From f3ed57787050c330ca74c566dad2d7812753e7ba Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 30 Sep 2025 22:37:13 -0400 Subject: [PATCH] Longitudinal: reimplement Gas Interceptor (comma Pedal) support (#1290) * init * bump * bump * bump * recheck openpilot long availability * bump * bump * bump * bump * bump * just base for now * bump * bump * bump * bump * flipped * apparently it's yucky, reverting most * bump * need to add for toyota * should've been remove * flipped * bump * no way * fix * test sdsu distance * final tuning for pedal * bump --- cereal/custom.capnp | 1 + opendbc_repo | 2 +- selfdrive/car/tests/test_car_interfaces.py | 2 +- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/longcontrol.py | 10 +++++--- selfdrive/controls/tests/test_longcontrol.py | 27 +++++++++++--------- selfdrive/selfdrived/selfdrived.py | 2 +- sunnypilot/selfdrive/car/car_specific.py | 10 ++++++-- 8 files changed, 35 insertions(+), 21 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index c87ac0d2e..674f109fc 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -299,6 +299,7 @@ struct CarParamsSP @0x80ae746ee2596b11 { safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags pcmCruiseSpeed @3 :Bool; intelligentCruiseButtonManagementAvailable @4 :Bool; + enableGasInterceptor @5 :Bool; neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; diff --git a/opendbc_repo b/opendbc_repo index 52d50f984..dfcfacb66 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 52d50f984191d63cf6a79398b21df2d2e3b9236e +Subproject commit dfcfacb667c6ff131d0e620c36e118f14a84dc4a diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 7c7a56300..821767ee4 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -60,7 +60,7 @@ class TestCarInterfaces: # Test controller initialization # TODO: wait until card refactor is merged to run controller a few times, # hypothesis also slows down significantly with just one more message draw - LongControl(car_params) + LongControl(car_params, car_params_sp) if car_params.steerControlType == CarParams.SteerControlType.angle: LatControlAngle(car_params, car_params_sp, car_interface) elif car_params.lateralTuning.which() == 'pid': diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3751efc87..3601ae310 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -58,7 +58,7 @@ class Controls(ControlsExt, ModelStateBase): self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None - self.LoC = LongControl(self.CP) + self.LoC = LongControl(self.CP, self.CP_SP) self.VM = VehicleModel(self.CP) self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6d4f92246..b8b00e2fb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -10,8 +10,11 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] LongCtrlState = car.CarControl.Actuators.LongControlState -def long_control_state_trans(CP, active, long_control_state, v_ego, +def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego, should_stop, brake_pressed, cruise_standstill): + # Gas Interceptor + cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor + stopping_condition = should_stop starting_condition = (not should_stop and not cruise_standstill and @@ -45,8 +48,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, return long_control_state class LongControl: - def __init__(self, CP): + def __init__(self, CP, CP_SP): self.CP = CP + self.CP_SP = CP_SP self.long_control_state = LongCtrlState.off self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), @@ -61,7 +65,7 @@ class LongControl: self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, + self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, CS.cruiseState.standstill) if self.long_control_state == LongCtrlState.off: diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index ab50810d8..cf0ab24e0 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, custom from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans @@ -8,49 +8,52 @@ class TestLongControlStateTransition: def test_stay_stopped(self): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid active = False - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.off def test_engage(): CP = car.CarParams.new_message() + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.off - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=True, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=True, cruise_standstill=False) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=True) assert next_state == LongCtrlState.stopping - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid def test_starting(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP_SP = custom.CarParamsSP.new_message() active = True current_state = LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.starting - next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, + next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.pid diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index a13c873fb..4bc75021f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -159,7 +159,7 @@ class SelfdriveD(CruiseHelper): self.mads = ModularAssistiveDrivingSystem(self) self.icbm = IntelligentCruiseButtonManagement(self.CP, self.CP_SP) - self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + self.car_events_sp = CarSpecificEventsSP(self.CP, self.CP_SP) CruiseHelper.__init__(self, self.CP) diff --git a/sunnypilot/selfdrive/car/car_specific.py b/sunnypilot/selfdrive/car/car_specific.py index 7e6f300d7..97ce26429 100644 --- a/sunnypilot/selfdrive/car/car_specific.py +++ b/sunnypilot/selfdrive/car/car_specific.py @@ -18,9 +18,9 @@ GearShifter = structs.CarState.GearShifter class CarSpecificEventsSP: - def __init__(self, CP: structs.CarParams, params): + def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP): self.CP = CP - self.params = params + self.CP_SP = CP_SP self.low_speed_alert = False @@ -42,4 +42,10 @@ class CarSpecificEventsSP: if self.low_speed_alert: events.add(EventName.belowSteerSpeed) + elif self.CP.brand == 'toyota': + if self.CP.openpilotLongitudinalControl: + if CS.cruiseState.standstill and not CS.brakePressed and self.CP_SP.enableGasInterceptor: + if events.has(EventName.resumeRequired): + events.remove(EventName.resumeRequired) + return events_sp