mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-08-05 16:26:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Longitudinal: reimplement Gas Interceptor (comma Pedal) support (#1290)

Browse Source 
* 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
This commit is contained in:
Jason Wen
2025-09-30 22:37:13 -04:00
committed by GitHub
parent 49e58a2532
commit f3ed577870
8 changed files with 35 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/custom.capnp
+1
View File
@@ -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;
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 52d50f9841...dfcfacb667
selfdrive/car/tests/test_car_interfaces.py
+1 -1
View File
@@ -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':
selfdrive/controls/controlsd.py
+1 -1
View File
@@ -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:
selfdrive/controls/lib/longcontrol.py
+7 -3
View File
@@ -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:
selfdrive/controls/tests/test_longcontrol.py
+15 -12
View File
@@ -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
selfdrive/selfdrived/selfdrived.py
+1 -1
View File
@@ -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)
sunnypilot/selfdrive/car/car_specific.py
+8 -2
View File
@@ -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