Controls - Speed Limit Controller - Confirm New Speed Limits

Don't automatically start using the new speed limit until it's been manually confirmed.

selfdrive/car/interfaces.py

@@ -563,6 +563,10 @@ class CarStateBase(ABC):
     self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
 
     # FrogPilot variables
+    self.cruise_decreased = False
+    self.cruise_decreased_previously = False
+    self.cruise_increased = False
+    self.cruise_increased_previously = False
     self.lkas_enabled = False
     self.lkas_previously_enabled = False
 

selfdrive/car/toyota/carstate.py

@@ -191,6 +191,11 @@ class CarState(CarStateBase):
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
     # FrogPilot CarState functions
+    self.cruise_decreased_previously = self.cruise_decreased
+    self.cruise_decreased = self.pcm_acc_status == 10
+    self.cruise_increased_previously = self.cruise_increased
+    self.cruise_increased = self.pcm_acc_status == 9
+
     fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1
     fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1
 

selfdrive/car/toyota/interface.py

@@ -160,6 +160,8 @@ class CarInterface(CarInterfaceBase):
 
     if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
       ret.buttonEvents = [
+        *create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}),
+        *create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}),
         *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
         *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
       ]

selfdrive/controls/controlsd.py

@@ -33,6 +33,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 from openpilot.system.hardware import HARDWARE
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
+from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
@@ -79,10 +80,13 @@ class Controls:
     self.onroad_distance_pressed = False
     self.openpilot_crashed_triggered = False
     self.speed_check = False
+    self.speed_limit_changed = False
 
     self.display_timer = 0
     self.drive_distance = 0
     self.drive_time = 0
+    self.previous_speed_limit = 0
+    self.speed_limit_timer = 0
 
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
@@ -475,7 +479,7 @@ class Controls:
   def state_transition(self, CS):
     """Compute conditional state transitions and execute actions on state transitions"""
 
-    self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.frogpilot_toggles)
+    self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.speed_limit_changed, self.frogpilot_toggles)
 
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
@@ -550,7 +554,7 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_toggles)
+          self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_toggles)
 
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES
@@ -956,8 +960,53 @@ class Controls:
     self.speed_check |= self.frogpilot_toggles.pause_lateral_below_signal and not (CS.leftBlinker or CS.rightBlinker)
     self.speed_check |= CS.standstill
 
+    if self.frogpilot_toggles.speed_limit_confirmation:
+      current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
+      desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
+
+      limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1
+
+      speed_limit_decreased = limit_changed and self.previous_speed_limit > desired_speed_limit
+      speed_limit_increased = limit_changed and self.previous_speed_limit < desired_speed_limit
+
+      self.previous_speed_limit = desired_speed_limit
+
+      if self.CP.pcmCruise and self.speed_limit_changed:
+        if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents):
+          self.params_memory.put_bool("SLCConfirmed", True)
+          self.params_memory.put_bool("SLCConfirmedPressed", True)
+        elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
+          self.params_memory.put_bool("SLCConfirmed", False)
+          self.params_memory.put_bool("SLCConfirmedPressed", True)
+
+      if speed_limit_decreased:
+        if self.frogpilot_toggles.speed_limit_confirmation_lower:
+          self.speed_limit_changed = True
+        else:
+          self.params_memory.put_bool("SLCConfirmed", True)
+      elif speed_limit_increased:
+        if self.frogpilot_toggles.speed_limit_confirmation_higher:
+          self.speed_limit_changed = True
+        else:
+          self.params_memory.put_bool("SLCConfirmed", True)
+
+      if self.params_memory.get_bool("SLCConfirmedPressed") or not abs(current_speed_limit - desired_speed_limit) > 1:
+        self.speed_limit_changed = False
+        self.params_memory.put_bool("SLCConfirmedPressed", False)
+
+      if self.speed_limit_changed:
+        self.speed_limit_timer += DT_CTRL
+        if self.speed_limit_timer >= 10:
+          self.speed_limit_changed = False
+          self.speed_limit_timer = 0
+      else:
+        self.speed_limit_timer = 0
+    else:
+      self.speed_limit_changed = False
+
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateral = self.always_on_lateral_active
+    FPCC.speedLimitChanged = self.speed_limit_changed
 
     return FPCC
 

selfdrive/controls/lib/drive_helpers.py

@@ -53,13 +53,13 @@ class VCruiseHelper:
   def v_cruise_initialized(self):
     return self.v_cruise_kph != V_CRUISE_UNSET
 
-  def update_v_cruise(self, CS, enabled, is_metric, frogpilot_toggles):
+  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
       if not self.CP.pcmCruise:
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_toggles)
+        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
@@ -69,7 +69,7 @@ class VCruiseHelper:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
 
-  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, frogpilot_toggles):
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press
     # would have the effect of both enabling and changing speed is checked after the state transition
     if not enabled:
@@ -96,6 +96,17 @@ class VCruiseHelper:
     if button_type is None:
       return
 
+    # Confirm or deny the new speed limit value
+    if speed_limit_changed:
+      if button_type == ButtonType.accelCruise:
+        self.params_memory.put_bool("SLCConfirmed", True)
+        self.params_memory.put_bool("SLCConfirmedPressed", True)
+        return
+      elif button_type == ButtonType.decelCruise:
+        self.params_memory.put_bool("SLCConfirmed", False)
+        self.params_memory.put_bool("SLCConfirmedPressed", True)
+        return
+
     # Don't adjust speed when pressing resume to exit standstill
     cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
     if button_type == ButtonType.accelCruise and cruise_standstill:
@@ -135,7 +146,7 @@ class VCruiseHelper:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_toggles) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_toggles) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
@@ -145,6 +156,9 @@ class VCruiseHelper:
     # 250kph or above probably means we never had a set speed
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
       self.v_cruise_kph = self.v_cruise_kph_last
+    else:
+      if desired_speed_limit != 0 and frogpilot_toggles.set_speed_limit:
+        self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH))
       else:
         self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
 

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -202,7 +202,14 @@ class FrogPilotPlanner:
     # 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
+      unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
+
+      if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0:
+        if self.params_memory.get_bool("SLCConfirmed"):
+          self.slc_target = unconfirmed_slc_target
+          self.params_memory.put_bool("SLCConfirmed", False)
+      elif unconfirmed_slc_target > 1:
+        self.slc_target = unconfirmed_slc_target
     else:
       self.slc_target = v_cruise + v_ego_diff if v_cruise != V_CRUISE_UNSET else 0
 
@@ -233,6 +240,7 @@ class FrogPilotPlanner:
 
     frogpilotPlan.slcSpeedLimit = self.slc_target
     frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
+    frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
 
     frogpilotPlan.tFollow = float(self.t_follow)
 

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -562,6 +562,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(const UIScene &scene) {
   speedLimitController = scene.speed_limit_controller;
   showSLCOffset = speedLimitController && scene.show_slc_offset;
   slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
+  speedLimitChanged = speedLimitController && scene.speed_limit_changed;
+  unconfirmedSpeedLimit = speedLimitController ? scene.unconfirmed_speed_limit : 0;
   useViennaSLCSign = scene.use_vienna_slc_sign;
 
   trafficModeActive = scene.traffic_mode_active;
@@ -577,6 +579,40 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
     map_settings_btn_bottom->setVisible(!hideBottomIcons);
     bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight);
   }
+
+  if (speedLimitChanged) {
+    drawSLCConfirmation(p);
+  }
+}
+
+void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
+  p.save();
+
+  QSize size = this->size();
+  int halfWidth = size.width() / 2;
+
+  QRect leftRect(0, 0, halfWidth, size.height());
+  QRect rightRect(halfWidth, 0, halfWidth, size.height());
+
+  p.setOpacity(0.5);
+  p.fillRect(leftRect, rightHandDM ? redColor() : greenColor());
+  p.fillRect(rightRect, rightHandDM ? greenColor() : redColor());
+  p.setOpacity(1.0);
+
+  p.setFont(InterFont(75, QFont::Bold));
+  p.setPen(Qt::white);
+
+  QString unitText = is_metric ? tr("kph") : tr("mph");
+  QString speedText = QString::number(std::nearbyint(unconfirmedSpeedLimit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
+  QString confirmText = tr("Confirm speed limit\n") + speedText;
+  QString ignoreText = tr("Ignore speed limit\n") + speedText;
+
+  QRect textRect(0, leftRect.height() / 2 - 225, halfWidth, leftRect.height() / 2);
+
+  p.drawText(textRect.translated(0, 0), Qt::AlignCenter, rightHandDM ? ignoreText : confirmText);
+  p.drawText(textRect.translated(halfWidth, 0), Qt::AlignCenter, rightHandDM ? confirmText : ignoreText);
+
+  p.restore();
 }
 
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -46,6 +46,7 @@ private:
   void paintFrogPilotWidgets(QPainter &p);
   void updateFrogPilotWidgets(const UIScene &scene);
 
+  void drawSLCConfirmation(QPainter &p);
   void drawStatusBar(QPainter &p);
 
   // FrogPilot variables
@@ -63,6 +64,7 @@ private:
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
   bool showSLCOffset;
+  bool speedLimitChanged;
   bool speedLimitController;
   bool trafficModeActive;
   bool useViennaSLCSign;
@@ -72,6 +74,7 @@ private:
   float distanceConversion;
   float slcSpeedLimitOffset;
   float speedConversion;
+  float unconfirmedSpeedLimit;
 
   int alertSize;
   int conditionalSpeed;

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -85,12 +85,25 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
   UIScene &scene = s->scene;
 
   // FrogPilot clickable widgets
+  QSize size = this->size();
+  QRect leftRect(0, 0, size.width() / 2, size.height());
+  QRect rightRect = leftRect.translated(size.width() / 2, 0);
+  bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
+  bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
+
   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 (isLeftSideClicked || isRightSideClicked) {
+    bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
+    paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed);
+    paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true);
+    return;
+  }
+
   if (isMaxSpeedClicked) {
     scene.reverse_cruise = !scene.reverse_cruise;
     params.putBoolNonBlocking("ReverseCruise", scene.reverse_cruise);

selfdrive/ui/ui.cc

@@ -228,6 +228,7 @@ static void update_state(UIState *s) {
   if (sm.updated("frogpilotCarControl")) {
     auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
     scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
+    scene.speed_limit_changed = scene.speed_limit_controller && frogpilotCarControl.getSpeedLimitChanged();
   }
   if (sm.updated("frogpilotCarState")) {
     auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState();
@@ -238,6 +239,7 @@ static void update_state(UIState *s) {
     scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
     scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
     scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
+    scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();

selfdrive/ui/ui.h

@@ -138,6 +138,7 @@ typedef struct UIScene {
   bool show_cem_status_bar;
   bool show_slc_offset;
   bool show_slc_offset_ui;
+  bool speed_limit_changed;
   bool speed_limit_controller;
   bool tethering_enabled;
   bool traffic_mode;
@@ -149,6 +150,7 @@ typedef struct UIScene {
   float lead_detection_threshold;
   float speed_limit;
   float speed_limit_offset;
+  float unconfirmed_speed_limit;
 
   int alert_size;
   int conditional_speed;