Pause Steering

frogpilot/controls/frogpilot_card.py

@@ -20,6 +20,7 @@ class FrogPilotCard:
     self.decel_pressed = False
     self.distancePressed_previously = False
     self.force_coast = False
+    self.pause_lateral = False
     self.pause_longitudinal = False
 
     self.gap_counter = 0
@@ -37,6 +38,8 @@ class FrogPilotCard:
       self.handle_experimental_mode(sm, frogpilot_toggles)
     elif sm["carControl"].longActive and getattr(frogpilot_toggles, f"force_coast_via_{key}"):
       self.force_coast = not self.force_coast
+    elif getattr(frogpilot_toggles, f"pause_lateral_via_{key}"):
+      self.pause_lateral = not self.pause_lateral
     elif sm["carControl"].longActive and getattr(frogpilot_toggles, f"pause_longitudinal_via_{key}"):
       self.pause_longitudinal = not self.pause_longitudinal
 
@@ -103,6 +106,7 @@ class FrogPilotCard:
     frogpilotCarState.distanceLongPressed = self.very_long_press_threshold > self.gap_counter >= self.long_press_threshold
     frogpilotCarState.distanceVeryLongPressed = self.gap_counter >= self.very_long_press_threshold
     frogpilotCarState.forceCoast = self.force_coast
+    frogpilotCarState.pauseLateral = self.pause_lateral
     frogpilotCarState.pauseLongitudinal = self.pause_longitudinal
 
     return frogpilotCarState

frogpilot/controls/frogpilot_planner.py

@@ -93,7 +93,9 @@ class FrogPilotPlanner:
 
     self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature
 
+    self.lateral_check = v_ego >= frogpilot_toggles.pause_lateral_below_speed
     self.lateral_check |= sm["carState"].standstill
+    self.lateral_check &= not sm["frogpilotCarState"].pauseLateral
 
     self.model_length = sm["modelV2"].position.x[-1]
 

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -12,6 +12,7 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent)
   pausedIcon = loadPixmap("../../frogpilot/assets/other_images/paused_icon.png", {widget_size, widget_size});
   speedIcon = loadPixmap("../../frogpilot/assets/other_images/speed_icon.png", {widget_size, widget_size});
   stopSignImg = loadPixmap("../../frogpilot/assets/other_images/stop_sign.png", {btn_size, btn_size});
+  turnIcon = loadPixmap("../../frogpilot/assets/other_images/turn_icon.png", {widget_size, widget_size});
 
   loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/lead_icon.gif", cemLeadIcon, QSize(widget_size, widget_size), this);
@@ -159,6 +160,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   forceCoast = frogpilotCarState.getForceCoast();
   laneWidthLeft = frogpilotPlan.getLaneWidthLeft();
   laneWidthRight = frogpilotPlan.getLaneWidthRight();
+  lateralPaused = frogpilotCarState.getPauseLateral();
   longitudinalPaused = frogpilotCarState.getPauseLongitudinal();
   redLight = frogpilotPlan.getRedLight();
   roadCurvature = frogpilotPlan.getRoadCurvature();
@@ -231,6 +233,13 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     }
   }
 
+  if (!hideBottomIcons && lateralPaused) {
+    paintLateralPaused(p);
+  } else {
+    lateralPausedPosition.setX(0);
+    lateralPausedPosition.setY(0);
+  }
+
   if (!hideBottomIcons && (forceCoast || longitudinalPaused)) {
     paintLongitudinalPaused(p);
   }
@@ -527,6 +536,35 @@ void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p)
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintLateralPaused(QPainter &p) {
+  if (dmIconPosition == QPoint(0, 0)) {
+    return;
+  }
+
+  p.save();
+
+  if (cemStatusPosition != QPoint(0, 0)) {
+    lateralPausedPosition = cemStatusPosition;
+  } else {
+    lateralPausedPosition.rx() = dmIconPosition.x();
+    lateralPausedPosition.ry() = dmIconPosition.y() - widget_size / 2;
+  }
+  lateralPausedPosition.rx() += rightHandDM ? -UI_BORDER_SIZE - widget_size - UI_BORDER_SIZE : UI_BORDER_SIZE + widget_size + UI_BORDER_SIZE;
+
+  QRect lateralWidget(lateralPausedPosition, QSize(widget_size, widget_size));
+
+  p.setBrush(blackColor(166));
+  p.setPen(QPen(QColor(bg_colors[STATUS_TRAFFIC_MODE_ENABLED]), 10));
+  p.drawRoundedRect(lateralWidget, 24, 24);
+
+  p.setOpacity(0.5);
+  p.drawPixmap(lateralWidget, turnIcon);
+  p.setOpacity(0.75);
+  p.drawPixmap(lateralWidget, pausedIcon);
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintLeadMetrics(QPainter &p, bool adjacent, QPointF *chevron, const cereal::RadarState::LeadData::Reader &lead_data) {
   float leadDistance = lead_data.getDRel() + (adjacent ? std::abs(lead_data.getYRel()) : 0.0f);
   float leadSpeed = std::max(lead_data.getVLead(), 0.0f);
@@ -602,7 +640,9 @@ void FrogPilotAnnotatedCameraWidget::paintLongitudinalPaused(QPainter &p) {
   p.save();
 
   QPoint longitudinalIconPosition;
-  if (cemStatusPosition != QPoint(0, 0)) {
+  if (lateralPausedPosition != QPoint(0, 0)) {
+    longitudinalIconPosition = lateralPausedPosition;
+  } else if (cemStatusPosition != QPoint(0, 0)) {
     longitudinalIconPosition = cemStatusPosition;
   } else {
     longitudinalIconPosition.rx() = dmIconPosition.x();

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -61,6 +61,7 @@ private:
   void paintCompass(QPainter &p);
   void paintCurveSpeedControl(QPainter &p);
   void paintCurveSpeedControlTraining(QPainter &p);
+  void paintLateralPaused(QPainter &p);
   void paintLongitudinalPaused(QPainter &p);
   void paintPedalIcons(QPainter &p);
   void paintRadarTracks(QPainter &p);
@@ -79,6 +80,7 @@ private:
   bool cscTraining;
   bool experimentalMode;
   bool forceCoast;
+  bool lateralPaused;
   bool longitudinalPaused;
   bool redLight;
 
@@ -118,9 +120,11 @@ private:
   QPixmap pausedIcon;
   QPixmap speedIcon;
   QPixmap stopSignImg;
+  QPixmap turnIcon;
 
   QPoint cemStatusPosition;
   QPoint compassPosition;
+  QPoint lateralPausedPosition;
 
   QSharedPointer<QMovie> cemCurveIcon;
   QSharedPointer<QMovie> cemLeadIcon;

selfdrive/controls/controlsd.py

@@ -116,7 +116,7 @@ class Controls:
     standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
     CC.latActive = (self.sm['selfdriveState'].active or self.sm['frogpilotCarState'].alwaysOnLateralEnabled) and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.CP.steerAtStandstill) and self.sm['frogpilotPlan'].lateralCheck
-    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
+    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['frogpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state