From dc9b652682a26d4a64f496fde2c00a9d1544c3e9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 14 Jun 2024 21:19:51 +0000 Subject: [PATCH 1/2] Sentry: Update fingerprinting events --- selfdrive/car/car_helpers.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index eec3b72081..5fcfe34a4f 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -219,7 +219,7 @@ def crash_log(candidate): while True: if is_connected_to_internet(): sentry.get_init() - sentry.capture_warning("fingerprinted %s" % candidate) + sentry.capture_info("fingerprinted %s" % candidate) break else: no_internet += 1 @@ -233,8 +233,7 @@ def crash_log2(fingerprints, fw): while True: if is_connected_to_internet(): sentry.get_init() - sentry.capture_warning("car doesn't match any fingerprints: %s" % fingerprints) - sentry.capture_warning("car doesn't match any fw: %s" % fw) + sentry.capture_warning("car doesn't match any fingerprints: %s" % repr(fingerprints)) break else: no_internet += 1 From e1ac25bdd81448cc067ff80dfafe88b38bfd1ada Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 16 Jun 2024 01:17:53 -0400 Subject: [PATCH 2/2] Custom Stock Longitudinal Control: GM Support This commit adds the custom cruise control logic in the GM interface and provides custom minimum cruise speed for GM. It also provides comprehensive updates in GM car controller to manage different scenarios in cruise control. Additionally, the update includes button control for cruise speed modification, maintaining a steady speed and considering curve speed hysteresis. Further, safety checks were implemented in the panda safety module for the GM to check cruise control actions. --- panda | 2 +- selfdrive/car/gm/carcontroller.py | 190 ++++++++++++++++++++++++ selfdrive/car/gm/interface.py | 3 +- selfdrive/controls/lib/drive_helpers.py | 8 + 4 files changed, 201 insertions(+), 2 deletions(-) diff --git a/panda b/panda index b1eaf46501..5b6fb2ac53 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit b1eaf465019be0b76d208a820face2a62a711a8f +Subproject commit 5b6fb2ac537554e65a0a1a0d7c97756d202a96a7 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