From a0b09e7154d2b29583c772a116a15f83ccdd9bbe Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 13 Sep 2024 07:40:33 -0400 Subject: [PATCH] hyundai: decouple completely --- selfdrive/car/hyundai/carcontroller.py | 201 +------------------------ 1 file changed, 3 insertions(+), 198 deletions(-) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 013b253e0b..b251ce9801 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -7,9 +7,8 @@ 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, LEGACY_SAFETY_MODE_CAR +from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -62,38 +61,10 @@ class CarController(CarControllerBase): self.lat_disengage_init = False self.lat_active_last = False - sub_services = ['longitudinalPlanSP'] if CP.openpilotLongitudinalControl: - sub_services.append('radarState') - # TODO: Always true, prep for future conditional refactoring - if sub_services: - self.sm = messaging.SubMaster(sub_services) + self.sm = messaging.SubMaster(['radarState']) 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 @@ -109,21 +80,9 @@ class CarController(CarControllerBase): return 19 if hud_control.leadVisible else 0 def update(self, CC, CS, now_nanos): - if not self.CP.pcmCruiseSpeed or (self.CP.openpilotLongitudinalControl and self.frame % 5 == 0): + if 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 @@ -165,19 +124,6 @@ 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 @@ -232,15 +178,6 @@ 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, @@ -250,21 +187,6 @@ 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: @@ -336,120 +258,3 @@ 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