mawei/openpilot
1
0
Fork 0
mirror of https://jihulab.com/mr-one/openpilot.git synced 2026-10-27 21:13:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

sunnypilot: use GPS service for map-based SCC

Browse Source
This commit is contained in:
1okko
2026-10-21 20:59:02 +08:00
parent cb72480647
commit f6fd048d7e
2 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/map_controller.py
+10 -6
View File
@@ -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()
openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py
+1 -1
View File
@@ -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)