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/frogpilot/functions/conditional_experimental_mode.py

@@ -99,6 +99,11 @@ class ConditionalExperimentalMode:
       self.status_value = 5
       return True
 
+    # Speed Limit Controller check
+    if self.params_memory.get_bool("SLCExperimentalMode"):
+      self.status_value = 6
+      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 = 7 if self.lead_detected else 8

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -10,6 +10,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 
 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
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
                  # MPH = [0.,  35,   35,  40,    40,  45,    45,  67,    67,   67, 123]
@@ -54,7 +55,11 @@ class FrogPilotPlanner:
     self.cem = ConditionalExperimentalMode()
     self.mtsc = MapTurnSpeedController()
 
+    self.override_slc = False
+
+    self.overridden_speed = 0
     self.mtsc_target = 0
+    self.slc_target = 0
     self.v_cruise = 0
 
     self.x_desired_trajectory = np.zeros(CONTROL_N)
@@ -99,8 +104,39 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise
 
+    # Pfeiferj's Speed Limit Controller
+    if self.speed_limit_controller:
+      SpeedLimitController.update_current_max_velocity(v_cruise)
+      self.slc_target = SpeedLimitController.desired_speed_limit
+
+      # Override SLC upon gas pedal press and reset upon brake/cancel button
+      if self.speed_limit_controller_override:
+        self.override_slc |= carState.gasPressed
+        self.override_slc &= enabled
+        self.override_slc &= v_ego > self.slc_target
+      else:
+        self.override_slc = False
+
+      self.overridden_speed *= enabled
+
+      # Use the override speed if SLC is being overridden
+      if self.override_slc:
+        if self.speed_limit_controller_override == 1:
+          # Set the max speed to the manual set speed
+          if carState.gasPressed:
+            self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise)
+          self.slc_target = self.overridden_speed
+        elif self.speed_limit_controller_override == 2:
+          self.overridden_speed = v_cruise
+          self.slc_target = v_cruise
+      if self.slc_target == 0:
+        self.slc_target = v_cruise
+    else:
+      self.overriden_speed = 0
+      self.slc_target = v_cruise
+
     v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
-    return min(v_cruise, self.mtsc_target) - v_ego_diff
+    return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff
 
   def publish_lateral(self, sm, pm, DH):
     frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan')
@@ -127,6 +163,11 @@ class FrogPilotPlanner:
     frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
     frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock
 
+    frogpilotLongitudinalPlan.slcOverridden = self.override_slc
+    frogpilotLongitudinalPlan.slcOverriddenSpeed = float(self.overridden_speed)
+    frogpilotLongitudinalPlan.slcSpeedLimit = float(self.slc_target)
+    frogpilotLongitudinalPlan.slcSpeedLimitOffset = float(SpeedLimitController.offset)
+
     pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
 
   def update_frogpilot_params(self, params):
@@ -163,3 +204,8 @@ class FrogPilotPlanner:
     self.lane_change_delay = params.get_int("LaneChangeTime") if self.nudgeless else 0
     self.lane_detection = params.get_bool("LaneDetection") if self.nudgeless else False
     self.one_lane_change = params.get_bool("OneLaneChange") if self.nudgeless else False
+
+    self.speed_limit_controller = params.get_bool("SpeedLimitController")
+    if self.speed_limit_controller:
+      self.speed_limit_controller_override = params.get_int("SLCOverride")
+      SpeedLimitController.update_frogpilot_params()

selfdrive/frogpilot/functions/speed_limit_controller.py

@@ -0,0 +1,119 @@
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+import json
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+
+class SpeedLimitController:
+  car_speed_limit: float = 0  # m/s
+  lst_speed_limit: float = 0  # m/s
+  map_speed_limit: float = 0  # m/s
+  nav_speed_limit: float = 0  # m/s
+
+  def __init__(self) -> None:
+    self.update_frogpilot_params()
+    self.write_car_state()
+    self.write_map_state()
+    self.write_nav_state()
+
+  def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
+    if load_state:
+      self.load_state()
+
+  @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:
+    params_memory.put_bool("SLCExperimentalMode", False)
+
+    limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [limit for limit in limits if limit > 0]
+
+    if self.highest and filtered_limits:
+      return max(filtered_limits)
+    if self.lowest and filtered_limits:
+      return min(filtered_limits)
+
+    priority_orders = [
+      ["nav", "car", "map"],
+      ["nav", "map", "car"],
+      ["nav", "map"],
+      ["nav", "car"],
+      ["nav"],
+      ["map", "car", "nav"],
+      ["map", "nav", "car"],
+      ["map", "nav"],
+      ["map", "car"],
+      ["map"],
+      ["car", "nav", "map"],
+      ["car", "map", "nav"],
+      ["car", "map"],
+      ["car", "nav"],
+      ["car"]
+    ]
+
+    if self.speed_limit_priority < len(priority_orders):
+      for source in priority_orders[self.speed_limit_priority]:
+        limit = getattr(self, f"{source}_speed_limit", 0)
+        if limit > 0:
+          self.prv_speed_limit = limit
+          return limit
+
+    if self.use_experimental_mode:
+      params_memory.put_bool("SLCExperimentalMode", True)
+      return 0
+    elif self.use_previous_limit:
+      if self.lst_speed_limit != self.prv_speed_limit:
+        params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100)
+      self.lst_speed_limit = self.prv_speed_limit
+      return self.prv_speed_limit
+
+    return 0
+
+  @property
+  def desired_speed_limit(self):
+    return self.speed_limit + self.offset if self.speed_limit else 0
+
+  def load_state(self):
+    self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
+    self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
+    self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
+
+  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_priority = params.get_int("SLCPriority")
+    self.highest = self.speed_limit_priority == 15
+    self.lowest = self.speed_limit_priority == 16
+
+    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_int("PreviousSpeedLimit") / 100
+
+SpeedLimitController = SpeedLimitController()