Dev UI

Dev UI

Update onroad.cc

Update car.capnp

Update car.capnp

Co-Authored-By: Comma Device <[email protected]>
Co-Authored-By: Willem Melching <[email protected]>

Author: 3 people

cereal/car.capnp

@@ -197,6 +197,10 @@ struct CarState {
   # clutch (manual transmission only)
   clutchPressed @28 :Bool;
 
+  lkMode @41 :Bool;
+  engineRPM @42 :Float32;
+  brakeToggle @43 :Bool;
+
   # which packets this state came from
   canMonoTimes @12: List(UInt64);
 

selfdrive/assets/img_brake_disc.png

@@ -13,6 +13,7 @@
 
 def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
   signals = [
+    ("ENGINE_RPM", "POWERTRAIN_DATA"),
     ("XMISSION_SPEED", "ENGINE_DATA"),
     ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"),
     ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"),
@@ -168,6 +169,7 @@ def __init__(self, CP):
     self.brake_switch_active = False
     self.cruise_setting = 0
     self.v_cruise_pcm_prev = 0
+    self.engineRPM = 0
 
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
@@ -229,6 +231,7 @@ def update(self, cp, cp_cam, cp_body):
     ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
       250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
     ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
+    self.engineRPM = cp.vl["POWERTRAIN_DATA"]["ENGINE_RPM"]
 
     # TODO: set for all cars
     if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,

selfdrive/car/honda/carstate.py

@@ -380,6 +380,10 @@ def init(CP, logcan, sendcan):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
 
+
+    ret.lkMode = self.CS.lkMode
+    ret.engineRPM = self.CS.engineRPM
+
     buttonEvents = []
 
     if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:

selfdrive/car/honda/interface.py

@@ -3,6 +3,7 @@
 #include <cmath>
 
 #include <QDebug>
+#include <QString>
 
 #include "selfdrive/common/timing.h"
 #include "selfdrive/ui/qt/util.h"
@@ -171,8 +172,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 OnroadHud::OnroadHud(QWidget *parent) : QWidget(parent) {
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size});
-
-  connect(this, &OnroadHud::valueChanged, [=] { update(); });
+  brake_img = loadPixmap("../assets/img_brake_disc.png", {img_size, img_size});
 }
 
 void OnroadHud::updateState(const UIState &s) {
@@ -185,27 +185,176 @@ void OnroadHud::updateState(const UIState &s) {
   if (cruise_set && !s.scene.is_metric) {
     maxspeed *= KM_TO_MILE;
   }
-  QString maxspeed_str = cruise_set ? QString::number(std::nearbyint(maxspeed)) : "N/A";
-  float cur_speed = sm["carState"].getCarState().getVEgo() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  QString maxspeed_str = cruise_set ? QString::number(std::nearbyint(maxspeed)) : "-";
+  float cur_speed = std::max(0.0, sm["carState"].getCarState().getVEgo() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH));
 
   setProperty("is_cruise_set", cruise_set);
   setProperty("speed", QString::number(std::nearbyint(cur_speed)));
   setProperty("maxSpeed", maxspeed_str);
   setProperty("speedUnit", s.scene.is_metric ? "km/h" : "mph");
   setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE);
   setProperty("status", s.status);
-
-  // update engageability and DM icons at 2Hz
-  if (sm.frame % (UI_FREQ / 2) == 0) {
-    setProperty("engageable", cs.getEngageable() || cs.getEnabled());
-    setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode());
-  }
+  setProperty("engageable", cs.getEngageable() || cs.getEnabled());
+  setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode());
+  // -1.96133 is -0.2g of braking force, which is where most one-pedal enabled cars turn on brakelight -wn2
+  setProperty("braking", sm["carControl"].getCarControl().getActuators().getAccel() < -1.96133 || sm["carState"].getCarState().getBrakePressed() || sm["carState"].getCarState().getBrakeLightsDEPRECATED());
+  
+  const auto leadOne = sm["radarState"].getRadarState().getLeadOne();
+  setProperty("lead_d_rel", leadOne.getDRel());
+  setProperty("lead_v_rel", leadOne.getVRel());
+  setProperty("lead_status", leadOne.getStatus());
+  setProperty("angleSteers", sm["carState"].getCarState().getSteeringAngleDeg());
+
+  // If your car uses INDI or LQR, adjust this accordingly. -wirelessnet2
+  setProperty("steerAngleDesired", sm["controlsState"].getControlsState().getLateralControlState().getPidState().getSteeringAngleDesiredDeg());
+  setProperty("engineRPM", sm["carState"].getCarState().getEngineRPM());
 }
 
 void OnroadHud::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   p.setRenderHint(QPainter::Antialiasing);
 
+int NvgWindow::devUiDrawElement(QPainter &p, int x, int y, const char* value, const char* label, const char* units, QColor &color) {
+  configFont(p, "Open Sans", 30 * 2, "SemiBold");
+  drawColoredText(p, x + 92, y + 80, QString(value), color);
+
+  configFont(p, "Open Sans", 28, "Regular");
+  drawText(p, x + 92, y + 80 + 42, QString(label), 255);
+
+  if (strlen(units) > 0) {
+    p.save();
+    p.translate(x + 54 + 30 - 3 + 92, y + 37 + 25);
+    p.rotate(-90);
+    drawText(p, 0, 0, QString(units), 255);
+    p.restore();
+  }
+
+  return 110;
+}
+
+void NvgWindow::drawLeftDevUi(QPainter &p, int x, int y) {
+  int rh = 5;
+  int ry = y;
+
+  // Add Relative Distance to Primary Lead Car
+  // Unit: Meters
+  if (true) {
+    char val_str[8];
+    char units_str[8];
+    QColor valueColor = QColor(255, 255, 255, 255);
+
+    if (lead_status) {
+      // Orange if close, Red if very close
+      if (lead_d_rel < 5) {
+        valueColor = QColor(255, 0, 0, 255); 
+      } else if (lead_d_rel < 15) {
+        valueColor = QColor(255, 188, 0, 255);
+      }
+      snprintf(val_str, sizeof(val_str), "%d", (int)lead_d_rel);
+    } else {
+      snprintf(val_str, sizeof(val_str), "-");
+    }
+
+    snprintf(units_str, sizeof(units_str), "m");
+
+    rh += devUiDrawElement(p, x, ry, val_str, "REL DIST", units_str, valueColor);
+    ry = y + rh;
+  }
+
+  // Add Relative Velocity vs Primary Lead Car
+  // Unit: kph if metric, else mph
+  if (true) {
+    char val_str[8];
+    QColor valueColor = QColor(255, 255, 255, 255);
+
+     if (lead_status) {
+       // Red if approaching faster than 10mph
+       // Orange if approaching (negative)
+       if (lead_v_rel < -4.4704) {
+        valueColor = QColor(255, 0, 0, 255); 
+       } else if (lead_v_rel < 0) {
+         valueColor = QColor(255, 188, 0, 255);
+       }
+
+       if (speedUnit == "mph") {
+         snprintf(val_str, sizeof(val_str), "%d", (int)(lead_v_rel * 2.236936)); //mph
+       } else {
+         snprintf(val_str, sizeof(val_str), "%d", (int)(lead_v_rel * 3.6)); //kph
+       }
+     } else {
+       snprintf(val_str, sizeof(val_str), "-");
+     }
+
+    rh += devUiDrawElement(p, x, ry, val_str, "REL SPEED", speedUnit.toStdString().c_str(), valueColor);
+    ry = y + rh;
+  }
+
+  // Add Real Steering Angle
+  // Unit: Degrees
+  if (true) {
+    char val_str[8];
+    QColor valueColor = QColor(255, 255, 255, 255);
+
+    // Red if large steering angle
+    // Orange if moderate steering angle
+    if (std::fabs(angleSteers) > 12) {
+      valueColor = QColor(255, 0, 0, 255);
+    } else if (std::fabs(angleSteers) > 6) {
+      valueColor = QColor(255, 188, 0, 255);
+    }
+
+    snprintf(val_str, sizeof(val_str), "%.0f%s%s", angleSteers , "°", "");
+
+    rh += devUiDrawElement(p, x, ry, val_str, "REAL STEER", "", valueColor);
+    ry = y + rh;
+  }
+
+  // Add Desired Steering Angle
+  // Unit: Degrees
+  if (false) {
+    char val_str[8];
+    QColor valueColor = QColor(255, 255, 255, 255);
+
+    if (status != STATUS_DISENGAGED) {
+      // Red if large steering angle
+      // Orange if moderate steering angle
+      if (std::fabs(angleSteers) > 12) {
+        valueColor = QColor(255, 0, 0, 255);
+      } else if (std::fabs(angleSteers) > 6) {
+        valueColor = QColor(255, 188, 0, 255);
+      }
+
+      snprintf(val_str, sizeof(val_str), "%.0f%s%s", steerAngleDesired, "°", "");
+    } else {
+      snprintf(val_str, sizeof(val_str), "-");
+    }
+
+    rh += devUiDrawElement(p, x, ry, val_str, "DESIR STEER", "", valueColor);
+    ry = y + rh;
+  }
+
+  // Add Engine RPM
+  // Unit: RPM
+  if (true) {
+    char val_str[8];
+    QColor valueColor = QColor(255, 255, 255, 255);
+
+    if(engineRPM == 0) {
+      snprintf(val_str, sizeof(val_str), "OFF");
+    } else {
+      snprintf(val_str, sizeof(val_str), "%d", (engineRPM));
+    }
+
+    rh += devUiDrawElement(p, x, ry, val_str, "ENG RPM", "", valueColor);
+    ry = y + rh;
+  }
+
+  rh += 25;
+  p.setBrush(QColor(0, 0, 0, 0));
+  QRect ldu(x, y, 184, rh);
+  p.drawRoundedRect(ldu, 20, 20); 
+}
+
   // Header gradient
   QLinearGradient bg(0, header_h - (header_h / 2.5), 0, header_h);
   bg.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.45));
@@ -241,10 +390,21 @@ void OnroadHud::paintEvent(QPaintEvent *event) {
              engage_img, bg_colors[status], 1.0);
   }
 
-  // dm icon
   if (!hideDM) {
-    drawIcon(p, radius / 2 + (bdr_s * 2), rect().bottom() - footer_h / 2,
+    // left Dev UI
+    drawLeftDevUi(p, bdr_s * 2, bdr_s * 2 + rc.height());
+
+    // dm icon
+    drawIcon(p, rc.center().x(), rect().bottom() - footer_h / 2 + 50,
              dm_img, QColor(0, 0, 0, 70), dmActive ? 1.0 : 0.2);
+
+    p.setOpacity(1.0); // dmActive will determine opacity of brake_img's bg without this :/ -wirelessnet2
+
+    // brake icon
+    if (false) {
+      drawIcon(p, rc.center().x() + 250, rect().bottom() - footer_h / 2 + 50,
+                brake_img, QColor(0, 0, 0, 70), braking ? 1.0 : 0.2);
+    }
   }
 }
 
@@ -258,7 +418,17 @@ void OnroadHud::drawText(QPainter &p, int x, int y, const QString &text, int alp
   p.drawText(real_rect.x(), real_rect.bottom(), text);
 }
 
-void OnroadHud::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) {
+void NvgWindow::drawColoredText(QPainter &p, int x, int y, const QString &text, QColor &color) {
+  QFontMetrics fm(p.font());
+  QRect init_rect = fm.boundingRect(text);
+  QRect real_rect = fm.boundingRect(init_rect, 0, text);
+  real_rect.moveCenter({x, y - real_rect.height() / 2});
+
+  p.setPen(color);
+  p.drawText(real_rect.x(), real_rect.bottom(), text);
+}
+
+void NvgWindow::drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity) {
   p.setPen(Qt::NoPen);
   p.setBrush(bg);
   p.drawEllipse(x - radius / 2, y - radius / 2, radius, radius);
@@ -320,7 +490,27 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) {
 
   // paint path
   QLinearGradient bg(0, height(), 0, height() / 4);
-  if (scene.end_to_end) {
+  // wirelessnet2's rainbow barf path
+  if (scene.enabled) {
+    // openpilot is not disengaged
+    if (scene.steeringPressed) {
+      // The user is applying torque to the steering wheel
+      bg.setColorAt(0, QColor(0, 191, 255, 255));
+      bg.setColorAt(1, QColor(0, 95, 128, 50));
+    } else {
+      // Draw colored track
+      int torqueScale = (int)std::fabs(510 * (float)scene.pidStateOutput);
+      int red_lvl = std::fmin(255, torqueScale);
+      int green_lvl = std::fmin(255, 510 - torqueScale);
+      bg.setColorAt(0, QColor(red_lvl, green_lvl, 0, 255));
+      bg.setColorAt(1, QColor((int)(0.5 * red_lvl), (int)(0.5 * green_lvl), 0, 50));
+    }
+  } else if (!scene.end_to_end) {
+    // Draw white track when disengaged and not end_to_end
+    bg.setColorAt(0, whiteColor());
+    bg.setColorAt(1, whiteColor(0));
+  } else {
+    // Draw red vision track when disengaged and end_to_end
     const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation();
     float orientation_future = 0;
     if (orientation.getZ().size() > 16) {
@@ -330,15 +520,11 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) {
     float curve_hue = fmax(70, 112 - (orientation_future * 420));
     // FIXME: painter.drawPolygon can be slow if hue is not rounded
     curve_hue = int(curve_hue * 100 + 0.5) / 100;
-
-    bg.setColorAt(0.0 / 1.5, QColor::fromHslF(148 / 360., 1.0, 0.5, 1.0));
-    bg.setColorAt(0.55 / 1.5, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.8));
-    bg.setColorAt(0.9 / 1.5, QColor::fromHslF(curve_hue / 360., 1.0, 0.65, 0.6));
-    bg.setColorAt(1.0, QColor::fromHslF(curve_hue / 360., 1.0, 0.65, 0.0));
-  } else {
-    bg.setColorAt(0, whiteColor());
-    bg.setColorAt(1, whiteColor(0));
+    bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4));
+    bg.setColorAt(0.75 / 1.5, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.35));
+    bg.setColorAt(1.0, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.0));
   }
+
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices.v, scene.track_vertices.cnt);
 }

selfdrive/ui/qt/onroad.cc

@@ -66,8 +66,46 @@ class OnroadAlerts : public QWidget {
 class NvgWindow : public CameraViewWidget {
   Q_OBJECT
 
+  Q_PROPERTY(bool braking MEMBER braking);
+  Q_PROPERTY(int lead_status MEMBER lead_status);
+  Q_PROPERTY(float lead_d_rel MEMBER lead_d_rel);
+  Q_PROPERTY(float lead_v_rel MEMBER lead_v_rel);
+  Q_PROPERTY(float angleSteers MEMBER angleSteers);
+  Q_PROPERTY(float steerAngleDesired MEMBER steerAngleDesired);
+  Q_PROPERTY(int engineRPM MEMBER engineRPM);
+
 public:
   explicit NvgWindow(VisionStreamType type, QWidget* parent = 0);
+  void updateState(const UIState &s);
+
+private:
+  void drawIcon(QPainter &p, int x, int y, QPixmap &img, QBrush bg, float opacity);
+  void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
+  void drawLeftDevUi(QPainter &p, int x, int y);
+  int devUiDrawElement(QPainter &p, int x, int y, const char* value, const char* label, const char* units, QColor &color);
+  void drawColoredText(QPainter &p, int x, int y, const QString &text, QColor &color);
+
+  QPixmap engage_img;
+  QPixmap dm_img;
+  QPixmap brake_img;
+  const int radius = 160;
+  const int img_size = (radius / 2) * 1.5;
+  QString speed;
+  QString speedUnit;
+  QString maxSpeed;
+  bool is_cruise_set = false;
+  bool engageable = false;
+  bool dmActive = false;
+  bool hideDM = false;
+  int status = STATUS_DISENGAGED;
+
+  bool braking = false;
+  int lead_status;
+  float lead_d_rel = 0;
+  float lead_v_rel = 0;
+  float angleSteers = 0;
+  float steerAngleDesired = 0;
+  int engineRPM = 0;
 
 protected:
   void paintGL() override;