Force openpilot to Coast

frogpilot/controls/frogpilot_card.py

@@ -19,6 +19,7 @@ class FrogPilotCard:
     self.always_on_lateral_allowed = False
     self.decel_pressed = False
     self.distancePressed_previously = False
+    self.force_coast = False
 
     self.gap_counter = 0
 
@@ -33,6 +34,8 @@ class FrogPilotCard:
   def handle_button_event(self, key, sm, frogpilot_toggles):
     if sm["carControl"].longActive and getattr(frogpilot_toggles, f"experimental_mode_via_{key}"):
       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
 
   def handle_experimental_mode(self, sm, frogpilot_toggles):
     if frogpilot_toggles.conditional_experimental_mode:
@@ -89,10 +92,13 @@ class FrogPilotCard:
     if any(be.pressed and be.type == ButtonType.lkas for be in carState.buttonEvents):
       self.handle_button_event("lkas", sm, frogpilot_toggles)
 
+    self.force_coast &= not (carState.brakePressed or carState.gasPressed)
+
     frogpilotCarState.accelPressed = self.accel_pressed
     frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled
     frogpilotCarState.decelPressed = self.decel_pressed
     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
 
     return frogpilotCarState

frogpilot/controls/lib/frogpilot_acceleration.py

@@ -64,6 +64,8 @@ class FrogPilotAcceleration:
 
     if self.frogpilot_planner.tracking_lead:
       self.min_accel = ACCEL_MIN
+    elif sm["frogpilotCarState"].forceCoast:
+      self.min_accel = A_CRUISE_MIN_ECO
     elif (eco_gear or sport_gear) and frogpilot_toggles.map_deceleration:
       if eco_gear:
         self.min_accel = A_CRUISE_MIN_ECO

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -7,6 +7,8 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent)
 
   curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size});
   curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1));
+  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});
 
   loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this);
@@ -149,6 +151,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   cscSpeed = frogpilotPlan.getCscSpeed();
   cscTraining = frogpilotPlan.getCscTraining();
   experimentalMode = selfdriveState.getExperimentalMode();
+  forceCoast = frogpilotCarState.getForceCoast();
   redLight = frogpilotPlan.getRedLight();
   roadCurvature = frogpilotPlan.getRoadCurvature();
   roadName = QString::fromStdString(params_memory.get("RoadName"));
@@ -220,6 +223,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     }
   }
 
+  if (!hideBottomIcons && (forceCoast)) {
+    paintLongitudinalPaused(p);
+  }
+
   if (frogpilot_toggles.value("radar_tracks").toBool()) {
     paintRadarTracks(p);
   }
@@ -451,6 +458,36 @@ void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p)
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintLongitudinalPaused(QPainter &p) {
+  if (dmIconPosition == QPoint(0, 0)) {
+    return;
+  }
+
+  p.save();
+
+  QPoint longitudinalIconPosition;
+  if (cemStatusPosition != QPoint(0, 0)) {
+    longitudinalIconPosition = cemStatusPosition;
+  } else {
+    longitudinalIconPosition.rx() = dmIconPosition.x();
+    longitudinalIconPosition.ry() = dmIconPosition.y() - widget_size / 2;
+  }
+  longitudinalIconPosition.rx() += rightHandDM ? -UI_BORDER_SIZE - widget_size - UI_BORDER_SIZE : UI_BORDER_SIZE + widget_size + UI_BORDER_SIZE;
+
+  QRect longitudinalWidget(longitudinalIconPosition, QSize(widget_size, widget_size));
+
+  p.setBrush(blackColor(166));
+  p.setPen(QPen(QColor(bg_colors[STATUS_TRAFFIC_MODE_ENABLED]), 10));
+  p.drawRoundedRect(longitudinalWidget, 24, 24);
+
+  p.setOpacity(0.5);
+  p.drawPixmap(longitudinalWidget, speedIcon);
+  p.setOpacity(0.75);
+  p.drawPixmap(longitudinalWidget, pausedIcon);
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintRadarTracks(QPainter &p) {
   if (radar_tracks.empty()) {
     return;

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -52,6 +52,7 @@ private:
   void paintCompass(QPainter &p);
   void paintCurveSpeedControl(QPainter &p);
   void paintCurveSpeedControlTraining(QPainter &p);
+  void paintLongitudinalPaused(QPainter &p);
   void paintRadarTracks(QPainter &p);
   void paintRoadName(QPainter &p);
   void paintStandstillTimer(QPainter &p);
@@ -66,6 +67,7 @@ private:
   bool cscControllingSpeed;
   bool cscTraining;
   bool experimentalMode;
+  bool forceCoast;
   bool redLight;
 
   int animationFrameIndex;
@@ -95,6 +97,8 @@ private:
 
   QPixmap curveSpeedIcon;
   QPixmap curveSpeedIconFlipped;
+  QPixmap pausedIcon;
+  QPixmap speedIcon;
   QPixmap stopSignImg;
 
   QPoint cemStatusPosition;

selfdrive/controls/controlsd.py

@@ -211,7 +211,7 @@ class Controls:
     cs.uiAccelCmd = float(self.LoC.pid.i)
     cs.ufAccelCmd = float(self.LoC.pid.f)
     cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
-                         (self.sm['selfdriveState'].state == State.softDisabling))
+                         (self.sm['selfdriveState'].state == State.softDisabling) or self.sm["frogpilotCarState"].forceCoast)
 
     lat_tuning = self.CP.lateralTuning.which()
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle: