mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-29 08:33:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

use v cruise cluster instead

Browse Source
This commit is contained in:
Jason Wen
2025-09-20 03:20:19 -04:00
parent a7a231ed30
commit 6edce28024
3 changed files with 24 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+6 -1
View File
@@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details.
from cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.constants import CV
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -41,6 +43,9 @@ 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]:
v_cruise_cluster_kph = min(sm['carState'].vCruiseCluster, V_CRUISE_MAX)
v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
long_enabled = sm['carControl'].enabled
long_override = sm['carControl'].cruiseControl.override
@@ -53,7 +58,7 @@ class LongitudinalPlannerSP:
self.resolver.update(v_ego, sm)
# Speed Limit Assist
self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster,
self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster,
self.resolver.speed_limit, self.resolver.speed_limit_offset, self.resolver.distance, self.events_sp)
targets = {
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+12 -20
View File
@@ -64,8 +64,8 @@ class SpeedLimitAssist:
self.v_ego = 0.
self.a_ego = 0.
self.v_offset = 0.
self.v_cruise_setpoint = 0.
self.v_cruise_setpoint_prev = 0.
self.v_cruise_cluster = 0.
self.v_cruise_cluster_prev = 0.
self.initial_max_set = False
self._speed_limit = 0.
self._speed_limit_offset = 0.
@@ -95,8 +95,8 @@ class SpeedLimitAssist:
return bool(self._speed_limit != self.speed_limit_prev)
@property
def v_cruise_setpoint_changed(self) -> bool:
return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev)
def v_cruise_cluster_changed(self) -> bool:
return bool(self.v_cruise_cluster != self.v_cruise_cluster_prev)
def get_v_target_from_control(self) -> float:
if self.is_enabled:
@@ -118,18 +118,10 @@ class SpeedLimitAssist:
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
def initial_max_set_confirmed(self) -> bool:
return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
return bool(abs(self.v_cruise_cluster - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
def detect_manual_cruise_change(self) -> bool:
# If cruise speed changed and it's not what SLA would set
if self.v_cruise_setpoint_changed:
expected_cruise = self.speed_limit_final
return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE)
return False
def update_calculations(self, v_cruise_setpoint: float) -> None:
self.v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
def update_calculations(self, v_cruise_cluster: float) -> None:
self.v_cruise_cluster = v_cruise_cluster if not np.isnan(v_cruise_cluster) else 0.0
# Update current velocity offset (error)
self.v_offset = self.speed_limit_final - self.v_ego
@@ -163,14 +155,14 @@ class SpeedLimitAssist:
else:
# ACTIVE
if self.state == SpeedLimitAssistState.active:
if self.detect_manual_cruise_change():
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
# ADAPTING
elif self.state == SpeedLimitAssistState.adapting:
if self.detect_manual_cruise_change():
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.active
@@ -235,7 +227,7 @@ class SpeedLimitAssist:
elif self.speed_limit_changed:
events_sp.add(EventNameSP.speedLimitChanged)
def update(self, long_enabled: bool, long_override: 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_cluster: float,
speed_limit: float, speed_limit_offset: float, distance: float, events_sp: EventsSP) -> None:
self.long_enabled = long_enabled
self.long_override = long_override
@@ -247,13 +239,13 @@ class SpeedLimitAssist:
self._distance = distance
self.update_params()
self.update_calculations(v_cruise_setpoint)
self.update_calculations(v_cruise_cluster)
self.is_enabled, self.is_active = self.update_state_machine()
self.update_events(events_sp)
# Update change tracking variables
self.speed_limit_prev = self._speed_limit
self.v_cruise_setpoint_prev = self.v_cruise_setpoint
self.v_cruise_cluster_prev = self.v_cruise_cluster
self.long_enabled_prev = self.long_enabled
self.output_v_target = self.get_v_target_from_control()
sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py
+6 -8
View File
@@ -68,14 +68,12 @@ class TestSpeedLimitAssist:
self.sla.speed_limit_prev = 0.
self.sla.last_valid_speed_limit_offsetted = 0.
self.sla._distance = 0.
self.sla.v_cruise_setpoint = 0.
self.sla.v_cruise_setpoint_prev = 0.
self.events_sp.clear()
def initialize_active_state(self, v_cruise_setpoint):
def initialize_active_state(self, initialize_v_cruise):
self.sla.state = SpeedLimitAssistState.active
self.sla.v_cruise_setpoint = v_cruise_setpoint
self.sla.v_cruise_setpoint_prev = v_cruise_setpoint
self.sla.v_cruise_cluster = initialize_v_cruise
self.sla.v_cruise_cluster_prev = initialize_v_cruise
def test_initial_state(self):
assert self.sla.state == SpeedLimitAssistState.disabled
@@ -135,7 +133,7 @@ class TestSpeedLimitAssist:
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.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED
self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.active
@@ -143,7 +141,7 @@ class TestSpeedLimitAssist:
def test_manual_cruise_change_detection(self):
self.sla.state = SpeedLimitAssistState.active
expected_cruise = SPEED_LIMITS['highway']
self.sla.v_cruise_setpoint_prev = expected_cruise
self.sla.v_cruise_cluster_prev = expected_cruise
different_cruise = SPEED_LIMITS['highway'] + 5
self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, 0, self.events_sp)
@@ -179,7 +177,7 @@ class TestSpeedLimitAssist:
def test_distance_based_adapting(self):
self.sla.state = SpeedLimitAssistState.adapting
self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
self.sla.v_cruise_cluster_prev = REQUIRED_INITIAL_MAX_SET_SPEED
distance = 100.0
current_speed = SPEED_LIMITS['highway']