From f6fd048d7e1f726e5e3e9281707ea9941f1f529a Mon Sep 17 00:00:00 2001 From: 1okko <15377594951@189.cn> Date: Wed, 21 Oct 2026 20:59:02 +0800 Subject: [PATCH] sunnypilot: use GPS service for map-based SCC --- .../lib/smart_cruise_control/map_controller.py | 16 ++++++++++------ .../smart_cruise_control/smart_cruise_control.py | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py index f3ed0fc07..0d85f060e 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py @@ -2,12 +2,14 @@ import json import math import platform +import openpilot.cereal.messaging as messaging from openpilot.cereal import custom +from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate +from openpilot.sunnypilot.navd.helpers import Coordinate from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState @@ -84,7 +86,8 @@ class SmartCruiseControlMap: self.target_lon = 0.0 self.frame = -1 - self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) + self.gps_location_service = get_gps_location_service(self.params) + self.last_position = Coordinate(0.0, 0.0) self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or [] def get_v_target_from_control(self) -> float: @@ -100,8 +103,9 @@ class SmartCruiseControlMap: if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: self.enabled = self.params.get_bool("SmartCruiseControlMap") - def update_calculations(self) -> None: - self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0) + def update_calculations(self, sm: messaging.SubMaster) -> None: + gps = sm[self.gps_location_service] + self.last_position = Coordinate(gps.latitude, gps.longitude) lat = self.last_position.latitude lon = self.last_position.longitude @@ -243,7 +247,7 @@ class SmartCruiseControlMap: return enabled, active - def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None: + def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None: self.long_enabled = long_enabled self.long_override = long_override self.v_ego = v_ego @@ -251,7 +255,7 @@ class SmartCruiseControlMap: self.v_cruise = v_cruise self.update_params() - self.update_calculations() + self.update_calculations(sm) self.is_enabled, self.is_active = self._update_state_machine() diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py index ff6c70663..62b0dcf59 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py @@ -15,5 +15,5 @@ class SmartCruiseControl: self.map = SmartCruiseControlMap() def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None: - self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise) + self.map.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)