+#include "selfdrive/ui/sunnypilot/qt/util.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
class ModelsPanel : public QWidget {
@@ -20,6 +21,7 @@ public:
private:
QString GetActiveModelName();
QString GetActiveModelInternalName();
+ QString GetActiveModelRef();
void updateModelManagerState();
void showEvent(QShowEvent *event) override;
@@ -36,6 +38,7 @@ private:
void updateLabels();
void handleCurrentModelLblBtnClicked();
void handleBundleDownloadProgress();
+ void refreshLaneTurnValueControl();
void showResetParamsDialog();
QProgressBar* createProgressBar(QWidget *parent);
QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar);
@@ -80,5 +83,6 @@ private:
Params params;
ButtonControlSP *clearModelCacheBtn;
ButtonControlSP *refreshAvailableModelsBtn;
-
+ ParamControlSP *lane_turn_desire_toggle;
+ OptionControlSP *lane_turn_value_control;
};
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc
index a1961cb1ea..8bdb7703f2 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc
@@ -11,12 +11,39 @@ SoftwarePanelSP::SoftwarePanelSP(QWidget *parent) : SoftwarePanel(parent) {
// branch selector
QObject::disconnect(targetBranchBtn, nullptr, nullptr, nullptr);
connect(targetBranchBtn, &ButtonControlSP::clicked, [=]() {
- InputDialog d(tr("Search Branch"), this, tr("Enter search keywords, or leave blank to list all branches."), false);
+ if (Hardware::get_device_type() == cereal::InitData::DeviceType::TICI) {
+ auto current = params.get("GitBranch");
+ QStringList allBranches = QString::fromStdString(params.get("UpdaterAvailableBranches")).split(",");
+ QStringList branches;
+ for (const QString &b : allBranches) {
+ if (b.endsWith("-tici")) {
+ branches.append(b);
+ }
+ }
+
+ for (QString b : {current.c_str(), "master-tici", "staging-tici", "release-tici"}) {
+ auto i = branches.indexOf(b);
+ if (i >= 0) {
+ branches.removeAt(i);
+ branches.insert(0, b);
+ }
+ }
+
+ QString cur = QString::fromStdString(params.get("UpdaterTargetBranch"));
+ QString selection = MultiOptionDialog::getSelection(tr("Select a branch"), branches, cur, this);
+ if (!selection.isEmpty()) {
+ params.put("UpdaterTargetBranch", selection.toStdString());
+ targetBranchBtn->setValue(QString::fromStdString(params.get("UpdaterTargetBranch")));
+ checkForUpdates();
+ }
+ } else {
+ InputDialog d(tr("Search Branch"), this, tr("Enter search keywords, or leave blank to list all branches."), false);
d.setMinLength(0);
const int ret = d.exec();
if (ret) {
searchBranches(d.text());
}
+ }
});
// Disable Updates toggle
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.cc
index 3d4e070963..96d945ab53 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.cc
@@ -34,6 +34,27 @@ SunnylinkPanel::SunnylinkPanel(QWidget *parent) : QFrame(parent) {
vlayout->setContentsMargins(50, 20, 50, 20);
auto *list = new ListWidget(this, false);
+
+ QVBoxLayout *titleLayout = new QVBoxLayout;
+ QLabel *title = new QLabel(tr("🚀 sunnylink 🚀"));
+ title->setStyleSheet("font-size: 90px; font-weight: 500; font-family: 'Noto Color Emoji';");
+ titleLayout->addWidget(title, 0, Qt::AlignCenter);
+
+ QLabel *sunnylinkDesc = new QLabel(""+
+ tr("For secure backup, restore, and remote configuration")+ "
");
+
+ QLabel *sponsorMsg = new QLabel(""+
+ tr("Sponsorship isn't required for basic backup/restore") + "
" +
+ tr("Click the sponsor button for more details")+ "
");
+
+ sunnylinkDesc->setStyleSheet("font-size: 40px; font-weight: 100; font-family: 'Noto';");
+ sponsorMsg->setStyleSheet("font-size: 35px; font-weight: 100; font-family: 'Noto';");
+
+ titleLayout->addWidget(sunnylinkDesc, 0, Qt::AlignCenter);
+ titleLayout->addWidget(sponsorMsg, 0, Qt::AlignCenter);
+
+ list->addItem(titleLayout);
+
QString sunnylinkEnabledBtnDesc = tr("This is the master switch, it will allow you to cutoff any sunnylink requests should you want to do that.");
sunnylinkEnabledBtn = new ParamControl(
"SunnylinkEnabled",
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
index 818da9b75e..e19760f2e1 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
@@ -28,6 +28,20 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
"../assets/offroad/icon_monitoring.png",
false,
},
+ {
+ "RainbowMode",
+ tr("Enable Tesla Rainbow Mode"),
+ RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "
" + tr("It")+ " " + tr("does not") + " " + tr("affect driving in any way.") + "",
+ "../assets/offroad/icon_monitoring.png",
+ false,
+ },
+ {
+ "StandstillTimer",
+ tr("Enable Standstill Timer"),
+ tr("Show a timer on the HUD when the car is at a standstill."),
+ "../assets/offroad/icon_monitoring.png",
+ false,
+ },
};
// Add regular toggles first
@@ -65,6 +79,15 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
list->addItem(chevron_info_settings);
param_watcher->addParam("ChevronInfo");
+ // Visuals: Developer UI Info (Dev UI)
+ std::vector dev_ui_settings_texts{tr("Off"), tr("Right"), tr("Right &&\nBottom")};
+ dev_ui_settings = new ButtonParamControlSP(
+ "DevUIInfo", tr("Developer UI"), tr("Display real-time parameters and metrics from various sources."),
+ "",
+ dev_ui_settings_texts,
+ 380);
+ list->addItem(dev_ui_settings);
+
sunnypilotScroller = new ScrollViewSP(list, this);
vlayout->addWidget(sunnypilotScroller);
@@ -83,4 +106,7 @@ void VisualsPanel::paramsRefresh() {
if (chevron_info_settings) {
chevron_info_settings->refresh();
}
+ if (dev_ui_settings) {
+ dev_ui_settings->refresh();
+ }
}
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h
index f342662c22..30ff31c301 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h
@@ -28,4 +28,5 @@ protected:
std::map toggles;
ParamWatcher * param_watcher;
ButtonParamControlSP *chevron_info_settings;
+ ButtonParamControlSP *dev_ui_settings;
};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc
index 3721a3d198..1d5567161a 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc
+++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc
@@ -14,3 +14,8 @@ AnnotatedCameraWidgetSP::AnnotatedCameraWidgetSP(VisionStreamType type, QWidget
void AnnotatedCameraWidgetSP::updateState(const UIState &s) {
AnnotatedCameraWidget::updateState(s);
}
+
+void AnnotatedCameraWidgetSP::showEvent(QShowEvent *event) {
+ AnnotatedCameraWidget::showEvent(event);
+ ui_update_params_sp(uiState());
+}
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h
index 46ce7d4be3..8c0a385657 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h
+++ b/selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h
@@ -15,4 +15,7 @@ class AnnotatedCameraWidgetSP : public AnnotatedCameraWidget {
public:
explicit AnnotatedCameraWidgetSP(VisionStreamType type, QWidget *parent = nullptr);
void updateState(const UIState &s) override;
+
+protected:
+ void showEvent(QShowEvent *event) override;
};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc
new file mode 100644
index 0000000000..292ba6f7bb
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.cc
@@ -0,0 +1,227 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+#include
+
+#include "common/util.h"
+#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h"
+
+
+// Add Relative Distance to Primary Lead Car
+// Unit: Meters
+UiElement DeveloperUi::getDRel(bool lead_status, float lead_d_rel) {
+ QString value = lead_status ? QString::number(lead_d_rel, 'f', 0) : "-";
+ QColor color = QColor(255, 255, 255, 255);
+
+ if (lead_status) {
+ // Orange if close, Red if very close
+ if (lead_d_rel < 5) {
+ color = QColor(255, 0, 0, 255);
+ } else if (lead_d_rel < 15) {
+ color = QColor(255, 188, 0, 255);
+ }
+ }
+
+ return UiElement(value, "REL DIST", "m", color);
+}
+
+// Add Relative Velocity vs Primary Lead Car
+// Unit: kph if metric, else mph
+UiElement DeveloperUi::getVRel(bool lead_status, float lead_v_rel, bool is_metric, const QString &speed_unit) {
+ QString value = lead_status ? QString::number(lead_v_rel * (is_metric ? MS_TO_KPH : MS_TO_MPH), 'f', 0) : "-";
+ QColor color = QColor(255, 255, 255, 255);
+
+ if (lead_status) {
+ // Red if approaching faster than 10mph
+ // Orange if approaching (negative)
+ if (lead_v_rel < -4.4704) {
+ color = QColor(255, 0, 0, 255);
+ } else if (lead_v_rel < 0) {
+ color = QColor(255, 188, 0, 255);
+ }
+ }
+
+ return UiElement(value, "REL SPEED", speed_unit, color);
+}
+
+// Add Real Steering Angle
+// Unit: Degrees
+UiElement DeveloperUi::getSteeringAngleDeg(float angle_steers, bool lat_active, bool steer_override) {
+ QString value = QString("%1%2%3").arg(QString::number(angle_steers, 'f', 1)).arg("°").arg("");
+ QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255);
+
+ // Red if large steering angle
+ // Orange if moderate steering angle
+ if (std::fabs(angle_steers) > 180) {
+ color = QColor(255, 0, 0, 255);
+ } else if (std::fabs(angle_steers) > 90) {
+ color = QColor(255, 188, 0, 255);
+ }
+
+ return UiElement(value, "REAL STEER", "", color);
+}
+
+// Add Actual Lateral Acceleration (roll compensated) when using Torque
+// Unit: m/s²
+UiElement DeveloperUi::getActualLateralAccel(float curvature, float v_ego, float roll, bool lat_active, bool steer_override) {
+ double actualLateralAccel = (curvature * pow(v_ego, 2)) - (roll * 9.81);
+
+ QString value = QString::number(actualLateralAccel, 'f', 2);
+ QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255);
+
+ return UiElement(value, "ACTUAL L.A.", "m/s²", color);
+}
+
+// Add Desired Steering Angle when using PID
+// Unit: Degrees
+UiElement DeveloperUi::getSteeringAngleDesiredDeg(bool lat_active, float steer_angle_desired, float angle_steers) {
+ QString value = lat_active ? QString("%1%2%3").arg(QString::number(steer_angle_desired, 'f', 1)).arg("°").arg("") : "-";
+ QColor color = QColor(255, 255, 255, 255);
+
+ if (lat_active) {
+ // Red if large steering angle
+ // Orange if moderate steering angle
+ if (std::fabs(angle_steers) > 180) {
+ color = QColor(255, 0, 0, 255);
+ } else if (std::fabs(angle_steers) > 90) {
+ color = QColor(255, 188, 0, 255);
+ } else {
+ color = QColor(0, 255, 0, 255);
+ }
+ }
+
+ return UiElement(value, "DESIRED STEER", "", color);
+}
+
+// Add Device Memory (RAM) Usage
+// Unit: Percent
+UiElement DeveloperUi::getMemoryUsagePercent(int memory_usage_percent) {
+ QString value = QString("%1%2").arg(QString::number(memory_usage_percent, 'd', 0)).arg("%");
+ QColor color = (memory_usage_percent > 85) ? QColor(255, 188, 0, 255) : QColor(255, 255, 255, 255);
+
+ return UiElement(value, "RAM", "", color);
+}
+
+// Add Vehicle Current Acceleration
+// Unit: m/s²
+UiElement DeveloperUi::getAEgo(float a_ego) {
+ QString value = QString::number(a_ego, 'f', 1);
+ QColor color = QColor(255, 255, 255, 255);
+
+ return UiElement(value, "ACC.", "m/s²", color);
+}
+
+// Add Relative Velocity to Primary Lead Car
+// Unit: kph if metric, else mph
+UiElement DeveloperUi::getVEgoLead(bool lead_status, float lead_v_rel, float v_ego, bool is_metric, const QString &speed_unit) {
+ QString value = lead_status ? QString::number((lead_v_rel + v_ego) * (is_metric ? MS_TO_KPH : MS_TO_MPH), 'f', 0) : "-";
+ QColor color = QColor(255, 255, 255, 255);
+
+ if (lead_status) {
+ // Red if approaching faster than 10mph
+ // Orange if approaching (negative)
+ if (lead_v_rel < -4.4704) {
+ color = QColor(255, 0, 0, 255);
+ } else if (lead_v_rel < 0) {
+ color = QColor(255, 188, 0, 255);
+ }
+ }
+
+ return UiElement(value, "L.S.", speed_unit, color);
+}
+
+// Add Friction Coefficient Raw from torqued
+// Unit: None
+UiElement DeveloperUi::getFrictionCoefficientFiltered(float friction_coefficient_filtered, bool live_valid) {
+ QString value = QString::number(friction_coefficient_filtered, 'f', 3);
+ QColor color = live_valid ? QColor(0, 255, 0, 255) : QColor(255, 255, 255, 255);
+
+ return UiElement(value, "FRIC.", "", color);
+}
+
+// Add Lateral Acceleration Factor Raw from torqued
+// Unit: m/s²
+UiElement DeveloperUi::getLatAccelFactorFiltered(float lat_accel_factor_filtered, bool live_valid) {
+ QString value = QString::number(lat_accel_factor_filtered, 'f', 3);
+ QColor color = live_valid ? QColor(0, 255, 0, 255) : QColor(255, 255, 255, 255);
+
+ return UiElement(value, "L.A.", "m/s²", color);
+}
+
+// Add Steering Torque from Car EPS
+// Unit: Newton Meters
+UiElement DeveloperUi::getSteeringTorqueEps(float steering_torque_eps) {
+ QString value = QString::number(std::fabs(steering_torque_eps), 'f', 1);
+ QColor color = QColor(255, 255, 255, 255);
+
+ return UiElement(value, "E.T.", "N·dm", color);
+}
+
+// Add Bearing Degree and Direction from Car (Compass)
+// Unit: Meters
+UiElement DeveloperUi::getBearingDeg(float bearing_accuracy_deg, float bearing_deg) {
+ QString value = (bearing_accuracy_deg != 180.00) ? QString("%1%2%3").arg(QString::number(bearing_deg, 'd', 0)).arg("°").arg("") : "-";
+ QColor color = QColor(255, 255, 255, 255);
+ QString dir_value;
+
+ if (bearing_accuracy_deg != 180.00) {
+ if (((bearing_deg >= 337.5) && (bearing_deg <= 360)) || ((bearing_deg >= 0) && (bearing_deg <= 22.5))) {
+ dir_value = "N";
+ } else if ((bearing_deg > 22.5) && (bearing_deg < 67.5)) {
+ dir_value = "NE";
+ } else if ((bearing_deg >= 67.5) && (bearing_deg <= 112.5)) {
+ dir_value = "E";
+ } else if ((bearing_deg > 112.5) && (bearing_deg < 157.5)) {
+ dir_value = "SE";
+ } else if ((bearing_deg >= 157.5) && (bearing_deg <= 202.5)) {
+ dir_value = "S";
+ } else if ((bearing_deg > 202.5) && (bearing_deg < 247.5)) {
+ dir_value = "SW";
+ } else if ((bearing_deg >= 247.5) && (bearing_deg <= 292.5)) {
+ dir_value = "W";
+ } else if ((bearing_deg > 292.5) && (bearing_deg < 337.5)) {
+ dir_value = "NW";
+ }
+ } else {
+ dir_value = "OFF";
+ }
+
+ return UiElement(QString("%1 | %2").arg(dir_value).arg(value), "B.D.", "", color);
+}
+
+// Add Altitude of Current Location
+// Unit: Meters
+UiElement DeveloperUi::getAltitude(float gps_accuracy, float altitude) {
+ QString value = (gps_accuracy != 0.00) ? QString::number(altitude, 'f', 1) : "-";
+ QColor color = QColor(255, 255, 255, 255);
+
+ return UiElement(value, "ALT.", "m", color);
+}
+
+// Add Actuators Output
+// Unit: Degree (angle) or m/s² (torque)
+UiElement DeveloperUi::getActuatorsOutputLateral(cereal::CarParams::SteerControlType steerControlType,
+ cereal::CarControl::Actuators::Reader &actuators,
+ float desiredCurvature, float v_ego, float roll, bool lat_active, bool steer_override) {
+ QString label;
+ QString value;
+ QString unit;
+
+ if (steerControlType == cereal::CarParams::SteerControlType::ANGLE) {
+ label = "DESIRED STEER";
+ value = QString("%1%2%3").arg(QString::number(actuators.getSteeringAngleDeg(), 'f', 1)).arg("°").arg("");
+ } else {
+ label = "DESIRED L.A.";
+ double desiredLateralAccel = (desiredCurvature * pow(v_ego, 2)) - (roll * 9.81);
+ value = QString::number(desiredLateralAccel, 'f', 2);
+ unit = "m/s²";
+ }
+
+ value = lat_active ? value : "-";
+ QColor color = lat_active ? (steer_override ? QColor(0x91, 0x9b, 0x95, 0xff) : QColor(0, 255, 0, 255)) : QColor(255, 255, 255, 255);
+
+ return UiElement(value, label, unit, color);
+}
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h
new file mode 100644
index 0000000000..0c5c472209
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h
@@ -0,0 +1,31 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+#pragma once
+
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h"
+
+class DeveloperUi {
+
+public:
+ static UiElement getDRel(bool lead_status, float lead_d_rel);
+ static UiElement getVRel(bool lead_status, float lead_v_rel, bool is_metric, const QString &speed_unit);
+ static UiElement getSteeringAngleDeg(float angle_steers, bool lat_active, bool steer_override);
+ static UiElement getActualLateralAccel(float curvature, float v_ego, float roll, bool lat_active, bool steer_override);
+ static UiElement getSteeringAngleDesiredDeg(bool lat_active, float steer_angle_desired, float angle_steers);
+ static UiElement getMemoryUsagePercent(int memory_usage_percent);
+ static UiElement getAEgo(float a_ego);
+ static UiElement getVEgoLead(bool lead_status, float lead_v_rel, float v_ego, bool is_metric, const QString &speed_unit);
+ static UiElement getFrictionCoefficientFiltered(float friction_coefficient_filtered, bool live_valid);
+ static UiElement getLatAccelFactorFiltered(float lat_accel_factor_filtered, bool live_valid);
+ static UiElement getSteeringTorqueEps(float steering_torque_eps);
+ static UiElement getBearingDeg(float bearing_accuracy_deg, float bearing_deg);
+ static UiElement getAltitude(float gps_accuracy, float altitude);
+ static UiElement getActuatorsOutputLateral(cereal::CarParams::SteerControlType steerControlType,
+ cereal::CarControl::Actuators::Reader &actuators,
+ float desiredCurvature, float v_ego, float roll, bool lat_active, bool steer_override);
+};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h
new file mode 100644
index 0000000000..3711e5ac05
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/qt/onroad/developer_ui/ui_elements.h
@@ -0,0 +1,19 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+#pragma once
+
+#include
+
+struct UiElement {
+ QString value{};
+ QString label{};
+ QString units{};
+ QColor color{};
+
+ explicit UiElement(const QString &value = "", const QString &label = "", const QString &units = "", const QColor &color = QColor(255, 255, 255, 255))
+ : value(value), label(label), units(units), color(color) {}
+};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
index 233ca59f98..fb2a69c24b 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
@@ -4,15 +4,310 @@
* This file is part of sunnypilot and is licensed under the MIT License.
* See the LICENSE.md file in the root directory for more details.
*/
+#include
#include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"
+#include "selfdrive/ui/qt/util.h"
+
+
HudRendererSP::HudRendererSP() {}
void HudRendererSP::updateState(const UIState &s) {
HudRenderer::updateState(s);
+
+ const SubMaster &sm = *(s.sm);
+ const bool cs_alive = sm.alive("controlsState");
+ const auto cs = sm["controlsState"].getControlsState();
+ const auto car_state = sm["carState"].getCarState();
+ const auto car_control = sm["carControl"].getCarControl();
+ const auto radar_state = sm["radarState"].getRadarState();
+ const auto is_gps_location_external = sm.rcv_frame("gpsLocationExternal") > 1;
+ const auto gpsLocation = is_gps_location_external ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation();
+ const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
+ const auto car_params = sm["carParams"].getCarParams();
+ const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+
+ static int reverse_delay = 0;
+ bool reverse_allowed = false;
+ if (int(car_state.getGearShifter()) != 4) {
+ reverse_delay = 0;
+ reverse_allowed = false;
+ } else {
+ reverse_delay += 50;
+ if (reverse_delay >= 1000) {
+ reverse_allowed = true;
+ }
+ }
+
+ reversing = reverse_allowed;
+ is_metric = s.scene.is_metric;
+
+ // Handle older routes where vEgoCluster is not set
+ v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
+ float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo();
+ speed = cs_alive ? std::max(0.0, v_ego) : 0.0;
+ speed *= is_metric ? MS_TO_KPH : MS_TO_MPH;
+
+ latActive = car_control.getLatActive();
+ steerOverride = car_state.getSteeringPressed();
+
+ devUiInfo = s.scene.dev_ui_info;
+
+ speedUnit = is_metric ? tr("km/h") : tr("mph");
+ lead_d_rel = radar_state.getLeadOne().getDRel();
+ lead_v_rel = radar_state.getLeadOne().getVRel();
+ lead_status = radar_state.getLeadOne().getStatus();
+ steerControlType = car_params.getSteerControlType();
+ actuators = car_control.getActuators();
+ torqueLateral = steerControlType == cereal::CarParams::SteerControlType::TORQUE;
+ angleSteers = car_state.getSteeringAngleDeg();
+ desiredCurvature = cs.getDesiredCurvature();
+ curvature = cs.getCurvature();
+ roll = sm["liveParameters"].getLiveParameters().getRoll();
+ memoryUsagePercent = sm["deviceState"].getDeviceState().getMemoryUsagePercent();
+ gpsAccuracy = is_gps_location_external ? gpsLocation.getHorizontalAccuracy() : 1.0; // External reports accuracy, internal does not.
+ altitude = gpsLocation.getAltitude();
+ vEgo = car_state.getVEgo();
+ aEgo = car_state.getAEgo();
+ steeringTorqueEps = car_state.getSteeringTorqueEps();
+ bearingAccuracyDeg = gpsLocation.getBearingAccuracyDeg();
+ bearingDeg = gpsLocation.getBearingDeg();
+ torquedUseParams = ltp.getUseParams();
+ latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
+ frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();
+ liveValid = ltp.getLiveValid();
+
+ standstillTimer = s.scene.standstill_timer;
+ isStandstill = car_state.getStandstill();
+ longOverride = car_control.getCruiseControl().getOverride();
+ smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
+ smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
}
void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
HudRenderer::draw(p, surface_rect);
+ if (!reversing) {
+ // Smart Cruise Control
+ int x_offset = -260;
+ int y1_offset = -80;
+ // int y2_offset = -140; // reserved for 2 icons
+
+ bool scc_vision_active_pulse = pulseElement(smartCruiseControlVisionFrame);
+ if ((smartCruiseControlVisionEnabled && !smartCruiseControlVisionActive) || (smartCruiseControlVisionActive && scc_vision_active_pulse)) {
+ drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y1_offset, "SCC-V");
+ }
+
+ if (smartCruiseControlVisionActive) {
+ smartCruiseControlVisionFrame++;
+ } else {
+ smartCruiseControlVisionFrame = 0;
+ }
+
+ // Bottom Dev UI
+ if (devUiInfo == 2) {
+ QRect rect_bottom(surface_rect.left(), surface_rect.bottom() - 60, surface_rect.width(), 61);
+ p.setPen(Qt::NoPen);
+ p.setBrush(QColor(0, 0, 0, 100));
+ p.drawRect(rect_bottom);
+ drawBottomDevUI(p, rect_bottom.left(), rect_bottom.center().y());
+ }
+
+ // Right Dev UI
+ if (devUiInfo != 0) {
+ QRect rect_right(surface_rect.right() - (UI_BORDER_SIZE * 2), UI_BORDER_SIZE * 1.5, 184, 170);
+ drawRightDevUI(p, surface_rect.right() - 184 - UI_BORDER_SIZE * 2, UI_BORDER_SIZE * 2 + rect_right.height());
+ }
+
+ // Standstill Timer
+ if (standstillTimer) {
+ drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53);
+ }
+ }
+}
+
+void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QColor color) {
+ QRect real_rect = p.fontMetrics().boundingRect(text);
+ real_rect.moveCenter({x, y - real_rect.height() / 2});
+ p.setPen(color);
+ p.drawText(real_rect.x(), real_rect.bottom(), text);
+}
+
+bool HudRendererSP::pulseElement(int frame) {
+ if (frame % UI_FREQ < (UI_FREQ / 2.5)) {
+ return false;
+ }
+
+ return true;
+}
+
+void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name) {
+ int x = surface_rect.center().x();
+ int y = surface_rect.height() / 4;
+
+ QString text = QString::fromStdString(name);
+ QFont font = InterFont(36, QFont::Bold);
+ p.setFont(font);
+
+ QFontMetrics fm(font);
+
+ int padding_v = 5;
+ int box_width = 160;
+ int box_height = fm.height() + padding_v * 2;
+
+ QRectF bg_rect(x - (box_width / 2) + x_offset,
+ y - (box_height / 2) + y_offset,
+ box_width, box_height);
+
+ QPainterPath boxPath;
+ boxPath.addRoundedRect(bg_rect, 10, 10);
+
+ int text_w = fm.horizontalAdvance(text);
+ qreal baseline_y = bg_rect.top() + padding_v + fm.ascent();
+ qreal text_x = bg_rect.center().x() - (text_w / 2.0);
+
+ QPainterPath textPath;
+ textPath.addText(QPointF(text_x, baseline_y), font, text);
+ boxPath = boxPath.subtracted(textPath);
+
+ p.setPen(Qt::NoPen);
+ p.setBrush(longOverride ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0xff, 0, 0xff));
+ p.drawPath(boxPath);
+}
+
+int HudRendererSP::drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) {
+
+ p.setFont(InterFont(28, QFont::Bold));
+ x += 92;
+ y += 80;
+ drawText(p, x, y, label);
+
+ p.setFont(InterFont(30 * 2, QFont::Bold));
+ y += 65;
+ drawText(p, x, y, value, color);
+
+ p.setFont(InterFont(28, QFont::Bold));
+
+ if (units.length() > 0) {
+ p.save();
+ x += 120;
+ y -= 25;
+ p.translate(x, y);
+ p.rotate(-90);
+ drawText(p, 0, 0, units);
+ p.restore();
+ }
+
+ return 130;
+}
+
+void HudRendererSP::drawRightDevUI(QPainter &p, int x, int y) {
+ int rh = 5;
+ int ry = y;
+
+ UiElement dRelElement = DeveloperUi::getDRel(lead_status, lead_d_rel);
+ rh += drawRightDevUIElement(p, x, ry, dRelElement.value, dRelElement.label, dRelElement.units, dRelElement.color);
+ ry = y + rh;
+
+ UiElement vRelElement = DeveloperUi::getVRel(lead_status, lead_v_rel, is_metric, speedUnit);
+ rh += drawRightDevUIElement(p, x, ry, vRelElement.value, vRelElement.label, vRelElement.units, vRelElement.color);
+ ry = y + rh;
+
+ UiElement steeringAngleDegElement = DeveloperUi::getSteeringAngleDeg(angleSteers, latActive, steerOverride);
+ rh += drawRightDevUIElement(p, x, ry, steeringAngleDegElement.value, steeringAngleDegElement.label, steeringAngleDegElement.units, steeringAngleDegElement.color);
+ ry = y + rh;
+
+ UiElement actuatorsOutputLateralElement = DeveloperUi::getActuatorsOutputLateral(steerControlType, actuators, desiredCurvature, vEgo, roll, latActive, steerOverride);
+ rh += drawRightDevUIElement(p, x, ry, actuatorsOutputLateralElement.value, actuatorsOutputLateralElement.label, actuatorsOutputLateralElement.units, actuatorsOutputLateralElement.color);
+ ry = y + rh;
+
+ UiElement actualLateralAccelElement = DeveloperUi::getActualLateralAccel(curvature, vEgo, roll, latActive, steerOverride);
+ rh += drawRightDevUIElement(p, x, ry, actualLateralAccelElement.value, actualLateralAccelElement.label, actualLateralAccelElement.units, actualLateralAccelElement.color);
+}
+
+int HudRendererSP::drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) {
+ p.setFont(InterFont(38, QFont::Bold));
+ QFontMetrics fm(p.font());
+ QRect init_rect = fm.boundingRect(label + " ");
+ QRect real_rect = fm.boundingRect(init_rect, 0, label + " ");
+ real_rect.moveCenter({x, y});
+
+ QRect init_rect2 = fm.boundingRect(value);
+ QRect real_rect2 = fm.boundingRect(init_rect2, 0, value);
+ real_rect2.moveTop(real_rect.top());
+ real_rect2.moveLeft(real_rect.right() + 10);
+
+ QRect init_rect3 = fm.boundingRect(units);
+ QRect real_rect3 = fm.boundingRect(init_rect3, 0, units);
+ real_rect3.moveTop(real_rect.top());
+ real_rect3.moveLeft(real_rect2.right() + 10);
+
+ p.setPen(Qt::white);
+ p.drawText(real_rect, Qt::AlignLeft | Qt::AlignVCenter, label);
+
+ p.setPen(color);
+ p.drawText(real_rect2, Qt::AlignRight | Qt::AlignVCenter, value);
+ p.drawText(real_rect3, Qt::AlignLeft | Qt::AlignVCenter, units);
+ return 430;
+}
+
+void HudRendererSP::drawBottomDevUI(QPainter &p, int x, int y) {
+ int rw = 90;
+
+ UiElement aEgoElement = DeveloperUi::getAEgo(aEgo);
+ rw += drawBottomDevUIElement(p, rw, y, aEgoElement.value, aEgoElement.label, aEgoElement.units, aEgoElement.color);
+
+ UiElement vEgoLeadElement = DeveloperUi::getVEgoLead(lead_status, lead_v_rel, vEgo, is_metric, speedUnit);
+ rw += drawBottomDevUIElement(p, rw, y, vEgoLeadElement.value, vEgoLeadElement.label, vEgoLeadElement.units, vEgoLeadElement.color);
+
+ if (torqueLateral && torquedUseParams) {
+ UiElement frictionCoefficientFilteredElement = DeveloperUi::getFrictionCoefficientFiltered(frictionCoefficientFiltered, liveValid);
+ rw += drawBottomDevUIElement(p, rw, y, frictionCoefficientFilteredElement.value, frictionCoefficientFilteredElement.label, frictionCoefficientFilteredElement.units, frictionCoefficientFilteredElement.color);
+
+ UiElement latAccelFactorFilteredElement = DeveloperUi::getLatAccelFactorFiltered(latAccelFactorFiltered, liveValid);
+ rw += drawBottomDevUIElement(p, rw, y, latAccelFactorFilteredElement.value, latAccelFactorFilteredElement.label, latAccelFactorFilteredElement.units, latAccelFactorFilteredElement.color);
+ } else {
+ UiElement steeringTorqueEpsElement = DeveloperUi::getSteeringTorqueEps(steeringTorqueEps);
+ rw += drawBottomDevUIElement(p, rw, y, steeringTorqueEpsElement.value, steeringTorqueEpsElement.label, steeringTorqueEpsElement.units, steeringTorqueEpsElement.color);
+
+ UiElement bearingDegElement = DeveloperUi::getBearingDeg(bearingAccuracyDeg, bearingDeg);
+ rw += drawBottomDevUIElement(p, rw, y, bearingDegElement.value, bearingDegElement.label, bearingDegElement.units, bearingDegElement.color);
+ }
+
+ UiElement altitudeElement = DeveloperUi::getAltitude(gpsAccuracy, altitude);
+ rw += drawBottomDevUIElement(p, rw, y, altitudeElement.value, altitudeElement.label, altitudeElement.units, altitudeElement.color);
+}
+
+void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
+ if (isStandstill) {
+ standstillElapsedTime += 1.0 / UI_FREQ;
+
+ int minute = static_cast(standstillElapsedTime / 60);
+ int second = static_cast(standstillElapsedTime - (minute * 60));
+
+ // stop sign for standstill timer
+ const int size = 190; // size
+ const float angle = M_PI / 8.0;
+
+ QPolygon octagon;
+ for (int i = 0; i < 8; i++) {
+ float curr_angle = angle + i * M_PI / 4.0;
+ int point_x = x + size / 2 * cos(curr_angle);
+ int point_y = y + size / 2 * sin(curr_angle);
+ octagon << QPoint(point_x, point_y);
+ }
+
+ p.setPen(QPen(Qt::white, 6));
+ p.setBrush(QColor(255, 90, 81, 200)); // red pastel
+ p.drawPolygon(octagon);
+
+ QString time_str = QString("%1:%2").arg(minute, 1, 10, QChar('0')).arg(second, 2, 10, QChar('0'));
+ p.setFont(InterFont(55, QFont::Bold));
+ p.setPen(Qt::white);
+ QRect timerTextRect = p.fontMetrics().boundingRect(QString(time_str));
+ timerTextRect.moveCenter({x, y});
+ p.drawText(timerTextRect, Qt::AlignCenter, QString(time_str));
+ } else {
+ standstillElapsedTime = 0.0;
+ }
}
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
index 1e98cd3a52..4c92835957 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
@@ -7,9 +7,8 @@
#pragma once
-#include
-
#include "selfdrive/ui/qt/onroad/hud.h"
+#include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h"
class HudRendererSP : public HudRenderer {
Q_OBJECT
@@ -18,4 +17,50 @@ public:
HudRendererSP();
void updateState(const UIState &s) override;
void draw(QPainter &p, const QRect &surface_rect) override;
+
+private:
+ Params params;
+ void drawText(QPainter &p, int x, int y, const QString &text, QColor color = Qt::white);
+ void drawRightDevUI(QPainter &p, int x, int y);
+ int drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color);
+ int drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color);
+ void drawBottomDevUI(QPainter &p, int x, int y);
+ void drawStandstillTimer(QPainter &p, int x, int y);
+ bool pulseElement(int frame);
+ void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
+
+ bool lead_status;
+ float lead_d_rel;
+ float lead_v_rel;
+ bool torqueLateral;
+ float angleSteers;
+ float desiredCurvature;
+ float curvature;
+ float roll;
+ int memoryUsagePercent;
+ int devUiInfo;
+ float gpsAccuracy;
+ float altitude;
+ float vEgo;
+ float aEgo;
+ float steeringTorqueEps;
+ float bearingAccuracyDeg;
+ float bearingDeg;
+ bool torquedUseParams;
+ float latAccelFactorFiltered;
+ float frictionCoefficientFiltered;
+ bool liveValid;
+ QString speedUnit;
+ bool latActive;
+ bool steerOverride;
+ bool reversing;
+ cereal::CarParams::SteerControlType steerControlType;
+ cereal::CarControl::Actuators::Reader actuators;
+ bool standstillTimer;
+ bool isStandstill;
+ float standstillElapsedTime;
+ bool longOverride;
+ bool smartCruiseControlVisionEnabled;
+ bool smartCruiseControlVisionActive;
+ int smartCruiseControlVisionFrame;
};
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.cc b/selfdrive/ui/sunnypilot/qt/onroad/model.cc
index af0177c344..5d92838f8a 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/model.cc
+++ b/selfdrive/ui/sunnypilot/qt/onroad/model.cc
@@ -48,5 +48,43 @@ void ModelRendererSP::drawPath(QPainter &painter, const cereal::ModelDataV2::Rea
painter.drawPolygon(right_blindspot_vertices);
}
}
- ModelRenderer::drawPath(painter, model, surface_rect.height());
+
+ bool rainbow = Params().getBool("RainbowMode");
+ //float v_ego = sm["carState"].getCarState().getVEgo();
+
+ if (rainbow) {
+ // Simple time-based animation
+ float time_offset = std::chrono::duration_cast(
+ std::chrono::steady_clock::now().time_since_epoch()).count() / 1000.0f;
+
+ // simple linear gradient from bottom to top
+ QLinearGradient bg(0, surface_rect.height(), 0, 0);
+
+ // evenly spaced colors across the spectrum
+ // The animation shifts the entire spectrum smoothly
+ float animation_speed = 40.0f; // speed vroom vroom
+ float hue_offset = fmod(time_offset * animation_speed, 360.0f);
+
+ // 6-8 color stops for smooth transitions more color makes it laggy
+ const int num_stops = 7;
+ for (int i = 0; i < num_stops; i++) {
+ float position = static_cast(i) / (num_stops - 1);
+
+ float hue = fmod(hue_offset + position * 360.0f, 360.0f);
+ float saturation = 0.9f;
+ float lightness = 0.6f;
+
+ // Alpha fades out towards the far end of the path
+ float alpha = 0.8f * (1.0f - position * 0.3f);
+
+ QColor color = QColor::fromHslF(hue / 360.0f, saturation, lightness, alpha);
+ bg.setColorAt(position, color);
+ }
+
+ painter.setBrush(bg);
+ painter.drawPolygon(track_vertices);
+ } else {
+ // Normal path rendering
+ ModelRenderer::drawPath(painter, model, surface_rect.height());
+ }
}
diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc
index ca85935d0b..2e066e88b5 100644
--- a/selfdrive/ui/sunnypilot/qt/util.cc
+++ b/selfdrive/ui/sunnypilot/qt/util.cc
@@ -110,3 +110,16 @@ QStringList searchFromList(const QString &query, const QStringList &list) {
}
return search_results;
}
+
+std::optional loadCerealEvent(Params& params, const std::string& _param) {
+ std::string bytes = params.get(_param);
+
+ try {
+ AlignedBuffer aligned_buf;
+ capnp::FlatArrayMessageReader cmsg(aligned_buf.align(bytes.data(), bytes.size()));
+ return cmsg.getRoot();
+ } catch (kj::Exception& e) {
+ qInfo() << "invalid " << QString::fromStdString(_param) << ":" << e.getDescription().cStr();
+ return std::nullopt;
+ }
+}
diff --git a/selfdrive/ui/sunnypilot/qt/util.h b/selfdrive/ui/sunnypilot/qt/util.h
index 089b5370cc..4b9d615ce5 100644
--- a/selfdrive/ui/sunnypilot/qt/util.h
+++ b/selfdrive/ui/sunnypilot/qt/util.h
@@ -15,8 +15,11 @@
#include
#include
+#include "selfdrive/ui/sunnypilot/ui.h"
+
QString getUserAgent(bool sunnylink = false);
std::optional getSunnylinkDongleId();
std::optional getParamIgnoringDefault(const std::string ¶m_name, const std::string &default_value);
QMap loadPlatformList();
QStringList searchFromList(const QString &query, const QStringList &list);
+std::optional loadCerealEvent(Params& params, const std::string& _param);
diff --git a/selfdrive/ui/sunnypilot/qt/widgets/controls.h b/selfdrive/ui/sunnypilot/qt/widgets/controls.h
index a840febfe7..03cb461385 100644
--- a/selfdrive/ui/sunnypilot/qt/widgets/controls.h
+++ b/selfdrive/ui/sunnypilot/qt/widgets/controls.h
@@ -750,3 +750,24 @@ public:
setFixedSize(400, 100);
}
};
+
+inline QString RainbowizeWords(const QString &text) {
+ const QStringList colors = {
+ "#FF6F61", // soft coral red
+ "#FFA177", // warm peach
+ "#FFD966", // soft golden yellow
+ "#88D498", // mint green
+ "#6EC6FF", // sky blue
+ "#A78BFA", // soft lavender
+ "#F78FB3" // rose pink
+ };
+
+ QString result;
+ QStringList words = text.split(' ');
+
+ for (int i = 0; i < words.size(); ++i) {
+ result += QString("%2 ").arg(colors[i % colors.size()]).arg(words[i].toHtmlEscaped());
+ }
+
+ return result.trimmed();
+ }
diff --git a/selfdrive/ui/sunnypilot/qt/widgets/external_storage.cc b/selfdrive/ui/sunnypilot/qt/widgets/external_storage.cc
new file mode 100644
index 0000000000..f951187511
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/qt/widgets/external_storage.cc
@@ -0,0 +1,170 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/widgets/external_storage.h"
+
+#include
+#include
+#include
+#include
+#include
+
+#include "common/params.h"
+#include "selfdrive/ui/qt/api.h"
+#include "selfdrive/ui/qt/widgets/input.h"
+#include "selfdrive/ui/sunnypilot/ui.h"
+
+ExternalStorageControl::ExternalStorageControl() :
+ ButtonControl(tr("External Storage"), "", tr("Extend your comma device's storage by inserting a USB drive into the aux port.")) {
+
+ QObject::connect(this, &ButtonControl::clicked, [=]() {
+ if (text() == tr("CHECK") || text() == tr("MOUNT")) {
+ mountStorage();
+ } else if (text() == tr("UNMOUNT")) {
+ unmountStorage();
+ } else if (text() == tr("FORMAT")) {
+ if (ConfirmationDialog::confirm(tr("Are you sure you want to format this drive? This will erase all data."), tr("Format"), this)) {
+ formatStorage();
+ }
+ }
+ });
+
+ QObject::connect(uiState(), &UIState::offroadTransition, this, &ExternalStorageControl::updateState);
+ updateState(!uiState()->scene.started);
+
+ refresh();
+}
+
+void ExternalStorageControl::updateState(bool offroad) {
+ setEnabled(offroad);
+}
+
+void ExternalStorageControl::debouncedRefresh() {
+ if (refreshPending) return;
+ refreshPending = true;
+
+ QTimer::singleShot(250, this, [=]() {
+ refreshPending = false;
+ refresh();
+ });
+}
+
+void ExternalStorageControl::refresh() {
+ QtConcurrent::run([=]() {
+ auto run = [](const QString &cmd) {
+ QProcess p;
+ p.start("sh", QStringList() << "-c" << cmd);
+ p.waitForFinished();
+ return p.exitCode() == 0;
+ };
+
+ bool isMounted = run("findmnt -n /mnt/external_realdata");
+ bool hasDrive = run("lsblk -f /dev/sdg");
+ bool hasFs = run("lsblk -f /dev/sdg1 | grep -q ext4");
+ bool hasLabel = run("sudo blkid /dev/sdg1 | grep -q 'LABEL=\"openpilot\"'");
+
+ QString info;
+ if (isMounted && hasLabel) {
+ QProcess df;
+ df.start("sh", QStringList() << "-c" << "df -h /mnt/external_realdata | awk 'NR==2 {print $3 \"/\" $2}'");
+ df.waitForFinished();
+ info = df.readAllStandardOutput().trimmed();
+ }
+
+ QMetaObject::invokeMethod(this, [=]() {
+ if (formatting) {
+ setValue(tr("formatting"));
+ setText(tr("FORMAT"));
+ setEnabled(false);
+ } else {
+ if (!hasDrive) {
+ setValue(tr("insert drive"));
+ setText(tr("CHECK"));
+ } else if (!hasFs || !hasLabel) {
+ setValue(tr("needs format"));
+ setText(tr("FORMAT"));
+ } else if (isMounted) {
+ setValue(info);
+ setText(tr("UNMOUNT"));
+ } else {
+ setValue("drive detected");
+ setText(tr("MOUNT"));
+ }
+ updateState(!uiState()->scene.started);
+ }
+ }, Qt::QueuedConnection);
+ });
+}
+
+void ExternalStorageControl::mountStorage() {
+ setValue(tr("mounting"));
+ setEnabled(false);
+
+ QtConcurrent::run([=]() {
+ QProcess process;
+ process.start("sh", QStringList() << "-c" <<
+ "sudo mount -o remount,rw / && "
+ "sudo mkdir -p /mnt/external_realdata && "
+ "grep -q '/dev/sdg1 /mnt/external_realdata' /etc/fstab || "
+ "echo '/dev/sdg1 /mnt/external_realdata ext4 defaults,nofail 0 2' | sudo tee -a /etc/fstab && "
+ "sudo systemctl daemon-reexec && "
+ "sudo mount /mnt/external_realdata && "
+ "sudo chown -R comma:comma /mnt/external_realdata && "
+ "sudo chmod -R 775 /mnt/external_realdata && "
+ "sudo mount -o remount,ro /");
+ process.waitForFinished();
+
+ QMetaObject::invokeMethod(this, [=]() {
+ debouncedRefresh();
+ }, Qt::QueuedConnection);
+ });
+}
+
+void ExternalStorageControl::unmountStorage() {
+ setValue(tr("unmounting"));
+ setEnabled(false);
+
+ QtConcurrent::run([=]() {
+ QProcess process;
+ process.start("sh", QStringList() << "-c" << "sudo umount /mnt/external_realdata");
+ process.waitForFinished();
+
+ QMetaObject::invokeMethod(this, [=]() {
+ debouncedRefresh();
+ }, Qt::QueuedConnection);
+ });
+}
+
+void ExternalStorageControl::formatStorage() {
+ unmountStorage();
+ formatting = true;
+ setValue(tr("formatting"));
+ setEnabled(false);
+
+ QProcess *process = new QProcess(this);
+ connect(process, static_cast(&QProcess::finished),
+ this, [=](int exitCode, QProcess::ExitStatus status) {
+ process->deleteLater();
+ formatting = false;
+ if (exitCode == 0 && status == QProcess::NormalExit) {
+ mountStorage();
+ } else {
+ setValue(tr("needs format"));
+ updateState(!uiState()->scene.started);
+ }
+ });
+ process->start("sh", QStringList() << "-c" <<
+ "sudo wipefs -a /dev/sdg && "
+ "sudo parted -s /dev/sdg mklabel gpt mkpart primary ext4 0% 100% && "
+ "sudo mkfs.ext4 -F -L openpilot /dev/sdg1"
+ );
+}
+
+void ExternalStorageControl::showEvent(QShowEvent *event) {
+ ButtonControl::showEvent(event);
+ QTimer::singleShot(100, this, &ExternalStorageControl::debouncedRefresh);
+}
diff --git a/selfdrive/ui/sunnypilot/qt/widgets/external_storage.h b/selfdrive/ui/sunnypilot/qt/widgets/external_storage.h
new file mode 100644
index 0000000000..d26eefd18c
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/qt/widgets/external_storage.h
@@ -0,0 +1,34 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include "system/hardware/hw.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+#define ButtonControl ButtonControlSP
+
+class ExternalStorageControl : public ButtonControl {
+ Q_OBJECT
+
+public:
+ ExternalStorageControl();
+
+protected:
+ void showEvent(QShowEvent *event) override;
+
+private:
+ Params params;
+
+ bool refreshPending = false;
+ bool formatting = false;
+ void updateState(bool offroad);
+ void refresh();
+ void debouncedRefresh();
+ void mountStorage();
+ void unmountStorage();
+ void formatStorage();
+};
diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc
index b2701356cc..7b10929bc6 100644
--- a/selfdrive/ui/sunnypilot/ui.cc
+++ b/selfdrive/ui/sunnypilot/ui.cc
@@ -18,13 +18,23 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
- "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP"
+ "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
+ "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
+ "carStateSP", "liveParameters"
});
// update timer
timer = new QTimer(this);
QObject::connect(timer, &QTimer::timeout, this, &UIStateSP::update);
timer->start(1000 / UI_FREQ);
+
+ // Param watcher for UIScene param updates
+ param_watcher = new ParamWatcher(this);
+ connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString ¶m_name, const QString ¶m_value) {
+ ui_update_params_sp(this);
+ });
+ param_watcher->addParam("DevUIInfo");
+ param_watcher->addParam("StandstillTimer");
}
// This method overrides completely the update method from the parent class intentionally.
@@ -39,6 +49,12 @@ void UIStateSP::update() {
emit uiUpdate(*this);
}
+void ui_update_params_sp(UIStateSP *s) {
+ auto params = Params();
+ s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
+ s->scene.standstill_timer = params.getBool("StandstillTimer");
+}
+
DeviceSP::DeviceSP(QObject *parent) : Device(parent) {
QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &DeviceSP::update);
QObject::connect(this, &Device::displayPowerChanged, this, &DeviceSP::handleDisplayPowerChanged);
diff --git a/selfdrive/ui/sunnypilot/ui.h b/selfdrive/ui/sunnypilot/ui.h
index cf8de1c4bb..393f997cbd 100644
--- a/selfdrive/ui/sunnypilot/ui.h
+++ b/selfdrive/ui/sunnypilot/ui.h
@@ -13,6 +13,7 @@
#include "selfdrive/ui/sunnypilot/qt/network/sunnylink/models/role_model.h"
#include "selfdrive/ui/sunnypilot/qt/network/sunnylink/models/sponsor_role_model.h"
#include "selfdrive/ui/ui.h"
+#include "selfdrive/ui/qt/util.h"
class UIStateSP : public UIState {
Q_OBJECT
@@ -73,6 +74,7 @@ private slots:
private:
std::vector sunnylinkRoles = {};
std::vector sunnylinkUsers = {};
+ ParamWatcher *param_watcher;
};
UIStateSP *uiStateSP();
@@ -92,3 +94,5 @@ private:
DeviceSP *deviceSP();
inline DeviceSP *device() { return deviceSP(); }
+
+void ui_update_params_sp(UIStateSP *s);
diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h
new file mode 100644
index 0000000000..c941be675c
--- /dev/null
+++ b/selfdrive/ui/sunnypilot/ui_scene.h
@@ -0,0 +1,13 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+typedef struct UISceneSP : UIScene {
+ int dev_ui_info = 0;
+ bool standstill_timer = false;
+} UISceneSP;
diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py
index 653395ba1d..68d1f0c185 100755
--- a/selfdrive/ui/tests/test_ui/run.py
+++ b/selfdrive/ui/tests/test_ui/run.py
@@ -29,7 +29,7 @@ UI_DELAY = 0.5 # may be slower on CI?
TEST_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"
STREAMS: list[tuple[VisionStreamType, CameraConfig, bytes]] = []
-OFFROAD_ALERTS = ['Offroad_StorageMissing', 'Offroad_IsTakingSnapshot']
+OFFROAD_ALERTS = ['Offroad_IsTakingSnapshot', ]
DATA: dict[str, capnp.lib.capnp._DynamicStructBuilder] = dict.fromkeys(
["carParams", "deviceState", "pandaStates", "controlsState", "selfdriveState",
"liveCalibration", "modelV2", "radarState", "driverMonitoringState", "carState",
diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts
index f72ea8ee83..ef7110e72e 100644
--- a/selfdrive/ui/translations/main_ar.ts
+++ b/selfdrive/ui/translations/main_ar.ts
@@ -2103,6 +2103,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts
index 193e2fd895..dde9241460 100644
--- a/selfdrive/ui/translations/main_de.ts
+++ b/selfdrive/ui/translations/main_de.ts
@@ -2085,6 +2085,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts
index 1d1723cde7..3b53e7f944 100644
--- a/selfdrive/ui/translations/main_es.ts
+++ b/selfdrive/ui/translations/main_es.ts
@@ -2087,6 +2087,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts
index 959321bb4b..45705defd9 100644
--- a/selfdrive/ui/translations/main_fr.ts
+++ b/selfdrive/ui/translations/main_fr.ts
@@ -2083,6 +2083,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts
index 28b5b00e51..3faa183954 100644
--- a/selfdrive/ui/translations/main_ja.ts
+++ b/selfdrive/ui/translations/main_ja.ts
@@ -2082,6 +2082,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts
index d69fda88b0..756cad6045 100644
--- a/selfdrive/ui/translations/main_ko.ts
+++ b/selfdrive/ui/translations/main_ko.ts
@@ -2096,6 +2096,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts
index c5171b19f7..0a886d435c 100644
--- a/selfdrive/ui/translations/main_pt-BR.ts
+++ b/selfdrive/ui/translations/main_pt-BR.ts
@@ -2087,6 +2087,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts
index 3bd2d84a92..7b89f2a694 100644
--- a/selfdrive/ui/translations/main_th.ts
+++ b/selfdrive/ui/translations/main_th.ts
@@ -2078,6 +2078,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts
index db9c15580a..0598d08364 100644
--- a/selfdrive/ui/translations/main_tr.ts
+++ b/selfdrive/ui/translations/main_tr.ts
@@ -2077,6 +2077,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts
index 2316360891..1f4db2d6d0 100644
--- a/selfdrive/ui/translations/main_zh-CHS.ts
+++ b/selfdrive/ui/translations/main_zh-CHS.ts
@@ -2082,6 +2082,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts
index 16bb393dea..1a8784846c 100644
--- a/selfdrive/ui/translations/main_zh-CHT.ts
+++ b/selfdrive/ui/translations/main_zh-CHT.ts
@@ -2082,6 +2082,22 @@ Warning: You are on a metered connection!
[Don't use] Enable sunnylink uploader
+
+ 🚀 sunnylink 🚀
+
+
+
+ For secure backup, restore, and remote configuration
+
+
+
+ Sponsorship isn't required for basic backup/restore
+
+
+
+ Click the sponsor button for more details
+
+
SunnylinkSponsorPopup
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 4b8f79785b..4cad883f7d 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -55,8 +55,7 @@ void update_state(UIState *s) {
}
if (sm.updated("wideRoadCameraState")) {
auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
- float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f;
- scene.light_sensor = std::max(100.0f - scale * cam_state.getExposureValPercent(), 0.0f);
+ scene.light_sensor = std::max(100.0f - cam_state.getExposureValPercent(), 0.0f);
} else if (!sm.allAliveAndValid({"wideRoadCameraState"})) {
scene.light_sensor = -1;
}
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index e78b573b66..5b3872b3d4 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -66,6 +66,11 @@ typedef struct UIScene {
uint64_t started_frame;
} UIScene;
+#ifdef SUNNYPILOT
+#include "sunnypilot/ui_scene.h"
+#define UIScene UISceneSP
+#endif
+
class UIState : public QObject {
Q_OBJECT
diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py
index c2bb5f77a4..c4a2c0ca11 100644
--- a/selfdrive/ui/ui_state.py
+++ b/selfdrive/ui/ui_state.py
@@ -105,10 +105,7 @@ class UIState:
# Handle wide road camera state updates
if self.sm.updated["wideRoadCameraState"]:
cam_state = self.sm["wideRoadCameraState"]
-
- # Scale factor based on sensor type
- scale = 6.0 if cam_state.sensor == 'ar0231' else 1.0
- self.light_sensor = max(100.0 - scale * cam_state.exposureValPercent, 0.0)
+ self.light_sensor = max(100.0 - cam_state.exposureValPercent, 0.0)
elif not self.sm.alive["wideRoadCameraState"] or not self.sm.valid["wideRoadCameraState"]:
self.light_sensor = -1
diff --git a/sunnypilot/__init__.py b/sunnypilot/__init__.py
index e69de29bb2..ab5441aa71 100644
--- a/sunnypilot/__init__.py
+++ b/sunnypilot/__init__.py
@@ -0,0 +1,7 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+PARAMS_UPDATE_PERIOD = 3 # seconds
diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py
index da9f4d7771..794f2ef8ff 100644
--- a/sunnypilot/mapd/live_map_data/debug.py
+++ b/sunnypilot/mapd/live_map_data/debug.py
@@ -37,15 +37,14 @@ def live_map_data_sp_thread():
def live_map_data_sp_thread_debug(gps_location_service):
- _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', gps_location_service])
+ _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
_sub_master.update()
v_ego = _sub_master['carState'].vEgo
- long_spl = _sub_master['longitudinalPlanSP'].speedLimit
- _policy = Policy.car_state_priority
- _resolver = SpeedLimitResolver(_policy)
- _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master)
- print(_speed_limit, _distance, _source, " <-> ", long_spl)
+ _resolver = SpeedLimitResolver()
+ _resolver.policy = Policy.car_state_priority
+ _resolver.update(v_ego, _sub_master)
+ print(_resolver.speed_limit, _resolver.distance, _resolver.source)
def main():
diff --git a/sunnypilot/mapd/mapd_manager.py b/sunnypilot/mapd/mapd_manager.py
index 9ebf07a7f4..1211c1ecc6 100755
--- a/sunnypilot/mapd/mapd_manager.py
+++ b/sunnypilot/mapd/mapd_manager.py
@@ -6,11 +6,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
-import time
import platform
import os
import glob
import shutil
+from datetime import datetime
from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper, config_realtime_process
@@ -56,7 +56,7 @@ def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None:
- params.put("OsmDownloadedDate", str(time.monotonic()))
+ params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
params.put_bool("OsmDbUpdatesCheck", False)
osm_download_locations = {
diff --git a/sunnypilot/modeld/fill_model_msg.py b/sunnypilot/modeld/fill_model_msg.py
index dadffc8433..a62c451efd 100644
--- a/sunnypilot/modeld/fill_model_msg.py
+++ b/sunnypilot/modeld/fill_model_msg.py
@@ -3,6 +3,7 @@ import capnp
import numpy as np
from cereal import log
from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan
+from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
from openpilot.sunnypilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -120,23 +121,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
modelV2_action.desiredCurvature = desired_curvature
# times at X_IDXS according to model plan
- PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
- PLAN_T_IDXS[0] = 0.0
- plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
- for xidx in range(1, ModelConstants.IDX_N):
- tidx = 0
- # increment tidx until we find an element that's further away than the current xidx
- while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]:
- tidx += 1
- if tidx == ModelConstants.IDX_N - 1:
- # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
- PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
- break
- # interpolate to find `t` for the current xidx
- current_x_val = plan_x[tidx]
- next_x_val = plan_x[tidx+1]
- p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan')
- PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
+ PLAN_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
# lane lines
modelV2.init('laneLines', 4)
diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py
index b94e166bc6..3d11ed23f4 100755
--- a/sunnypilot/modeld/modeld.py
+++ b/sunnypilot/modeld/modeld.py
@@ -177,7 +177,7 @@ def main(demo=False):
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
# messaging
- pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+ pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
publish_state = PublishState()
@@ -304,6 +304,7 @@ def main(demo=False):
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
+ mdv2sp_send = messaging.new_message('modelDataV2SP')
action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL)
fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
@@ -316,6 +317,7 @@ def main(demo=False):
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+ mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
@@ -323,6 +325,7 @@ def main(demo=False):
pm.send('modelV2', modelv2_send)
pm.send('drivingModelData', drivingdata_send)
pm.send('cameraOdometry', posenet_send)
+ pm.send('modelDataV2SP', mdv2sp_send)
last_vipc_frame_id = meta_main.frame_id
diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py
index c7de698f61..ee0eb48684 100644
--- a/sunnypilot/modeld_v2/fill_model_msg.py
+++ b/sunnypilot/modeld_v2/fill_model_msg.py
@@ -3,6 +3,7 @@ import capnp
import numpy as np
from cereal import log
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+from openpilot.sunnypilot.models.helpers import plan_x_idxs_helper
from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -118,8 +119,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
# action (includes lateral planning now)
modelV2.action = action
- # times at X_IDXS of edges and lines aren't used
- LINE_T_IDXS: list[float] = []
+ # times at X_IDXS of edges and lines
+ LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
# lane lines
modelV2.init('laneLines', 4)
diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py
index 1a420e6f1a..0fd45940d8 100755
--- a/sunnypilot/modeld_v2/modeld.py
+++ b/sunnypilot/modeld_v2/modeld.py
@@ -27,7 +27,7 @@ from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase
from openpilot.sunnypilot.models.helpers import get_active_bundle
from openpilot.sunnypilot.models.runners.helpers import get_model_runner
-PROCESS_NAME = "selfdrive.modeld.modeld"
+PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
class FrameMeta:
@@ -77,42 +77,47 @@ class ModelState(ModelStateBase):
self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32)
# Temporal input: shape is [batch, history, features]
if len(shape) == 3 and shape[1] > 1:
- buffer_history_len = max(100, (shape[1] * 4 if shape[1] < 100 else shape[1])) # Allow for higher history buffers in the future
+ buffer_history_len = shape[1] * 4 if shape[1] < 99 else shape[1] # Allow for higher history buffers in the future
feature_len = shape[2]
- self.temporal_buffers[key] = np.zeros((1, buffer_history_len, feature_len), dtype=np.float32)
features_buffer_shape = self.model_runner.input_shapes.get('features_buffer')
if shape[1] in (24, 25) and features_buffer_shape is not None and features_buffer_shape[1] == 24: # 20Hz
+ buffer_history_len = (features_buffer_shape[1] + 1) * 4
step = int(-buffer_history_len / shape[1])
self.temporal_idxs_map[key] = np.arange(step, step * (shape[1] + 1), step)[::-1]
elif shape[1] == 25: # Split
skip = buffer_history_len // shape[1]
self.temporal_idxs_map[key] = np.arange(buffer_history_len)[-1 - (skip * (shape[1] - 1))::skip]
- elif shape[1] == buffer_history_len: # non20hz
- self.temporal_idxs_map[key] = np.arange(buffer_history_len)
+ elif shape[1] >= 99: # non20hz
+ self.temporal_idxs_map[key] = np.arange(shape[1])
+ self.temporal_buffers[key] = np.zeros((1, buffer_history_len, feature_len), dtype=np.float32)
@property
def mlsim(self) -> bool:
return bool(self.generation is not None and self.generation >= 11)
+ @property
+ def desire_key(self) -> str:
+ return next(key for key in self.numpy_inputs if key.startswith('desire'))
+
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
- inputs['desire'][0] = 0
- new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
- self.prev_desire[:] = inputs['desire']
- self.temporal_buffers['desire'][0,:-1] = self.temporal_buffers['desire'][0,1:]
- self.temporal_buffers['desire'][0,-1] = new_desire
+ inputs[self.desire_key][0] = 0
+ new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0)
+ self.prev_desire[:] = inputs[self.desire_key]
+ self.temporal_buffers[self.desire_key][0,:-1] = self.temporal_buffers[self.desire_key][0,1:]
+ self.temporal_buffers[self.desire_key][0,-1] = new_desire
# Roll buffer and assign based on desire.shape[1] value
- if self.temporal_buffers['desire'].shape[1] > self.numpy_inputs['desire'].shape[1]:
- skip = self.temporal_buffers['desire'].shape[1] // self.numpy_inputs['desire'].shape[1]
- self.numpy_inputs['desire'][:] = (
- self.temporal_buffers['desire'][0].reshape(self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], skip, -1).max(axis=2))
+ if self.temporal_buffers[self.desire_key].shape[1] > self.numpy_inputs[self.desire_key].shape[1]:
+ skip = self.temporal_buffers[self.desire_key].shape[1] // self.numpy_inputs[self.desire_key].shape[1]
+ self.numpy_inputs[self.desire_key][:] = (self.temporal_buffers[self.desire_key][0].reshape(
+ self.numpy_inputs[self.desire_key].shape[0], self.numpy_inputs[self.desire_key].shape[1], skip, -1).max(axis=2))
else:
- self.numpy_inputs['desire'][:] = self.temporal_buffers['desire'][0, self.temporal_idxs_map['desire']]
+ self.numpy_inputs[self.desire_key][:] = self.temporal_buffers[self.desire_key][0, self.temporal_idxs_map[self.desire_key]]
for key in self.numpy_inputs:
- if key in inputs and key not in ['desire']:
+ if key in inputs and key not in [self.desire_key]:
self.numpy_inputs[key][:] = inputs[key]
imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.model_runner.vision_input_names}
@@ -156,10 +161,11 @@ class ModelState(ModelStateBase):
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.mlsim)
- if v_ego > self.MIN_LAT_CONTROL_SPEED:
- desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
- else:
- desired_curvature = prev_action.desiredCurvature
+ if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models
+ if v_ego > self.MIN_LAT_CONTROL_SPEED:
+ desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)
+ else:
+ desired_curvature = prev_action.desiredCurvature
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop))
@@ -202,7 +208,7 @@ def main(demo=False):
cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
# messaging
- pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+ pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"])
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
publish_state = PublishState()
@@ -306,7 +312,7 @@ def main(demo=False):
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.model_runner.vision_input_names}
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.model_runner.vision_input_names}
inputs:dict[str, np.ndarray] = {
- 'desire': vec_desire,
+ model.desire_key: vec_desire,
'traffic_convention': traffic_convention,
}
@@ -322,6 +328,7 @@ def main(demo=False):
modelv2_send = messaging.new_message('modelV2')
drivingdata_send = messaging.new_message('drivingModelData')
posenet_send = messaging.new_message('cameraOdometry')
+ mdv2sp_send = messaging.new_message('modelDataV2SP')
action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
prev_action = action
@@ -336,6 +343,7 @@ def main(demo=False):
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+ mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
@@ -343,6 +351,7 @@ def main(demo=False):
pm.send('modelV2', modelv2_send)
pm.send('drivingModelData', drivingdata_send)
pm.send('cameraOdometry', posenet_send)
+ pm.send('modelDataV2SP', mdv2sp_send)
last_vipc_frame_id = meta_main.frame_id
diff --git a/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py b/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py
index 8a0cfd97c8..f664db31b3 100644
--- a/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py
+++ b/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py
@@ -8,12 +8,16 @@ import openpilot.sunnypilot.modeld_v2.modeld as modeld_module
ModelState = modeld_module.ModelState
-
# These are the shapes extracted/loaded from the model onnx
SHAPE_MODE_PARAMS = [
- ({'desire': (1, 25, 8), 'features_buffer': (1, 25, 512), 'prev_desired_curv': (1, 25, 1)}, 'split'),
- ({'desire': (1, 25, 8), 'features_buffer': (1, 24, 512), 'prev_desired_curv': (1, 25, 1)}, '20hz'),
- ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1)}, 'non20hz'),
+ ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "nav_features": (1, 256), "nav_instructions": (1, 150)}, 'non20hz'), # Optimus Prime
+ ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "lat_planner_state": (1, 4),}, 'non20hz'), # farmville
+ ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "lateral_control_params": (1, 2), "prev_desired_curv": (1, 100, 1)}, 'non20hz'), # wd40
+ ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1), "lateral_control_params": (1, 2),}, 'non20hz'), # NTS
+ ({'desire': (1, 25, 8), 'features_buffer': (1, 24, 512)}, '20hz'), # NPR
+ ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1), "lateral_control_params": (1, 2),}, 'non20hz'), # NTS
+ ({'desire': (1, 25, 8), 'features_buffer': (1, 25, 512)}, 'split'), # Steam Powered v2
+ ({'desire_pulse': (1, 25, 8), 'features_buffer': (1, 25, 512)}, 'split'), # desire rename
]
@@ -95,9 +99,7 @@ def get_expected_indices(shape, constants, mode, key=None):
idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1]
return idxs
elif mode == 'non20hz':
- if key and shape[1] == constants.FULL_HISTORY_BUFFER_LEN:
- return np.arange(constants.FULL_HISTORY_BUFFER_LEN)
- return None
+ return np.arange(shape[1])
return None
@@ -108,6 +110,8 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches):
for key in shapes:
buf = state.temporal_buffers.get(key, None)
idxs = state.temporal_idxs_map.get(key, None)
+ if buf is None:
+ continue # not all shapes are 3D, and the non-3D ones are not buffered
# Buffer shape logic
if mode == 'split':
expected_shape = (1, constants.FULL_HISTORY_BUFFER_LEN, shapes[key][2])
@@ -116,10 +120,7 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches):
expected_shape = (1, constants.FULL_HISTORY_BUFFER_LEN, shapes[key][2])
expected_idxs = get_expected_indices(shapes[key], constants, '20hz', key)
elif mode == 'non20hz':
- if key == 'features_buffer':
- expected_shape = (1, shapes[key][1]*4, shapes[key][2])
- else:
- expected_shape = (1, shapes[key][1], shapes[key][2])
+ expected_shape = (1, shapes[key][1], shapes[key][2])
expected_idxs = get_expected_indices(shapes[key], constants, 'non20hz', key)
assert buf is not None, f"{key}: buffer not found"
@@ -130,10 +131,10 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches):
assert idxs is None or idxs.size == 0, f"{key}: buffer idxs should be None or empty"
-def legacy_buffer_update(buf, new_val, mode, key, constants, idxs):
+def legacy_buffer_update(buf, new_val, mode, key, constants, idxs, input_shape, prev_desire=None):
# This is what we compare the new dynamic logic to, to ensure it does the same thing
if mode == 'split':
- if key == 'desire':
+ if key == 'desire' or key.startswith('desire'):
buf[0,:-1] = buf[0,1:]
buf[0,-1] = new_val
return buf.reshape((1, constants.INPUT_HISTORY_BUFFER_LEN, constants.TEMPORAL_SKIP, -1)).max(axis=2)
@@ -173,15 +174,22 @@ def legacy_buffer_update(buf, new_val, mode, key, constants, idxs):
return legacy_buf[idxs]
elif mode == 'non20hz':
if key == 'desire':
- length = new_val.shape[0]
- buf[0,:-1,:length] = buf[0,1:,:length]
- buf[0,-1,:length] = new_val[:length]
+ desire_len = constants.DESIRE_LEN
+ if prev_desire is None:
+ prev_desire = np.zeros(desire_len, dtype=np.float32)
+ # Set first element to zero
+ new_val = new_val.copy()
+ new_val[0] = 0
+ # Shift buffer by desire len
+ buf[0][:-desire_len] = buf[0][desire_len:]
+ # Only insert new desire if rising edge
+ buf[0][-desire_len:] = np.where(new_val - prev_desire > 0.99, new_val, 0)
+ prev_desire[:] = new_val
return buf[0]
elif key == 'features_buffer':
- feature_len = new_val.shape[0]
- buf[0,:-1,:feature_len] = buf[0,1:,:feature_len]
- buf[0,-1,:feature_len] = new_val[:feature_len]
- return buf[0]
+ buf[0, :-1] = buf[0, 1:]
+ buf[0, -1] = new_val
+ return buf[0, -input_shape[1]:] # (99, 512)
elif key == 'prev_desired_curv':
length = new_val.shape[0]
buf[0,:-length,0] = buf[0,length:,0]
@@ -191,32 +199,18 @@ def legacy_buffer_update(buf, new_val, mode, key, constants, idxs):
def dynamic_buffer_update(state, key, new_val, mode):
- if key == 'desire':
- state.temporal_buffers['desire'][0,:-1] = state.temporal_buffers['desire'][0,1:]
- state.temporal_buffers['desire'][0,-1] = new_val
- if state.temporal_buffers['desire'].shape[1] > state.numpy_inputs['desire'].shape[1]:
- skip = state.temporal_buffers['desire'].shape[1] // state.numpy_inputs['desire'].shape[1]
- return state.temporal_buffers['desire'][0].reshape(
- state.numpy_inputs['desire'].shape[0], state.numpy_inputs['desire'].shape[1], skip, -1
- ).max(axis=2)
- else:
- return state.temporal_buffers['desire'][0, state.temporal_idxs_map['desire']]
-
- inputs = {'desire': np.zeros((1, state.constants.DESIRE_LEN), dtype=np.float32)}
- for k, tb in state.temporal_buffers.items():
- if k in state.temporal_idxs_map:
- continue
- buf_len = tb.shape[1]
- if k in state.numpy_inputs:
- out_len = state.numpy_inputs[k].shape[1]
- if out_len <= buf_len:
- state.temporal_idxs_map[k] = np.arange(buf_len)[-out_len:]
- else:
- state.temporal_idxs_map[k] = np.arange(buf_len)
- else:
- state.temporal_idxs_map[k] = np.arange(buf_len)
+ if key == 'desire' or key.startswith('desire'):
+ inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32)
+ for k, v in state.model_runner.input_shapes.items() if k != key}
+ inputs[key] = new_val.copy()
+ # ModelState.run expects desire as a pulse, so we zero the first element.
+ inputs[key][0] = 0
+ state.run({}, {}, inputs, prepare_only=False)
+ return state.numpy_inputs[key]
if key == 'features_buffer':
+ inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32)
+ for k, v in state.model_runner.input_shapes.items() if k != 'features_buffer'}
def run_model_stub():
return {
'hidden_state': np.asarray(new_val, dtype=np.float32).reshape(1, -1),
@@ -226,6 +220,8 @@ def dynamic_buffer_update(state, key, new_val, mode):
return state.numpy_inputs['features_buffer'][0]
if key == 'prev_desired_curv':
+ inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32)
+ for k, v in state.model_runner.input_shapes.items() if k != 'prev_desired_curv'}
def run_model_stub():
return {
'hidden_state': np.zeros((1, state.constants.FEATURE_LEN), dtype=np.float32),
@@ -241,16 +237,27 @@ def dynamic_buffer_update(state, key, new_val, mode):
@pytest.mark.parametrize("key", ["desire", "features_buffer", "prev_desired_curv"])
def test_buffer_update_equivalence(shapes, mode, key, apply_patches):
state = ModelState(None)
+ if key == "desire":
+ desire_keys = [k for k in shapes.keys() if k.startswith('desire')]
+ if desire_keys:
+ actual_key = desire_keys[0] # Use the first (and likely only) desire key
+ else:
+ actual_key = key
+
+ if actual_key not in state.numpy_inputs:
+ pytest.skip()
+
constants = DummyModelRunner(shapes).constants
- buf = state.temporal_buffers.get(key, None)
- idxs = state.temporal_idxs_map.get(key, None)
- input_shape = shapes[key]
+ buf = state.temporal_buffers.get(actual_key, None)
+ idxs = state.temporal_idxs_map.get(actual_key, None)
+ input_shape = shapes[actual_key]
+ prev_desire = np.zeros(constants.DESIRE_LEN, dtype=np.float32) if key == 'desire' else None
+
for step in range(20): # multiple steps to ensure history is built up
new_val = np.full((input_shape[2],), step, dtype=np.float32)
- expected = legacy_buffer_update(buf, new_val, mode, key, constants, idxs)
- actual = dynamic_buffer_update(state, key, new_val, mode)
- # Model returns the reduced numpy_inputs history, compare the last n entries so the test is checking the same slices.
+ expected = legacy_buffer_update(buf, new_val, mode, actual_key, constants, idxs, input_shape, prev_desire)
+ actual = dynamic_buffer_update(state, actual_key, new_val, mode)
if expected is not None and actual is not None and expected.shape != actual.shape:
if expected.ndim == 2 and actual.ndim == 2 and expected.shape[1] == actual.shape[1]:
expected = expected[-actual.shape[0]:]
- assert np.allclose(actual, expected), f"{mode} {key}: dynamic buffer update does not match legacy logic"
+ assert np.allclose(actual, expected), f"{mode} {actual_key}: dynamic buffer update does not match legacy logic"
diff --git a/sunnypilot/models/fetcher.py b/sunnypilot/models/fetcher.py
index 60a222a36c..3be6e0b46c 100644
--- a/sunnypilot/models/fetcher.py
+++ b/sunnypilot/models/fetcher.py
@@ -8,6 +8,7 @@ See the LICENSE.md file in the root directory for more details.
import time
import requests
+from requests.exceptions import (SSLError, RequestException, HTTPError)
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from sunnypilot.models.helpers import is_bundle_version_compatible
@@ -66,6 +67,7 @@ class ModelParser:
model_bundle.is20hz = bundle.get("is_20hz", False)
model_bundle.minimumSelectorVersion = int(bundle["minimum_selector_version"])
model_bundle.overrides = ModelParser._parse_overrides(bundle.get("overrides", {}))
+ model_bundle.ref = bundle.get("ref")
return model_bundle
@@ -121,19 +123,36 @@ class ModelFetcher:
self.model_cache = ModelCache(params)
self.model_parser = ModelParser()
- def _fetch_and_cache_models(self) -> list[custom.ModelManagerSP.ModelBundle]:
- """Fetches fresh model data from remote and updates cache"""
+ def _fetch_and_cache_models(self) -> list[custom.ModelManagerSP.ModelBundle] | None:
+ """Fetches fresh model data from remote and updates cache.
+ Returns None on transport errors. Raises on 404 and other fatal HTTP errors.
+ """
try:
response = requests.get(self.MODEL_URL, timeout=10)
- response.raise_for_status()
- json_data = response.json()
+ # Explicitly handle 404 differently
+ if response.status_code == 404:
+ cloudlog.error(f"Models URL returned 404 Not Found: {self.MODEL_URL}")
+ raise HTTPError(f"404 Not Found: {self.MODEL_URL}", response=response)
+
+ # Raise for any other 4xx/5xx
+ response.raise_for_status()
+
+ json_data = response.json()
self.model_cache.set(json_data)
cloudlog.debug("Successfully updated models cache")
return self.model_parser.parse_models(json_data)
- except Exception:
- cloudlog.exception("Error fetching models")
- raise
+
+ except ConnectionError as e:
+ cloudlog.warning(f"DNS/connection error while fetching models: {e}")
+ except SSLError as e:
+ cloudlog.warning(f"SSL error while fetching models: {e}")
+ except RequestException as e:
+ cloudlog.warning(f"Request transport error while fetching models: {e}")
+ except Exception as e:
+ cloudlog.exception(f"Unexpected error fetching models: {e}")
+
+ return None
def get_available_bundles(self) -> list[custom.ModelManagerSP.ModelBundle]:
"""Gets the list of available models, with smart cache handling"""
@@ -143,12 +162,12 @@ class ModelFetcher:
cloudlog.debug("Using valid cached models data")
return self.model_parser.parse_models(cached_data)
- try:
- return self._fetch_and_cache_models()
- except Exception:
- if not cached_data:
- cloudlog.exception("Failed to fetch fresh data and no cache available")
- raise
+ fetched_bundles = self._fetch_and_cache_models()
+ if fetched_bundles is not None:
+ return fetched_bundles
+
+ if not cached_data:
+ cloudlog.warning("Failed to fetch fresh data and no cache available")
cloudlog.warning("Failed to fetch fresh data. Using expired cache as fallback")
return self.model_parser.parse_models(cached_data)
diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py
index 79241cd831..20b94fb611 100644
--- a/sunnypilot/models/helpers.py
+++ b/sunnypilot/models/helpers.py
@@ -19,7 +19,7 @@ from openpilot.system.hardware.hw import Paths
from pathlib import Path
# see the README.md for more details on the model selector versioning
-CURRENT_SELECTOR_VERSION = 9
+CURRENT_SELECTOR_VERSION = 10
REQUIRED_MIN_SELECTOR_VERSION = 9
USE_ONNX = os.getenv('USE_ONNX', PC)
@@ -185,3 +185,27 @@ def load_meta_constants(model_metadata):
meta = MetaTombRaider
return meta
+
+
+# The following method(s) are modeld helper methods
+def plan_x_idxs_helper(constants, plan, model_output) -> list[float]:
+ # times at X_IDXS according to plan.
+ LINE_T_IDXS = [np.nan] * constants.IDX_N
+ LINE_T_IDXS[0] = 0.0
+ plan_x = model_output['plan'][0, :, plan.POSITION][:, 0].tolist()
+ for xidx in range(1, constants.IDX_N):
+ tidx = 0
+ # increment tidx until we find an element that's further away than the current xidx
+ while tidx < constants.IDX_N - 1 and plan_x[tidx + 1] < constants.X_IDXS[xidx]:
+ tidx += 1
+ if tidx == constants.IDX_N - 1:
+ # if the plan doesn't extend far enough, set plan_t to the max value (10s), then break
+ LINE_T_IDXS[xidx] = constants.T_IDXS[constants.IDX_N - 1]
+ break
+ # interpolate to find `t` for the current xidx
+ current_x_val = plan_x[tidx]
+ next_x_val = plan_x[tidx + 1]
+ p = (constants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
+ next_x_val - current_x_val) > 1e-9 else float('nan')
+ LINE_T_IDXS[xidx] = p * constants.T_IDXS[tidx + 1] + (1 - p) * constants.T_IDXS[tidx]
+ return LINE_T_IDXS
diff --git a/sunnypilot/models/tests/model_hash b/sunnypilot/models/tests/model_hash
index f66baa7e71..33d7d86e28 100644
--- a/sunnypilot/models/tests/model_hash
+++ b/sunnypilot/models/tests/model_hash
@@ -1 +1 @@
-2ff2f49176a13bc7f856645d785b3b838a5c7ecf7f6cb37699fa0459ebf12453
\ No newline at end of file
+70406ab4dd66d0e384734a8a56632ae4a62bc9670c2e630a0f71588c4e212cd8
\ No newline at end of file
diff --git a/sunnypilot/selfdrive/assets/icons/star-empty.png b/sunnypilot/selfdrive/assets/icons/star-empty.png
new file mode 100644
index 0000000000..bf60dec374
--- /dev/null
+++ b/sunnypilot/selfdrive/assets/icons/star-empty.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3731604f80e83a1fdb7c258baf6530b81190eeec82e6443172e84a35b7a74c02
+size 1088
diff --git a/sunnypilot/selfdrive/assets/icons/star-filled.png b/sunnypilot/selfdrive/assets/icons/star-filled.png
new file mode 100644
index 0000000000..3667231bf0
--- /dev/null
+++ b/sunnypilot/selfdrive/assets/icons/star-filled.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0c2a513b7f2da004f145b7d689654cc65137f1b146d484fbce7ce727a297b62c
+size 861
diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py
index f443aeee0e..716e3e1c93 100644
--- a/sunnypilot/selfdrive/car/cruise_ext.py
+++ b/sunnypilot/selfdrive/car/cruise_ext.py
@@ -7,19 +7,46 @@ See the LICENSE.md file in the root directory for more details.
import numpy as np
from cereal import car
+from opendbc.car import structs
from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
ButtonType = car.CarState.ButtonEvent.Type
+CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
+ ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
+ ButtonType.cancel: 0, ButtonType.mainCruise: 0}
+
+V_CRUISE_MIN = 8
+
+
+def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
+ # increment timer for buttons still pressed
+ for k in button_timers:
+ if button_timers[k] > 0:
+ button_timers[k] += 1
+
+ for b in CS.buttonEvents:
+ if b.type.raw in button_timers:
+ # Start/end timer and store current state on change of button pressed
+ button_timers[b.type.raw] = 1 if b.pressed else 0
+
+
class VCruiseHelperSP:
- def __init__(self) -> None:
+ def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
+ self.CP = CP
+ self.CP_SP = CP_SP
self.params = Params()
+ self.v_cruise_min = 0
+ self.enabled_prev = False
self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
+ self.enable_button_timers = CRUISE_BUTTON_TIMER
+
def read_custom_set_speed_params(self) -> None:
self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
@@ -39,3 +66,26 @@ class VCruiseHelperSP:
v_cruise_delta = v_cruise_delta * actual_increment
return round_to_nearest, v_cruise_delta
+
+ def get_minimum_set_speed(self, is_metric: bool) -> None:
+ if self.CP_SP.pcmCruiseSpeed:
+ self.v_cruise_min = V_CRUISE_MIN
+ return
+
+ self.v_cruise_min = get_minimum_set_speed(is_metric)
+
+ def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
+ # special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise
+ if not self.CP_SP.pcmCruiseSpeed:
+ update_manual_button_timers(CS, self.enable_button_timers)
+ button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers)
+
+ if enabled and not self.enabled_prev:
+ self.enabled_prev = not button_pressed
+ enabled = False
+ elif not enabled:
+ self.enabled_prev = enabled
+
+ return enabled and self.enabled_prev
+
+ return enabled
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py
similarity index 100%
rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/__init__.py
rename to sunnypilot/selfdrive/car/intelligent_cruise_button_management/__init__.py
diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py
new file mode 100644
index 0000000000..0b0957b828
--- /dev/null
+++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py
@@ -0,0 +1,137 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from cereal import car, custom
+from opendbc.car import structs, apply_hysteresis
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
+from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers
+
+LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState
+SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
+
+ALLOWED_SPEED_THRESHOLD = 1.8 # m/s, ~4 MPH
+HYST_GAP = 0.75
+INACTIVE_TIMER = 0.4
+
+
+SEND_BUTTONS = {
+ State.increasing: SendButtonState.increase,
+ State.decreasing: SendButtonState.decrease,
+}
+
+
+class IntelligentCruiseButtonManagement:
+ def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+ self.CP = CP
+ self.CP_SP = CP_SP
+
+ self.v_target = 0
+ self.v_cruise_cluster = 0
+ self.v_cruise_min = 0
+ self.cruise_button = SendButtonState.none
+ self.state = State.inactive
+ self.pre_active_timer = 0
+
+ self.is_ready = False
+ self.is_ready_prev = False
+ self.v_target_ms_last = 0.0
+ self.is_metric = False
+
+ self.cruise_button_timers = CRUISE_BUTTON_TIMER
+
+ @property
+ def v_cruise_equal(self) -> bool:
+ return self.v_target == self.v_cruise_cluster
+
+ def update_calculations(self, CS: car.CarState) -> None:
+ speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
+ ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
+ v_cruise_ms = CS.vCruise * CV.KPH_TO_MS
+
+ # all targets in m/s
+ v_targets = {
+ LongitudinalPlanSource.cruise: v_cruise_ms
+ }
+ source = min(v_targets, key=lambda k: v_targets[k])
+ v_target_ms = v_targets[source]
+
+ self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv)
+
+ self.v_target = round(self.v_target_ms_last * speed_conv)
+ self.v_cruise_min = get_minimum_set_speed(self.is_metric)
+ self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv)
+
+ def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState:
+ self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+ # HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE
+ if self.state != State.inactive:
+ if not self.is_ready:
+ self.state = State.inactive
+
+ else:
+ # PRE_ACTIVE
+ if self.state == State.preActive:
+ if self.pre_active_timer <= 0:
+ if self.v_cruise_equal:
+ self.state = State.holding
+
+ elif self.v_target > self.v_cruise_cluster:
+ self.state = State.increasing
+
+ elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
+ self.state = State.decreasing
+
+ # HOLDING
+ elif self.state == State.holding:
+ if not self.v_cruise_equal:
+ self.state = State.preActive
+
+ # ACCELERATING
+ elif self.state == State.increasing:
+ if self.v_target <= self.v_cruise_cluster:
+ self.state = State.holding
+
+ # DECELERATING
+ elif self.state == State.decreasing:
+ if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min:
+ self.state = State.holding
+
+ # INACTIVE
+ elif self.state == State.inactive:
+ if self.is_ready and not self.is_ready_prev:
+ self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL)
+ self.state = State.preActive
+
+ send_button = SEND_BUTTONS.get(self.state, SendButtonState.none)
+
+ return send_button
+
+ def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
+ update_manual_button_timers(CS, self.cruise_button_timers)
+
+ allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD
+ ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \
+ not CC.cruiseControl.resume
+ button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
+
+ self.is_ready = ready and not button_pressed
+
+ def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None:
+ if self.CP_SP.pcmCruiseSpeed:
+ return
+
+ self.is_metric = is_metric
+
+ self.update_calculations(CS)
+ self.update_readiness(CS, CC)
+
+ self.cruise_button = self.update_state_machine()
+
+ self.is_ready_prev = self.is_ready
diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py
new file mode 100644
index 0000000000..eb4bbdecb5
--- /dev/null
+++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py
@@ -0,0 +1,8 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+def get_minimum_set_speed(is_metric: bool) -> int:
+ return 30 if is_metric else 20
diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py
index cd7a24b63b..55244caa57 100644
--- a/sunnypilot/selfdrive/car/interfaces.py
+++ b/sunnypilot/selfdrive/car/interfaces.py
@@ -43,11 +43,21 @@ def _initialize_neural_network_lateral_control(CI: CarInterfaceBase, CP: structs
CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
+def _initialize_intelligent_cruise_button_management(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
+ if params is None:
+ params = Params()
+
+ icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement")
+ if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl:
+ CP_SP.pcmCruiseSpeed = False
+
+
def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
CP = CI.CP
CP_SP = CI.CP_SP
_initialize_neural_network_lateral_control(CI, CP, CP_SP, params)
+ _initialize_intelligent_cruise_button_management(CP, CP_SP, params)
def initialize_params(params) -> list[dict[str, Any]]:
diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py
index 7e06ac77c1..e0f9326bf8 100644
--- a/sunnypilot/selfdrive/controls/controlsd_ext.py
+++ b/sunnypilot/selfdrive/controls/controlsd_ext.py
@@ -73,7 +73,9 @@ class ControlsExt:
# MADS state
CC_SP.mads = sm['selfdriveStateSP'].mads
- CC_SP.params = self.param_store.publish()
+ CC_SP.params = self.param_store.param_list
+
+ CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
return CC_SP
diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py
new file mode 100644
index 0000000000..00ce026abb
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py
@@ -0,0 +1,45 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from cereal import custom
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+
+
+class LaneTurnController:
+ def __init__(self, desire_helper):
+ self.DH = desire_helper
+ self.turn_direction = custom.TurnDirection.none
+ self.params = Params()
+ self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+ self.param_read_counter = 0
+ self.enabled = self.params.get_bool("LaneTurnDesire")
+
+ def read_params(self):
+ self.enabled = self.params.get_bool("LaneTurnDesire")
+ value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+ self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
+
+ def update_params(self) -> None:
+ if self.param_read_counter % 50 == 0:
+ self.read_params()
+ self.param_read_counter += 1
+
+ def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
+ if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+ self.turn_direction = custom.TurnDirection.turnLeft
+ elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+ self.turn_direction = custom.TurnDirection.turnRight
+ else:
+ self.turn_direction = custom.TurnDirection.none
+
+ def get_turn_direction(self):
+ if not self.enabled:
+ return custom.TurnDirection.none
+ return self.turn_direction
diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py
index fe54f46ca7..6e134276e5 100644
--- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py
+++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py
@@ -9,23 +9,28 @@ from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkL
class LatControlTorqueExt(NeuralNetworkLateralControl):
- def __init__(self, lac_torque, CP, CP_SP):
- super().__init__(lac_torque, CP, CP_SP)
+ def __init__(self, lac_torque, CP, CP_SP, CI):
+ super().__init__(lac_torque, CP, CP_SP, CI)
- def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+ def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel,
- desired_curvature, actual_curvature):
+ desired_curvature, actual_curvature, steer_limited_by_safety, output_torque):
self._ff = ff
+ self._pid = pid
self._pid_log = pid_log
self._setpoint = setpoint
self._measurement = measurement
+ self._roll_compensation = roll_compensation
self._lateral_accel_deadzone = lateral_accel_deadzone
self._desired_lateral_accel = desired_lateral_accel
self._actual_lateral_accel = actual_lateral_accel
self._desired_curvature = desired_curvature
self._actual_curvature = actual_curvature
+ self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel
+ self._steer_limited_by_safety = steer_limited_by_safety
+ self._output_torque = output_torque
self.update_calculations(CS, VM, desired_lateral_accel)
self.update_neural_network_feedforward(CS, params, calibrated_pose)
- return self._ff, self._pid_log
+ return self._pid_log, self._output_torque
diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py
index 644f28573a..1965d50b51 100644
--- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py
+++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py
@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
import math
import numpy as np
+from openpilot.common.pid import PIDController
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -43,9 +44,10 @@ def get_lookahead_value(future_vals, current_val):
class LatControlTorqueExtBase:
- def __init__(self, lac_torque, CP, CP_SP):
+ def __init__(self, lac_torque, CP, CP_SP, CI):
self.model_v2 = None
self.model_valid = False
+ self.lac_torque = lac_torque
self.torque_params = lac_torque.torque_params
self.actual_lateral_jerk: float = 0.0
@@ -53,17 +55,22 @@ class LatControlTorqueExtBase:
self.lateral_jerk_measurement: float = 0.0
self.lookahead_lateral_jerk: float = 0.0
- self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel
+ self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
self._ff = 0.0
+ self._pid = PIDController(0.0, 0.0, k_f=0.0)
self._pid_log = None
self._setpoint = 0.0
self._measurement = 0.0
+ self._roll_compensation = 0.0
self._lateral_accel_deadzone = 0.0
self._desired_lateral_accel = 0.0
self._actual_lateral_accel = 0.0
self._desired_curvature = 0.0
self._actual_curvature = 0.0
+ self._gravity_adjusted_lateral_accel = 0.0
+ self._steer_limited_by_safety = False
+ self._output_torque = 0.0
# twilsonco's Lateral Neural Network Feedforward
# Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
index e3dee73912..e20e80f99c 100644
--- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
@@ -7,22 +7,28 @@ See the LICENSE.md file in the root directory for more details.
from cereal import messaging, custom
from opendbc.car import structs
-from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
from openpilot.sunnypilot.models.helpers import get_active_bundle
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+Source = custom.LongitudinalPlanSP.LongitudinalPlanSource
class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, mpc):
self.events_sp = EventsSP()
+ self.resolver = SpeedLimitResolver()
+
self.dec = DynamicExperimentalController(CP, mpc)
- self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+ self.scc = SmartCruiseControl()
self.slc = SpeedLimitController(CP)
+ self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+ self.source = Source.cruise
@property
def mlsim(self) -> bool:
@@ -35,16 +41,26 @@ class LongitudinalPlannerSP:
return self.dec.mode()
- def update_v_cruise(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> float:
+ def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
self.events_sp.clear()
- self.slc.update(sm, v_ego, a_ego, v_cruise, self.events_sp)
+ self.scc.update(sm, v_ego, a_ego, v_cruise)
- v_cruise_slc = self.slc.speed_limit_offseted if self.slc.is_active else V_CRUISE_UNSET
+ # Speed Limit Control
+ self.resolver.update(v_ego, sm)
+ v_cruise_slc = self.slc.update(sm['carControl'].longActive, v_ego, a_ego, sm['carState'].vCruiseCluster,
+ self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp)
- v_cruise_final = min(v_cruise, v_cruise_slc)
+ targets = {
+ Source.cruise: (v_cruise, a_ego),
+ Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target),
+ Source.sla: (v_cruise_slc, a_ego),
+ }
- return v_cruise_final
+ self.source = min(targets, key=lambda k: targets[k][0])
+ v_target, a_target = targets[self.source]
+
+ return v_target, a_target
def update(self, sm: messaging.SubMaster) -> None:
self.dec.update(sm)
@@ -55,6 +71,7 @@ class LongitudinalPlannerSP:
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+ longitudinalPlanSP.longitudinalPlanSource = self.source
longitudinalPlanSP.events = self.events_sp.to_msg()
# Dynamic Experimental Control
@@ -63,13 +80,26 @@ class LongitudinalPlannerSP:
dec.enabled = self.dec.enabled()
dec.active = self.dec.active()
+ # Smart Cruise Control
+ smartCruiseControl = longitudinalPlanSP.smartCruiseControl
+ # Vision Turn Speed Control
+ sccVision = smartCruiseControl.vision
+ sccVision.state = self.scc.vision.state
+ sccVision.vTarget = float(self.scc.vision.output_v_target)
+ sccVision.aTarget = float(self.scc.vision.output_a_target)
+ sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc)
+ sccVision.maxPredictedLateralAccel = float(self.scc.vision.max_pred_lat_acc)
+ sccVision.enabled = self.scc.vision.is_enabled
+ sccVision.active = self.scc.vision.is_active
+
# Speed Limit Control
slc = longitudinalPlanSP.slc
slc.state = self.slc.state
slc.enabled = self.slc.is_enabled
slc.active = self.slc.is_active
- slc.speedLimit = float(self.slc.speed_limit)
+ slc.speedLimit = float(self.resolver.speed_limit)
slc.speedLimitOffset = float(self.slc.speed_limit_offset)
- slc.distToSpeedLimit = float(self.slc.distance)
+ slc.distToSpeedLimit = float(self.resolver.distance)
+ slc.source = self.resolver.source
pm.send('longitudinalPlanSP', plan_sp_send)
diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py
index 77d46ace34..1738a11e49 100644
--- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py
+++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py
@@ -9,6 +9,8 @@ import math
import numpy as np
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from opendbc.sunnypilot.car.interfaces import LatControlInputs
+from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -30,8 +32,8 @@ def roll_pitch_adjust(roll, pitch):
class NeuralNetworkLateralControl(LatControlTorqueExtBase):
- def __init__(self, lac_torque, CP, CP_SP):
- super().__init__(lac_torque, CP, CP_SP)
+ def __init__(self, lac_torque, CP, CP_SP, CI):
+ super().__init__(lac_torque, CP, CP_SP, CI)
self.params = Params()
self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
@@ -57,14 +59,44 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
self.error_deque = deque(maxlen=history_check_frames[0])
self.past_future_len = len(self.past_times) + len(self.nn_future_times)
+ @property
+ def _nnlc_enabled(self):
+ return self.enabled and self.model_valid and self.has_nn_model
+
+ def update_limits(self):
+ if not self._nnlc_enabled:
+ return
+
+ self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
def update_lateral_lag(self, lag):
super().update_lateral_lag(lag)
self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
+ def update_feedforward_torque_space(self, CS):
+ torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
+ self.torque_params, gravity_adjusted=False)
+ torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
+ self.torque_params, gravity_adjusted=False)
+ self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+ self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
+ CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
+ self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
+ FRICTION_THRESHOLD, self.torque_params)
+
+ def update_output_torque(self, CS):
+ freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+ self._output_torque = self._pid.update(self._pid_log.error,
+ feedforward=self._ff,
+ speed=CS.vEgo,
+ freeze_integrator=freeze_integrator)
+
def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
- if not self.enabled or not self.model_valid or not self.has_nn_model:
+ if not self._nnlc_enabled:
return
+ self.update_feedforward_torque_space(CS)
+
low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
@@ -128,3 +160,5 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase):
# apply friction override for cars with low NN friction response
if self.model.friction_override:
self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+ self.update_output_torque(CS)
diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py
index 01ddec68ab..009e3d96af 100644
--- a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py
+++ b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py
@@ -3,6 +3,7 @@ from parameterized import parameterized
from cereal import car, log, messaging
from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
@@ -41,7 +42,7 @@ def generate_modelV2():
class TestNeuralNetworkLateralControl:
- @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
+ @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
def test_saturation(self, car_name):
params = Params()
params.put_bool("NeuralNetworkLateralControl", True)
@@ -57,6 +58,7 @@ class TestNeuralNetworkLateralControl:
VM = VehicleModel(CP)
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI)
+ torque_params = CP.lateralTuning.torque
CS = car.CarState.new_message()
CS.vEgo = 30
@@ -77,17 +79,23 @@ class TestNeuralNetworkLateralControl:
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
+ controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+ controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True)
assert lac_log.saturated
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
+ controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+ controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False)
assert not lac_log.saturated
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
+ controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+ controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False)
assert lac_log.saturated
diff --git a/sunnypilot/selfdrive/controls/lib/param_store.py b/sunnypilot/selfdrive/controls/lib/param_store.py
index 2ef3473187..65a0175340 100644
--- a/sunnypilot/selfdrive/controls/lib/param_store.py
+++ b/sunnypilot/selfdrive/controls/lib/param_store.py
@@ -4,39 +4,41 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
-import capnp
-
from cereal import custom
-
from opendbc.car import structs
from openpilot.common.params import Params
+from sunnypilot.sunnylink.utils import get_param_as_byte
+
class ParamStore:
keys: list[str]
- values: dict[str, str]
+ _params: dict[str, custom.CarControlSP.Param]
def __init__(self, CP: structs.CarParams):
universal_params: list[str] = []
brand_params: list[str] = []
self.keys = universal_params + brand_params
- self.values = {}
- self.cached_params_list: list[capnp.lib.capnp._DynamicStructBuilder] | None = None
+ self._params = {}
self.frame = 0
def update(self, params: Params) -> None:
- if self.frame % 300 == 0:
- old_values = dict(self.values)
- self.values = {k: params.get(k) or "0" for k in self.keys}
- if old_values != self.values:
- self.cached_params_list = None
-
self.frame += 1
+ if self.frame % 300 != 0:
+ return
- def publish(self) -> list[capnp.lib.capnp._DynamicStructBuilder]:
- if self.cached_params_list is None:
- # TODO-SP: Why are we doing a list instead of a dictionary here?
- self.cached_params_list = [custom.CarControlSP.Param(key=k, value=self.values[k]) for k in self.keys]
- return self.cached_params_list
+ for key in self.keys:
+ param_type = params.get_type(key).name.lower() # Using string instead of number because its "loose" dependency, and could change by OP at anytime.
+
+ # Over engineering opportunity: It's possible this conversion is slow, we may check the value as params returns it for cache purposes. Not today.
+ param_value = get_param_as_byte(key, params)
+ if (existing_param := self._params.get(key)) is not None and existing_param.value == param_value:
+ continue
+
+ self._params[key] = custom.CarControlSP.Param(key=key, value=param_value, type=param_type)
+
+ @property
+ def param_list(self) -> list[custom.CarControlSP.Param]:
+ return [v for k,v in self._params.items()]
diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py
new file mode 100644
index 0000000000..c66c2c392a
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.py
@@ -0,0 +1,19 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import cereal.messaging as messaging
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+
+
+class SmartCruiseControl:
+ def __init__(self):
+ self.vision = SmartCruiseControlVision()
+
+ def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> None:
+ long_enabled = sm['carControl'].enabled
+ long_override = sm['carControl'].cruiseControl.override
+
+ self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py
new file mode 100644
index 0000000000..9f6efffb55
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py
@@ -0,0 +1,104 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom, log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+
+def generate_modelV2():
+ model = messaging.new_message('modelV2')
+ position = log.XYZTData.new_message()
+ speed = 30
+ position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+ model.modelV2.position = position
+ orientation = log.XYZTData.new_message()
+ curvature = 0.05
+ orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+ orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+ model.modelV2.orientation = orientation
+ orientationRate = log.XYZTData.new_message()
+ orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
+ model.modelV2.orientationRate = orientationRate
+ velocity = log.XYZTData.new_message()
+ velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+ velocity.x[0] = float(speed) # always start at current speed
+ model.modelV2.velocity = velocity
+ acceleration = log.XYZTData.new_message()
+ acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+ acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+ model.modelV2.acceleration = acceleration
+
+ return model
+
+
+def generate_carState():
+ car_state = messaging.new_message('carState')
+ speed = 30
+ v_cruise = 50
+ car_state.carState.vEgo = float(speed)
+ car_state.carState.standstill = False
+ car_state.carState.vCruise = float(v_cruise * 3.6)
+
+ return car_state
+
+
+def generate_controlsState():
+ controls_state = messaging.new_message('controlsState')
+ controls_state.controlsState.curvature = 0.05
+
+ return controls_state
+
+
+class TestSmartCruiseControlVision:
+
+ def setup_method(self):
+ self.params = Params()
+ self.reset_params()
+ self.scc_v = SmartCruiseControlVision()
+
+ mdl = generate_modelV2()
+ cs = generate_carState()
+ controls_state = generate_controlsState()
+ self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
+
+ def reset_params(self):
+ self.params.put_bool("SmartCruiseControlVision", True)
+
+ def test_initial_state(self):
+ assert self.scc_v.state == VisionState.disabled
+ assert not self.scc_v.is_active
+ assert self.scc_v.output_v_target == V_CRUISE_UNSET
+ assert self.scc_v.output_a_target == 0.
+
+ def test_system_disabled(self):
+ self.params.put_bool("SmartCruiseControlVision", False)
+ self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+ for _ in range(int(10. / DT_MDL)):
+ self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+ assert self.scc_v.state == VisionState.disabled
+ assert not self.scc_v.is_active
+
+ def test_disabled(self):
+ for _ in range(int(10. / DT_MDL)):
+ self.scc_v.update(self.sm, False, False, 0., 0., 0.)
+ assert self.scc_v.state == VisionState.disabled
+
+ def test_transition_disabled_to_enabled(self):
+ for _ in range(int(10. / DT_MDL)):
+ self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+ assert self.scc_v.state == VisionState.enabled
+
+ # TODO-SP: mock modelV2 data to test other states
diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py
new file mode 100644
index 0000000000..f12a00f23e
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py
@@ -0,0 +1,205 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
+ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
+
+_MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h
+
+_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state.
+_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops.
+
+_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state.
+
+_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state.
+_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle.
+
+_A_LAT_REG_MAX = 2. # Maximum lateral acceleration
+
+_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting.
+
+# Lookup table for the minimum smooth deceleration during the ENTERING state
+# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
+_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state
+_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead
+
+# Lookup table for the acceleration for the TURNING state
+# depending on the current lateral acceleration of the vehicle.
+_TURNING_ACC_V = [0.5, 0., -0.4] # acc value
+_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc
+
+_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn.
+
+
+class SmartCruiseControlVision:
+ v_target: float = 0
+ a_target: float = 0.
+ v_ego: float = 0.
+ a_ego: float = 0.
+ output_v_target: float = V_CRUISE_UNSET
+ output_a_target: float = 0.
+
+ def __init__(self):
+ self.params = Params()
+ self.frame = -1
+ self.long_enabled = False
+ self.long_override = False
+ self.is_enabled = False
+ self.is_active = False
+ self.enabled = self.params.get_bool("SmartCruiseControlVision")
+ self.v_cruise_setpoint = 0.
+
+ self.state = VisionState.disabled
+ self.current_lat_acc = 0.
+ self.max_pred_lat_acc = 0.
+
+ def get_a_target_from_control(self) -> float:
+ return self.a_target
+
+ def get_v_target_from_control(self) -> float:
+ if self.is_active:
+ return max(self.v_target, _MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+
+ return V_CRUISE_UNSET
+
+ def _update_params(self) -> None:
+ if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+ self.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+ def _update_calculations(self, sm: messaging.SubMaster) -> None:
+ if not self.long_enabled:
+ return
+ else:
+ rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+ vel_plan = np.array(sm['modelV2'].velocity.x)
+
+ self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+
+ # get the maximum lat accel from the model
+ predicted_lat_accels = rate_plan * vel_plan
+ self.max_pred_lat_acc = np.amax(predicted_lat_accels)
+
+ # get the maximum curve based on the current velocity
+ v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations
+ max_curve = self.max_pred_lat_acc / (v_ego**2)
+
+ # Get the target velocity for the maximum curve
+ self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+
+ def _update_state_machine(self) -> tuple[bool, bool]:
+ # ENABLED, ENTERING, TURNING, LEAVING
+ if self.state != VisionState.disabled:
+ # longitudinal and feature disable always have priority in a non-disabled state
+ if not self.long_enabled or not self.enabled:
+ self.state = VisionState.disabled
+ elif self.long_override:
+ self.state = VisionState.overriding
+
+ else:
+ # ENABLED
+ if self.state == VisionState.enabled:
+ # Do not enter a turn control cycle if the speed is low.
+ if self.v_ego <= _MIN_V:
+ pass
+ # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
+ elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+ self.state = VisionState.entering
+
+ # OVERRIDING
+ elif self.state == VisionState.overriding:
+ if not self.long_override:
+ self.state = VisionState.enabled
+
+ # ENTERING
+ elif self.state == VisionState.entering:
+ # Transition to Turning if current lateral acceleration is over the threshold.
+ if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+ self.state = VisionState.turning
+ # Abort if the predicted lateral acceleration drops
+ elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
+ self.state = VisionState.enabled
+
+ # TURNING
+ elif self.state == VisionState.turning:
+ # Transition to Leaving if current lateral acceleration drops below a threshold.
+ if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
+ self.state = VisionState.leaving
+
+ # LEAVING
+ elif self.state == VisionState.leaving:
+ # Transition back to Turning if current lateral acceleration goes back over the threshold.
+ if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+ self.state = VisionState.turning
+ # Finish if current lateral acceleration goes below a threshold.
+ elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+ self.state = VisionState.enabled
+
+ # DISABLED
+ elif self.state == VisionState.disabled:
+ if self.long_enabled and self.enabled:
+ if self.long_override:
+ self.state = VisionState.overriding
+ else:
+ self.state = VisionState.enabled
+
+ enabled = self.state in ENABLED_STATES
+ active = self.state in ACTIVE_STATES
+
+ return enabled, active
+
+ def _update_solution(self) -> float:
+ # DISABLED, ENABLED
+ if self.state not in ACTIVE_STATES:
+ # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
+ # the smooth deceleration.
+ a_target = self.a_ego
+ # ENTERING
+ elif self.state == VisionState.entering:
+ # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
+ a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
+ # TURNING
+ elif self.state == VisionState.turning:
+ # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
+ a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
+ # LEAVING
+ elif self.state == VisionState.leaving:
+ # When leaving, we provide a comfortable acceleration to regain speed.
+ a_target = _LEAVING_ACC
+ else:
+ raise NotImplementedError(f"SCC-V state not supported: {self.state}")
+
+ return a_target
+
+ def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
+ v_cruise_setpoint: float) -> None:
+ self.long_enabled = long_enabled
+ self.long_override = long_override
+ self.v_ego = v_ego
+ self.a_ego = a_ego
+ self.v_cruise_setpoint = v_cruise_setpoint
+
+ self._update_params()
+ self._update_calculations(sm)
+
+ self.is_enabled, self.is_active = self._update_state_machine()
+ self.a_target = self._update_solution()
+
+ self.output_v_target = self.get_v_target_from_control()
+ self.output_a_target = self.get_a_target_from_control()
+
+ self.frame += 1
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py
similarity index 54%
rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py
rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py
index b0a6675491..2ac5b6a6f0 100644
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py
@@ -1,12 +1,10 @@
from cereal import custom
-DEBUG = True
-PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates.
-TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal.
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimitAssistState
-# Lookup table for speed limit percent offset depending on speed.
-LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h
-LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h
+PARAMS_UPDATE_PERIOD = 3. # secs. Time between parameter updates.
+DISABLED_GUARD_PERIOD = 2 # secs.
+PRE_ACTIVE_GUARD_PERIOD = 5 # secs. Time to wait after activation before considering temp deactivation signal.
# Constants for Limit controllers.
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
@@ -16,4 +14,7 @@ LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on lim
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
-SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
+# Speed Limit Assist Auto mode constants
+REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params
+CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial
+FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py
similarity index 64%
rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py
rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py
index 4a460b3335..15e8ab8ad2 100644
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py
@@ -1,12 +1,6 @@
from enum import IntEnum
-class Source(IntEnum):
- none = 0
- car_state = 1
- map_data = 2
-
-
class Policy(IntEnum):
map_data_only = 0
car_state_only = 1
@@ -15,11 +9,7 @@ class Policy(IntEnum):
combined = 4
-class Engage(IntEnum):
- auto = 0
-
-
class OffsetType(IntEnum):
- default = 0
+ off = 0
fixed = 1
percentage = 2
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py
new file mode 100644
index 0000000000..de9ebecaac
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py
@@ -0,0 +1,255 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from cereal import custom
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \
+ SpeedLimitAssistState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+EventNameSP = custom.OnroadEventSP.EventName
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
+
+
+class SpeedLimitAssist:
+ _speed_limit: float
+ _distance: float
+ _source: custom.LongitudinalPlanSP.SpeedLimitSource
+ v_ego: float
+ a_ego: float
+ v_offset: float
+ last_valid_speed_limit_final: float
+
+ def __init__(self, CP):
+ self.params = Params()
+ self.CP = CP
+ self.frame = -1
+ self.long_engaged_timer = 0
+ self.pre_active_timer = 0
+ self.is_metric = self.params.get_bool("IsMetric")
+ self.enabled = self.params.get_bool("SpeedLimitAssist")
+ self.op_engaged = False
+ self.op_engaged_prev = False
+ self.is_enabled = False
+ self.is_active = False
+ self.v_ego = 0.
+ self.a_ego = 0.
+ self.v_offset = 0.
+ self.v_cruise_setpoint = 0.
+ self.v_cruise_setpoint_prev = 0.
+ self.initial_max_set = False
+ self._speed_limit = 0.
+ self.speed_limit_prev = 0.
+ self.last_valid_speed_limit_final = 0.
+ self._distance = 0.
+ self._source = SpeedLimitSource.none
+ self.state = SpeedLimitAssistState.disabled
+ self._state_prev = SpeedLimitAssistState.disabled
+ self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
+
+ self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+ self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+
+ # Solution functions mapped to respective states
+ self.acceleration_solutions = {
+ SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
+ SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
+ SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
+ SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
+ SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
+ SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
+ }
+
+ @property
+ def speed_limit_final(self) -> float:
+ return self._speed_limit + self.speed_limit_offset
+
+ @property
+ def speed_limit_changed(self) -> bool:
+ return bool(self._speed_limit != self.speed_limit_prev)
+
+ @property
+ def speed_limit_offset(self) -> float:
+ return self.get_offset(self.offset_type, self.offset_value)
+
+ @property
+ def v_cruise_setpoint_changed(self) -> bool:
+ return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev)
+
+ def get_v_target_from_control(self) -> float:
+ if self.is_enabled:
+ # If we have a current valid speed limit, use it
+ if self._speed_limit > 0:
+ self.last_valid_speed_limit_final = self.speed_limit_final
+ return self.speed_limit_final
+
+ # If no current speed limit but we have a last valid one, use that
+ if self.last_valid_speed_limit_final > 0:
+ return self.last_valid_speed_limit_final
+
+ # Fallback
+ return V_CRUISE_UNSET
+
+ def get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
+ if offset_type == OffsetType.off:
+ return 0
+ elif offset_type == OffsetType.fixed:
+ return offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
+ elif offset_type == OffsetType.percentage:
+ return offset_value * 0.01 * self._speed_limit
+ else:
+ raise NotImplementedError("Offset not supported")
+
+ def update_params(self) -> None:
+ if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+ self.enabled = self.params.get_bool("SpeedLimitAssist")
+ self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True))
+ self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
+ self.is_metric = self.params.get_bool("IsMetric")
+
+ def initial_max_set_confirmed(self) -> bool:
+ return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE)
+
+ def detect_manual_cruise_change(self) -> bool:
+ # If cruise speed changed and it's not what SLC would set
+ if self.v_cruise_setpoint_changed:
+ expected_cruise = self.speed_limit_final
+ return bool(abs(self.v_cruise_setpoint - expected_cruise) > CRUISE_SPEED_TOLERANCE)
+
+ return False
+
+ def update_calculations(self, v_cruise_setpoint: float) -> None:
+ self.v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
+
+ # Update current velocity offset (error)
+ self.v_offset = self.speed_limit_final - self.v_ego
+
+ def get_current_acceleration_as_target(self) -> float:
+ return self.a_ego
+
+ def get_adapting_state_target_acceleration(self) -> float:
+ if self._distance > 0:
+ return (self.speed_limit_final ** 2 - self.v_ego ** 2) / (2. * self._distance)
+
+ return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+ def get_active_state_target_acceleration(self) -> float:
+ return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
+
+ def update_state_machine(self):
+ self._state_prev = self.state
+
+ self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
+ self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+ # ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
+ if self.state != SpeedLimitAssistState.disabled:
+ if not self.op_engaged or not self.enabled:
+ self.state = SpeedLimitAssistState.disabled
+ self.initial_max_set = False
+
+ else:
+ # ACTIVE
+ if self.state == SpeedLimitAssistState.active:
+ if self.detect_manual_cruise_change():
+ self.state = SpeedLimitAssistState.inactive
+ elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
+ self.state = SpeedLimitAssistState.adapting
+
+ # ADAPTING
+ elif self.state == SpeedLimitAssistState.adapting:
+ if self.detect_manual_cruise_change():
+ self.state = SpeedLimitAssistState.inactive
+ elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
+ self.state = SpeedLimitAssistState.active
+
+ # PENDING
+ elif self.state == SpeedLimitAssistState.pending:
+ if self._speed_limit > 0:
+ if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+ self.state = SpeedLimitAssistState.adapting
+ else:
+ self.state = SpeedLimitAssistState.active
+
+ # PRE_ACTIVE
+ elif self.state == SpeedLimitAssistState.preActive:
+ if self.initial_max_set_confirmed():
+ self.initial_max_set = True
+ if self._speed_limit > 0:
+ if self.v_offset < LIMIT_SPEED_OFFSET_TH:
+ self.state = SpeedLimitAssistState.adapting
+ else:
+ self.state = SpeedLimitAssistState.active
+ else:
+ self.state = SpeedLimitAssistState.pending
+ elif self.pre_active_timer <= PRE_ACTIVE_GUARD_PERIOD:
+ # Timeout - session ended
+ self.state = SpeedLimitAssistState.inactive
+
+ # INACTIVE
+ elif self.state == SpeedLimitAssistState.inactive:
+ pass
+
+ # DISABLED
+ elif self.state == SpeedLimitAssistState.disabled:
+ if self.op_engaged and self.enabled:
+ if not self.op_engaged_prev:
+ self.pre_active_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
+
+ elif self.pre_active_timer <= 0:
+ self.state = SpeedLimitAssistState.preActive
+ self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+ self.initial_max_set = False
+
+ enabled = self.state in ENABLED_STATES
+ active = self.state in ACTIVE_STATES
+
+ return enabled, active
+
+ def update_events(self, events_sp: EventsSP) -> None:
+ if self.state == SpeedLimitAssistState.preActive and self._state_prev != SpeedLimitAssistState.preActive:
+ events_sp.add(EventNameSP.speedLimitPreActive)
+ elif self.is_active:
+ if self._state_prev not in ACTIVE_STATES:
+ events_sp.add(EventNameSP.speedLimitActive)
+ elif self.speed_limit_changed:
+ events_sp.add(EventNameSP.speedLimitChanged)
+
+ def update(self, long_active: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float,
+ speed_limit: float, distance: float, source: custom.LongitudinalPlanSP.SpeedLimitSource, events_sp: EventsSP) -> float:
+ self.op_engaged = long_active
+ self.v_ego = v_ego
+ self.a_ego = a_ego
+
+ self._speed_limit = speed_limit
+ self._distance = distance
+ self._source = source
+
+ self.update_params()
+ self.update_calculations(v_cruise_setpoint)
+ self.is_enabled, self.is_active = self.update_state_machine()
+ self.update_events(events_sp)
+
+ # Update change tracking variables
+ self.speed_limit_prev = self._speed_limit
+ self.v_cruise_setpoint_prev = self.v_cruise_setpoint
+ self.op_engaged_prev = self.op_engaged
+ self.frame += 1
+
+ v_target = self.get_v_target_from_control()
+
+ return v_target
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py
new file mode 100644
index 0000000000..ddc3252992
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py
@@ -0,0 +1,132 @@
+import time
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
+
+
+class SpeedLimitResolver:
+ _limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+ _distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float]
+ _v_ego: float
+ speed_limit: float
+ distance: float
+ source: custom.LongitudinalPlanSP.SpeedLimitSource
+
+ def __init__(self):
+ self.params = Params()
+ self.frame = -1
+
+ self._gps_location_service = get_gps_location_service(self.params)
+ self._limit_solutions = {} # Store for speed limit solutions from different sources
+ self._distance_solutions = {} # Store for distance to current speed limit start for different sources
+
+ self.policy = self.params.get("SpeedLimitAssistPolicy", return_default=True)
+ self._policy_to_sources_map = {
+ Policy.car_state_only: [SpeedLimitSource.car],
+ Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
+ Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
+ Policy.map_data_only: [SpeedLimitSource.map],
+ Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
+ }
+ for source in ALL_SOURCES:
+ self._reset_limit_sources(source)
+
+ def update_params(self):
+ if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+ self.policy = Policy(self.params.get("SpeedLimitAssistPolicy", return_default=True))
+ self.change_policy(self.policy)
+
+ def change_policy(self, policy: Policy) -> None:
+ self.policy = policy
+
+ def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimitSource) -> None:
+ self._limit_solutions[source] = 0.
+ self._distance_solutions[source] = 0.
+
+ def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None:
+ """Get limit solutions from each data source"""
+ self._get_from_car_state(sm)
+ self._get_from_map_data(sm)
+
+ def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
+ self._reset_limit_sources(SpeedLimitSource.car)
+ self._limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
+ self._distance_solutions[SpeedLimitSource.car] = 0.
+
+ def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
+ self._reset_limit_sources(SpeedLimitSource.map)
+ self._process_map_data(sm)
+
+ def _process_map_data(self, sm: messaging.SubMaster) -> None:
+ gps_data = sm[self._gps_location_service]
+ map_data = sm['liveMapDataSP']
+
+ gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
+ if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
+ return
+
+ speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
+ next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
+
+ self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
+
+ def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
+ gps_data = sm[self._gps_location_service]
+ map_data = sm['liveMapDataSP']
+
+ distance_since_fix = self._v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
+ distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
+
+ self._limit_solutions[SpeedLimitSource.map] = speed_limit
+ self._distance_solutions[SpeedLimitSource.map] = 0.
+
+ if 0. < next_speed_limit < self._v_ego:
+ adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
+ adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
+
+ if distance_to_speed_limit_ahead <= adapt_distance:
+ self._limit_solutions[SpeedLimitSource.map] = next_speed_limit
+ self._distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
+
+ def _consolidate(self) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimitSource]:
+ source = self._get_source_solution_according_to_policy()
+ speed_limit = self._limit_solutions[source] if source else 0.
+ distance = self._distance_solutions[source] if source else 0.
+
+ return speed_limit, distance, source
+
+ def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimitSource:
+ sources_for_policy = self._policy_to_sources_map[self.policy]
+
+ if self.policy != Policy.combined:
+ # They are ordered in the order of preference, so we pick the first that's non zero
+ for source in sources_for_policy:
+ return source if self._limit_solutions[source] > 0. else SpeedLimitSource.none
+
+ limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
+ sources = np.array(sources_for_policy, dtype=int)
+
+ if len(limits) > 0:
+ min_idx = np.argmin(limits)
+ return sources[min_idx]
+
+ return SpeedLimitSource.none
+
+ def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
+ self._v_ego = v_ego
+ self.update_params()
+ self._resolve_limit_sources(sm)
+
+ self.speed_limit, self.distance, self.source = self._consolidate()
+
+ self.frame += 1
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py
new file mode 100644
index 0000000000..fa17494d2e
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py
@@ -0,0 +1,247 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pytest
+
+from cereal import custom
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import SpeedLimitAssistState, REQUIRED_INITIAL_MAX_SET_SPEED, \
+ PRE_ACTIVE_GUARD_PERIOD
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist, ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
+
+ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
+
+SPEED_LIMITS = {
+ 'residential': 25 * CV.MPH_TO_MS, # 25 mph
+ 'city': 35 * CV.MPH_TO_MS, # 35 mph
+ 'highway': 65 * CV.MPH_TO_MS, # 65 mph
+ 'freeway': 80 * CV.MPH_TO_MS, # 80 mph
+}
+
+
+class TestSpeedLimitAssist:
+
+ def setup_method(self):
+ self.params = Params()
+ self.reset_custom_params()
+ self.events_sp = EventsSP()
+ CI = self._setup_platform(TOYOTA.TOYOTA_RAV4_TSS2_2022)
+ self.sla = SpeedLimitAssist(CI.CP)
+ self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)
+
+ def teardown_method(self, method):
+ self.reset_state()
+
+ def _setup_platform(self, car_name):
+ CarInterface = interfaces[car_name]
+ CP = CarInterface.get_non_essential_params(car_name)
+ CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+ CI = CarInterface(CP, CP_SP)
+ sunnypilot_interfaces.setup_interfaces(CI, self.params)
+ return CI
+
+ def reset_custom_params(self):
+ self.params.put_bool("SpeedLimitAssist", True)
+ self.params.put_bool("IsMetric", False)
+ self.params.put("SpeedLimitOffsetType", 0)
+ self.params.put("SpeedLimitValueOffset", 0)
+
+ def reset_state(self):
+ self.sla.state = SpeedLimitAssistState.disabled
+ self.sla.frame = -1
+ self.sla.last_op_engaged_frame = 0
+ self.sla.op_engaged = False
+ self.sla.op_engaged_prev = False
+ self.sla.initial_max_set = False
+ self.sla._speed_limit = 0.
+ self.sla.speed_limit_prev = 0.
+ self.sla.last_valid_speed_limit_offsetted = 0.
+ self.sla._distance = 0.
+ self.sla._source = SpeedLimitSource.none
+ self.sla.v_cruise_setpoint = 0.
+ self.sla.v_cruise_setpoint_prev = 0.
+ self.events_sp.clear()
+
+ def initialize_active_state(self, v_cruise_setpoint):
+ self.sla.state = SpeedLimitAssistState.active
+ self.sla.v_cruise_setpoint = v_cruise_setpoint
+ self.sla.v_cruise_setpoint_prev = v_cruise_setpoint
+
+ def test_initial_state(self):
+ assert self.sla.state == SpeedLimitAssistState.disabled
+ assert not self.sla.is_enabled
+ assert not self.sla.is_active
+ assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
+
+ def test_disabled(self):
+ self.params.put_bool("SpeedLimitAssist", False)
+ for _ in range(int(10. / DT_MDL)):
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.disabled
+
+ def test_transition_disabled_to_preactive(self):
+ for _ in range(int(3. / DT_MDL)):
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.preActive
+ assert self.sla.is_enabled and not self.sla.is_active
+
+ def test_preactive_to_active_with_max_speed_confirmation(self):
+ self.sla.state = SpeedLimitAssistState.preActive
+ v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.active
+ assert self.sla.is_enabled and self.sla.is_active
+ assert v_cruise_sla == SPEED_LIMITS['city']
+
+ def test_preactive_timeout_to_inactive(self):
+ self.sla.state = SpeedLimitAssistState.preActive
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+ for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.inactive
+
+ def test_preactive_to_pending_no_speed_limit(self):
+ self.sla.state = SpeedLimitAssistState.preActive
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.pending
+ assert self.sla.is_enabled and not self.sla.is_active
+
+ def test_pending_to_active_when_speed_limit_available(self):
+ self.sla.state = SpeedLimitAssistState.pending
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.active
+
+ def test_pending_to_adapting_when_below_speed_limit(self):
+ self.sla.state = SpeedLimitAssistState.pending
+ _ = self.sla.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.adapting
+ assert self.sla.is_enabled and self.sla.is_active
+
+ def test_active_to_adapting_transition(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+ _ = self.sla.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.adapting
+
+ def test_adapting_to_active_transition(self):
+ self.sla.state = SpeedLimitAssistState.adapting
+ self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.active
+
+ def test_manual_cruise_change_detection(self):
+ self.sla.state = SpeedLimitAssistState.active
+ expected_cruise = SPEED_LIMITS['highway']
+ self.sla.v_cruise_setpoint_prev = expected_cruise
+
+ different_cruise = SPEED_LIMITS['highway'] + 5
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.inactive
+
+ @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [
+ (OffsetType.fixed, 5, SPEED_LIMITS['city'], 5 * CV.MPH_TO_MS), # 5 MPH fixed offset
+ (OffsetType.percentage, 10, SPEED_LIMITS['city'], 0.1 * SPEED_LIMITS['city']), # 10% offset
+ (OffsetType.off, 0, SPEED_LIMITS['city'], 0), # Off
+ (OffsetType.fixed, 10, SPEED_LIMITS['highway'], 10 * CV.MPH_TO_MS), # Different speed, fixed offset
+ (OffsetType.percentage, 5, SPEED_LIMITS['highway'], 0.05 * SPEED_LIMITS['highway']), # Different speed, percentage
+ ])
+ def test_offset_calculations(self, offset_type, offset_value, speed_limit, expected_offset):
+ self.sla._speed_limit = speed_limit
+ actual_offset = self.sla.get_offset(offset_type, offset_value)
+ assert actual_offset == pytest.approx(expected_offset, rel=0.01)
+
+ def test_rapid_speed_limit_changes(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+ speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']]
+
+ for _, speed_limit in enumerate(speed_limits):
+ _ = self.sla.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state in ACTIVE_STATES
+
+ def test_invalid_speed_limits_handling(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+ self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city']
+
+ invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
+
+ for invalid_limit in invalid_limits:
+ v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp)
+ assert isinstance(v_cruise_sla, (int, float))
+ assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0
+
+ def test_stale_data_handling(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+ old_speed_limit = SPEED_LIMITS['city']
+ self.sla.last_valid_speed_limit_final = old_speed_limit
+
+ v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state in ACTIVE_STATES
+ assert v_cruise_sla == old_speed_limit
+
+ def test_different_speed_limit_sources(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+ for source in (SpeedLimitSource.car, SpeedLimitSource.map):
+ v_cruise_sla = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp)
+ assert v_cruise_sla != V_CRUISE_UNSET
+
+ def test_distance_based_adapting(self):
+ self.sla.state = SpeedLimitAssistState.adapting
+ self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED
+
+ distance = 100.0
+ current_speed = SPEED_LIMITS['highway']
+ target_speed = SPEED_LIMITS['city']
+
+ v_cruise_sla = self.sla.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.adapting
+ assert v_cruise_sla == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
+
+ def test_long_disengaged_to_disabled(self):
+ self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED)
+
+ v_cruise_sla = self.sla.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'],
+ 0, SpeedLimitSource.car, self.events_sp)
+ assert self.sla.state == SpeedLimitAssistState.disabled
+ assert v_cruise_sla == V_CRUISE_UNSET
+
+ def test_maintain_states_with_no_changes(self):
+ """Test that states are maintained when no significant changes occur"""
+ test_states = [
+ SpeedLimitAssistState.preActive,
+ SpeedLimitAssistState.pending,
+ SpeedLimitAssistState.active,
+ SpeedLimitAssistState.adapting
+ ]
+
+ for state in test_states:
+ self.sla.state = state
+ self.sla.op_engaged = True
+ if state in [SpeedLimitAssistState.pending, SpeedLimitAssistState.active, SpeedLimitAssistState.adapting]:
+ self.sla.initial_max_set = True
+
+ initial_state = state
+
+ _ = self.sla.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp)
+
+ assert self.sla.state in ALL_STATES # Sanity check
+
+ if initial_state == SpeedLimitAssistState.preActive:
+ assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
+ elif initial_state in ACTIVE_STATES:
+ assert self.sla.state in ACTIVE_STATES
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py
similarity index 59%
rename from sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py
rename to sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py
index 62a9329958..14433dc293 100644
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py
+++ b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py
@@ -4,11 +4,13 @@ import time
import pytest
from pytest_mock import MockerFixture
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE
+from cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE
-# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
def create_mock(properties, mocker: MockerFixture):
@@ -52,79 +54,85 @@ def setup_sm_mock(mocker: MockerFixture):
parametrized_policies = pytest.mark.parametrize(
"policy, sm_key, function_key", [
- (Policy.car_state_only, 'carStateSP', 'car_state'),
- (Policy.car_state_priority, 'carStateSP', 'car_state'),
- (Policy.map_data_only, 'liveMapDataSP', 'map_data'),
- (Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
+ (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
+ (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
+ (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
+ (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
],
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
)
-@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"])
+@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
class TestSpeedLimitResolverValidation:
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_initial_state(self, resolver_class, policy):
- resolver = resolver_class(policy)
- for source in Source:
+ resolver = resolver_class()
+ resolver.policy = policy
+ for source in ALL_SOURCES:
if source in resolver._limit_solutions:
assert resolver._limit_solutions[source] == 0.
assert resolver._distance_solutions[source] == 0.
@parametrized_policies
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
- resolver = resolver_class(policy)
+ resolver = resolver_class()
+ resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
# Assert the resolver
- speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
- assert speed_limit == source_speed_limit
- assert source == Source[function_key]
+ resolver.update(source_speed_limit, sm_mock)
+ assert resolver.speed_limit == source_speed_limit
+ assert resolver.source == ALL_SOURCES[function_key]
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
- resolver = resolver_class(Policy.combined)
+ resolver = resolver_class()
+ resolver.policy = Policy.combined
sm_mock = setup_sm_mock(mocker)
- socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data}
+ socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
minimum_key, minimum_speed_limit = min(
((key, sm_mock[key].speedLimit) for key in
socket_to_source.keys()), key=lambda x: x[1])
# Assert the resolver
- speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock)
- assert speed_limit == minimum_speed_limit
- assert source == socket_to_source[minimum_key]
+ resolver.update(minimum_speed_limit, sm_mock)
+ assert resolver.speed_limit == minimum_speed_limit
+ assert resolver.source == socket_to_source[minimum_key]
@parametrized_policies
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
- resolver = resolver_class(policy)
+ resolver = resolver_class()
+ resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
# Assert the parsing
- speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
- assert resolver._limit_solutions[Source[function_key]] == source_speed_limit
- assert resolver._distance_solutions[Source[function_key]] == 0.
+ resolver.update(source_speed_limit, sm_mock)
+ assert resolver._limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
+ assert resolver._distance_solutions[ALL_SOURCES[function_key]] == 0.
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
v_ego = 50
- resolver = resolver_class(policy)
+ resolver = resolver_class()
+ resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
- _speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock)
+ resolver.update(v_ego, sm_mock)
# After resolution
- assert _speed_limit is not None
- assert _distance is not None
- assert _source is not None
+ assert resolver.speed_limit is not None
+ assert resolver.distance is not None
+ assert resolver.source is not None
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
- resolver = resolver_class(policy)
+ resolver = resolver_class()
+ resolver.policy = policy
sm_mock = mocker.MagicMock()
sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
resolver._get_from_map_data(sm_mock)
- assert resolver._limit_solutions[Source.map_data] == 0.
- assert resolver._distance_solutions[Source.map_data] == 0.
+ assert resolver._limit_solutions[SpeedLimitSource.map] == 0.
+ assert resolver._distance_solutions[SpeedLimitSource.map] == 0.
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py
deleted file mode 100644
index b15c63bf05..0000000000
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/helpers.py
+++ /dev/null
@@ -1,19 +0,0 @@
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import DEBUG, SpeedLimitControlState
-from openpilot.common.swaglog import cloudlog
-
-
-def debug(msg):
- if not DEBUG:
- return
- cloudlog.debug(msg)
-
-
-def description_for_state(speed_limit_control_state):
- if speed_limit_control_state == SpeedLimitControlState.inactive:
- return 'INACTIVE'
- if speed_limit_control_state == SpeedLimitControlState.tempInactive:
- return 'TEMP_INACTIVE'
- if speed_limit_control_state == SpeedLimitControlState.adapting:
- return 'ADAPTING'
- if speed_limit_control_state == SpeedLimitControlState.active:
- return 'ACTIVE'
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
deleted file mode 100644
index ab94e64ca2..0000000000
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
+++ /dev/null
@@ -1,284 +0,0 @@
-import numpy as np
-import time
-
-from cereal import messaging, custom
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
- PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-from openpilot.selfdrive.modeld.constants import ModelConstants
-
-EventNameSP = custom.OnroadEventSP.EventName
-
-ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting)
-ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempInactive, *ACTIVE_STATES)
-
-
-class SpeedLimitController:
- _speed_limit: float
- _distance: float
- _source: Source
- _v_ego: float
- _a_ego: float
- _v_offset: float
-
- def __init__(self, CP):
- self.params = Params()
- self._CP = CP
- self._policy = self.params.get("SpeedLimitControlPolicy", return_default=True)
- self._resolver = SpeedLimitResolver(self._policy)
- self._last_params_update = 0.0
- self._last_op_engaged_time = 0.0
- self._is_metric = self.params.get_bool("IsMetric")
- self._enabled = self.params.get_bool("SpeedLimitControl")
- self._op_engaged = False
- self._op_engaged_prev = False
- self._v_ego = 0.
- self._a_ego = 0.
- self._v_offset = 0.
- self._v_cruise_setpoint = 0.
- self._v_cruise_setpoint_prev = 0.
- self._v_cruise_setpoint_changed = False
- self._speed_limit = 0.
- self._speed_limit_prev = 0.
- self._speed_limit_changed = False
- self._distance = 0.
- self._source = Source.none
- self._state = SpeedLimitControlState.inactive
- self._state_prev = SpeedLimitControlState.inactive
- self._gas_pressed = False
- self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
-
- self._offset_type = OffsetType(self.params.get("SpeedLimitWarningValueOffset", return_default=True))
- self._offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
- self._warning_type = self.params.get("SpeedLimitWarningType", return_default=True)
- self._warning_offset_type = OffsetType(self.params.get("SpeedLimitWarningOffsetType", return_default=True))
- self._warning_offset_value = self.params.get("SpeedLimitWarningValueOffset", return_default=True)
- self._engage_type = self._read_engage_type_param()
- self._current_time = 0.
- self._v_cruise_rounded = 0.
- self._v_cruise_prev_rounded = 0.
- self._speed_limit_offsetted_rounded = 0.
- self._speed_limit_warning_offsetted_rounded = 0.
- self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
-
- # Mapping functions to state transitions
- self.state_transition_strategy = {
- # Transition functions for each state
- SpeedLimitControlState.inactive: self.transition_state_from_inactive,
- SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive,
- SpeedLimitControlState.adapting: self.transition_state_from_adapting,
- SpeedLimitControlState.active: self.transition_state_from_active,
- SpeedLimitControlState.preActive: self.transition_state_from_pre_active,
- }
-
- # FIXME-SP: unused?
- # Solution functions mapped to respective states
- self.acceleration_solutions = {
- # Solution functions for each state
- SpeedLimitControlState.tempInactive: self.get_current_acceleration_as_target,
- SpeedLimitControlState.inactive: self.get_current_acceleration_as_target,
- SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration,
- SpeedLimitControlState.active: self.get_active_state_target_acceleration,
- SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
- }
-
- @property
- def state(self) -> SpeedLimitControlState:
- return self._state
-
- @state.setter
- def state(self, value) -> None:
- if value != self._state:
- debug(f'Speed Limit Controller state: {description_for_state(value)}')
-
- if value == SpeedLimitControlState.tempInactive:
- # Reset previous speed limit to current value as to prevent going out of tempInactive in
- # a single cycle when the speed limit changes at the same time the user has temporarily deactivated it.
- self._speed_limit_prev = self._speed_limit
-
- self._state = value
-
- @property
- def is_enabled(self) -> bool:
- return self.state in ENABLED_STATES and self._enabled
-
- @property
- def is_active(self) -> bool:
- return self.state in ACTIVE_STATES and self._enabled
-
- @property
- def speed_limit_offseted(self) -> float:
- return self._speed_limit + self.speed_limit_offset
-
- @property
- def speed_limit_offset(self) -> float:
- return self._get_offset(self._offset_type, self._offset_value)
-
- @property
- def speed_limit_warning_offset(self) -> float:
- return self._get_offset(self._warning_offset_type, self._warning_offset_value)
-
- @property
- def speed_limit(self) -> float:
- return self._speed_limit
-
- @property
- def distance(self) -> float:
- return self._distance
-
- @property
- def source(self) -> Source:
- return self._source
-
- def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
- if offset_type == OffsetType.default:
- return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit)
- elif offset_type == OffsetType.fixed:
- return offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
- elif offset_type == OffsetType.percentage:
- return offset_value * 0.01 * self._speed_limit
- else:
- raise NotImplementedError("Offset not supported")
-
- def _update_v_cruise_setpoint_prev(self) -> None:
- self._v_cruise_setpoint_prev = self._v_cruise_setpoint
-
- def _update_params(self) -> None:
- if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD:
- self._enabled = self.params.get_bool("SpeedLimitControl")
- self._offset_type = OffsetType(self.params.get("SpeedLimitWarningValueOffset", return_default=True))
- self._offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
- self._warning_type = self.params.get("SpeedLimitWarningType", return_default=True)
- self._warning_offset_type = OffsetType(self.params.get("SpeedLimitWarningOffsetType", return_default=True))
- self._warning_offset_value = self.params.get("SpeedLimitWarningValueOffset", return_default=True)
- self._policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True))
- self._is_metric = self.params.get_bool("IsMetric")
- self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
- self._resolver.change_policy(self._policy)
- self._engage_type = self._read_engage_type_param()
-
- self._last_params_update = self._current_time
-
- def _read_engage_type_param(self) -> Engage:
- if self._pcm_cruise_op_long:
- return Engage.auto
-
- return Engage.auto
-
- def _update_calculations(self) -> None:
- # Update current velocity offset (error)
- self._v_offset = self.speed_limit_offseted - self._v_ego
-
- # Track the time op becomes active to prevent going to tempInactive right away after
- # op enabling since controlsd will change the cruise speed every time on enabling and this will
- # cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
- # speed.
- if not self._op_engaged_prev and self._op_engaged:
- self._last_op_engaged_time = self._current_time
-
- # Update change tracking variables
- self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
- self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
- self._speed_limit_prev = self._speed_limit
- self._update_v_cruise_setpoint_prev() # always for Engage.auto
- self._op_engaged_prev = self._op_engaged
-
- self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor))
- self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor))
- self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._speed_factor))
- self._speed_limit_warning_offsetted_rounded = 0 if self._speed_limit == 0 else \
- int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor))
-
- def transition_state_from_inactive(self) -> None:
- """ Make state transition from inactive state """
- if self._engage_type == Engage.auto:
- if self._v_offset < LIMIT_SPEED_OFFSET_TH:
- self.state = SpeedLimitControlState.adapting
- else:
- self.state = SpeedLimitControlState.active
-
- def transition_state_from_temp_inactive(self) -> None:
- """ Make state transition from temporary inactive state """
- if self._speed_limit_changed:
- if self._engage_type == Engage.auto:
- self.state = SpeedLimitControlState.inactive
-
- def transition_state_from_pre_active(self) -> None:
- """ Make state transition from preActive state """
-
- def transition_state_from_adapting(self) -> None:
- """ Make state transition from adapting state """
- if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
- self.state = SpeedLimitControlState.active
-
- def transition_state_from_active(self) -> None:
- """ Make state transition from active state """
- if self._engage_type == Engage.auto:
- if self._v_offset < LIMIT_SPEED_OFFSET_TH:
- self.state = SpeedLimitControlState.adapting
-
- def _state_transition(self) -> None:
- self._state_prev = self._state
-
- # In any case, if op is disabled, or speed limit control is disabled or no valid speed limit
- # or gas is pressed, deactivate.
- if not self._op_engaged or not self._enabled or self._speed_limit == 0:
- self.state = SpeedLimitControlState.inactive
- return
-
- # In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
- # Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations
- # due to controlsd logic changing cruise setpoint when going active.
- if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \
- self._current_time > (self._last_op_engaged_time + TEMP_INACTIVE_GUARD_PERIOD):
- self.state = SpeedLimitControlState.tempInactive
- return
-
- self.state_transition_strategy[self.state]()
-
- self._update_v_cruise_setpoint_prev() # always for Engage.auto
-
- def get_current_acceleration_as_target(self) -> float:
- """ When state is inactive or tempInactive, preserve current acceleration """
- return self._a_ego
-
- def get_adapting_state_target_acceleration(self) -> float:
- """ In adapting state, calculate target acceleration based on speed limit and current velocity """
- if self.distance > 0:
- return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)
-
- return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
- def get_active_state_target_acceleration(self) -> float:
- """ In active state, aim to keep speed constant around control time horizon """
- return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
-
- def _update_events(self, events_sp: EventsSP) -> None:
- if self.is_active:
- if self._engage_type == Engage.auto:
- if self._state_prev not in ACTIVE_STATES:
- events_sp.add(EventNameSP.speedLimitActive)
- elif self._speed_limit_changed != 0:
- events_sp.add(EventNameSP.speedLimitValueChange)
-
- def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise_setpoint: float, events_sp: EventsSP) -> None:
- _car_state = sm['carState']
- self._op_engaged = sm['carControl'].longActive
- self._v_ego = v_ego
- self._a_ego = a_ego
- self._v_cruise_setpoint = v_cruise_setpoint if not np.isnan(v_cruise_setpoint) else 0.0
- self._gas_pressed = _car_state.gasPressed
- self._current_time = time.monotonic()
-
- self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)
-
- self._update_params()
- self._update_calculations()
- self._state_transition()
- self._update_events(events_sp)
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
deleted file mode 100644
index 43f78087b2..0000000000
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
+++ /dev/null
@@ -1,119 +0,0 @@
-import time
-import numpy as np
-
-from cereal import messaging
-from openpilot.common.gps import get_gps_location_service
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
-from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug
-
-
-class SpeedLimitResolver:
- _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources
- _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources
- _v_ego: float
- _current_speed_limit: float
-
- def __init__(self, policy: Policy):
- self._gps_location_service = get_gps_location_service(Params())
- self._limit_solutions = {}
- self._distance_solutions = {}
-
- self._policy = policy
- self._policy_to_sources_map = {
- Policy.car_state_only: [Source.car_state],
- Policy.car_state_priority: [Source.car_state, Source.map_data],
- Policy.map_data_priority: [Source.map_data, Source.car_state],
- Policy.map_data_only: [Source.map_data],
- Policy.combined: [Source.car_state, Source.map_data],
- }
- for source in Source:
- self._reset_limit_sources(source)
-
- def change_policy(self, policy: Policy) -> None:
- self._policy = policy
-
- def _reset_limit_sources(self, source: Source) -> None:
- self._limit_solutions[source] = 0.
- self._distance_solutions[source] = 0.
-
- def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]:
- self._v_ego = v_ego
- self._current_speed_limit = current_speed_limit
-
- self._resolve_limit_sources(sm)
- return self._consolidate()
-
- def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None:
- """Get limit solutions from each data source"""
- self._get_from_car_state(sm)
- self._get_from_map_data(sm)
-
- def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
- self._reset_limit_sources(Source.car_state)
- self._limit_solutions[Source.car_state] = sm['carStateSP'].speedLimit
- self._distance_solutions[Source.car_state] = 0.
-
- def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
- self._reset_limit_sources(Source.map_data)
- self._process_map_data(sm)
-
- def _process_map_data(self, sm: messaging.SubMaster) -> None:
- gps_data = sm[self._gps_location_service]
- map_data = sm['liveMapDataSP']
-
- gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
- if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
- debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
- return
-
- speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
- next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
-
- self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
-
- def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
- gps_data = sm[self._gps_location_service]
- map_data = sm['liveMapDataSP']
-
- distance_since_fix = self._v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
- distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
-
- self._limit_solutions[Source.map_data] = speed_limit
- self._distance_solutions[Source.map_data] = 0.
-
- if 0. < next_speed_limit < self._v_ego:
- adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
- adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
-
- if distance_to_speed_limit_ahead <= adapt_distance:
- self._limit_solutions[Source.map_data] = next_speed_limit
- self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead
-
- def _consolidate(self) -> tuple[float, float, Source]:
- source = self._get_source_solution_according_to_policy()
- self.speed_limit = self._limit_solutions[source] if source else 0.
- self.distance = self._distance_solutions[source] if source else 0.
- self.source = source or Source.none
-
- debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
- return self.speed_limit, self.distance, self.source
-
- def _get_source_solution_according_to_policy(self) -> Source | None:
- sources_for_policy = self._policy_to_sources_map[self._policy]
-
- if self._policy != Policy.combined:
- # They are ordered in the order of preference, so we pick the first that's non zero
- for source in sources_for_policy:
- if self._limit_solutions[source] > 0.:
- return Source(source)
-
- limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
- sources = np.array([source.value for source in sources_for_policy], dtype=int)
-
- if len(limits) > 0:
- min_idx = np.argmin(limits)
- return Source(sources[min_idx])
-
- return None
diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py
deleted file mode 100644
index 0f1d489ee8..0000000000
--- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/state.py
+++ /dev/null
@@ -1,58 +0,0 @@
-"""
-Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-from cereal import custom
-from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
-
-EventNameSP = custom.OnroadEventSP.EventName
-State = custom.LongitudinalPlanSP.SpeedLimitControlState
-
-ACTIVE_STATES = (State.active, State.adapting)
-ENABLED_STATES = (State.preActive, State.tempInactive, *ACTIVE_STATES)
-
-
-class StateMachine:
- def __init__(self):
- self.state = State.inactive
-
- def update(self, events_sp: EventsSP) -> tuple[bool, bool]:
- # INACTIVE
- if self.state == State.inactive:
- if events_sp.has(EventNameSP.speedLimitAdapting):
- self.state = State.adapting
- elif events_sp.has(EventNameSP.speedLimitActive):
- self.state = State.active
-
- # ACTIVE
- elif self.state == State.active:
- if events_sp.has(EventNameSP.speedLimitDisable):
- self.state = State.inactive
- elif events_sp.has(EventNameSP.speedLimitUserCancel):
- self.state = State.tempInactive
- elif events_sp.has(EventNameSP.speedLimitAdapting):
- self.state = State.adapting
-
- # ADAPTING
- elif self.state == State.adapting:
- if events_sp.has(EventNameSP.speedLimitDisable):
- self.state = State.inactive
- elif events_sp.has(EventNameSP.speedLimitUserCancel):
- self.state = State.tempInactive
- elif events_sp.has(EventNameSP.speedLimitReached):
- self.state = State.active
-
- # TEMP INACTIVE
- elif self.state == State.tempInactive:
- if events_sp.has(EventNameSP.speedLimitDisable):
- self.state = State.inactive
- elif events_sp.has(EventNameSP.speedLimitValueChange):
- # When speed limit changes, reactivate
- self.state = State.inactive
-
- enabled = self.state in ENABLED_STATES
- active = self.state in ACTIVE_STATES
-
- return enabled, active
diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py
new file mode 100644
index 0000000000..5633ed6efc
--- /dev/null
+++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py
@@ -0,0 +1,113 @@
+import pytest
+from cereal import log
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+
+
+class TurnDirection:
+ none = 0
+ turnLeft = 1
+ turnRight = 2
+
+
+@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
+ (True, False, 5, False, False, TurnDirection.turnLeft),
+ (False, True, 6, False, False, TurnDirection.turnRight),
+ (True, False, 9, False, False, TurnDirection.none),
+ (True, False, 7, True, False, TurnDirection.none),
+ (False, True, 6, False, True, TurnDirection.none),
+ (False, False, 5, False, False, TurnDirection.none),
+ (True, True, 5, False, False, TurnDirection.none),
+])
+def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
+ dh = DesireHelper()
+ controller = LaneTurnController(dh)
+ controller.enabled = True
+ controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+ controller.turn_direction = TurnDirection.none
+ controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
+ assert controller.get_turn_direction() == expected
+
+
+def test_lane_turn_desire_disabled():
+ dh = DesireHelper()
+ controller = LaneTurnController(dh)
+ controller.enabled = False
+ controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+ controller.turn_direction = TurnDirection.none
+ controller.update_lane_turn(False, False, True, False, 7)
+ assert controller.get_turn_direction() == TurnDirection.none
+
+
+def test_lane_turn_overrides_lane_change():
+ dh = DesireHelper()
+ controller = LaneTurnController(dh)
+ controller.enabled = True
+ controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+ controller.turn_direction = TurnDirection.none
+ # left turn desire
+ controller.update_lane_turn(False, False, True, False, 5)
+ assert controller.get_turn_direction() == TurnDirection.turnLeft
+ # right turn desire
+ controller.update_lane_turn(False, False, False, True, 6)
+ assert controller.get_turn_direction() == TurnDirection.turnRight
+ # no turn
+ controller.update_lane_turn(False, False, False, False, 7)
+ assert controller.get_turn_direction() == TurnDirection.none
+
+
+@pytest.mark.parametrize("v_ego,expected", [
+ (8.93, TurnDirection.turnLeft), # just below threshold
+ (8.96, TurnDirection.none), # above threshold
+ (8.95, TurnDirection.none), # just above threshold
+])
+def test_lane_turn_desire_speed_boundary(v_ego, expected):
+ dh = DesireHelper()
+ controller = LaneTurnController(dh)
+ controller.enabled = True
+ controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+ controller.turn_direction = TurnDirection.none
+ controller.update_lane_turn(False, True, True, False, v_ego)
+ assert controller.get_turn_direction() == expected
+
+
+class DummyCarState:
+ def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+ steeringPressed=False, steeringTorque=0, brakePressed=False):
+ self.vEgo = vEgo
+ self.leftBlinker = leftBlinker
+ self.rightBlinker = rightBlinker
+ self.leftBlindspot = leftBlindspot
+ self.rightBlindspot = rightBlindspot
+ self.steeringPressed = steeringPressed
+ self.steeringTorque = steeringTorque
+ self.brakePressed = brakePressed
+
+@pytest.fixture
+def set_lane_turn_params():
+ params = Params()
+ params.put("LaneTurnDesire", True)
+ params.put("LaneTurnValue", 20.0)
+
+@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
+ # Lane turn desire overrides lane change desire
+ (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft),
+ (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight),
+ # Lane change desire only (no turn desires)
+ (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+ steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft),
+ (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+ steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight),
+ # No desire (inactive)
+ (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
+ (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none), # No blinkers? no desire!
+])
+def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
+ dh = DesireHelper()
+ dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+ for _ in range(10):
+ dh.update(carstate, lateral_active, lane_change_prob)
+ assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py
index f39fafefdb..035c5f13ee 100644
--- a/sunnypilot/selfdrive/selfdrived/events.py
+++ b/sunnypilot/selfdrive/selfdrived/events.py
@@ -17,7 +17,7 @@ EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
- speedLimit = sm['longitudinalPlanSP'].slc.speedLimit
+ speedLimit = sm['longitudinalPlanSP'].sla.speedLimit
speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
return Alert(
@@ -147,16 +147,43 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.WARNING: NoEntryAlert("Pedal Pressed")
},
- EventNameSP.speedLimitActive: {
+ EventNameSP.laneTurnLeft: {
ET.WARNING: Alert(
- "Set speed changed to match posted speed limit",
+ "Turning Left",
"",
AlertStatus.normal, AlertSize.small,
- Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.),
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
},
- EventNameSP.speedLimitValueChange: {
- ET.WARNING: speed_limit_adjust_alert,
+ EventNameSP.laneTurnRight: {
+ ET.WARNING: Alert(
+ "Turning Right",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
},
+ EventNameSP.speedLimitActive: {
+ ET.WARNING: Alert(
+ "Automatically adjusting",
+ "to the posted speed limit",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+ },
+
+ EventNameSP.speedLimitChanged: {
+ ET.WARNING: Alert(
+ "Set speed changed",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+ },
+
+ EventNameSP.speedLimitPreActive: {
+ ET.WARNING: Alert(
+ "Auto Speed Limit Control: Activation Required",
+ "Manually change set speed to 80 MPH to activate",
+ AlertStatus.normal, AlertSize.mid,
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.),
+ },
}
diff --git a/sunnypilot/sunnylink/athena/sunnylinkd.py b/sunnypilot/sunnylink/athena/sunnylinkd.py
index 90eae1dfe8..25a77c367b 100755
--- a/sunnypilot/sunnylink/athena/sunnylinkd.py
+++ b/sunnypilot/sunnylink/athena/sunnylinkd.py
@@ -3,7 +3,9 @@
from __future__ import annotations
import base64
+import errno
import gzip
+import json
import os
import ssl
import threading
@@ -17,11 +19,11 @@ from openpilot.common.swaglog import cloudlog
from openpilot.system.athena.athenad import ws_send, jsonrpc_handler, \
recv_queue, UploadQueueCache, upload_queue, cur_upload_items, backoff, ws_manage, log_handler, start_local_proxy_shim, upload_handler
from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException,
- create_connection)
+ create_connection, WebSocketConnectionClosedException)
import cereal.messaging as messaging
from sunnypilot.sunnylink.api import SunnylinkApi
-from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready
+from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, get_param_as_byte, save_param_from_base64_encoded_string
SUNNYLINK_ATHENA_HOST = os.getenv('SUNNYLINK_ATHENA_HOST', 'wss://ws.stg.api.sunnypilot.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
@@ -107,10 +109,13 @@ def ws_recv(ws: WebSocket, end_event: threading.Event) -> None:
except WebSocketTimeoutException:
ns_since_last_ping = int(time.monotonic() * 1e9) - last_ping
if ns_since_last_ping > SUNNYLINK_RECONNECT_TIMEOUT_S * 1e9:
- cloudlog.exception("sunnylinkd.ws_recv.timeout")
+ cloudlog.warning("sunnylinkd.ws_recv.timeout")
end_event.set()
- except Exception:
- cloudlog.exception("sunnylinkd.ws_recv.exception")
+ except Exception as e:
+ if isinstance(e, WebSocketConnectionClosedException):
+ cloudlog.warning(f"sunnylinkd.ws_recv.{type(e).__name__}")
+ else:
+ cloudlog.exception("sunnylinkd.ws_recv.exception")
end_event.set()
@@ -137,11 +142,15 @@ def ws_queue(end_event: threading.Event) -> None:
sunnylink_api.resume_queued(timeout=29)
resume_requested = True
tries = 0
- except Exception:
- cloudlog.exception("sunnylinkd.ws_queue.resume_queued.exception")
+ except Exception as e:
+ if isinstance(e, (ConnectionError, TimeoutError)):
+ cloudlog.warning(f"sunnylinkd.ws_queue.resume_queued.{type(e).__name__}")
+ else:
+ cloudlog.exception("sunnylinkd.ws_queue.resume_queued.exception")
+
resume_requested = False
tries += 1
- time.sleep(backoff(tries)) # Wait for the backoff time before the next attempt
+ time.sleep(backoff(tries))
if end_event.is_set():
cloudlog.debug("end_event is set, exiting ws_queue thread")
@@ -171,16 +180,26 @@ def getParamsAllKeys() -> list[str]:
@dispatcher.add_method
def getParams(params_keys: list[str], compression: bool = False) -> str | dict[str, str]:
+ params = Params()
+
try:
- params = Params()
- params_dict: dict[str, bytes] = {key: params.get(key) or b'' for key in params_keys}
+ param_keys_validated = [key for key in params_keys if key in getParamsAllKeys()]
+ params_dict: dict[str, list[dict[str, str | bool | int]]] = {"params": []}
+ for key in param_keys_validated:
+ value = get_param_as_byte(key)
+ if value is None:
+ continue
- # Compress the values before encoding to base64 as output from params.get is bytes and same for compression
- if compression:
- params_dict = {key: gzip.compress(value) for key, value in params_dict.items()}
+ params_dict["params"].append({
+ "key": key,
+ "value": base64.b64encode(gzip.compress(value) if compression else value).decode('utf-8'),
+ "type": int(params.get_type(key).value),
+ "is_compressed": compression
+ })
- # Last step is to encode the values to base64 and decode to utf-8 for JSON serialization
- return {key: base64.b64encode(value).decode('utf-8') for key, value in params_dict.items()}
+ response = {str(param.get('key')): str(param.get('value')) for param in params_dict.get("params", [])}
+ response |= {"params": json.dumps(params_dict.get("params", []))} # Upcoming for settings v1
+ return response
except Exception as e:
cloudlog.exception("sunnylinkd.getParams.exception", e)
@@ -189,15 +208,9 @@ def getParams(params_keys: list[str], compression: bool = False) -> str | dict[s
@dispatcher.add_method
def saveParams(params_to_update: dict[str, str], compression: bool = False) -> None:
- params = Params()
- params_dict = {key: base64.b64decode(value) for key, value in params_to_update.items()}
-
- if compression:
- params_dict = {key: gzip.decompress(value) for key, value in params_dict.items()}
-
- for key, value in params_dict.items():
+ for key, value in params_to_update.items():
try:
- params.put(key, value)
+ save_param_from_base64_encoded_string(key, value, compression)
except Exception as e:
cloudlog.error(f"sunnylinkd.saveParams.exception {e}")
@@ -252,14 +265,19 @@ def main(exit_event: threading.Event = None):
handle_long_poll(ws, exit_event)
except (KeyboardInterrupt, SystemExit):
break
- except (ConnectionError, TimeoutError, WebSocketException):
+ except Exception as e:
conn_retries += 1
params.remove("LastSunnylinkPingTime")
- except Exception:
- cloudlog.exception("sunnylinkd.main.exception")
- conn_retries += 1
- params.remove("LastSunnylinkPingTime")
+ if isinstance(e, (ConnectionError, TimeoutError, WebSocketException)):
+ cloudlog.warning(f"sunnylinkd.main.{type(e).__name__}")
+ elif isinstance(e, OSError):
+ name = errno.errorcode.get(e.errno or -1, "UNKNOWN")
+ msg = f"sunnylinkd.main.OSError.{name} ({e.errno})"
+ is_expected_error = e.errno in (errno.ENETDOWN, errno.ENETRESET, errno.ENETUNREACH)
+ cloudlog.warning(msg) if is_expected_error else cloudlog.exception(msg)
+ else:
+ cloudlog.exception("sunnylinkd.main.exception")
time.sleep(backoff(conn_retries))
diff --git a/sunnypilot/sunnylink/backups/manager.py b/sunnypilot/sunnylink/backups/manager.py
index f98088a1fb..e52b547afe 100644
--- a/sunnypilot/sunnylink/backups/manager.py
+++ b/sunnypilot/sunnylink/backups/manager.py
@@ -12,7 +12,7 @@ from enum import Enum
from typing import Any
from openpilot.common.git import get_branch
-from openpilot.common.params import Params, ParamKeyType, ParamKeyFlag
+from openpilot.common.params import Params, ParamKeyFlag
from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.system.version import get_version
@@ -20,6 +20,7 @@ from openpilot.system.version import get_version
from cereal import messaging, custom
from sunnypilot.sunnylink.api import SunnylinkApi
from sunnypilot.sunnylink.backups.utils import decrypt_compressed_data, encrypt_compress_data, SnakeCaseEncoder
+from sunnypilot.sunnylink.utils import get_param_as_byte, save_param_from_base64_encoded_string
class OperationType(Enum):
@@ -74,7 +75,7 @@ class BackupManagerSP:
config_data = {}
params_to_backup = [k.decode('utf-8') for k in self.params.all_keys(ParamKeyFlag.BACKUP)]
for param in params_to_backup:
- value = str(self.params.get(param)).encode('utf-8')
+ value = get_param_as_byte(param)
if value is not None:
config_data[param] = base64.b64encode(value).decode('utf-8')
return config_data
@@ -113,6 +114,7 @@ class BackupManagerSP:
payload = json.loads(json.dumps(backup_info.to_dict(), cls=SnakeCaseEncoder))
self._update_progress(75.0, OperationType.BACKUP)
+ cloudlog.debug(f"Uploading backup with payload: {json.dumps(payload)}")
# Upload to sunnylink
result = self.api.api_get(
f"backup/{self.device_id}",
@@ -124,9 +126,11 @@ class BackupManagerSP:
if result:
self.backup_status = custom.BackupManagerSP.Status.completed
self._update_progress(100.0, OperationType.BACKUP)
+ cloudlog.info("Backup successfully created and uploaded")
else:
self.backup_status = custom.BackupManagerSP.Status.failed
self.last_error = "Failed to upload backup"
+ cloudlog.error(result)
self._report_status()
return bool(self.backup_status == custom.BackupManagerSP.Status.completed)
@@ -169,8 +173,7 @@ class BackupManagerSP:
self._update_progress(75.0, OperationType.RESTORE)
# Apply configuration
- all_values_encoded = self._get_metadata_value(backup_metadata, "all_values_encoded", "false")
- self._apply_config(config_data, str(all_values_encoded).lower() == "true")
+ self._apply_config(config_data)
self.restore_status = custom.BackupManagerSP.Status.completed
self._update_progress(100.0, OperationType.RESTORE)
@@ -183,7 +186,7 @@ class BackupManagerSP:
self._report_status()
return False
- def _apply_config(self, config_data: dict[str, str], all_values_encoded: bool = False) -> None:
+ def _apply_config(self, config_data: dict[str, str]) -> None:
"""Applies configuration data from a backup, but only for parameters marked as backupable."""
backupable_params = [k.decode('utf-8') for k in self.params.all_keys(ParamKeyFlag.BACKUP)]
backupable_set_lower = {p.lower() for p in backupable_params}
@@ -195,26 +198,8 @@ class BackupManagerSP:
if param.lower() in backupable_set_lower:
# Find real param name (with correct casing)
real_param = next(p for p in backupable_params if p.lower() == param.lower())
- param_type = self.params.get_type(real_param)
try:
- value = base64.b64decode(encoded_value) if all_values_encoded else encoded_value
-
- if param_type != ParamKeyType.BYTES:
- value = value.decode('utf-8') # type: ignore
-
- if param_type == ParamKeyType.STRING:
- value = value
- elif param_type == ParamKeyType.BOOL:
- value = value.lower() in ('true', '1', 'yes') # type: ignore
- elif param_type == ParamKeyType.INT:
- value = int(value) # type: ignore
- elif param_type == ParamKeyType.FLOAT:
- value = float(value) # type: ignore
- elif param_type == ParamKeyType.TIME:
- value = str(value)
- elif param_type == ParamKeyType.JSON:
- value = json.loads(value)
- self.params.put(real_param, value)
+ save_param_from_base64_encoded_string(real_param, encoded_value)
restored_count += 1
except Exception as e:
cloudlog.error(f"Failed to restore param {param}: {str(e)}")
@@ -264,8 +249,8 @@ class BackupManagerSP:
# Check for backup command
if self.params.get_bool("BackupManager_CreateBackup"):
try:
- await self.create_backup()
- reset_progress = True
+ if await self.create_backup():
+ reset_progress = True
finally:
self.params.remove("BackupManager_CreateBackup")
diff --git a/sunnypilot/sunnylink/utils.py b/sunnypilot/sunnylink/utils.py
index 35714eafe3..91c0788795 100644
--- a/sunnypilot/sunnylink/utils.py
+++ b/sunnypilot/sunnylink/utils.py
@@ -1,5 +1,8 @@
+import base64
+import gzip
+import json
from sunnypilot.sunnylink.api import SunnylinkApi, UNREGISTERED_SUNNYLINK_DONGLE_ID
-from openpilot.common.params import Params
+from openpilot.common.params import Params, ParamKeyType
from openpilot.system.version import is_prebuilt
@@ -55,3 +58,59 @@ def get_api_token():
sunnylink_api = SunnylinkApi(sunnylink_dongle_id)
token = sunnylink_api.get_token()
print(f"API Token: {token}")
+
+
+def get_param_as_byte(param_name: str, params=None) -> bytes | None:
+ """Get a parameter as bytes. Returns None if the parameter does not exist."""
+ params = params or Params() # Use existing Params instance if provided
+ param = params.get(param_name)
+ if param is None:
+ return None
+
+ param_type = params.get_type(param_name)
+ if param_type == ParamKeyType.BYTES:
+ return bytes(param)
+ elif param_type == ParamKeyType.JSON:
+ return json.dumps(param).encode('utf-8')
+ return str(param).encode('utf-8')
+
+
+def save_param_from_base64_encoded_string(param_name: str, base64_encoded_data: str, is_compressed=False) -> None:
+ """Save a parameter from bytes. Overwrites the parameter if it already exists."""
+ params = Params()
+ # Find real param name (with correct casing)
+ param_type = params.get_type(param_name)
+ value = base64.b64decode(base64_encoded_data)
+
+ if is_compressed:
+ value = gzip.decompress(value)
+
+ # We convert to string anything that isn't bytes first. We later transform further.
+ param_value = _convert_param_to_type(value, param_type)
+ params.put(param_name, param_value)
+
+
+def _convert_param_to_type(value: bytes, param_type: ParamKeyType) -> bytes | str | int | float | bool | dict | None:
+ """
+ Convert a byte value to the specified param type. Used internally when getting a Param to convert it to the right type.
+ If this method looks familiar, it's because on SP we have a similar one in openpilot/sunnypilot/car/__init__.py.
+ """
+
+ # We convert to string anything that isn't bytes first. We later transform further.
+ if param_type != ParamKeyType.BYTES:
+ value = value.decode('utf-8') # type: ignore
+
+ if param_type == ParamKeyType.STRING:
+ value = value
+ elif param_type == ParamKeyType.BOOL:
+ value = value.lower() in ('true', '1', 'yes') # type: ignore
+ elif param_type == ParamKeyType.INT:
+ value = int(value) # type: ignore
+ elif param_type == ParamKeyType.FLOAT:
+ value = float(value) # type: ignore
+ elif param_type == ParamKeyType.TIME:
+ value = str(value) # type: ignore
+ elif param_type == ParamKeyType.JSON:
+ value = json.loads(value)
+
+ return value
diff --git a/sunnypilot/system/hardware/c3/README.md b/sunnypilot/system/hardware/c3/README.md
new file mode 100644
index 0000000000..f74a210191
--- /dev/null
+++ b/sunnypilot/system/hardware/c3/README.md
@@ -0,0 +1,3 @@
+# C3 specific hardware code
+
+`c3` is known as `tici` and comma three by comma. Not to confuse it with `c3x` which is known as `tizi`.
\ No newline at end of file
diff --git a/sunnypilot/system/hardware/c3/agnos.json b/sunnypilot/system/hardware/c3/agnos.json
new file mode 100644
index 0000000000..941a4956bf
--- /dev/null
+++ b/sunnypilot/system/hardware/c3/agnos.json
@@ -0,0 +1,84 @@
+[
+ {
+ "name": "xbl",
+ "url": "https://commadist.azureedge.net/agnosupdate/xbl-effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b.img.xz",
+ "hash": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
+ "hash_raw": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
+ "size": 3282256,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "ed61a650bea0c56652dd0fc68465d8fc722a4e6489dc8f257630c42c6adcdc89"
+ },
+ {
+ "name": "xbl_config",
+ "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c.img.xz",
+ "hash": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
+ "hash_raw": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
+ "size": 98124,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "b12801ffaa81e58e3cef914488d3b447e35483ba549b28c6cd9deb4814c3265f"
+ },
+ {
+ "name": "abl",
+ "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
+ "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+ "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+ "size": 274432,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
+ },
+ {
+ "name": "aop",
+ "url": "https://commadist.azureedge.net/agnosupdate/aop-21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9.img.xz",
+ "hash": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
+ "hash_raw": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
+ "size": 184364,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "c1be2f4aac5b3af49b904b027faec418d05efd7bd5144eb4fdfcba602bcf2180"
+ },
+ {
+ "name": "devcfg",
+ "url": "https://commadist.azureedge.net/agnosupdate/devcfg-d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620.img.xz",
+ "hash": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
+ "hash_raw": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
+ "size": 40336,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "17b229668b20305ff8fa3cd5f94716a3aaa1e5bf9d1c24117eff7f2f81ae719f"
+ },
+ {
+ "name": "boot",
+ "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
+ "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
+ "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
+ "size": 18515968,
+ "sparse": false,
+ "full_check": true,
+ "has_ab": true,
+ "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
+ },
+ {
+ "name": "system",
+ "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
+ "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
+ "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
+ "size": 5368709120,
+ "sparse": true,
+ "full_check": false,
+ "has_ab": true,
+ "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
+ "alt": {
+ "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
+ "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
+ "size": 5368709120
+ }
+ }
+]
\ No newline at end of file
diff --git a/sunnypilot/system/hardware/c3/launch_chffrplus.sh b/sunnypilot/system/hardware/c3/launch_chffrplus.sh
new file mode 100755
index 0000000000..45cc950537
--- /dev/null
+++ b/sunnypilot/system/hardware/c3/launch_chffrplus.sh
@@ -0,0 +1,92 @@
+#!/usr/bin/env bash
+
+SP_C3_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
+DIR="$( cd "$SP_C3_DIR/../../../.." >/dev/null 2>&1 && pwd )"
+
+source "$SP_C3_DIR/launch_env.sh"
+
+function agnos_init {
+ # TODO: move this to agnos
+ sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta
+
+ # set success flag for current boot slot
+ sudo abctl --set_success
+
+ # TODO: do this without udev in AGNOS
+ # udev does this, but sometimes we startup faster
+ sudo chgrp gpu /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
+ sudo chmod 660 /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
+
+
+ if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then
+ AGNOS_PY="$DIR/system/hardware/tici/agnos.py"
+ MANIFEST="$SP_C3_DIR/agnos.json"
+ if $AGNOS_PY --verify $MANIFEST; then
+ sudo reboot
+ fi
+ $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST
+ fi
+}
+
+function launch {
+ # Remove orphaned git lock if it exists on boot
+ [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock
+
+ # Check to see if there's a valid overlay-based update available. Conditions
+ # are as follows:
+ #
+ # 1. The DIR init file has to exist, with a newer modtime than anything in
+ # the DIR Git repo. This checks for local development work or the user
+ # switching branches/forks, which should not be overwritten.
+ # 2. The FINALIZED consistent file has to exist, indicating there's an update
+ # that completed successfully and synced to disk.
+
+ if [ -f "${DIR}/.overlay_init" ]; then
+ find ${DIR}/.git -newer ${DIR}/.overlay_init | grep -q '.' 2> /dev/null
+ if [ $? -eq 0 ]; then
+ echo "${DIR} has been modified, skipping overlay update installation"
+ else
+ if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then
+ if [ ! -d /data/safe_staging/old_openpilot ]; then
+ echo "Valid overlay update found, installing"
+ LAUNCHER_LOCATION="${BASH_SOURCE[0]}"
+
+ mv $DIR /data/safe_staging/old_openpilot
+ mv "${STAGING_ROOT}/finalized" $DIR
+ cd $DIR
+
+ echo "Restarting launch script ${LAUNCHER_LOCATION}"
+ unset AGNOS_VERSION
+ exec "${LAUNCHER_LOCATION}"
+ else
+ echo "openpilot backup found, not updating"
+ # TODO: restore backup? This means the updater didn't start after swapping
+ fi
+ fi
+ fi
+ fi
+
+ # handle pythonpath
+ ln -sfn $(pwd) /data/pythonpath
+ export PYTHONPATH="$PWD"
+
+ # hardware specific init
+ if [ -f /AGNOS ]; then
+ agnos_init
+ fi
+
+ # write tmux scrollback to a file
+ tmux capture-pane -pq -S-1000 > /tmp/launch_log
+
+ # start manager
+ cd $DIR/system/manager
+ if [ ! -f $DIR/prebuilt ]; then
+ ./build.py
+ fi
+ ./manager.py
+
+ # if broken, keep on screen error
+ while true; do sleep 1; done
+}
+
+launch
diff --git a/sunnypilot/system/hardware/c3/launch_env.sh b/sunnypilot/system/hardware/c3/launch_env.sh
new file mode 100755
index 0000000000..4c011c6ac0
--- /dev/null
+++ b/sunnypilot/system/hardware/c3/launch_env.sh
@@ -0,0 +1,13 @@
+#!/usr/bin/env bash
+
+export OMP_NUM_THREADS=1
+export MKL_NUM_THREADS=1
+export NUMEXPR_NUM_THREADS=1
+export OPENBLAS_NUM_THREADS=1
+export VECLIB_MAXIMUM_THREADS=1
+
+if [ -z "$AGNOS_VERSION" ]; then
+ export AGNOS_VERSION="12.8"
+fi
+
+export STAGING_ROOT="/data/safe_staging"
diff --git a/system/athena/athenad.py b/system/athena/athenad.py
index f97b8e55bb..42c9cf8a1c 100755
--- a/system/athena/athenad.py
+++ b/system/athena/athenad.py
@@ -381,20 +381,22 @@ def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: str = N
return {"success": 1}
-def scan_dir(path: str, prefix: str) -> list[str]:
+def scan_dir(path: str, prefix: str, base: str | None = None) -> list[str]:
+ if base is None:
+ base = path
files = []
# only walk directories that match the prefix
# (glob and friends traverse entire dir tree)
with os.scandir(path) as i:
for e in i:
- rel_path = os.path.relpath(e.path, Paths.log_root())
+ rel_path = os.path.relpath(e.path, base)
if e.is_dir(follow_symlinks=False):
# add trailing slash
rel_path = os.path.join(rel_path, '')
# if prefix is a partial dir name, current dir will start with prefix
# if prefix is a partial file name, prefix with start with dir name
if rel_path.startswith(prefix) or prefix.startswith(rel_path):
- files.extend(scan_dir(e.path, prefix))
+ files.extend(scan_dir(e.path, prefix, base))
else:
if rel_path.startswith(prefix):
files.append(rel_path)
@@ -402,7 +404,12 @@ def scan_dir(path: str, prefix: str) -> list[str]:
@dispatcher.add_method
def listDataDirectory(prefix='') -> list[str]:
- return scan_dir(Paths.log_root(), prefix)
+ internal_files = scan_dir(Paths.log_root(), prefix, Paths.log_root())
+ try:
+ external_files = scan_dir(Paths.log_root_external(), prefix, Paths.log_root_external())
+ except FileNotFoundError:
+ external_files = []
+ return sorted(set(internal_files + external_files))
@dispatcher.add_method
@@ -427,8 +434,13 @@ def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlRespo
failed.append(file.fn)
continue
- path = os.path.join(Paths.log_root(), file.fn)
- if not os.path.exists(path) and not os.path.exists(strip_zst_extension(path)):
+ path_internal = os.path.join(Paths.log_root(), file.fn)
+ path_external = os.path.join(Paths.log_root_external(), file.fn)
+ if os.path.exists(path_internal) or os.path.exists(strip_zst_extension(path_internal)):
+ path = path_internal
+ elif os.path.exists(path_external) or os.path.exists(strip_zst_extension(path_external)):
+ path = path_external
+ else:
failed.append(file.fn)
continue
diff --git a/system/camerad/SConscript b/system/camerad/SConscript
index fe5cf87b78..734f748a2a 100644
--- a/system/camerad/SConscript
+++ b/system/camerad/SConscript
@@ -4,7 +4,7 @@ libs = [common, 'OpenCL', messaging, visionipc, gpucommon]
if arch != "Darwin":
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
- 'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
+ 'cameras/cdm.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
if GetOption("extras") and arch == "x86_64":
diff --git a/system/camerad/cameras/hw.h b/system/camerad/cameras/hw.h
index d299627ce9..f20a1b3ade 100644
--- a/system/camerad/cameras/hw.h
+++ b/system/camerad/cameras/hw.h
@@ -13,7 +13,7 @@ typedef enum {
ISP_BPS_PROCESSED, // fully processed image through the BPS
} SpectraOutputType;
-// For the comma 3/3X three camera platform
+// For the comma 3X three camera platform
struct CameraConfig {
int camera_num;
diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc
index 47ae9061f4..caf7871573 100644
--- a/system/camerad/cameras/spectra.cc
+++ b/system/camerad/cameras/spectra.cc
@@ -1004,8 +1004,7 @@ bool SpectraCamera::openSensor() {
};
// Figure out which sensor we have
- if (!init_sensor_lambda(new AR0231) &&
- !init_sensor_lambda(new OX03C10) &&
+ if (!init_sensor_lambda(new OX03C10) &&
!init_sensor_lambda(new OS04C10)) {
LOGE("** sensor %d FAILED bringup, disabling", cc.camera_num);
enabled = false;
diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc
deleted file mode 100644
index e4ae29f079..0000000000
--- a/system/camerad/sensors/ar0231.cc
+++ /dev/null
@@ -1,136 +0,0 @@
-#include
-#include
-
-#include "system/camerad/sensors/sensor.h"
-
-namespace {
-
-const size_t AR0231_REGISTERS_HEIGHT = 2;
-// TODO: this extra height is universal and doesn't apply per camera
-const size_t AR0231_STATS_HEIGHT = 2 + 8;
-
-const float sensor_analog_gains_AR0231[] = {
- 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3
- 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7
- 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11
- 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass
-
-} // namespace
-
-AR0231::AR0231() {
- image_sensor = cereal::FrameData::ImageSensor::AR0231;
- bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR;
- pixel_size_mm = 0.003;
- data_word = true;
- frame_width = 1928;
- frame_height = 1208;
- frame_stride = (frame_width * 12 / 8) + 4;
- extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT;
-
- registers_offset = 0;
- frame_offset = AR0231_REGISTERS_HEIGHT;
- stats_offset = AR0231_REGISTERS_HEIGHT + frame_height;
-
- start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231));
- init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231));
- probe_reg_addr = 0x3000;
- probe_expected_data = 0x354;
- bits_per_pixel = 12;
- mipi_format = CAM_FORMAT_MIPI_RAW_12;
- frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead
- mclk_frequency = 19200000; //Hz
-
- readout_time_ns = 22850000;
-
- dc_gain_factor = 2.5;
- dc_gain_min_weight = 0;
- dc_gain_max_weight = 1;
- dc_gain_on_grey = 0.2;
- dc_gain_off_grey = 0.3;
- exposure_time_min = 2; // with HDR, fastest ss
- exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms
- analog_gain_min_idx = 0x1; // 0.25x
- analog_gain_rec_idx = 0x6; // 0.8x
- analog_gain_max_idx = 0xD; // 4.0x
- analog_gain_cost_delta = 0;
- analog_gain_cost_low = 0.1;
- analog_gain_cost_high = 5.0;
- for (int i = 0; i <= analog_gain_max_idx; i++) {
- sensor_analog_gains[i] = sensor_analog_gains_AR0231[i];
- }
- min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx];
- max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx];
- target_grey_factor = 1.0;
-
- black_level = 168;
- color_correct_matrix = {
- 0x000000af, 0x00000ff9, 0x00000fd8,
- 0x00000fbc, 0x000000bb, 0x00000009,
- 0x00000fb6, 0x00000fe0, 0x000000ea,
- };
- for (int i = 0; i < 65; i++) {
- float fx = i / 64.0;
- const float gamma_k = 0.75;
- const float gamma_b = 0.125;
- const float mp = 0.01; // ideally midpoint should be adaptive
- const float rk = 9 - 100*mp;
- // poly approximation for s curve
- fx = (fx > mp) ?
- ((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) :
- ((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b);
- gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
- }
- prepare_gamma_lut();
- linearization_lut = {
- 0x02000000, 0x02000000, 0x02000000, 0x02000000,
- 0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff,
- 0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff,
- 0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff,
- 0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff,
- 0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff,
- 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
- 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
- 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
- };
- linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff};
- vignetting_lut = {
- 0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2,
- 0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7,
- 0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d,
- 0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed,
- 0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1,
- 0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17,
- 0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93,
- 0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e,
- 0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6,
- 0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee,
- 0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8,
- 0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676,
- 0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22,
- };
-}
-
-std::vector AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const {
- uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g;
- return {
- {0x3366, analog_gain_reg},
- {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
- {0x3012, (uint16_t)exposure_time},
- };
-}
-
-int AR0231::getSlaveAddress(int port) const {
- assert(port >= 0 && port <= 2);
- return (int[]){0x20, 0x30, 0x20}[port];
-}
-
-float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {
- // Cost of ev diff
- float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10;
- // Cost of absolute gain
- float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low;
- score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m;
- // Cost of changing gain
- score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0;
- return score;
-}
diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h
deleted file mode 100644
index e0872a673a..0000000000
--- a/system/camerad/sensors/ar0231_registers.h
+++ /dev/null
@@ -1,121 +0,0 @@
-#pragma once
-
-const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
-const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
-
-const struct i2c_random_wr_payload init_array_ar0231[] = {
- {0x301A, 0x0018}, // RESET_REGISTER
-
- // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
-
- // CLOCK Settings
- // input clock is 19.2 / 2 * 0x37 = 528 MHz
- // pixclk is 528 / 6 = 88 MHz
- // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
- // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
- {0x302A, 0x0006}, // VT_PIX_CLK_DIV
- {0x302C, 0x0001}, // VT_SYS_CLK_DIV
- {0x302E, 0x0002}, // PRE_PLL_CLK_DIV
- {0x3030, 0x0037}, // PLL_MULTIPLIER
- {0x3036, 0x000C}, // OP_PIX_CLK_DIV
- {0x3038, 0x0001}, // OP_SYS_CLK_DIV
-
- // FORMAT
- {0x3040, 0xC000}, // READ_MODE
- {0x3004, 0x0000}, // X_ADDR_START_
- {0x3008, 0x0787}, // X_ADDR_END_
- {0x3002, 0x0000}, // Y_ADDR_START_
- {0x3006, 0x04B7}, // Y_ADDR_END_
- {0x3032, 0x0000}, // SCALING_MODE
- {0x30A2, 0x0001}, // X_ODD_INC_
- {0x30A6, 0x0001}, // Y_ODD_INC_
- {0x3402, 0x0788}, // X_OUTPUT_CONTROL
- {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
- {0x3064, 0x1982}, // SMIA_TEST
- {0x30BA, 0x11F2}, // DIGITAL_CTRL
-
- // Enable external trigger and disable GPIO outputs
- {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
- {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
- {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
-
- // Readout timing
- {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
- {0x300A, 0x0855}, // FRAME_LENGTH_LINES
- {0x3042, 0x0000}, // EXTRA_DELAY
-
- // Readout Settings
- {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
- {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
- {0x3342, 0x1212}, // MIPI_F1_PDT_EDT
- {0x3346, 0x1212}, // MIPI_F2_PDT_EDT
- {0x334A, 0x1212}, // MIPI_F3_PDT_EDT
- {0x334E, 0x1212}, // MIPI_F4_PDT_EDT
- {0x3344, 0x0011}, // MIPI_F1_VDT_VC
- {0x3348, 0x0111}, // MIPI_F2_VDT_VC
- {0x334C, 0x0211}, // MIPI_F3_VDT_VC
- {0x3350, 0x0311}, // MIPI_F4_VDT_VC
- {0x31B0, 0x0053}, // FRAME_PREAMBLE
- {0x31B2, 0x003B}, // LINE_PREAMBLE
- {0x301A, 0x001C}, // RESET_REGISTER
-
- // Noise Corrections
- {0x3092, 0x0C24}, // ROW_NOISE_CONTROL
- {0x337A, 0x0C80}, // DBLC_SCALE0
- {0x3370, 0x03B1}, // DBLC
- {0x3044, 0x0400}, // DARK_CONTROL
-
- // Enable temperature sensor
- {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
- {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
-
- // Enable dead pixel correction using
- // the 1D line correction scheme
- {0x31E0, 0x0003},
-
- // HDR Settings
- {0x3082, 0x0004}, // OPERATION_MODE_CTRL
- {0x3238, 0x0444}, // EXPOSURE_RATIO
-
- {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
- {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
- {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
- {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
-
- // TODO: do these have to be lower than LINE_LENGTH_PCK?
- {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
- {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
-
- {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
- {0x33DA, 0x0000}, // COMPANDING
- {0x318E, 0x0200}, // PRE_HDR_GAIN_EN
-
- // DLO Settings
- {0x3100, 0x4000}, // DLO_CONTROL0
- {0x3280, 0x0CCC}, // T1 G1
- {0x3282, 0x0CCC}, // T1 R
- {0x3284, 0x0CCC}, // T1 B
- {0x3286, 0x0CCC}, // T1 G2
- {0x3288, 0x0FA0}, // T2 G1
- {0x328A, 0x0FA0}, // T2 R
- {0x328C, 0x0FA0}, // T2 B
- {0x328E, 0x0FA0}, // T2 G2
-
- // Initial Gains
- {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
- {0x3366, 0xFF77}, // ANALOG_GAIN (1x)
-
- {0x3060, 0x3333}, // ANALOG_COLOR_GAIN
-
- {0x3362, 0x0000}, // DC GAIN
-
- {0x305A, 0x00F8}, // red gain
- {0x3058, 0x0122}, // blue gain
- {0x3056, 0x009A}, // g1 gain
- {0x305C, 0x009A}, // g2 gain
-
- {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
-
- // Initial Integration Time
- {0x3012, 0x0005},
-};
diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h
index d4be3cf036..96aa8b604f 100644
--- a/system/camerad/sensors/sensor.h
+++ b/system/camerad/sensors/sensor.h
@@ -10,7 +10,6 @@
#include "media/cam_sensor.h"
#include "cereal/gen/cpp/log.capnp.h"
-#include "system/camerad/sensors/ar0231_registers.h"
#include "system/camerad/sensors/ox03c10_registers.h"
#include "system/camerad/sensors/os04c10_registers.h"
@@ -88,17 +87,6 @@ public:
};
};
-class AR0231 : public SensorInfo {
-public:
- AR0231();
- std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
- float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
- int getSlaveAddress(int port) const override;
-
-private:
- mutable std::map> ar0231_register_lut;
-};
-
class OX03C10 : public SensorInfo {
public:
OX03C10();
diff --git a/system/hardware/base.py b/system/hardware/base.py
index b457ea4e17..ce97bf294d 100644
--- a/system/hardware/base.py
+++ b/system/hardware/base.py
@@ -65,6 +65,10 @@ class ThermalConfig:
return ret
class LPABase(ABC):
+ @abstractmethod
+ def bootstrap(self) -> None:
+ pass
+
@abstractmethod
def list_profiles(self) -> list[Profile]:
pass
@@ -89,6 +93,9 @@ class LPABase(ABC):
def switch_profile(self, iccid: str) -> None:
pass
+ def is_comma_profile(self, iccid: str) -> bool:
+ return any(iccid.startswith(prefix) for prefix in ('8985235',))
+
class HardwareBase(ABC):
@staticmethod
def get_cmdline() -> dict[str, str]:
diff --git a/system/hardware/esim.py b/system/hardware/esim.py
index 58ead6593f..909ad41e03 100755
--- a/system/hardware/esim.py
+++ b/system/hardware/esim.py
@@ -3,10 +3,32 @@
import argparse
import time
from openpilot.system.hardware import HARDWARE
+from openpilot.system.hardware.base import LPABase
+
+
+def bootstrap(lpa: LPABase) -> None:
+ print('┌──────────────────────────────────────────────────────────────────────────────┐')
+ print('│ WARNING, PLEASE READ BEFORE PROCEEDING │')
+ print('│ │')
+ print('│ this is an irreversible operation that will remove the comma-provisioned │')
+ print('│ profile. │')
+ print('│ │')
+ print('│ after this operation, you must purchase a new eSIM from comma in order to │')
+ print('│ use the comma prime subscription again. │')
+ print('└──────────────────────────────────────────────────────────────────────────────┘')
+ print()
+ for severity in ('sure', '100% sure'):
+ print(f'are you {severity} you want to proceed? (y/N) ', end='')
+ confirm = input()
+ if confirm != 'y':
+ print('aborting')
+ exit(0)
+ lpa.bootstrap()
if __name__ == '__main__':
parser = argparse.ArgumentParser(prog='esim.py', description='manage eSIM profiles on your comma device', epilog='comma.ai')
+ parser.add_argument('--bootstrap', action='store_true', help='bootstrap the eUICC (required before downloading profiles)')
parser.add_argument('--backend', choices=['qmi', 'at'], default='qmi', help='use the specified backend, defaults to qmi')
parser.add_argument('--switch', metavar='iccid', help='switch to profile')
parser.add_argument('--delete', metavar='iccid', help='delete profile (warning: this cannot be undone)')
@@ -16,7 +38,10 @@ if __name__ == '__main__':
mutated = False
lpa = HARDWARE.get_sim_lpa()
- if args.switch:
+ if args.bootstrap:
+ bootstrap(lpa)
+ mutated = True
+ elif args.switch:
lpa.switch_profile(args.switch)
mutated = True
elif args.delete:
diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py
index d702334fa8..0144a9df03 100755
--- a/system/hardware/hardwared.py
+++ b/system/hardware/hardwared.py
@@ -6,7 +6,6 @@ import struct
import threading
import time
from collections import OrderedDict, namedtuple
-from pathlib import Path
import psutil
@@ -24,7 +23,7 @@ from openpilot.system.statsd import statlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware.power_monitoring import PowerMonitoring
from openpilot.system.hardware.fan_controller import TiciFanController
-from openpilot.system.version import terms_version, training_version
+from openpilot.system.version import terms_version, training_version, get_build_metadata
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
@@ -326,18 +325,22 @@ def hardware_thread(end_event, hw_queue) -> None:
startup_conditions["not_always_offroad"] = not offroad_mode
onroad_conditions["not_always_offroad"] = not offroad_mode
+ # if an unsupported device and branch is detected, going onroad is blocked
+ # only allow going onroad when:
+ # - TIZI, or
+ # - TICI and channel_type is "tici"
+ build_metadata = get_build_metadata()
+ is_unsupported_combo = TICI and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
+ startup_conditions["not_tici"] = not is_unsupported_combo
+ onroad_conditions["not_tici"] = not is_unsupported_combo
+ set_offroad_alert("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
+
# if the temperature enters the danger zone, go offroad to cool down
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
extra_text = f"{offroad_comp_temp:.1f}C"
show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)
- # TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
- if TICI and HARDWARE.get_device_type() == "tici":
- if not os.path.isfile("/persist/comma/living-in-the-moment"):
- if not Path("/data/media").is_mount():
- set_offroad_alert_if_changed("Offroad_StorageMissing", True)
-
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
diff --git a/system/hardware/hw.py b/system/hardware/hw.py
index 5e40fff136..d24857e8bd 100644
--- a/system/hardware/hw.py
+++ b/system/hardware/hw.py
@@ -20,6 +20,10 @@ class Paths:
else:
return '/data/media/0/realdata/'
+ @staticmethod
+ def log_root_external() -> str:
+ return '/mnt/external_realdata/'
+
@staticmethod
def swaglog_root() -> str:
if PC:
diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json
index 941a4956bf..d93963cf2c 100644
--- a/system/hardware/tici/agnos.json
+++ b/system/hardware/tici/agnos.json
@@ -23,14 +23,14 @@
},
{
"name": "abl",
- "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
- "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
- "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+ "url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
+ "hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
+ "hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"size": 274432,
"sparse": false,
"full_check": true,
"has_ab": true,
- "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
+ "ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee"
},
{
"name": "aop",
@@ -56,29 +56,29 @@
},
{
"name": "boot",
- "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
- "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
- "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
- "size": 18515968,
+ "url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz",
+ "hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
+ "hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
+ "size": 17442816,
"sparse": false,
"full_check": true,
"has_ab": true,
- "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
+ "ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a"
},
{
"name": "system",
- "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
- "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
- "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
- "size": 5368709120,
+ "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz",
+ "hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9",
+ "hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
+ "size": 4718592000,
"sparse": true,
"full_check": false,
"has_ab": true,
- "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
+ "ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727",
"alt": {
- "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
- "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
- "size": 5368709120
+ "hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
+ "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img",
+ "size": 4718592000
}
}
]
\ No newline at end of file
diff --git a/system/hardware/tici/all-partitions.json b/system/hardware/tici/all-partitions.json
index 5891e2748a..ebffc01dfd 100644
--- a/system/hardware/tici/all-partitions.json
+++ b/system/hardware/tici/all-partitions.json
@@ -152,14 +152,14 @@
},
{
"name": "abl",
- "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
- "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
- "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
+ "url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
+ "hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
+ "hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"size": 274432,
"sparse": false,
"full_check": true,
"has_ab": true,
- "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
+ "ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee"
},
{
"name": "aop",
@@ -339,62 +339,62 @@
},
{
"name": "boot",
- "url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
- "hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
- "hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
- "size": 18515968,
+ "url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz",
+ "hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
+ "hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
+ "size": 17442816,
"sparse": false,
"full_check": true,
"has_ab": true,
- "ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
+ "ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a"
},
{
"name": "system",
- "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
- "hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
- "hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
- "size": 5368709120,
+ "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz",
+ "hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9",
+ "hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
+ "size": 4718592000,
"sparse": true,
"full_check": false,
"has_ab": true,
- "ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
+ "ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727",
"alt": {
- "hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
- "url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
- "size": 5368709120
+ "hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
+ "url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img",
+ "size": 4718592000
}
},
{
"name": "userdata_90",
- "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a.img.xz",
- "hash": "6a11d448bac50467791809339051eed2894aae971c37bf6284b3b972a99ba3ac",
- "hash_raw": "602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a",
+ "url": "https://commadist.azureedge.net/agnosupdate/userdata_90-b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9.img.xz",
+ "hash": "bea163e6fb6ac6224c7f32619affb5afb834cd859971b0cab6d8297dd0098f0a",
+ "hash_raw": "b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9",
"size": 96636764160,
"sparse": true,
"full_check": true,
"has_ab": false,
- "ondevice_hash": "e014d92940a696bf8582807259820ab73948b950656ed83a45da738f26083705"
+ "ondevice_hash": "f4841c6ae3207197886e5efbd50f44cc24822680d7b785fa2d2743c657f23287"
},
{
"name": "userdata_89",
- "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48.img.xz",
- "hash": "748e31a5fc01fc256c012e359c3382d1f98cce98feafe8ecc0fca3e47caef116",
- "hash_raw": "4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48",
+ "url": "https://commadist.azureedge.net/agnosupdate/userdata_89-3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167.img.xz",
+ "hash": "b5458a29dd7d4a4c9b7ad77b8baa5f804142ac78d97c6668839bf2a650e32518",
+ "hash_raw": "3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167",
"size": 95563022336,
"sparse": true,
"full_check": true,
"has_ab": false,
- "ondevice_hash": "c181b93050787adcfef730c086bcb780f28508d84e6376d9b80d37e5dc02b55e"
+ "ondevice_hash": "1dc10c542d3b019258fc08dc7dfdb49d9abad065e46d030b89bc1a2e0197f526"
},
{
"name": "userdata_30",
- "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668.img.xz",
- "hash": "09ff390e639e4373d772e1688d05a5ac77a573463ed1deeff86390686fa686f9",
- "hash_raw": "80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668",
+ "url": "https://commadist.azureedge.net/agnosupdate/userdata_30-1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c.img.xz",
+ "hash": "687d178cfc91be5d7e8aa1333405b610fdce01775b8333bd0985b81642b94eea",
+ "hash_raw": "1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c",
"size": 32212254720,
"sparse": true,
"full_check": true,
"has_ab": false,
- "ondevice_hash": "2c01ab470c02121c721ff6afc25582437e821686207f3afef659387afb69c507"
+ "ondevice_hash": "9ddbd1dae6ee7dc919f018364cf2f29dad138c9203c5a49aea0cbb9bf2e137e5"
}
]
\ No newline at end of file
diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py
index f6b29ec0ce..bfdcc6ddaf 100755
--- a/system/hardware/tici/amplifier.py
+++ b/system/hardware/tici/amplifier.py
@@ -61,18 +61,6 @@ BASE_CONFIG = [
]
CONFIGS = {
- "tici": [
- AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
- AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
- AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
- AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
-
- *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
- *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
- *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
- *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
- *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
- ],
"tizi": [
AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
diff --git a/system/hardware/tici/esim.py b/system/hardware/tici/esim.py
index b489286f50..391ba45531 100644
--- a/system/hardware/tici/esim.py
+++ b/system/hardware/tici/esim.py
@@ -40,6 +40,7 @@ class TiciLPA(LPABase):
self._process_notifications()
def download_profile(self, qr: str, nickname: str | None = None) -> None:
+ self._check_bootstrapped()
msgs = self._invoke('profile', 'download', '-a', qr)
self._validate_successful(msgs)
new_profile = next((m for m in msgs if m['payload']['message'] == 'es8p_meatadata_parse'), None)
@@ -54,6 +55,7 @@ class TiciLPA(LPABase):
self._validate_successful(self._invoke('profile', 'nickname', iccid, nickname))
def switch_profile(self, iccid: str) -> None:
+ self._check_bootstrapped()
self._validate_profile_exists(iccid)
latest = self.get_active_profile()
if latest and latest.iccid == iccid:
@@ -61,6 +63,33 @@ class TiciLPA(LPABase):
self._validate_successful(self._invoke('profile', 'enable', iccid))
self._process_notifications()
+ def bootstrap(self) -> None:
+ """
+ find all comma-provisioned profiles and delete them. they conflict with user-provisioned profiles
+ and must be deleted.
+
+ **note**: this is a **very** destructive operation. you **must** purchase a new comma SIM in order
+ to use comma prime again.
+ """
+ if self._is_bootstrapped():
+ return
+
+ for p in self.list_profiles():
+ if self.is_comma_profile(p.iccid):
+ self._disable_profile(p.iccid)
+ self.delete_profile(p.iccid)
+
+ def _disable_profile(self, iccid: str) -> None:
+ self._validate_successful(self._invoke('profile', 'disable', iccid))
+ self._process_notifications()
+
+ def _check_bootstrapped(self) -> None:
+ assert self._is_bootstrapped(), 'eUICC is not bootstrapped, please bootstrap before performing this operation'
+
+ def _is_bootstrapped(self) -> bool:
+ """ check if any comma provisioned profiles are on the eUICC """
+ return not any(self.is_comma_profile(iccid) for iccid in (p.iccid for p in self.list_profiles()))
+
def _invoke(self, *cmd: str):
proc = subprocess.Popen(['sudo', '-E', 'lpac'] + list(cmd), stdout=subprocess.PIPE, stderr=subprocess.PIPE, env=self.env)
try:
diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py
index 35f9916c31..0f50acdc38 100644
--- a/system/hardware/tici/hardware.py
+++ b/system/hardware/tici/hardware.py
@@ -425,9 +425,6 @@ class Tici(HardwareBase):
# pandad core
affine_irq(3, "spi_geni") # SPI
- if "tici" in self.get_device_type():
- affine_irq(3, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB)
- affine_irq(3, "xhci-hcd:usb1") # internal panda USB (also modem)
try:
pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip()
subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid])
@@ -446,22 +443,20 @@ class Tici(HardwareBase):
cmds = []
- if self.get_device_type() in ("tici", "tizi"):
+ if self.get_device_type() in ("tizi", ):
# clear out old blue prime initial APN
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn="')
cmds += [
+ # SIM hot swap
+ 'AT+QSIMDET=1,0',
+ 'AT+QSIMSTAT=1',
+
# configure modem as data-centric
'AT+QNVW=5280,0,"0102000000000000"',
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
]
- if self.get_device_type() == "tizi":
- # SIM hot swap, not routed on tici
- cmds += [
- 'AT+QSIMDET=1,0',
- 'AT+QSIMSTAT=1',
- ]
elif manufacturer == 'Cavli Inc.':
cmds += [
'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM
@@ -492,7 +487,7 @@ class Tici(HardwareBase):
# eSIM prime
dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
- if sim_id.startswith('8985235') and not os.path.exists(dest):
+ if self.get_sim_lpa().is_comma_profile(sim_id) and not os.path.exists(dest):
with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
dat = f.read()
dat = dat.replace("sim-id=", f"sim-id={sim_id}")
@@ -503,6 +498,14 @@ class Tici(HardwareBase):
os.system(f"sudo cp {tf.name} {dest}")
os.system(f"sudo nmcli con load {dest}")
+ def reboot_modem(self):
+ modem = self.get_modem()
+ for state in (0, 1):
+ try:
+ modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+ except Exception:
+ pass
+
def get_networks(self):
r = {}
diff --git a/system/hardware/tici/pins.py b/system/hardware/tici/pins.py
index 747278d1ec..bdbea591fb 100644
--- a/system/hardware/tici/pins.py
+++ b/system/hardware/tici/pins.py
@@ -1,5 +1,3 @@
-# TODO: these are also defined in a header
-
# GPIO pin definitions
class GPIO:
# both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
@@ -26,7 +24,4 @@ class GPIO:
CAM2_RSTN = 12
# Sensor interrupts
- BMX055_ACCEL_INT = 21
- BMX055_GYRO_INT = 23
- BMX055_MAGN_INT = 87
LSM_INT = 84
diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py
index db0fab884c..4fbde81673 100644
--- a/system/hardware/tici/tests/test_power_draw.py
+++ b/system/hardware/tici/tests/test_power_draw.py
@@ -31,9 +31,9 @@ class Proc:
PROCS = [
- Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
- Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']),
- Proc(['dmonitoringmodeld'], 0.6, msgs=['driverStateV2']),
+ Proc(['camerad'], 1.65, atol=0.4, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
+ Proc(['modeld'], 1.24, atol=0.2, msgs=['modelV2']),
+ Proc(['dmonitoringmodeld'], 0.65, atol=0.35, msgs=['driverStateV2']),
Proc(['encoderd'], 0.23, msgs=[]),
]
diff --git a/system/journald.py b/system/journald.py
new file mode 100755
index 0000000000..37158b9251
--- /dev/null
+++ b/system/journald.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+import json
+import subprocess
+
+import cereal.messaging as messaging
+from openpilot.common.swaglog import cloudlog
+
+
+def main():
+ pm = messaging.PubMaster(['androidLog'])
+ cmd = ['journalctl', '-f', '-o', 'json']
+ proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, text=True)
+ assert proc.stdout is not None
+ try:
+ for line in proc.stdout:
+ line = line.strip()
+ if not line:
+ continue
+ try:
+ kv = json.loads(line)
+ except json.JSONDecodeError:
+ cloudlog.exception("failed to parse journalctl output")
+ continue
+
+ msg = messaging.new_message('androidLog')
+ entry = msg.androidLog
+ entry.ts = int(kv.get('__REALTIME_TIMESTAMP', 0))
+ entry.message = json.dumps(kv)
+ if '_PID' in kv:
+ entry.pid = int(kv['_PID'])
+ if 'PRIORITY' in kv:
+ entry.priority = int(kv['PRIORITY'])
+ if 'SYSLOG_IDENTIFIER' in kv:
+ entry.tag = kv['SYSLOG_IDENTIFIER']
+
+ pm.send('androidLog', msg)
+ finally:
+ proc.terminate()
+ proc.wait()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/system/logcatd/.gitignore b/system/logcatd/.gitignore
deleted file mode 100644
index c66f7622d9..0000000000
--- a/system/logcatd/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-logcatd
diff --git a/system/logcatd/SConscript b/system/logcatd/SConscript
deleted file mode 100644
index 39c45d1093..0000000000
--- a/system/logcatd/SConscript
+++ /dev/null
@@ -1,3 +0,0 @@
-Import('env', 'messaging', 'common')
-
-env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[messaging, common, 'systemd'])
diff --git a/system/logcatd/logcatd_systemd.cc b/system/logcatd/logcatd_systemd.cc
deleted file mode 100644
index 54b3782132..0000000000
--- a/system/logcatd/logcatd_systemd.cc
+++ /dev/null
@@ -1,75 +0,0 @@
-#include
-
-#include
-#include
-#include