diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 766e1ad..b460a0d 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -122,6 +122,8 @@ class CarController: # Gas/regen, brakes, and UI commands - all at 25Hz if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping + at_full_stop = CC.longActive and CS.out.standstill + near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) interceptor_gas_cmd = 0 # Pitch compensated acceleration; @@ -133,7 +135,11 @@ class CarController: # ASCM sends max regen when not enabled self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = 0 + elif near_stop and stopping and not CC.cruiseControl.resume: + self.apply_gas = self.params.INACTIVE_REGEN + self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE)) else: + # Normal operation brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V) self.apply_gas = int(round(interp(accel if self.long_pitch else actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) self.apply_brake = int(round(interp(brake_accel if self.long_pitch else actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) @@ -145,10 +151,14 @@ class CarController: # gas interceptor only used for full long control on cars without ACC interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive) + if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill: + # "Tap" the accelerator pedal to re-engage ACC + interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS + self.apply_brake = 0 + self.apply_gas = self.params.INACTIVE_REGEN + idx = (self.frame // 4) % 4 - at_full_stop = CC.longActive and CS.out.standstill - near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) 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 @@ -163,6 +173,10 @@ class CarController: at_full_stop = at_full_stop and stopping friction_brake_bus = CanBus.POWERTRAIN + if self.CP.autoResumeSng: + resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume + at_full_stop = at_full_stop and not resume + # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f668f7d..bd66676 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -336,6 +336,11 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiV = [0.1, 0.1] ret.longitudinalTuning.kf = 0.15 ret.stoppingDecelRate = 0.8 + else: # Pedal used for SNG, ACC for longitudinal control otherwise + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG + ret.startingState = True + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 elif candidate in CC_ONLY_CAR: ret.flags |= GMFlags.CC_LONG.value @@ -391,7 +396,7 @@ class CarInterface(CarInterfaceBase): if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and (self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)): events.add(EventName.belowEngageSpeed) - if ret.cruiseState.standstill and not self.disable_resumeRequired: + if ret.cruiseState.standstill and not self.CP.autoResumeSng and not self.disable_resumeRequired: events.add(EventName.resumeRequired) self.resumeRequired_shown = True diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index a3cec03..da77af5 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -19,7 +19,9 @@ class CarControllerParams: STEER_DRIVER_ALLOWANCE = 65 STEER_DRIVER_MULTIPLIER = 4 STEER_DRIVER_FACTOR = 100 - NEAR_STOP_BRAKE_PHASE = 0.5 # m/s + NEAR_STOP_BRAKE_PHASE = 0.25 # m/s + SNG_INTERCEPTOR_GAS = 18. / 255. + SNG_TIME = 30 # frames until the above is reached # Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera" ADAS_KEEPALIVE_STEP = 100