Speed limit controller

Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself.

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

selfdrive/navd/navd.py

@@ -18,6 +18,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     parse_banner_instructions)
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
+
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
 REROUTE_COUNTER_MIN = 3
@@ -230,6 +232,12 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = 0
+      SpeedLimitController.write_nav_state()
+
+      if SpeedLimitController.desired_speed_limit != 0:
+        msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
       self.pm.send('navInstruction', msg)
       return
 
@@ -304,6 +312,16 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
+      SpeedLimitController.write_nav_state()
+    if not self.localizer_valid or ('maxspeed' not in closest.annotations):
+      SpeedLimitController.load_state()
+      SpeedLimitController.nav_speed_limit = 0
+      SpeedLimitController.write_nav_state()
+
+    if SpeedLimitController.desired_speed_limit != 0:
+      msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
 
     # Speed limit sign type
     if 'speedLimitSign' in step: