diff --git a/frogpilot/assets/other_images/paused_icon.png b/frogpilot/assets/other_images/paused_icon.png new file mode 100644 index 00000000..40c87941 Binary files /dev/null and b/frogpilot/assets/other_images/paused_icon.png differ diff --git a/frogpilot/assets/other_images/speed_icon.png b/frogpilot/assets/other_images/speed_icon.png new file mode 100644 index 00000000..8c022fb6 Binary files /dev/null and b/frogpilot/assets/other_images/speed_icon.png differ diff --git a/frogpilot/controls/frogpilot_card.py b/frogpilot/controls/frogpilot_card.py index 26836484..91716cae 100644 --- a/frogpilot/controls/frogpilot_card.py +++ b/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 diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index b91703bc..e5400256 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/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 diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 4da26987..ae304571 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/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; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 04acd9c1..e69fcf04 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/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; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 10be8188..503c029d 100644 --- a/selfdrive/controls/controlsd.py +++ b/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: