diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index c18d85c50e..013b253e0b 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -7,9 +7,9 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus -from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR, LEGACY_SAFETY_MODE_CAR from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.sunnypilot.custom_stock_longitudinal_controller.car.hyundai.controller import CustomStockLongitudinalController +from openpilot.selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -62,16 +62,41 @@ class CarController(CarControllerBase): self.lat_disengage_init = False self.lat_active_last = False + sub_services = ['longitudinalPlanSP'] if CP.openpilotLongitudinalControl: - self.sm = messaging.SubMaster(['radarState']) + sub_services.append('radarState') + # TODO: Always true, prep for future conditional refactoring + if sub_services: + self.sm = messaging.SubMaster(sub_services) self.param_s = Params() + self.last_speed_limit_sign_tap_prev = False + self.speed_limit = 0. + self.speed_limit_offset = 0 + self.timer = 0 + self.final_speed_kph = 0 + self.init_speed = 0 + self.current_speed = 0 + self.v_set_dis = 0 + self.v_cruise_min = 0 + self.button_type = 0 + self.button_select = 0 + self.button_count = 0 + self.target_speed = 0 + self.t_interval = 7 + self.slc_active_stock = False + self.sl_force_active_timer = 0 + self.v_tsc_state = 0 + self.slc_state = 0 + self.m_tsc_state = 0 + self.cruise_button = None + self.speed_diff = 0 + self.v_tsc = 0 + self.m_tsc = 0 + self.steady_speed = 0 self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop") self.lead_distance = 0 - if not CP.pcmCruiseSpeed: - self.custom_stock_longitudinal_controller = CustomStockLongitudinalController(self, CP) - def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float: lead_one = self.sm["radarState"].leadOne lead_two = self.sm["radarState"].leadTwo @@ -84,11 +109,21 @@ class CarController(CarControllerBase): return 19 if hud_control.leadVisible else 0 def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed: - self.custom_stock_longitudinal_controller.update(CS) - elif self.CP.openpilotLongitudinalControl: + if not self.CP.pcmCruiseSpeed or (self.CP.openpilotLongitudinalControl and self.frame % 5 == 0): self.sm.update(0) + if not self.CP.pcmCruiseSpeed: + if self.sm.updated['longitudinalPlanSP']: + self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState + self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState + self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState + self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit + self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset + self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed + self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed + + self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1) + actuators = CC.actuators hud_control = CC.hudControl @@ -130,6 +165,19 @@ class CarController(CarControllerBase): blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False + if not self.CP.pcmCruiseSpeed: + if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap: + self.sl_force_active_timer = self.frame + self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False) + self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap + + sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) + sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) + sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False)) + slc_active = not sl_inactive and not sl_temp_inactive + + self.slc_active_stock = slc_active + self.lat_active_last = CC.latActive escc = self.CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC.value @@ -184,6 +232,15 @@ class CarController(CarControllerBase): else: # button presses can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False)) + if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed: + if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS: + # TODO: resume for alt button cars + pass + else: + self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) + if self.cruise_button is not None: + if self.frame % 2 == 0: + can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button)) else: can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, @@ -193,6 +250,21 @@ class CarController(CarControllerBase): if not self.CP.openpilotLongitudinalControl: can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True)) + if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed: + self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) + if self.cruise_button is not None: + if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR: + send_freq = 1 + if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2: + send_freq = 5 + # send resume at a max freq of 10Hz + if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq: + # send 25 messages at a time to increases the likelihood of cruise buttons being accepted + can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP)] * 25) + if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq: + self.last_button_frame = self.frame + elif self.frame % 2 == 0: + can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25) # Parse lead distance from radarState and display the corresponding distance in the car's cluster if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0: @@ -221,9 +293,6 @@ class CarController(CarControllerBase): if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl and not escc: can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) - if not self.CP.pcmCruiseSpeed: - can_sends.extend(self.custom_stock_longitudinal_controller.create_mock_button_messages(CS, CC)) - new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer @@ -267,3 +336,120 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame return can_sends + + # multikyd methods, sunnyhaibin logic + def get_cruise_buttons_status(self, CS): + if not CS.out.cruiseState.enabled or CS.cruise_buttons[-1] != Buttons.NONE: + self.timer = 40 + elif self.timer: + self.timer -= 1 + else: + return 1 + return 0 + + def get_target_speed(self, v_cruise_kph_prev): + v_cruise_kph = v_cruise_kph_prev + if self.slc_state > 1: + v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH + if not self.slc_active_stock: + v_cruise_kph = v_cruise_kph_prev + return v_cruise_kph + + def get_button_type(self, button_type): + self.type_status = "type_" + str(button_type) + self.button_picker = getattr(self, self.type_status, lambda: "default") + return self.button_picker() + + def reset_button(self): + if self.button_type != 3: + self.button_type = 0 + + def type_default(self): + self.button_type = 0 + return None + + def type_0(self): + self.button_count = 0 + self.target_speed = self.init_speed + self.speed_diff = self.target_speed - self.v_set_dis + if self.target_speed > self.v_set_dis: + self.button_type = 1 + elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min: + self.button_type = 2 + return None + + def type_1(self): + cruise_button = Buttons.RES_ACCEL + self.button_count += 1 + if self.target_speed <= self.v_set_dis: + self.button_count = 0 + self.button_type = 3 + elif self.button_count > 5: + self.button_count = 0 + self.button_type = 3 + return cruise_button + + def type_2(self): + cruise_button = Buttons.SET_DECEL + self.button_count += 1 + if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min: + self.button_count = 0 + self.button_type = 3 + elif self.button_count > 5: + self.button_count = 0 + self.button_type = 3 + return cruise_button + + def type_3(self): + cruise_button = None + self.button_count += 1 + if self.button_count > self.t_interval: + self.button_type = 0 + return cruise_button + + def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev): + if self.v_tsc_state != 0: + vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH + if int(vision_v_cruise_kph) == int(v_cruise_kph_prev): + vision_v_cruise_kph = 255 + else: + vision_v_cruise_kph = 255 + if self.m_tsc_state > 1: + map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH + if int(map_v_cruise_kph) == 0.0: + map_v_cruise_kph = 255 + else: + map_v_cruise_kph = 255 + curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH) + return min(target_speed_kph, curve_speed) + + def get_button_control(self, CS, final_speed, v_cruise_kph_prev): + self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)) + self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH)) + cruise_button = self.get_button_type(self.button_type) + return cruise_button + + def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)): + if cur_speed > self.steady_speed: + self.steady_speed = cur_speed + elif cur_speed < self.steady_speed - hyst: + self.steady_speed = cur_speed + return self.steady_speed + + def get_cruise_buttons(self, CS, v_cruise_kph_prev): + cruise_button = None + if not self.get_cruise_buttons_status(CS): + pass + elif CS.out.cruiseState.enabled: + set_speed_kph = self.get_target_speed(v_cruise_kph_prev) + if self.slc_state > 1: + target_speed_kph = set_speed_kph + else: + target_speed_kph = min(v_cruise_kph_prev, set_speed_kph) + if self.v_tsc_state != 0 or self.m_tsc_state > 1: + self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev) + else: + self.final_speed_kph = target_speed_kph + + cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses + return cruise_button