diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 6a6a0846b7..0ccd1f144b 100755 --- a/selfdrive/navd/navd.py +++ b/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']) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index d5e81bc9e0..db56fcdcfc 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -5,7 +5,6 @@ #include -#include "common/transformations/coordinates.hpp" #include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/ui.h" diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index a23c8990d3..ed843610a8 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -149,8 +149,3 @@ std::pair 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; -} diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 6c6a8d12fb..4d6e9b5382 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -29,4 +29,3 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList polyline_to_coordinate_list(const QString &polylineString); std::optional coordinate_from_param(const std::string ¶m); std::pair map_format_distance(float d, bool is_metric); -double angle_difference(double angle1, double angle2);