diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index ef9a32789..86a257925 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -304,6 +304,9 @@ class CarController(CarControllerBase): else: apply_steer = 0 + if (self.CP.flags & GMFlags.CC_LONG.value) and CC.enabled and not CS.out.cruiseState.enabled: # Send 0 so Panda doesn't error + apply_steer = 0 + # shift previous steer timestamp self.prev_steer_ts_ns = self.last_steer_ts_ns self.last_steer_ts_ns = now_nanos @@ -371,7 +374,9 @@ class CarController(CarControllerBase): if self.CP.flags & GMFlags.CC_LONG.value: if CC.longActive and CS.out.vEgo > self.CP.minEnableSpeed: # Using extend instead of append since the message is only sent intermittently - can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators)) + can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, frogpilot_toggles)) + elif CC.enabled and self.frame % 52 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise: + can_sends.extend(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET)) if self.CP.enableGasInterceptor: can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx)) if self.CP.carFingerprint not in CC_ONLY_CAR: diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 3bbf2969b..254736996 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -205,44 +205,41 @@ def create_regen_paddle_command(packer, bus, press_regen_paddle): } return packer.make_can_msg("EBCMRegenPaddle", bus, values) -def create_gm_cc_spam_command(packer, controller, CS, actuators): - if controller.params_.get_bool("IsMetric"): - _CV = CV.MS_TO_KPH - RATE_UP_MAX = 0.04 - RATE_DOWN_MAX = 0.04 - else: - _CV = CV.MS_TO_MPH - RATE_UP_MAX = 0.2 - RATE_DOWN_MAX = 0.2 - - accel = actuators.accel * _CV # m/s/s to mph/s - speedSetPoint = int(round(CS.out.cruiseState.speed * _CV)) +def create_gm_cc_spam_command(packer, controller, CS, actuators, frogpilot_toggles): + accel = actuators.accel + Vego = CS.out.vEgo cruiseBtn = CruiseButtons.INIT - if speedSetPoint == CS.CP.minEnableSpeed and accel < -1: + if abs(accel) <= 0.15: + rate = 1 + else: + rate = 0.2 + + MS_CONVERT = CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH + + speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT)) + + if accel > 0: + DesiredSetPoint = int(round((Vego * 1.01 + 3 * accel) * MS_CONVERT)) # 1.01 factor to match cluster speed better + else: # accel <= 0 + DesiredSetPoint = int(round((Vego * 1.01 + 3 * accel) * MS_CONVERT)) + + if CS.CP.minEnableSpeed - (DesiredSetPoint / MS_CONVERT) > 3.25: cruiseBtn = CruiseButtons.CANCEL controller.apply_speed = 0 - rate = 0.04 - elif accel < 0: + elif DesiredSetPoint < speedSetPoint and speedSetPoint > CS.CP.minEnableSpeed * MS_CONVERT + 1: cruiseBtn = CruiseButtons.DECEL_SET - if speedSetPoint > (CS.out.vEgo * _CV) + 3.0: # If accel is changing directions, bring set speed to current speed as fast as possible - rate = RATE_DOWN_MAX - else: - rate = max(-1 / accel, RATE_DOWN_MAX) controller.apply_speed = speedSetPoint - 1 - elif accel > 0: + elif DesiredSetPoint > speedSetPoint: cruiseBtn = CruiseButtons.RES_ACCEL - if speedSetPoint < (CS.out.vEgo * _CV) - 3.0: - rate = RATE_UP_MAX - else: - rate = max(1 / accel, RATE_UP_MAX) controller.apply_speed = speedSetPoint + 1 else: + cruiseBtn = CruiseButtons.INIT controller.apply_speed = speedSetPoint - rate = float('inf') # Check rlogs closely - our message shouldn't show up on the pt bus for us # Or bus 2, since we're forwarding... but I think it does + # TODO: Cleanup the timing - normal is every 30ms... if (cruiseBtn != CruiseButtons.INIT) and ((controller.frame - controller.last_button_frame) * DT_CTRL > rate): controller.last_button_frame = controller.frame idx = (CS.buttons_counter + 1) % 4 # Need to predict the next idx for '22-23 EUV diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 2d8e846ef..e9fc7db59 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -296,9 +296,9 @@ class CarInterface(CarInterfaceBase): if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph - ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed + ret.longitudinalTuning.kpV = [0., 5., 2.] # set lower end to 0 since we can't drive below that speed ret.longitudinalTuning.deadzoneBP = [0., 1.] - ret.longitudinalTuning.deadzoneV = [0.56, 0.56] # == 2 km/h/s, 1.25 mph/s + ret.longitudinalTuning.deadzoneV = [0.9, 0.9] # == 2 km/h/s, 1.25 mph/s ret.longitudinalActuatorDelay = 1. # TODO: measure this ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.1] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0911de46c..dba18a9f0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -205,6 +205,8 @@ class Controls: self.frogpilot_toggles = get_frogpilot_toggles() + self.frogpilot_toggles.is_metric = self.is_metric + def set_initial_state(self): if REPLAY: controls_state = self.params.get("ReplayControlsState")