From b64d5a0fa4df9ea130056d91cfe68d20f6b4d208 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 22 Sep 2025 09:39:22 -0400
Subject: [PATCH] liveMapDataSP: parse bearing from GPS (#1279)

* simpler approach pls

* fix
---
 .../mapd/live_map_data/base_map_data.py       | 22 +++----------------
 sunnypilot/mapd/live_map_data/osm_map_data.py | 17 +++++++++++---
 2 files changed, 17 insertions(+), 22 deletions(-)

diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py
index 6c7679420a..f386eab96b 100644
--- a/sunnypilot/mapd/live_map_data/base_map_data.py
+++ b/sunnypilot/mapd/live_map_data/base_map_data.py
@@ -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()
diff --git a/sunnypilot/mapd/live_map_data/osm_map_data.py b/sunnypilot/mapd/live_map_data/osm_map_data.py
index 1eaf76dc8d..c5187a8e04 100644
--- a/sunnypilot/mapd/live_map_data/osm_map_data.py
+++ b/sunnypilot/mapd/live_map_data/osm_map_data.py
@@ -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))