dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

selfdrive/navd/helpers.py

@@ -4,9 +4,12 @@ import json
 import math
 from typing import Any, Dict, List, Optional, Tuple, Union, cast
 
-from common.conversions import Conversions
-from common.numpy_fast import clip
-from common.params import Params
+from openpilot.common.conversions import Conversions
+from openpilot.common.numpy_fast import clip
+from openpilot.common.params import Params
+
+DIRECTIONS = ('left', 'right', 'straight')
+MODIFIABLE_DIRECTIONS = ('left', 'right')
 
 EARTH_MEAN_RADIUS = 6371007.2
 SPEED_CONVERSIONS = {
@@ -119,8 +122,10 @@ def coordinate_from_param(param: str, params: Optional[Params] = None) -> Option
 
 
 def string_to_direction(direction: str) -> str:
-  for d in ['left', 'right', 'straight']:
+  for d in DIRECTIONS:
     if d in direction:
+      if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
+        return 'slight' + d.capitalize()
       return d
   return 'none'
 

selfdrive/navd/map_renderer.h

@@ -49,6 +49,7 @@ private:
   }
 
   QTimer* timer;
+  bool ever_loaded = false;
 
 public slots:
   void updatePosition(QMapbox::Coordinate position, float bearing);

selfdrive/navd/map_renderer.py

@@ -7,8 +7,8 @@ import numpy as np
 import polyline
 from cffi import FFI
 
-from common.ffi_wrapper import suffix
-from common.basedir import BASEDIR
+from openpilot.common.ffi_wrapper import suffix
+from openpilot.common.basedir import BASEDIR
 
 HEIGHT = WIDTH = SIZE = 256
 METERS_PER_PIXEL = 2

selfdrive/navd/navd.py

@@ -5,23 +5,20 @@ import os
 import threading
 
 import requests
-import numpy as np
 
 import cereal.messaging as messaging
 from cereal import log
-from common.api import Api
-from common.params import Params
-from common.realtime import Ratekeeper
-from common.transformations.coordinates import ecef2geodetic
-from selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
+from openpilot.common.api import Api
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     distance_along_geometry, maxspeed_to_ms,
                                     minimum_distance,
                                     parse_banner_instructions)
-from system.swaglog import cloudlog
+from openpilot.system.swaglog import cloudlog
 
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
-VALID_POS_STD = 50.0
 REROUTE_COUNTER_MIN = 3
 
 
@@ -79,21 +76,13 @@ class RouteEngine:
 
   def update_location(self):
     location = self.sm['liveLocationKalman']
-    laikad = self.sm['gnssMeasurements']
+    self.gps_ok = location.gpsOK
 
-    locationd_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-    laikad_valid = laikad.positionECEF.valid and np.linalg.norm(laikad.positionECEF.std) < VALID_POS_STD
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
 
-    self.localizer_valid = locationd_valid or laikad_valid
-    self.gps_ok = location.gpsOK or laikad_valid
-
-    if locationd_valid:
+    if self.localizer_valid:
       self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
       self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
-    elif laikad_valid:
-      geodetic = ecef2geodetic(laikad.positionECEF.value)
-      self.last_position = Coordinate(geodetic[0], geodetic[1])
-      self.last_bearing = None
 
   def recompute_route(self):
     if self.last_position is None:
@@ -357,7 +346,7 @@ class RouteEngine:
 
 def main(sm=None, pm=None):
   if sm is None:
-    sm = messaging.SubMaster(['liveLocationKalman', 'gnssMeasurements', 'managerState'])
+    sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
   if pm is None:
     pm = messaging.PubMaster(['navInstruction', 'navRoute'])
 

selfdrive/navd/set_destination.py

@@ -2,7 +2,7 @@
 import json
 import sys
 
-from common.params import Params
+from openpilot.common.params import Params
 
 if __name__ == "__main__":
   params = Params()

selfdrive/navd/tests/test_map_renderer.py

@@ -8,7 +8,7 @@ import cereal.messaging as messaging
 
 from typing import Any
 from cereal.visionipc import VisionIpcClient, VisionStreamType
-from selfdrive.manager.process_config import managed_processes
+from openpilot.selfdrive.manager.process_config import managed_processes
 
 LLK_DECIMATION = 10
 CACHE_PATH = "/data/mbgl-cache-navd.db"