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>
Co-Authored-By: Jason Wen <haibin.wen3@gmail.com>

selfdrive/frogpilot/functions/conditional_experimental_mode.py

@@ -3,6 +3,7 @@ from openpilot.common.numpy_fast import interp
 
 from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, MovingAverageCalculator, PROBABILITY
 
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
 
 # Lookup table for stop sign / stop light detection
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
@@ -74,6 +75,11 @@ class ConditionalExperimentalMode:
       self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
       return True
 
+    # Speed Limit Controller check
+    if SpeedLimitController.experimental_mode:
+      self.status_value = 9
+      return True
+
     # Speed check
     if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead):
       self.status_value = 10 if self.lead_detected else 11

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -12,6 +12,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING
 
 from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
 
 
 class FrogPilotPlanner:
@@ -24,8 +25,12 @@ class FrogPilotPlanner:
     self.cem = ConditionalExperimentalMode(self.params_memory)
     self.mtsc = MapTurnSpeedController()
 
+    self.override_slc = False
+
+    self.overridden_speed = 0
     self.mtsc_target = 0
     self.road_curvature = 0
+    self.slc_target = 0
     self.stop_distance = 0
     self.v_cruise = 0
 
@@ -106,7 +111,40 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise
 
-    targets = [self.mtsc_target]
+    # Pfeiferj's Speed Limit Controller
+    if self.speed_limit_controller:
+      SpeedLimitController.update_speed_limits()
+      unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
+
+      # Check if the new speed limit has been confirmed by the user
+      if self.speed_limit_confirmation:
+        if self.params_memory.get_bool("SLCConfirmed") or self.slc_target == 0:
+          self.slc_target = unconfirmed_slc_target
+          self.params_memory.put_bool("SLCConfirmed", False)
+      else:
+        self.slc_target = unconfirmed_slc_target
+
+      # Override SLC upon gas pedal press and reset upon brake/cancel button
+      self.override_slc &= self.overridden_speed > self.slc_target
+      self.override_slc |= carState.gasPressed and v_ego > self.slc_target > CRUISING_SPEED
+      self.override_slc &= enabled and not carState.standstill
+
+      # Use the override speed if SLC is being overridden
+      if self.override_slc:
+        if self.speed_limit_controller_override == 1:
+          # Set the speed limit to the manual set speed
+          if carState.gasPressed:
+            self.overridden_speed = v_ego + v_ego_diff
+          self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
+        elif self.speed_limit_controller_override == 2:
+          # Set the speed limit to the max set speed
+          self.overridden_speed = v_cruise + v_cruise_diff
+      else:
+        self.overridden_speed = 0
+    else:
+      self.slc_target = v_cruise
+
+    targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
     filtered_targets = [target for target in targets if target > CRUISING_SPEED]
 
     return min(filtered_targets) if filtered_targets else v_cruise
@@ -129,6 +167,12 @@ class FrogPilotPlanner:
 
     frogpilotPlan.redLight = self.cem.red_light_detected
 
+    frogpilotPlan.slcOverridden = bool(self.override_slc)
+    frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
+    frogpilotPlan.slcSpeedLimit = float(self.slc_target)
+    frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+    frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
+
     pm.send('frogpilotPlan', frogpilot_plan_send)
 
   def update_frogpilot_params(self, params):
@@ -171,3 +215,8 @@ class FrogPilotPlanner:
       self.mtsc_curvature_check = params.get_bool("MTSCCurvatureCheck")
       self.mtsc_limit = params.get_float("MTSCLimit") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
       self.params_memory.put_float("MapTargetLatA", 2 * (params.get_int("MTSCAggressiveness") / 100))
+
+    self.speed_limit_controller = params.get_bool("SpeedLimitController")
+    if self.speed_limit_controller:
+      self.speed_limit_confirmation = params.get_bool("SLCConfirmation")
+      self.speed_limit_controller_override = params.get_int("SLCOverride")

selfdrive/frogpilot/functions/speed_limit_controller.py

@@ -0,0 +1,117 @@
+import json
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+
+class SpeedLimitController:
+  car_speed_limit: float = 0  # m/s
+  map_speed_limit: float = 0  # m/s
+  nav_speed_limit: float = 0  # m/s
+  prv_speed_limit: float = 0  # m/s
+
+  def __init__(self) -> None:
+    self.update_frogpilot_params()
+
+  def update_speed_limits(self):
+    self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit") or '0')
+    self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit") or '0')
+    self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit") or '0')
+
+    if params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
+  @property
+  def offset(self) -> float:
+    if self.speed_limit < 14:
+      return self.offset1
+    elif self.speed_limit < 24:
+      return self.offset2
+    elif self.speed_limit < 29:
+      return self.offset3
+    else:
+      return self.offset4
+
+  @property
+  def speed_limit(self) -> float:
+    limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [limit for limit in limits if limit >= 1]
+
+    if self.highest and filtered_limits:
+      return max(filtered_limits)
+    elif self.lowest and filtered_limits:
+      return min(filtered_limits)
+
+    speed_limits = {
+      "Dashboard": self.car_speed_limit,
+      "Navigation": self.nav_speed_limit,
+      "Offline Maps": self.map_speed_limit,
+    }
+
+    priorities = [
+      self.speed_limit_priority1,
+      self.speed_limit_priority2,
+      self.speed_limit_priority3,
+    ]
+
+    for priority in priorities:
+      speed_limit_value = speed_limits.get(priority, 0)
+      if speed_limit_value >= 1:
+        return speed_limit_value
+
+    if self.use_previous_limit:
+      return self.prv_speed_limit
+
+    return 0
+
+  def update_previous_limit(self, speed_limit: float):
+    if self.prv_speed_limit != speed_limit:
+      params.put_float("PreviousSpeedLimit", speed_limit)
+      self.prv_speed_limit = speed_limit
+
+  @property
+  def desired_speed_limit(self) -> float:
+    if self.speed_limit != 0:
+      self.update_previous_limit(self.speed_limit)
+      return self.speed_limit + self.offset
+    else:
+      return 0
+
+  @property
+  def experimental_mode(self) -> bool:
+    return self.speed_limit == 0 and self.use_experimental_mode
+
+  def write_car_state(self):
+    params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
+
+  def write_map_state(self):
+    params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
+
+  def write_nav_state(self):
+    params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
+
+  def update_frogpilot_params(self):
+    conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
+
+    self.offset1 = params.get_int("Offset1") * conversion
+    self.offset2 = params.get_int("Offset2") * conversion
+    self.offset3 = params.get_int("Offset3") * conversion
+    self.offset4 = params.get_int("Offset4") * conversion
+
+    self.speed_limit_priority1 = params.get("SLCPriority1", encoding='utf-8')
+    self.speed_limit_priority2 = params.get("SLCPriority2", encoding='utf-8')
+    self.speed_limit_priority3 = params.get("SLCPriority3", encoding='utf-8')
+
+    self.highest = self.speed_limit_priority1 == "Highest"
+    self.lowest = self.speed_limit_priority1 == "Lowest"
+
+    slc_fallback = params.get_int("SLCFallback")
+    self.use_experimental_mode = slc_fallback == 1
+    self.use_previous_limit = slc_fallback == 2
+
+    self.prv_speed_limit = params.get_float("PreviousSpeedLimit")
+
+SpeedLimitController = SpeedLimitController()