diff --git a/cereal/custom.capnp b/cereal/custom.capnp index bbb3b1f1d8..a6b17d1a1f 100644 --- a/cereal/custom.capnp +++ b/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 { diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 7e30577897..2871882c99 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 = { diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py index c66c2c392a..2e09f4e155 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index f12a00f23e..491b179039 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/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. diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py index a4f0e6ddef..a8661b8575 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py +++ b/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() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py index fa17494d2e..904493a2ba 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py +++ b/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