navd: locationd is only trusted source (#29803)

* finish revert for https://github.com/commaai/openpilot/pull/27579

* comment out

* clean up
old-commit-hash: 96fd66e4e2

selfdrive/navd/navd.py

@@ -5,14 +5,12 @@ import os
 import threading
 
 import requests
-import numpy as np
 
 import cereal.messaging as messaging
 from cereal import log
 from openpilot.common.api import Api
 from openpilot.common.params import Params
 from openpilot.common.realtime import Ratekeeper
-from openpilot.common.transformations.coordinates import ecef2geodetic
 from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     distance_along_geometry, maxspeed_to_ms,
                                     minimum_distance,
@@ -21,7 +19,6 @@ 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/ui/qt/maps/map.cc

@@ -5,7 +5,6 @@
 
 #include <QDebug>
 
-#include "common/transformations/coordinates.hpp"
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/ui.h"

selfdrive/ui/qt/maps/map_helpers.cc

@@ -149,8 +149,3 @@ std::pair<QString, QString> map_format_distance(float d, bool is_metric) {
                         : std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("ft")};
   }
 }
-
-double angle_difference(double angle1, double angle2) {
-  double diff = fmod(angle2 - angle1 + 180.0, 360.0) - 180.0;
-  return diff < -180.0 ? diff + 360.0 : diff;
-}

selfdrive/ui/qt/maps/map_helpers.h

@@ -29,4 +29,3 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList<QGeoCo
 QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString);
 std::optional<QMapbox::Coordinate> coordinate_from_param(const std::string &param);
 std::pair<QString, QString> map_format_distance(float d, bool is_metric);
-double angle_difference(double angle1, double angle2);