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>

release/files_common

@@ -572,5 +572,6 @@ selfdrive/frogpilot/controls/lib/frogpilot_functions.py
 selfdrive/frogpilot/controls/lib/frogpilot_variables.py
 selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py
 selfdrive/frogpilot/controls/lib/model_manager.py
+selfdrive/frogpilot/controls/lib/speed_limit_controller.py
 selfdrive/frogpilot/fleet_manager/fleet_manager.py
 selfdrive/frogpilot/navigation/mapd.py

selfdrive/controls/controlsd.py

@@ -898,7 +898,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

@@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 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
 
@@ -61,6 +62,7 @@ class FrogPilotPlanner:
     self.frame = 0
     self.mtsc_target = 0
     self.road_curvature = 0
+    self.slc_target = 0
     self.speed_jerk = 0
 
   def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, radarState, frogpilot_toggles):
@@ -201,7 +203,14 @@ class FrogPilotPlanner:
     else:
       self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
 
-    targets = [self.mtsc_target]
+    # Pfeiferj's Speed Limit Controller
+    if frogpilot_toggles.speed_limit_controller:
+      SpeedLimitController.update(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles)
+      self.slc_target = SpeedLimitController.desired_speed_limit
+    else:
+      self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
+
+    targets = [self.mtsc_target, self.slc_target - v_ego_diff]
     filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
 
     return min(filtered_targets)
@@ -224,6 +233,9 @@ class FrogPilotPlanner:
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel
 
+    frogpilotPlan.slcSpeedLimit = self.slc_target
+    frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+
     frogpilotPlan.vCruise = float(self.v_cruise)
 
     pm.send('frogpilotPlan', frogpilot_plan_send)

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -5,6 +5,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, CRUISING_SPEED, PROBABILITY
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
 SLOW_DOWN_DISTANCE = [20, 30., 50., 70., 80., 90., 105., 120.]
@@ -55,6 +56,10 @@ class ConditionalExperimentalMode:
       self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
       return True
 
+    if SpeedLimitController.experimental_mode:
+      self.status_value = 9
+      return True
+
     if (not self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit) or (self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit_lead):
       self.status_value = 11 if self.lead_detected else 12
       return True

selfdrive/frogpilot/controls/lib/speed_limit_controller.py

@@ -0,0 +1,129 @@
+# 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
+TO_DEGREES = 180 / math.pi
+
+# 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.nav_speed_limit = 0  # m/s
+    self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
+
+  def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles):
+    self.write_map_state(v_ego)
+    self.nav_speed_limit = navigationSpeedLimit
+
+    self.frogpilot_toggles = frogpilot_toggles
+
+  def get_param_memory(self, key, is_json=False, default=None):
+    try:
+      data = self.params_memory.get(key)
+      if not is_json and data is not None:
+        return float(data.decode('utf-8'))
+      return json.loads(data) if is_json else data
+    except:
+      return default
+
+  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, default={})
+    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, default={})
+    lat = position.get("latitude", 0)
+    lon = position.get("longitude", 0)
+
+    if self.prv_speed_limit < 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)
+      max_d = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+    if self.prv_speed_limit > 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)
+      max_d = self.frogpilot_toggles.map_speed_lookahead_higher_lower * v_ego
+      if d < max_d:
+        self.map_speed_limit = next_map_speed_limit_value
+
+  @property
+  def speed_limit(self):
+    limits = [self.map_speed_limit, self.nav_speed_limit]
+    filtered_limits = [limit for limit in limits if limit is not None and limit > 1]
+
+    if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits:
+      return float(max(filtered_limits))
+    elif self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits:
+      return float(min(filtered_limits))
+
+    speed_limits = {
+      "Offline Maps": self.map_speed_limit,
+      "Navigation": self.nav_speed_limit,
+    }
+
+    priorities = [
+      self.frogpilot_toggles.speed_limit_priority1,
+      self.frogpilot_toggles.speed_limit_priority2,
+      self.frogpilot_toggles.speed_limit_priority3,
+    ]
+
+    for priority in priorities:
+      if priority in speed_limits and speed_limits[priority] in filtered_limits:
+        return float(speed_limits[priority])
+
+    if self.frogpilot_toggles.use_previous_limit:
+      return float(self.prv_speed_limit)
+
+    return 0
+
+  @property
+  def offset(self):
+    if self.speed_limit < 13.5:
+      return self.frogpilot_toggles.offset1
+    elif self.speed_limit < 24:
+      return self.frogpilot_toggles.offset2
+    elif self.speed_limit < 29:
+      return self.frogpilot_toggles.offset3
+    else:
+      return self.frogpilot_toggles.offset4
+
+  @property
+  def desired_speed_limit(self):
+    if self.speed_limit > 1:
+      self.update_previous_limit(self.speed_limit)
+      return self.speed_limit + self.offset
+    else:
+      return 0
+
+  def update_previous_limit(self, speed_limit):
+    if self.prv_speed_limit != speed_limit:
+      self.params.put_float("PreviousSpeedLimit", speed_limit)
+      self.prv_speed_limit = speed_limit
+
+  @property
+  def experimental_mode(self):
+    return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode
+
+SpeedLimitController = SpeedLimitController()

selfdrive/navd/navd.py

@@ -74,6 +74,8 @@ class RouteEngine:
     self.approaching_intersection = False
     self.approaching_turn = False
 
+    self.nav_speed_limit = 0
+
   def update(self):
     self.sm.update(0)
 
@@ -273,6 +275,7 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
+      self.nav_speed_limit = 0
       self.pm.send('navInstruction', msg)
       return
 
@@ -347,6 +350,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:
@@ -402,6 +408,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.cc

@@ -104,6 +104,9 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   QRect maxSpeedRect(7, 25, 225, 225);
   bool isMaxSpeedClicked = maxSpeedRect.contains(e->pos()) && scene.reverse_cruise_ui;
 
+  QRect speedLimitRect(7, 250, 225, 225);
+  bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()) && scene.show_slc_offset_ui;
+
   if (isMaxSpeedClicked) {
     bool currentReverseCruise = scene.reverse_cruise;
     uiState()->scene.reverse_cruise = !currentReverseCruise;
@@ -112,6 +115,13 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
     return;
   }
 
+  if (isSpeedLimitClicked) {
+    bool currentShowSLCOffset = scene.show_slc_offset;
+    scene.show_slc_offset = !currentShowSLCOffset;
+    params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset);
+    return;
+  }
+
   if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
     if (clickTimer.isActive()) {
       clickTimer.stop();
@@ -353,11 +363,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);
@@ -393,6 +406,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)) : "–";
 
@@ -468,12 +482,24 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
     p.setPen(QPen(blackColor(), 6));
     p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
 
+    p.save();
+    p.setOpacity(1.0);
+    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);
     }
+    p.restore();
+  }
 
   // EU (Vienna style) sign
   if (has_eu_speed_limit) {
@@ -483,10 +509,20 @@ 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.save();
+    p.setOpacity(1.0);
     p.setPen(blackColor());
+    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);
     }
+    p.restore();
+  }
 
   // current speed
   p.setFont(InterFont(176, QFont::Bold));
@@ -825,6 +861,11 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   onroadDistanceButton = scene.onroad_distance_button;
 
+  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;
 }
 

selfdrive/ui/qt/onroad.h

@@ -152,11 +152,15 @@ private:
   bool onroadDistanceButton;
   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/ui.cc

@@ -238,6 +238,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();
@@ -292,6 +294,11 @@ void ui_update_frogpilot_params(UIState *s) {
   bool quality_of_life_controls = params.getBool("QOLControls");
   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");
 }
 
 void UIState::updateStatus() {

selfdrive/ui/ui.h

@@ -201,13 +201,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;