diff --git a/frogpilot/assets/other_images/turn_icon.png b/frogpilot/assets/other_images/turn_icon.png new file mode 100644 index 000000000..6b1377976 Binary files /dev/null and b/frogpilot/assets/other_images/turn_icon.png differ diff --git a/frogpilot/controls/frogpilot_card.py b/frogpilot/controls/frogpilot_card.py index b3cad5f7c..eca6e388a 100644 --- a/frogpilot/controls/frogpilot_card.py +++ b/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 diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 7cb686f52..e1a393eb6 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/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] diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 917fa298e..dc7894158 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/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(); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index ff4a849d1..9e2ed12bc 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/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 cemCurveIcon; QSharedPointer cemLeadIcon; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f65102e8f..930a8a593 100644 --- a/selfdrive/controls/controlsd.py +++ b/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