From 7bca95dbb89dc6b969871cc7f6bf4306d91f60cc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 16:44:38 +0200 Subject: [PATCH] navd: speed limits only when localizer is valid (#24845) --- selfdrive/navd/navd.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index fcd53d8b60..509653a46d 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -32,6 +32,7 @@ class RouteEngine: self.last_bearing = None self.gps_ok = False + self.localizer_valid = False self.nav_destination = None self.step_idx = None @@ -73,9 +74,9 @@ class RouteEngine: location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK - localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid - if localizer_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]) @@ -202,7 +203,7 @@ class RouteEngine: if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): closest = geometry[closest_idx - 1] - if 'maxspeed' in closest.annotations: + if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] # Speed limit sign type