goldmaxxxxing

openpilot/selfdrive/controls/controlsd.py

@@ -102,7 +102,6 @@ def state_control(self):
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
-    actuators.longControlState = self.LoC.long_control_state
 
     # Enable blinkers while lane changing
     if model_v2.meta.laneChangeState != LaneChangeState.off:
@@ -117,6 +116,7 @@ def state_control(self):
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
     actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
+    actuators.longControlState = self.LoC.long_control_state
 
     # Steering PID loop and lateral MPC
     # Reset desired curvature to current to avoid violating the limits on engage

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -6,6 +6,7 @@
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+TAKEOVER_ACCEL_JERK = 2.0  # m/s^3, only limits increasing accel on takeover
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -15,6 +16,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
   starting_condition = (not should_stop and
                         not cruise_standstill and
                         not brake_pressed)
+  use_starting_state = CP.startingState and v_ego <= CP.vEgoStarting
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -23,13 +25,13 @@ 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:
+      elif use_starting_state:
         long_control_state = LongCtrlState.starting
       else:
         long_control_state = LongCtrlState.pid
 
     elif long_control_state == LongCtrlState.stopping:
-      if starting_condition and CP.startingState:
+      if starting_condition and use_starting_state:
         long_control_state = LongCtrlState.starting
       elif starting_condition:
         long_control_state = LongCtrlState.pid
@@ -49,15 +51,18 @@ def __init__(self, CP):
                              (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
                              rate=1 / DT_CTRL)
     self.last_output_accel = 0.0
+    self.takeover_active = False
 
   def reset(self):
     self.pid.reset()
+    self.takeover_active = False
 
   def update(self, active, CS, a_target, should_stop, accel_limits):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
+    prev_long_control_state = self.long_control_state
     self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
@@ -78,8 +83,20 @@ def update(self, active, CS, a_target, should_stop, accel_limits):
 
     else:  # LongCtrlState.pid
       error = a_target - CS.aEgo
-      output_accel = self.pid.update(error, speed=CS.vEgo,
-                                     feedforward=a_target)
+      starting_takeover = prev_long_control_state == LongCtrlState.off
+      raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target,
+                                         freeze_integrator=starting_takeover or self.takeover_active)
+
+      if starting_takeover:
+        # Match measured acceleration when it is safer than the request, then
+        # smoothly add acceleration. Never delay a stronger decel request.
+        output_accel = min(raw_output_accel, CS.aEgo)
+        self.takeover_active = output_accel < raw_output_accel
+      elif self.takeover_active:
+        output_accel = min(raw_output_accel, self.last_output_accel + TAKEOVER_ACCEL_JERK * DT_CTRL)
+        self.takeover_active = output_accel < raw_output_accel
+      else:
+        output_accel = raw_output_accel
 
     self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -54,7 +54,7 @@
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 6.0
+STOP_DISTANCE = 3.0
 CRUISE_MIN_ACCEL = -1.2
 CRUISE_MAX_ACCEL = 1.6
 MIN_X_LEAD_FACTOR = 0.5

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,7 +1,8 @@
-from opendbc.car.structs import car
-from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
-
+import pytest
 
+from opendbc.car.structs import car
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.longcontrol import TAKEOVER_ACCEL_JERK, LongControl, LongCtrlState, long_control_state_trans
 
 
 class TestLongControlStateTransition:
@@ -54,3 +55,75 @@ def test_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
+
+  current_state = LongCtrlState.off
+  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 long_control_params(ki=0.0):
+  CP = car.CarParams.new_message()
+  CP.longitudinalTuning.kpBP = [0.]
+  CP.longitudinalTuning.kpV = [0.]
+  CP.longitudinalTuning.kiBP = [0.]
+  CP.longitudinalTuning.kiV = [ki]
+  return CP
+
+
+def run_control(long_control, CS, a_target, active=True):
+  return long_control.update(active, CS, a_target, should_stop=False, accel_limits=(-3.5, 2.0))
+
+
+def test_positive_takeover_is_smooth():
+  long_control = LongControl(long_control_params(ki=1.0))
+  CS = car.CarState.new_message(vEgo=10.0, aEgo=0.2)
+
+  assert run_control(long_control, CS, 1.0, active=False) == 0.0
+  assert run_control(long_control, CS, 1.0) == pytest.approx(CS.aEgo)
+  assert run_control(long_control, CS, 1.0) == pytest.approx(CS.aEgo + TAKEOVER_ACCEL_JERK * DT_CTRL)
+  assert long_control.pid.i == 0.0
+
+
+def test_takeover_does_not_delay_decel():
+  long_control = LongControl(long_control_params())
+  CS = car.CarState.new_message(vEgo=10.0, aEgo=0.2)
+
+  assert run_control(long_control, CS, -1.0) == pytest.approx(-1.0)
+  assert not long_control.takeover_active
+
+
+def test_decel_during_takeover_is_immediate():
+  long_control = LongControl(long_control_params())
+  CS = car.CarState.new_message(vEgo=10.0, aEgo=0.0)
+
+  assert run_control(long_control, CS, 1.0) == 0.0
+  assert run_control(long_control, CS, 1.0) == pytest.approx(TAKEOVER_ACCEL_JERK * DT_CTRL)
+  assert run_control(long_control, CS, -1.0) == pytest.approx(-1.0)
+  assert not long_control.takeover_active
+
+
+def test_standstill_starting_kick_is_unchanged():
+  CP = long_control_params()
+  CP.startingState = True
+  CP.startAccel = 1.0
+  CP.vEgoStarting = 0.1
+  long_control = LongControl(CP)
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0)
+
+  assert run_control(long_control, CS, 1.0) == CP.startAccel
+  assert long_control.long_control_state == LongCtrlState.starting
+
+
+def test_short_override_restarts_takeover():
+  long_control = LongControl(long_control_params())
+  CS = car.CarState.new_message(vEgo=10.0, aEgo=0.0)
+
+  assert run_control(long_control, CS, 0.5) == 0.0
+  assert run_control(long_control, CS, 0.5) == pytest.approx(TAKEOVER_ACCEL_JERK * DT_CTRL)
+  assert run_control(long_control, CS, 0.5, active=False) == 0.0
+  assert run_control(long_control, CS, 0.5) == 0.0
+  assert run_control(long_control, CS, 0.5) == pytest.approx(TAKEOVER_ACCEL_JERK * DT_CTRL)