Merge remote-tracking branch 'sunnypilot/sunnypilot/master-new' into feature/slc

# Conflicts:
#	cereal/custom.capnp
#	common/params_keys.h
#	sunnypilot/mapd/live_map_data/__init__.py
#	sunnypilot/mapd/live_map_data/base_map_data.py
#	sunnypilot/mapd/live_map_data/debug.py
#	sunnypilot/mapd/live_map_data/osm_map_data.py
#	sunnypilot/mapd/live_map_data/standalone.py
#	sunnypilot/mapd/mapd_installer.py
#	sunnypilot/mapd/mapd_manager.py

cereal/custom.capnp

@@ -257,21 +257,7 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
   speedLimitAheadValid @2 :Bool;
   speedLimitAhead @3 :Float32;
   speedLimitAheadDistance @4 :Float32;
-  turnSpeedLimitValid @5 :Bool;
-  turnSpeedLimit @6 :Float32;
-  turnSpeedLimitEndDistance @7 :Float32;
-  turnSpeedLimitSign @8 :Int16;
-  turnSpeedLimitsAhead @9 :List(Float32);
-  turnSpeedLimitsAheadDistances @10 :List(Float32);
-  turnSpeedLimitsAheadSigns @11 :List(Int16);
-  lastGpsTimestamp @12 :Int64;  # Milliseconds since January 1, 1970.
-  currentRoadName @13 :Text;
-  lastGpsLatitude @14 :Float64;
-  lastGpsLongitude @15 :Float64;
-  lastGpsSpeed @16 :Float32;
-  lastGpsBearingDeg @17 :Float32;
-  lastGpsAccuracy @18 :Float32;
-  lastGpsBearingAccuracyDeg @19 :Float32;
+  roadName @5 :Text;
 }
 
 struct CustomReserved9 @0xa1680744031fdb2d {

sunnypilot/mapd/live_map_data/__init__.py

@@ -1,4 +1,9 @@
-from cereal import custom
+"""
+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.
+"""
 from openpilot.common.swaglog import cloudlog
 
 LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -1,10 +1,16 @@
+"""
+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 time
 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.sunnypilot.navd.helpers import Coordinate
+from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
 
 
 class BaseMapData(ABC):
@@ -16,8 +22,15 @@ class BaseMapData(ABC):
     self._sm = messaging.SubMaster(['livePose', 'carControl'] + self._gps_packets)
     self._pm = messaging.PubMaster(['liveMapDataSP'])
 
+    self.gps_location_service = get_gps_location_service(self.params)
+    self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service])
+    self.pm = messaging.PubMaster(['liveMapDataSP'])
+
+    self.last_position = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_altitude = None
+
   @abstractmethod
-  def update_location(self, current_location: Coordinate | None) -> None:
+  def update_location(self) -> None:
     pass
 
   @abstractmethod
@@ -25,45 +38,31 @@ class BaseMapData(ABC):
     pass
 
   @abstractmethod
-  def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]:
+  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
     pass
 
   @abstractmethod
   def get_current_road_name(self) -> str:
     pass
 
-  def _is_gps_data_valid(self) -> bool:
-    all_sock_alive = self._sm.all_alive(service_list=[self._gps_location_service])
-    all_sock_valid = self._sm.all_valid(service_list=[self._gps_location_service])
-    return bool(all_sock_alive and all_sock_valid)
-
-  def get_current_location(self) -> Coordinate | None:
-    if not self._sm.updated[self._gps_location_service] or not self._sm.valid[self._gps_location_service]:
-      return None
-
-    _last_gps = self._sm[self._gps_location_service]
+  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.updated[self._gps_location_service] or (time.monotonic() - self._sm.logMonoTime[self._gps_location_service] / 1e9) > 2.0
-    if not gps_ok and self._sm['livePose'].inputsOK:
+    gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0
+    if not gps_ok and self.sm['livePose'].inputsOK:
       return None
 
-    result = Coordinate(_last_gps.latitude, _last_gps.longitude)
-    result.annotations['unixTimestampMillis'] = _last_gps.unixTimestampMillis
-    result.annotations['speed'] = _last_gps.speed
-    result.annotations['bearingDeg'] = _last_gps.bearingDeg
-    result.annotations['accuracy'] = _last_gps.horizontalAccuracy
-    result.annotations['bearingAccuracyDeg'] = _last_gps.bearingAccuracyDeg
-
-    return result
+    # livePose has these data, but aren't on cereal
+    self.last_position = Coordinate(gps.latitude, gps.longitude)
+    self.last_altitude = gps.altitude
 
   def publish(self) -> None:
     speed_limit = self.get_current_speed_limit()
-    current_road_name = self.get_current_road_name()
-    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance(self._last_gps)
+    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
 
     mapd_sp_send = messaging.new_message('liveMapDataSP')
-    mapd_sp_send.valid = self._is_gps_data_valid()
+    mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose'])
     live_map_data = mapd_sp_send.liveMapDataSP
 
     if self._last_gps:
@@ -80,12 +79,12 @@ class BaseMapData(ABC):
     live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0)
     live_map_data.speedLimitAhead = next_speed_limit
     live_map_data.speedLimitAheadDistance = next_speed_limit_distance
-    live_map_data.currentRoadName = current_road_name
+    live_map_data.roadName = self.get_current_road_name()
 
-    self._pm.send('liveMapDataSP', mapd_sp_send)
+    self.pm.send('liveMapDataSP', mapd_sp_send)
 
   def tick(self) -> None:
-    self._sm.update()
-    self._last_gps = self.get_current_location()
-    self.update_location(self._last_gps)
+    self.sm.update()
+    self.get_current_location()
+    self.update_location()
     self.publish()

sunnypilot/mapd/live_map_data/debug.py

@@ -1,3 +1,9 @@
+"""
+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.
+"""
 # DISCLAIMER: This code is intended principally for development and debugging purposes.
 # Although it provides a standalone entry point to the program, users should refer
 # to the actual implementations for consumption. Usage outside of development scenarios

sunnypilot/mapd/live_map_data/osm_map_data.py

@@ -1,9 +1,16 @@
+"""
+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 platform
 
 from openpilot.common.params import Params
 from openpilot.sunnypilot.navd.helpers import Coordinate
 from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
+from openpilot.sunnypilot.navd.helpers import Coordinate
 
 
 class OsmMapData(BaseMapData):
@@ -12,16 +19,17 @@ class OsmMapData(BaseMapData):
     self.params = Params()
     self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
 
-  def update_location(self, current_location: Coordinate | None) -> None:
-    if not current_location:
+  def update_location(self) -> None:
+    if self.last_position is None or self.last_altitude is None:
       return
 
-    last_gps_position_for_osm = {
-      "latitude": current_location.latitude,
-      "longitude": current_location.longitude,
-      "bearing": current_location.annotations.get("bearingDeg", 0)
+    params = {
+      "latitude": self.last_position.latitude,
+      "longitude": self.last_position.longitude,
+      "altitude": self.last_altitude,
     }
-    self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm))
+
+    self.mem_params.put("LastGPSPosition", json.dumps(params))
 
   def get_current_speed_limit(self) -> float:
     return float(self.mem_params.get("MapSpeedLimit", encoding='utf8') or 0.0)
@@ -29,7 +37,7 @@ class OsmMapData(BaseMapData):
   def get_current_road_name(self) -> str:
     return self.mem_params.get("RoadName", encoding='utf8') or ""
 
-  def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]:
+  def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
     next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8')
     next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {}
     next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
@@ -39,6 +47,6 @@ class OsmMapData(BaseMapData):
 
     if next_speed_limit_latitude and next_speed_limit_longitude:
       next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
-      next_speed_limit_distance = (current_location or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
+      next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
 
     return next_speed_limit, next_speed_limit_distance

sunnypilot/mapd/live_map_data/standalone.py

@@ -1,3 +1,9 @@
+"""
+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.
+"""
 # DISCLAIMER: This code is intended principally for development and debugging purposes.
 # Although it provides a standalone entry point to the program, users should refer
 # to the actual implementations for consumption. Usage outside of development scenarios

sunnypilot/mapd/mapd_installer.py

@@ -1,3 +1,9 @@
+"""
+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.
+"""
 #!/usr/bin/env python3
 import logging
 import os
@@ -81,10 +87,10 @@ class MapdInstallManager:
     Params().put("MapdVersion", version)
 
   @staticmethod
-  def get_installed_version() -> str | None:
-    return Params().get("MapdVersion", encoding="utf-8")
+  def get_installed_version() -> str:
+    return Params().get("MapdVersion", encoding="utf-8") or ""
 
-  def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool | None:
+  def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
     max_retries = 10
     for retries in range(max_retries + 1):
       self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
@@ -97,21 +103,23 @@ class MapdInstallManager:
         if return_on_failure and retries == max_retries:
           return False
 
-  def non_prebuilt_install(self) -> bool | None:
+    return False
+
+  def non_prebuilt_install(self) -> None:
     sm = messaging.SubMaster(['deviceState'])
     metered = sm['deviceState'].networkMetered
 
     if metered:
       self._spinner.update("Can't proceed with mapd install since network is metered!")
       time.sleep(5)
-      return False
+      return
 
     try:
       self.ensure_directories_exist()
       if not self.download_needed():
         self._spinner.update("Mapd is good!")
         time.sleep(0.1)
-        return True
+        return
 
       if self.wait_for_internet_connection(return_on_failure=True):
         self._spinner.update(f"Downloading pfeiferj's mapd [{install_manager.get_installed_version()}] => [{VERSION}].")

sunnypilot/mapd/mapd_manager.py

@@ -1,3 +1,9 @@
+"""
+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 time
 import platform