overriding state

cereal/custom.capnp

@@ -205,6 +205,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     pending @3; # Awaiting new speed limit.
     adapting @4; # Reducing speed to match new speed limit.
     active @5; # Cruising at speed limit.
+    overriding @6; # System overriding with manual control.
   }
 
   enum SpeedLimitSource {

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -42,13 +42,16 @@ class LongitudinalPlannerSP:
     return self.dec.mode()
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
     self.events_sp.clear()
 
-    self.scc.update(sm, v_ego, a_ego, v_cruise)
+    self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
 
     # Speed Limit Assist
     self.resolver.update(v_ego, sm)
-    v_cruise_sla = self.sla.update(sm['carControl'].longActive, v_ego, a_ego, sm['carState'].vCruiseCluster,
+    v_cruise_sla = self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster,
                                    self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp)
 
     targets = {

sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py

@@ -12,8 +12,5 @@ class SmartCruiseControl:
   def __init__(self):
     self.vision = SmartCruiseControlVision()
 
-  def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> None:
-    long_enabled = sm['carControl'].enabled
-    long_override = sm['carControl'].cruiseControl.override
-
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
     self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -102,7 +102,7 @@ class SmartCruiseControlVision:
       self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
 
   def _update_state_machine(self) -> tuple[bool, bool]:
-    # ENABLED, ENTERING, TURNING, LEAVING
+    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
     if self.state != VisionState.disabled:
       # longitudinal and feature disable always have priority in a non-disabled state
       if not self.long_enabled or not self.enabled:
@@ -163,7 +163,7 @@ class SmartCruiseControlVision:
     return enabled, active
 
   def _update_solution(self) -> float:
-    # DISABLED, ENABLED
+    # DISABLED, ENABLED, OVERRIDING
     if self.state not in ACTIVE_STATES:
       # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
       # the smooth deceleration.

sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py

@@ -22,7 +22,7 @@ EventNameSP = custom.OnroadEventSP.EventName
 SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
 
 ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
-ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, SpeedLimitAssistState.overriding, *ACTIVE_STATES)
 
 
 class SpeedLimitAssist:
@@ -42,8 +42,9 @@ class SpeedLimitAssist:
     self.pre_active_timer = 0
     self.is_metric = self.params.get_bool("IsMetric")
     self.enabled = self.params.get_bool("SpeedLimitAssist")
-    self.op_engaged = False
-    self.op_engaged_prev = False
+    self.long_enabled = False
+    self.long_enabled_prev = False
+    self.long_override = False
     self.is_enabled = False
     self.is_active = False
     self.v_ego = 0.
@@ -156,11 +157,13 @@ class SpeedLimitAssist:
     self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
     self.pre_active_timer = max(0, self.pre_active_timer - 1)
 
-    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
+    # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE, OVERRIDING
     if self.state != SpeedLimitAssistState.disabled:
-      if not self.op_engaged or not self.enabled:
+      if not self.long_enabled or not self.enabled:
         self.state = SpeedLimitAssistState.disabled
         self.initial_max_set = False
+      elif self.long_override:
+        self.state = SpeedLimitAssistState.overriding
 
       else:
         # ACTIVE
@@ -200,14 +203,22 @@ class SpeedLimitAssist:
             # Timeout - session ended
             self.state = SpeedLimitAssistState.inactive
 
+        # OVERRIDING
+        elif self.state == SpeedLimitAssistState.overriding:
+          if not self.long_override:
+            self.state = SpeedLimitAssistState.preActive
+
         # INACTIVE
         elif self.state == SpeedLimitAssistState.inactive:
           pass
 
     # DISABLED
     elif self.state == SpeedLimitAssistState.disabled:
-      if self.op_engaged and self.enabled:
-        if not self.op_engaged_prev:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = SpeedLimitAssistState.overriding
+
+        elif not self.long_enabled_prev:
           self.pre_active_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
 
         elif self.pre_active_timer <= 0:
@@ -229,9 +240,10 @@ class SpeedLimitAssist:
       elif self.speed_limit_changed:
         events_sp.add(EventNameSP.speedLimitChanged)
 
-  def update(self, long_active: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float,
+  def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float,
              speed_limit: float, distance: float, source: custom.LongitudinalPlanSP.SpeedLimitSource, events_sp: EventsSP) -> float:
-    self.op_engaged = long_active
+    self.long_enabled = long_enabled
+    self.long_override = long_override
     self.v_ego = v_ego
     self.a_ego = a_ego
 
@@ -247,7 +259,7 @@ class SpeedLimitAssist:
     # Update change tracking variables
     self.speed_limit_prev = self._speed_limit
     self.v_cruise_setpoint_prev = self.v_cruise_setpoint
-    self.op_engaged_prev = self.op_engaged
+    self.long_enabled_prev = self.long_enabled
     self.frame += 1
 
     v_target = self.get_v_target_from_control()

sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py

@@ -90,58 +90,58 @@ class TestSpeedLimitAssist:
   def test_disabled(self):
     self.params.put_bool("SpeedLimitAssist", False)
     for _ in range(int(10. / DT_MDL)):
-      _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.disabled
 
   def test_transition_disabled_to_preactive(self):
     for _ in range(int(3. / DT_MDL)):
-      _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.preActive
     assert self.sla.is_enabled and not self.sla.is_active
 
   def test_preactive_to_active_with_max_speed_confirmation(self):
     self.sla.state = SpeedLimitAssistState.preActive
-    v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.active
-    assert self.sla.is_enabled and self.sla.is_active
+    assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}"
     assert v_cruise_sla == SPEED_LIMITS['city']
 
   def test_preactive_timeout_to_inactive(self):
     self.sla.state = SpeedLimitAssistState.preActive
-    _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
 
     for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
-      _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.inactive
 
   def test_preactive_to_pending_no_speed_limit(self):
     self.sla.state = SpeedLimitAssistState.preActive
-    _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.pending
     assert self.sla.is_enabled and not self.sla.is_active
 
   def test_pending_to_active_when_speed_limit_available(self):
     self.sla.state = SpeedLimitAssistState.pending
-    _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.active
 
   def test_pending_to_adapting_when_below_speed_limit(self):
     self.sla.state = SpeedLimitAssistState.pending
-    _ = self.sla.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.adapting
     assert self.sla.is_enabled and self.sla.is_active
 
   def test_active_to_adapting_transition(self):
     self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
 
-    _ = self.sla.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.adapting
 
   def test_adapting_to_active_transition(self):
     self.sla.state = SpeedLimitAssistState.adapting
     self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
 
-    _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.active
 
   def test_manual_cruise_change_detection(self):
@@ -150,7 +150,7 @@ class TestSpeedLimitAssist:
     self.sla.v_cruise_setpoint_prev = expected_cruise
 
     different_cruise = SPEED_LIMITS['highway'] + 5
-    _ = self.sla.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+    _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.inactive
 
   @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [
@@ -170,7 +170,7 @@ class TestSpeedLimitAssist:
     speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']]
 
     for _, speed_limit in enumerate(speed_limits):
-      _ = self.sla.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp)
+      _ = self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state in ACTIVE_STATES
 
   def test_invalid_speed_limits_handling(self):
@@ -180,7 +180,7 @@ class TestSpeedLimitAssist:
     invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
 
     for invalid_limit in invalid_limits:
-      v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp)
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp)
       assert isinstance(v_cruise_sla, (int, float))
       assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0
 
@@ -189,7 +189,7 @@ class TestSpeedLimitAssist:
     old_speed_limit = SPEED_LIMITS['city']
     self.sla.last_valid_speed_limit_final = old_speed_limit
 
-    v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp)
+    v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state in ACTIVE_STATES
     assert v_cruise_sla == old_speed_limit
 
@@ -197,7 +197,7 @@ class TestSpeedLimitAssist:
     self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
 
     for source in (SpeedLimitSource.car, SpeedLimitSource.map):
-      v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp)
+      v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp)
       assert v_cruise_sla != V_CRUISE_UNSET
 
   def test_distance_based_adapting(self):
@@ -208,14 +208,14 @@ class TestSpeedLimitAssist:
     current_speed = SPEED_LIMITS['highway']
     target_speed = SPEED_LIMITS['city']
 
-    v_cruise_sla = self.sla.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp)
+    v_cruise_sla = self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.adapting
     assert v_cruise_sla == target_speed  # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
 
   def test_long_disengaged_to_disabled(self):
     self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
 
-    v_cruise_sla = self.sla.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'],
+    v_cruise_sla = self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'],
                                    0, SpeedLimitSource.car, self.events_sp)
     assert self.sla.state == SpeedLimitAssistState.disabled
     assert v_cruise_sla == V_CRUISE_UNSET
@@ -237,7 +237,7 @@ class TestSpeedLimitAssist:
 
       initial_state = state
 
-      _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+      _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
 
       assert self.sla.state in ALL_STATES  # Sanity check