mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-15 15:22:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

liveMapDataSP: parse bearing from GPS (#1279)

Browse Source 
* simpler approach pls

* fix
This commit is contained in:
Jason Wen
2025-09-22 09:39:22 -04:00
committed by GitHub
parent 005c6aed95
commit 6a1121d4ad
2 changed files with 17 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/mapd/live_map_data/base_map_data.py
+3 -19
View File
@@ -9,8 +9,7 @@ from abc import abstractmethod, ABC
from cereal import messaging
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
from openpilot.sunnypilot.navd.helpers import coordinate_from_param
class BaseMapData(ABC):
@@ -23,8 +22,8 @@ class BaseMapData(ABC):
ignore_valid=gps_packets, poll='livePose')
self.pm = messaging.PubMaster(['liveMapDataSP'])
self.last_bearing = None
self.last_position = coordinate_from_param("LastGPSPosition", self.params)
self.last_altitude = None
@abstractmethod
def update_location(self) -> None:
@@ -42,20 +41,6 @@ class BaseMapData(ABC):
def get_current_road_name(self) -> str:
pass
def get_current_location(self) -> None:
gps = self.sm[self.gps_location_service]
# ignore the message if the fix is invalid
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0
if not gps_ok and self.sm['livePose'].inputsOK:
return
# livePose has these data, but aren't on cereal
self.last_position = Coordinate(gps.latitude, gps.longitude)
self.last_altitude = gps.altitude
return
def publish(self) -> None:
speed_limit = self.get_current_speed_limit()
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
@@ -74,7 +59,6 @@ class BaseMapData(ABC):
self.pm.send('liveMapDataSP', mapd_sp_send)
def tick(self) -> None:
self.sm.update()
self.get_current_location()
self.sm.update(0)
self.update_location()
self.publish()
sunnypilot/mapd/live_map_data/osm_map_data.py
+14 -3
View File
@@ -5,9 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -15,17 +17,26 @@ from openpilot.sunnypilot.navd.helpers import Coordinate
class OsmMapData(BaseMapData):
def __init__(self):
super().__init__()
self.params = Params()
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
def update_location(self) -> None:
if self.last_position is None or self.last_altitude is None:
gps = self.sm[self.gps_location_service]
gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0
if gps_ok:
self.last_bearing = gps.bearingDeg * 180/math.pi
self.last_position = Coordinate(gps.latitude, gps.longitude)
if not gps_ok and self.sm['livePose'].inputsOK:
return
if self.last_position is None:
return
params = {
"latitude": self.last_position.latitude,
"longitude": self.last_position.longitude,
"altitude": self.last_altitude,
"bearing": self.last_bearing,
}
self.mem_params.put("LastGPSPosition", json.dumps(params))