diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5d18b77097..4b0c0066a6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -369,6 +369,7 @@ struct CarControlSP @0xa5cd762cd951a455 { leadOne @2 :LeadData; leadTwo @3 :LeadData; intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement; + speed @5 :Float32; struct Param { key @0 :Text; diff --git a/sunnypilot/navd/navigationd.py b/sunnypilot/navd/navigationd.py index 06586c867b..90de2e095e 100755 --- a/sunnypilot/navd/navigationd.py +++ b/sunnypilot/navd/navigationd.py @@ -25,7 +25,7 @@ class Navigationd: self.mapbox = MapboxIntegration() self.nav_instructions = NavigationInstructions() - self.sm = messaging.SubMaster(['carState', 'liveLocationKalman']) + self.sm = messaging.SubMaster(['carControlSP', 'liveLocationKalman']) self.pm = messaging.PubMaster(['navigationd']) self.rk = Ratekeeper(3) # 3 Hz @@ -82,7 +82,7 @@ class Navigationd: nav_data: dict = {} if self.allow_navigation and self.last_position is not None: if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude): - v_ego = max(self.sm['carState'].vEgo, 0.) + v_ego = float(max(self.sm['carControlSP'].speed, 0.0)) nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude, v_ego) nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric) diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 8caeeaeabc..d18e35424d 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -72,6 +72,8 @@ class ControlsExt: CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement + CC_SP.speed = sm['carState'].vEgo + return CC_SP @staticmethod