From a1dbcfd62be9285037e3740eb9723b2f7999e4b4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 8 Jul 2024 03:38:29 +0000 Subject: [PATCH 1/7] Revert "MADS: Honda Nidec: Fix enforce cruise cancel with stock PCM" --- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/interfaces.py | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d412f81142..ae8746d958 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -242,7 +242,7 @@ class CarState(CarStateBase): ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"] - ret.cruiseState.enabled = self.pcm_cruise_enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0 + ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0 ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 1aba4d3119..457b095e6a 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -298,7 +298,7 @@ class CarInterface(CarInterfaceBase): if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed: if ret.gasPressed and not ret.cruiseState.enabled: self.CS.accEnabled = False - self.CS.accEnabled = self.CS.pcm_cruise_enabled + self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled ret, self.CS = self.get_sp_common_state(ret, self.CS, min_enable_speed_pcm=(self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9c535244b8..a402a7797a 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -778,7 +778,6 @@ class CarStateBase(ABC): self.mads_enabled = False self.prev_mads_enabled = False self.control_initialized = False - self.pcm_cruise_enabled = False Q = [[0.0, 0.0], [0.0, 100.0]] R = 0.3 From 64a34f97aa70a88a875b7f50cf20f6bcb46c467a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 8 Jul 2024 09:13:37 -0400 Subject: [PATCH 2/7] Revert "remove nav apt dependencies (#32792)" This reverts commit 9a084a73c5c3e6386bc5f21c46062d46878f6ca2. --- tools/install_ubuntu_dependencies.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/install_ubuntu_dependencies.sh b/tools/install_ubuntu_dependencies.sh index 4373760ce6..1fc0709095 100755 --- a/tools/install_ubuntu_dependencies.sh +++ b/tools/install_ubuntu_dependencies.sh @@ -55,7 +55,8 @@ function install_ubuntu_common_requirements() { portaudio19-dev \ qml-module-qtquick2 \ qtmultimedia5-dev \ - qtdeclarative5-dev \ + qtlocation5-dev \ + qtpositioning5-dev \ qttools5-dev-tools \ libqt5svg5-dev \ libqt5serialbus5-dev \ From a64b17b680679ccbc4013f068e6d35fcb8afdf62 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 9 Jul 2024 22:16:30 -0400 Subject: [PATCH 3/7] fix division by 0 and negative values --- selfdrive/ui/qt/onroad/annotated_camera.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 59624c4f46..a99517d33d 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1535,7 +1535,8 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState } if (chevron_data == 3 || chevron_data == 4) { position = (chevron_data == 3) ? 0 : 2; - chevron_text[position].append(QString::number(d_rel / v_ego, 'f', 1) + " " + "s"); + float val = (v_ego != 0.0f) ? std::max(0.0f, d_rel / v_ego) : 0.0f; + chevron_text[position].append(QString::number(val, 'f', 1) + " " + "s"); } int str_w = 200; // Width of the text box, might need adjustment From ba129989868274ed4bf8d91806ffeeb0620b3cbd Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 9 Jul 2024 22:59:44 -0400 Subject: [PATCH 4/7] ui: onroad: `drawLead` cleanup --- selfdrive/ui/qt/onroad/annotated_camera.cc | 30 +++++++++++++--------- selfdrive/ui/qt/onroad/annotated_camera.h | 3 ++- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 7e5629c4ef..009ff76138 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1490,13 +1490,14 @@ void AnnotatedCameraWidget::rocketFuel(QPainter &p) { } void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, - int num, float radar_d_rel, float v_ego, float radar_v_rel, int chevron_data, bool isMetric) { + int num, const cereal::CarState::Reader &car_data, int chevron_data) { painter.save(); const float speedBuff = 10.; const float leadBuff = 40.; const float d_rel = lead_data.getDRel(); const float v_rel = lead_data.getVRel(); + const float v_ego = car_data.getVEgo(); float fillAlpha = 0; if (d_rel < leadBuff) { @@ -1524,22 +1525,29 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState painter.drawPolygon(chevron, std::size(chevron)); if (num == 0) { // Display metrics to the 0th lead car - QStringList chevron_text[2]; + int chevron_types = 2; + QStringList chevron_text[chevron_types]; + int position; + float val; if (chevron_data == 1 || chevron_data == 3) { - chevron_text[0].append(QString::number(radar_d_rel,'f', 0) + " " + "m"); + position = 0; + val = std::max(0.0f, d_rel); + chevron_text[position].append(QString::number(val,'f', 0) + " " + "m"); } if (chevron_data == 2 || chevron_data == 3) { - chevron_text[chevron_data - 2].append(QString::number((radar_v_rel + v_ego) * (isMetric ? MS_TO_KPH : MS_TO_MPH),'f', 0) + " " + (isMetric ? "km/h" : "mph")); + position = (chevron_data == 2) ? 0 : 1; + val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast(MS_TO_KPH) : static_cast(MS_TO_MPH))); + chevron_text[position].append(QString::number(val,'f', 0) + " " + (is_metric ? "km/h" : "mph")); } - int str_w = 200; // Width of the text box, might need adjustment - int str_h = 50; // Height of the text box, adjust as necessary + float str_w = 200; // Width of the text box, might need adjustment + float str_h = 50; // Height of the text box, adjust as necessary painter.setFont(InterFont(45, QFont::Bold)); // Calculate the center of the chevron and adjust the text box position float text_y = y + sz + 12; // Position the text at the bottom of the chevron QRect textRect(x - str_w / 2, text_y, str_w, str_h); // Adjust the rectangle to center the text horizontally at the chevron's bottom QPoint shadow_offset(2, 2); - for (int i = 0; i < 2; ++i) { + for (int i = 0; i < chevron_types; ++i) { if (!chevron_text[i].isEmpty()) { painter.setPen(QColor(0x0, 0x0, 0x0, 200)); // Draw shadow painter.drawText(textRect.translated(shadow_offset.x(), shadow_offset.y() + i * str_h), Qt::AlignBottom | Qt::AlignHCenter, chevron_text[i].at(0)); @@ -1623,17 +1631,15 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) { if (s->scene.longitudinal_control && sm.rcv_frame("radarState") > s->scene.started_frame) { auto radar_state = sm["radarState"].getRadarState(); + auto car_state = sm["carState"].getCarState(); update_leads(s, radar_state, model.getPosition()); auto lead_one = radar_state.getLeadOne(); auto lead_two = radar_state.getLeadTwo(); - float v_ego = sm["carState"].getCarState().getVEgo(); - float radar_d_rel = radar_state.getLeadOne().getDRel(); - float radar_v_rel = radar_state.getLeadOne().getVRel(); if (lead_one.getStatus()) { - drawLead(painter, lead_one, s->scene.lead_vertices[0], 0, radar_d_rel, v_ego, radar_v_rel, s->scene.chevron_data, s->scene.is_metric); + drawLead(painter, lead_one, s->scene.lead_vertices[0], 0, car_state, s->scene.chevron_data); } if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { - drawLead(painter, lead_two, s->scene.lead_vertices[1], 1, radar_d_rel, v_ego, radar_v_rel, s->scene.chevron_data, s->scene.is_metric); + drawLead(painter, lead_two, s->scene.lead_vertices[1], 1, car_state, s->scene.chevron_data); } rocketFuel(painter); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 94bd12c8c0..29b9c4833b 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -211,7 +211,8 @@ protected: void showEvent(QShowEvent *event) override; void updateFrameMat() override; void drawLaneLines(QPainter &painter, const UIState *s); - void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, int num, float radar_d_rel, float v_ego, float radar_v_rel, int chevron_data, bool isMetric); + void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, + int num, const cereal::CarState::Reader &car_data, int chevron_data); void drawHud(QPainter &p); void drawDriverState(QPainter &painter, const UIState *s); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } From ebce2d15cb9d05f7e4c4c27fe565e24b97760c30 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 9 Jul 2024 23:49:48 -0400 Subject: [PATCH 5/7] sync --- selfdrive/ui/qt/onroad/annotated_camera.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c9faed9ba2..fdea2a30dd 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1540,9 +1540,11 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState chevron_text[position].append(QString::number(val,'f', 0) + " " + (is_metric ? "km/h" : "mph")); } if (chevron_data == 3 || chevron_data == 4) { + float STOP_DISTANCE = 6.0; position = (chevron_data == 3) ? 0 : 2; - val = (v_ego != 0.0f) ? std::max(0.0f, d_rel / v_ego) : 0.0f; - chevron_text[position].append(QString::number(val, 'f', 1) + " " + "s"); + val = (v_ego > 0) ? std::min(std::max(0.0f, (d_rel + STOP_DISTANCE) / v_ego), 200.0f) : 0.0f; + bool invalid_val = val == 200.0f || val == 0.0f; + chevron_text[position].append(invalid_val ? "---" : QString::number(val, 'f', 1) + " " + "s"); } float str_w = 200; // Width of the text box, might need adjustment From 6c481e5a0759c6684fe7ebf01ddc1278a414efd1 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 9 Jul 2024 23:54:47 -0400 Subject: [PATCH 6/7] more --- selfdrive/ui/qt/onroad/annotated_camera.cc | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index fdea2a30dd..5e3ee832e6 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1525,25 +1525,26 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState painter.drawPolygon(chevron, std::size(chevron)); if (num == 0) { // Display metrics to the 0th lead car - int chevron_types = 3; + const int chevron_types = 3; + const int chevron_all = chevron_types + 1; // All metrics QStringList chevron_text[chevron_types]; int position; float val; - if (chevron_data == 1 || chevron_data == 4) { + if (chevron_data == 1 || chevron_data == chevron_all) { position = 0; val = std::max(0.0f, d_rel); chevron_text[position].append(QString::number(val,'f', 0) + " " + "m"); } - if (chevron_data == 2 || chevron_data == 4) { + if (chevron_data == 2 || chevron_data == chevron_all) { position = (chevron_data == 2) ? 0 : 1; val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast(MS_TO_KPH) : static_cast(MS_TO_MPH))); chevron_text[position].append(QString::number(val,'f', 0) + " " + (is_metric ? "km/h" : "mph")); } - if (chevron_data == 3 || chevron_data == 4) { - float STOP_DISTANCE = 6.0; + if (chevron_data == 3 || chevron_data == chevron_all) { + const float STOP_DISTANCE = 6.0; position = (chevron_data == 3) ? 0 : 2; val = (v_ego > 0) ? std::min(std::max(0.0f, (d_rel + STOP_DISTANCE) / v_ego), 200.0f) : 0.0f; - bool invalid_val = val == 200.0f || val == 0.0f; + const bool invalid_val = val == 200.0f || val == 0.0f; chevron_text[position].append(invalid_val ? "---" : QString::number(val, 'f', 1) + " " + "s"); } From f20973d4cf23f5984fd4b687fcaf0d40dc4ead17 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 10 Jul 2024 11:27:07 -0400 Subject: [PATCH 7/7] cleaner --- selfdrive/ui/qt/onroad/annotated_camera.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 5e3ee832e6..bb19798e92 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1541,11 +1541,11 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState chevron_text[position].append(QString::number(val,'f', 0) + " " + (is_metric ? "km/h" : "mph")); } if (chevron_data == 3 || chevron_data == chevron_all) { - const float STOP_DISTANCE = 6.0; position = (chevron_data == 3) ? 0 : 2; - val = (v_ego > 0) ? std::min(std::max(0.0f, (d_rel + STOP_DISTANCE) / v_ego), 200.0f) : 0.0f; - const bool invalid_val = val == 200.0f || val == 0.0f; - chevron_text[position].append(invalid_val ? "---" : QString::number(val, 'f', 1) + " " + "s"); + val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f; + + QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---"; + chevron_text[position].append(ttc_str); } float str_w = 200; // Width of the text box, might need adjustment