Controls - Speed Limit Controller

Automatically adjust the max speed to match the current speed limit using 'Open Street Maps', 'Navigate On openpilot', or your car's dashboard (Toyotas/Lexus/HKG only).

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>

selfdrive/controls/controlsd.py

@@ -880,7 +880,7 @@ class Controls:
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode:
-        self.experimental_mode = self.params.get_bool("ExperimentalMode")
+        self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.frogpilot_toggles.speed_limit_controller and SpeedLimitController.experimental_mode
       self.personality = self.read_personality_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -17,6 +17,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY
 from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 GearShifter = car.CarState.GearShifter
 
@@ -58,6 +59,7 @@ class FrogPilotPlanner:
     self.model_length = 0
     self.mtsc_target = 0
     self.road_curvature = 0
+    self.slc_target = 0
     self.speed_jerk = 0
     self.tracked_model_length = 0
     self.v_cruise = 0
@@ -232,6 +234,13 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
+    # Pfeiferj's Speed Limit Controller
+    if frogpilot_toggles.speed_limit_controller:
+      SpeedLimitController.update(controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles)
+      self.slc_target = SpeedLimitController.desired_speed_limit
+    else:
+      self.slc_target = 0
+
     if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled:
       self.forcing_stop = True
       self.v_cruise = -1
@@ -251,7 +260,7 @@ class FrogPilotPlanner:
       self.forcing_stop = False
       self.tracked_model_length = 0
 
-      targets = [self.mtsc_target]
+      targets = [self.mtsc_target, self.slc_target - v_ego_diff]
       self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
 
   def publish(self, sm, pm, frogpilot_toggles):
@@ -278,6 +287,9 @@ class FrogPilotPlanner:
     frogpilotPlan.maxAcceleration = float(self.max_accel)
     frogpilotPlan.minAcceleration = float(self.min_accel)
 
+    frogpilotPlan.slcSpeedLimit = self.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+
     frogpilotPlan.vCruise = self.v_cruise
 
     pm.send('frogpilotPlan', frogpilot_plan_send)

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -3,6 +3,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 MODEL_LENGTH = ModelConstants.IDX_N
 PLANNER_TIME = ModelConstants.T_IDXS[MODEL_LENGTH - 1]
@@ -62,6 +63,10 @@ class ConditionalExperimentalMode:
       self.status_value = 15 if not self.frogpilot_planner.forcing_stop else 16
       return True
 
+    if SpeedLimitController.experimental_mode:
+      self.status_value = 17
+      return True
+
     return False
 
   def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles):

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,126 @@
+# PFEIFER - SLC - Modified by FrogAi for FrogPilot
+import json
+import math
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx - ax) / 2) * math.sin((bx - ax) / 2) + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2) * math.sin((by - ay) / 2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+  return R * c  # in meters
+
+class SpeedLimitController:
+  def __init__(self):
+    self.frogpilot_toggles = FrogPilotVariables.toggles
+
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.map_speed_limit = 0  # m/s
+    self.max_speed_limit = 0  # m/s
+    self.nav_speed_limit = 0  # m/s
+    self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
+
+  def get_param_memory(self, key, is_json=False):
+    param_value = self.params_memory.get(key)
+    if param_value is None:
+      return {} if is_json else 0.0
+    return json.loads(param_value) if is_json else float(param_value)
+
+  def update_previous_limit(self, speed_limit):
+    if self.prv_speed_limit != speed_limit:
+      self.params.put_float_nonblocking("PreviousSpeedLimit", speed_limit)
+      self.prv_speed_limit = speed_limit
+
+  def update(self, enabled, navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles):
+    self.write_map_state(v_ego)
+    self.nav_speed_limit = navigationSpeedLimit
+
+    self.max_speed_limit = v_cruise if enabled else 0
+
+    self.frogpilot_toggles = frogpilot_toggles
+
+  def write_map_state(self, v_ego):
+    self.map_speed_limit = self.get_param_memory("MapSpeedLimit")
+
+    next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True)
+    next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0)
+    next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0)
+    next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0)
+
+    position = self.get_param_memory("LastGPSPosition", is_json=True)
+    lat = position.get("latitude", 0)
+    lon = position.get("longitude", 0)
+
+    if next_map_speed_limit_value > 1:
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
+
+      if self.prv_speed_limit < next_map_speed_limit_value:
+        max_d = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego
+      else:
+        max_d = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego
+
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+  @property
+  def experimental_mode(self):
+    return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode
+
+  @property
+  def desired_speed_limit(self):
+    if self.speed_limit > 1:
+      self.update_previous_limit(self.speed_limit)
+      return self.speed_limit + self.offset
+    return 0
+
+  @property
+  def offset(self):
+    if self.speed_limit < 13.5:
+      return self.frogpilot_toggles.offset1
+    if self.speed_limit < 24:
+      return self.frogpilot_toggles.offset2
+    if self.speed_limit < 29:
+      return self.frogpilot_toggles.offset3
+    return self.frogpilot_toggles.offset4
+
+  @property
+  def speed_limit(self):
+    limits = [self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [float(limit) for limit in limits if limit > 1]
+
+    if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits:
+      return max(filtered_limits)
+    if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits:
+      return min(filtered_limits)
+
+    speed_limits = {
+      "Offline Maps": self.map_speed_limit,
+      "Navigation": self.nav_speed_limit,
+    }
+
+    for priority in [
+      self.frogpilot_toggles.speed_limit_priority1,
+      self.frogpilot_toggles.speed_limit_priority2,
+      self.frogpilot_toggles.speed_limit_priority3,
+    ]:
+      if speed_limits.get(priority, 0) in filtered_limits:
+        return speed_limits[priority]
+
+    if self.frogpilot_toggles.use_previous_limit:
+      return self.prv_speed_limit
+
+    if self.frogpilot_toggles.use_set_speed:
+      return self.max_speed_limit
+
+    return 0
+
+SpeedLimitController = SpeedLimitController()

selfdrive/navd/navd.py

@@ -74,6 +74,8 @@ class RouteEngine:
     self.approaching_turn = False
     self.update_toggles = False
 
+    self.nav_speed_limit = 0
+
   def update(self):
     self.sm.update(0)
 
@@ -276,6 +278,7 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
+      self.nav_speed_limit = 0
       self.pm.send('navInstruction', msg)
       return
 
@@ -350,6 +353,9 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+      self.nav_speed_limit = closest.annotations['maxspeed']
+    if not self.localizer_valid or ('maxspeed' not in closest.annotations):
+      self.nav_speed_limit = 0
 
     # Speed limit sign type
     if 'speedLimitSign' in step:
@@ -405,6 +411,7 @@ class RouteEngine:
 
     frogpilotNavigation.approachingIntersection = self.approaching_intersection
     frogpilotNavigation.approachingTurn = self.approaching_turn
+    frogpilotNavigation.navigationSpeedLimit = self.nav_speed_limit
 
     self.pm.send('frogpilotNavigation', frogpilot_plan_send)
 

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -54,11 +54,14 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
 
   auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
-  speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
+  speedLimit = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
   speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  if (speedLimitController) {
+    speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
+  }
 
-  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
-  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
+  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
+  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) && !(speedLimitController && !useViennaSLCSign) || (speedLimitController && useViennaSLCSign);
   is_metric = s.scene.is_metric;
   speedUnit =  s.scene.is_metric ? tr("km/h") : tr("mph");
   hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE);
@@ -91,6 +94,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
 
   QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
+  QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
   QString speedStr = QString::number(std::nearbyint(speed));
   QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
 
@@ -166,11 +170,20 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.setPen(QPen(blackColor(), 6));
     p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
 
-    p.setFont(InterFont(28, QFont::DemiBold));
-    p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
-    p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
-    p.setFont(InterFont(70, QFont::Bold));
-    p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+    if (speedLimitController && showSLCOffset) {
+      p.setFont(InterFont(28, QFont::DemiBold));
+      p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+      p.setFont(InterFont(70, QFont::Bold));
+      p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+      p.setFont(InterFont(50, QFont::DemiBold));
+      p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+    } else {
+      p.setFont(InterFont(28, QFont::DemiBold));
+      p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+      p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+      p.setFont(InterFont(70, QFont::Bold));
+      p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+    }
   }
 
   // EU (Vienna style) sign
@@ -181,9 +194,16 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.setPen(QPen(Qt::red, 20));
     p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
 
-    p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
     p.setPen(blackColor());
-    p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
+    if (showSLCOffset) {
+      p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+      p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
+      p.setFont(InterFont(40, QFont::DemiBold));
+      p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
+    } else {
+      p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+      p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
+    }
   }
 
   // current speed
@@ -549,6 +569,11 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
 
   reverseCruise = scene.reverse_cruise;
 
+  speedLimitController = scene.speed_limit_controller;
+  showSLCOffset = speedLimitController && scene.show_slc_offset;
+  slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
+  useViennaSLCSign = scene.use_vienna_slc_sign;
+
   trafficModeActive = scene.traffic_mode_active;
 }
 
@@ -590,6 +615,7 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
     {14, tr("Experimental Mode activated for slower lead")},
     {15, tr("Experimental Mode activated for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))},
     {16, tr("Experimental Mode forced on for stop light") + (mapOpen ? tr("") : tr(" or stop sign"))},
+    {17, tr("Experimental Mode activated due to no speed limit")},
   };
 
   if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -61,11 +61,15 @@ private:
   bool reverseCruise;
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
+  bool showSLCOffset;
+  bool speedLimitController;
   bool trafficModeActive;
+  bool useViennaSLCSign;
 
   float accelerationConversion;
   float cruiseAdjustment;
   float distanceConversion;
+  float slcSpeedLimitOffset;
   float speedConversion;
 
   int alertSize;

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -98,6 +98,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   QPoint pos = e->pos();
 
   QRect maxSpeedRect(7, 25, 225, 225);
+  QRect speedLimitRect(7, 250, 225, 225);
 
   if (maxSpeedRect.contains(pos) && scene.reverse_cruise_ui) {
     scene.reverse_cruise = !scene.reverse_cruise;
@@ -106,6 +107,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
     return;
   }
 
+  if (speedLimitRect.contains(pos) && scene.show_slc_offset_ui) {
+    scene.show_slc_offset = !scene.show_slc_offset;
+    params.putBoolNonBlocking("ShowSLCOffset", scene.show_slc_offset);
+    return;
+  }
+
   if (scene.experimental_mode_via_screen && pos != timeoutPoint) {
     if (clickTimer.isActive()) {
       clickTimer.stop();

selfdrive/ui/ui.cc

@@ -237,6 +237,8 @@ static void update_state(UIState *s) {
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
     scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
+    scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
+    scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -306,6 +308,11 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   scene.reverse_cruise = quality_of_life_controls && params.getBool("ReverseCruise");
   scene.reverse_cruise_ui = params.getBool("ReverseCruiseUI");
 
+  scene.speed_limit_controller = scene.longitudinal_control && params.getBool("SpeedLimitController");
+  scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset");
+  scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI");
+  scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna");
+
   scene.tethering_config = params.getInt("TetheringEnabled");
   if (scene.tethering_config == 2) {
     WifiManager(s).setTetheringEnabled(true);

selfdrive/ui/ui.h

@@ -139,13 +139,19 @@ typedef struct UIScene {
   bool right_hand_drive;
   bool show_aol_status_bar;
   bool show_cem_status_bar;
+  bool show_slc_offset;
+  bool show_slc_offset_ui;
+  bool speed_limit_controller;
   bool tethering_enabled;
   bool traffic_mode;
   bool traffic_mode_active;
   bool use_kaofui_icons;
+  bool use_vienna_slc_sign;
 
   float adjusted_cruise;
   float lead_detection_threshold;
+  float speed_limit;
+  float speed_limit_offset;
 
   int alert_size;
   int conditional_speed;