diff --git a/panda b/panda index e7e91de5f5..31cfc3cdbb 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e7e91de5f5bca290c30395511a8b3223d1d1ec26 +Subproject commit 31cfc3cdbb7d1d2f220eac6e9f49604c25917bf2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 2aa4811e4e..9f5c8ed9a8 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,4 +1,6 @@ from cereal import car +import cereal.messaging as messaging +from openpilot.common.params import Params from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_CTRL @@ -7,6 +9,7 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons from openpilot.selfdrive.car.interfaces import CarControllerBase +from selfdrive.controls.lib.drive_helpers import GM_V_CRUISE_MIN VisualAlert = car.CarControl.HUDControl.VisualAlert NetworkLocation = car.CarParams.NetworkLocation @@ -39,6 +42,36 @@ class CarController(CarControllerBase): self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + self.sm = messaging.SubMaster(['longitudinalPlan']) + self.param_s = Params() + self.is_metric = self.param_s.get_bool("IsMetric") + self.speed_limit_control_enabled = False + self.last_speed_limit_sign_tap = False + 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 + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -47,9 +80,40 @@ class CarController(CarControllerBase): if hud_v_cruise > 70: hud_v_cruise = 0 + if not self.CP.pcmCruiseSpeed: + self.sm.update(0) + + 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 + + if self.frame % 200 == 0: + self.speed_limit_control_enabled = self.param_s.get_bool("EnableSlc") + self.is_metric = self.param_s.get_bool("IsMetric") + self.last_speed_limit_sign_tap = self.param_s.get_bool("LastSpeedLimitSignTap") + self.v_cruise_min = GM_V_CRUISE_MIN[self.is_metric] * (CV.KPH_TO_MPH if not self.is_metric else 1) + # Send CAN commands. can_sends = [] + if not self.CP.pcmCruiseSpeed: + if not self.last_speed_limit_sign_tap_prev and self.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 = self.last_speed_limit_sign_tap + + sl_force_active = self.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0)) + sl_inactive = not sl_force_active and (not self.speed_limit_control_enabled or (True if self.slc_state == 0 else False)) + sl_temp_inactive = not sl_force_active and (self.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 + # Steering (Active: 50Hz, inactive: 10Hz) steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP @@ -150,6 +214,15 @@ class CarController(CarControllerBase): self.last_button_frame = self.frame can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL)) + if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and not self.CP.pcmCruiseSpeed and CS.out.cruiseState.enabled: + self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise) + if self.cruise_button is not None: + 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 = 3 + if self.frame % 12 < 6: # thanks to mochi86420 for the magic numbers + can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, (CS.button_counter + 2) % 4, self.cruise_button)) + if self.CP.networkLocation == NetworkLocation.fwdCamera: # Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1 if self.frame % 10 == 0: @@ -163,3 +236,120 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + + # multikyd methods, sunnyhaibin logic + def get_cruise_buttons_status(self, CS): + if not CS.out.cruiseState.enabled or CS.cruise_buttons != CruiseButtons.UNPRESS or CS.cruise_buttons != CruiseButtons.INIT: + 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 = CruiseButtons.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 = CruiseButtons.DECEL_SET + 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 self.is_metric else 1)) + self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not self.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 diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 465109eb4c..f68b31a291 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -88,6 +88,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] + ret.customStockLongAvailable = True if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct @@ -239,7 +240,7 @@ class CarInterface(CarInterfaceBase): else: self.CS.madsEnabled = False - if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0): + if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed: if any(b.type == ButtonType.cancel for b in buttonEvents): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) if self.get_sp_pedal_disengage(ret): diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 61edf7aade..89d165107b 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -65,6 +65,10 @@ VOLKSWAGEN_V_CRUISE_MIN = { True: 30, False: int(20 * CV.MPH_TO_KPH), } +GM_V_CRUISE_MIN = { + True: 30, + False: int(20 * CV.MPH_TO_KPH), +} SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState @@ -202,6 +206,8 @@ class VCruiseHelper: initial = MAZDA_V_CRUISE_MIN[is_metric] elif self.CP.carName == "volkswagen": initial = VOLKSWAGEN_V_CRUISE_MIN[is_metric] + elif self.CP.carName == "gm": + initial = GM_V_CRUISE_MIN[is_metric] # 250kph or above probably means we never had a set speed if any(b.type in resume_buttons for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: @@ -234,6 +240,8 @@ class VCruiseHelper: self.v_cruise_min = MAZDA_V_CRUISE_MIN[is_metric] elif self.CP.carName == "volkswagen": self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[is_metric] + elif self.CP.carName == "gm": + self.v_cruise_min = GM_V_CRUISE_MIN[is_metric] self.is_metric_prev = is_metric