From 09b2f4bbe03460513ef2ec0c267f65fe2f69f990 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 18 Oct 2026 19:07:32 -0700 Subject: [PATCH] mapd: fix mapd GPS position handling (#2064) * mapd: restore the position mapd reads from llk * turn it on * mapd ui * Revert "mapd ui" This reverts commit 857d4302972bbd6b186e60174f75cb4cbaf96226. * Revert "turn it on" This reverts commit 2a5d9a62beea943ea589c62a5c19f6b8a1e451b4. --- .../mapd/live_map_data/osm_map_data.py | 16 +++++ openpilot/sunnypilot/mapd/mapd_manager.py | 4 ++ .../mapd/tests/test_osm_map_data.py | 70 +++++++++++++++++++ 3 files changed, 90 insertions(+) create mode 100644 openpilot/sunnypilot/mapd/tests/test_osm_map_data.py diff --git a/openpilot/sunnypilot/mapd/live_map_data/osm_map_data.py b/openpilot/sunnypilot/mapd/live_map_data/osm_map_data.py index c22fb7da4..3073b372a 100644 --- a/openpilot/sunnypilot/mapd/live_map_data/osm_map_data.py +++ b/openpilot/sunnypilot/mapd/live_map_data/osm_map_data.py @@ -4,6 +4,7 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. 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 @@ -26,6 +27,21 @@ class OsmMapData(BaseMapData): self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + if self.last_position is None: + return + + params = { + "latitude": self.last_position.latitude, + "longitude": self.last_position.longitude, + } + + if self.last_bearing is not None: + params['bearing'] = self.last_bearing + + # mapd reads its position from the memory params as JSON, so write it for the mapd binary. + # LastGPSPosition is registered as BYTES (ubloxd owns the persistent copy for pigeond), + # so the JSON payload has to be encoded here. + self.mem_params.put("LastGPSPosition", json.dumps(params).encode(), block=True) def get_current_speed_limit(self) -> float: return float(self.mem_params.get("MapSpeedLimit") or 0.0) diff --git a/openpilot/sunnypilot/mapd/mapd_manager.py b/openpilot/sunnypilot/mapd/mapd_manager.py index 886a1c606..d6acd529a 100755 --- a/openpilot/sunnypilot/mapd/mapd_manager.py +++ b/openpilot/sunnypilot/mapd/mapd_manager.py @@ -121,6 +121,10 @@ def update_osm_db() -> None: if not mem_params.get("OSMDownloadBounds"): mem_params.put("OSMDownloadBounds", "", block=True) + # mapd reads LastGPSPosition from the memory params as JSON, and it is registered as BYTES upstream + if not mem_params.get("LastGPSPosition"): + mem_params.put("LastGPSPosition", b"{}", block=True) + def main_thread(): update_installed_version(VERSION, params) diff --git a/openpilot/sunnypilot/mapd/tests/test_osm_map_data.py b/openpilot/sunnypilot/mapd/tests/test_osm_map_data.py new file mode 100644 index 000000000..20b755bcb --- /dev/null +++ b/openpilot/sunnypilot/mapd/tests/test_osm_map_data.py @@ -0,0 +1,70 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +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 + +import openpilot.cereal.messaging as messaging +from openpilot.cereal import log +from openpilot.common.params import Params +from openpilot.common.test import OpenpilotTestCase +from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData + + +class TestOsmMapData(OpenpilotTestCase): + """The mapd binary reads its position from the memory params as JSON, so mapd_manager has to feed it.""" + + LATITUDE = 32.875134 + LONGITUDE = -117.209777 + ALTITUDE = 42.0 + BEARING = 90.0 + + def setup_method(self): + self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params() + self.mem_params.remove("LastGPSPosition") + self.map_data = OsmMapData() + self.pm = messaging.PubMaster(['liveLocationKalman']) + + def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool): + msg = messaging.new_message('liveLocationKalman', valid=True) + location = msg.liveLocationKalman + location.status = status + location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid} + location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True} + return msg + + def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None: + # the first exchange synchronizes the PUB/SUB connection, the second one is actually received + self.pm.send('liveLocationKalman', self.location_message(status, position_valid)) + self.map_data.sm.update(1000) + self.pm.send('liveLocationKalman', self.location_message(status, position_valid)) + self.map_data.sm.update(1000) + + def read_position(self) -> dict: + value = self.mem_params.get("LastGPSPosition") + self.assertIsNotNone(value, "LastGPSPosition was not written for mapd") + return json.loads(value) + + def test_writes_position_for_mapd(self): + self.send_location(log.LiveLocationKalman.Status.valid, True) + self.map_data.tick() + + position = self.read_position() + self.assertAlmostEqual(position['latitude'], self.LATITUDE, delta=1e-6) + self.assertAlmostEqual(position['longitude'], self.LONGITUDE, delta=1e-6) + self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6) + + def test_invalid_localizer_keeps_last_known_position(self): + self.send_location(log.LiveLocationKalman.Status.valid, True) + self.map_data.tick() + + self.send_location(log.LiveLocationKalman.Status.uninitialized, False) + self.map_data.tick() + + position = self.read_position() + self.assertAlmostEqual(position['latitude'], self.LATITUDE, delta=1e-6) + self.assertAlmostEqual(position['longitude'], self.LONGITUDE, delta=1e-6)