diff --git a/opendbc_repo b/opendbc_repo index 78a1c9e73d..938043e24a 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 78a1c9e73de2e0a081cb2bb41f7f10281b9eeabf +Subproject commit 938043e24a0ff1a3a6f81a4c793822d4733f09ef diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 977add1285..07abd0d5d3 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -10,8 +10,7 @@ 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, - should_stop, brake_pressed, cruise_standstill): +def long_control_state_trans(active, long_control_state, should_stop, brake_pressed, cruise_standstill): starting_condition = (not should_stop and not cruise_standstill and not brake_pressed) @@ -23,22 +22,17 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, if long_control_state == LongCtrlState.off: if not starting_condition: long_control_state = LongCtrlState.stopping - elif CP.startingState: - long_control_state = LongCtrlState.starting else: long_control_state = LongCtrlState.pid elif long_control_state == LongCtrlState.stopping: - if starting_condition and CP.startingState: - long_control_state = LongCtrlState.starting - elif starting_condition: + if starting_condition: long_control_state = LongCtrlState.pid - elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]: + elif long_control_state == LongCtrlState.pid: if should_stop: long_control_state = LongCtrlState.stopping - elif v_ego > CP.vEgoStarting: - long_control_state = LongCtrlState.pid + return long_control_state class LongControl: @@ -58,9 +52,8 @@ 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, - should_stop, CS.brakePressed, - CS.cruiseState.standstill) + self.long_control_state = long_control_state_trans(active, self.long_control_state, should_stop, + CS.brakePressed, CS.cruiseState.standstill) if self.long_control_state == LongCtrlState.off: self.reset() output_accel = 0. @@ -72,10 +65,6 @@ class LongControl: output_accel -= self.CP.stoppingDecelRate * DT_CTRL self.reset() - elif self.long_control_state == LongCtrlState.starting: - output_accel = self.CP.startAccel - self.reset() - else: # LongCtrlState.pid error = a_target - CS.aEgo output_accel = self.pid.update(error, speed=CS.vEgo, diff --git a/openpilot/selfdrive/controls/tests/test_longcontrol.py b/openpilot/selfdrive/controls/tests/test_longcontrol.py index f69c54a3cd..2c52132068 100644 --- a/openpilot/selfdrive/controls/tests/test_longcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_longcontrol.py @@ -1,56 +1,40 @@ -from opendbc.car.structs import car from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans - - class TestLongControlStateTransition: def test_stay_stopped(self): - CP = car.CarParams.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(active, current_state, 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(active, current_state, 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(active, current_state, 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(active, current_state, 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(active, current_state, should_stop=False, brake_pressed=False, cruise_standstill=False) assert next_state == LongCtrlState.off - def test_engage(self): - CP = car.CarParams.new_message() - active = True - current_state = LongCtrlState.off - next_state = long_control_state_trans(CP, 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, - 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, - 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, - should_stop=False, brake_pressed=False, cruise_standstill=False) - assert next_state == LongCtrlState.pid - - def test_starting(self): - CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) - active = True - current_state = LongCtrlState.starting - next_state = long_control_state_trans(CP, 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, - should_stop=False, brake_pressed=False, cruise_standstill=False) - assert next_state == LongCtrlState.pid +def test_engage(): + active = True + current_state = LongCtrlState.off + next_state = long_control_state_trans(active, current_state, + should_stop=True, brake_pressed=False, cruise_standstill=False) + assert next_state == LongCtrlState.stopping + next_state = long_control_state_trans(active, current_state, + should_stop=False, brake_pressed=True, cruise_standstill=False) + assert next_state == LongCtrlState.stopping + next_state = long_control_state_trans(active, current_state, + should_stop=False, brake_pressed=False, cruise_standstill=True) + assert next_state == LongCtrlState.stopping + next_state = long_control_state_trans(active, current_state, + should_stop=False, brake_pressed=False, cruise_standstill=False) + assert next_state == LongCtrlState.pid