From 379b7cf8b6573a448ebd1eba4b64c2b588a74d81 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 4 Oct 2022 01:16:38 +0800 Subject: [PATCH 01/62] proclogd: fix wrong type for rss (#25923) rss is long --- system/proclogd/proclog.cc | 2 +- system/proclogd/proclog.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/system/proclogd/proclog.cc b/system/proclogd/proclog.cc index cbe3b53493..09ab4f559e 100644 --- a/system/proclogd/proclog.cc +++ b/system/proclogd/proclog.cc @@ -95,7 +95,7 @@ std::optional procStat(std::string stat) { .num_threads = stol(v[StatPos::num_threads - 1]), .starttime = stoull(v[StatPos::starttime - 1]), .vms = stoul(v[StatPos::vsize - 1]), - .rss = stoul(v[StatPos::rss - 1]), + .rss = stol(v[StatPos::rss - 1]), .processor = stoi(v[StatPos::processor - 1]), }; return p; diff --git a/system/proclogd/proclog.h b/system/proclogd/proclog.h index 9ed53d1bac..49f97cdd36 100644 --- a/system/proclogd/proclog.h +++ b/system/proclogd/proclog.h @@ -20,8 +20,8 @@ struct ProcCache { struct ProcStat { int pid, ppid, processor; char state; - long cutime, cstime, priority, nice, num_threads; - unsigned long utime, stime, vms, rss; + long cutime, cstime, priority, nice, num_threads, rss; + unsigned long utime, stime, vms; unsigned long long starttime; std::string name; }; From 50b8fc19b692d0d4d0ef2dc9161fef6cf9d3013d Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Mon, 3 Oct 2022 13:48:56 -0500 Subject: [PATCH 02/62] VW PQ: Use correct brake signal (#25952) --- selfdrive/car/volkswagen/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index cf4a252b65..3e99ca8252 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -170,7 +170,7 @@ class CarState(CarStateBase): ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"]) + ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremslichtschalter"]) ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) # Update gear and/or clutch position data. From 6393d29b0437417fe15cbc9eca5c87534531e9bd Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 3 Oct 2022 12:26:12 -0700 Subject: [PATCH 03/62] pre-commit: test translations (#25695) * add test translations to precommit * fix test translations pre-commit hook * revert * fix that * add to release files add to release files * fix * don't run test on stripped dir * fix --- .github/workflows/selfdrive_tests.yaml | 2 +- .pre-commit-config.yaml | 7 +++++++ release/files_common | 2 ++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 9606c05631..d4971a4339 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -62,7 +62,7 @@ jobs: cp .pylintrc $STRIPPED_DIR cp mypy.ini $STRIPPED_DIR cd $STRIPPED_DIR - ${{ env.RUN }} "pre-commit run --all" + ${{ env.RUN }} "SKIP=test_translations pre-commit run --all" build_all: name: build all diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b1696e823c..25b8490f92 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -70,3 +70,10 @@ repos: - --quiet - --force - -j8 +- repo: local + hooks: + - id: test_translations + name: test translations + entry: selfdrive/ui/tests/test_translations.py + language: script + pass_filenames: false diff --git a/release/files_common b/release/files_common index 5783edd070..0c341eb3f6 100644 --- a/release/files_common +++ b/release/files_common @@ -305,6 +305,8 @@ selfdrive/ui/soundd/soundd selfdrive/ui/soundd/.gitignore selfdrive/ui/translations/*.ts selfdrive/ui/translations/languages.json +selfdrive/ui/update_translations.py +selfdrive/ui/tests/test_translations.py selfdrive/ui/qt/*.cc selfdrive/ui/qt/*.h From 2a0ce3e8b871b4e58b01b56a37bb8cfaf77d8b0a Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 3 Oct 2022 13:10:35 -0700 Subject: [PATCH 04/62] direct model feedback issues to GitHub discussions (#25953) * add links to give model feedback in github discussions * use correct link --- .github/ISSUE_TEMPLATE/bug_report.yml | 1 + .github/ISSUE_TEMPLATE/config.yml | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index b1a14076ea..47eb6c216f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -8,6 +8,7 @@ body: value: > Before creating a **bug report**, please check the following: * If the issue likely only affects your car model or make, go back and open a **car bug report** instead. + * If the issue is related to the driving or driver monitoring models, you should open a [discussion](https://github.com/commaai/openpilot/discussions/categories/model-feedback) instead. * Ensure you're running the latest openpilot release. * Ensure you're using officially supported hardware. Issues running on PCs have a different issue template. * Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 2c2deb17ba..45a8af0aaf 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -1,8 +1,11 @@ blank_issues_enabled: false contact_links: + - name: Report model bugs + url: https://github.com/commaai/openpilot/discussions/categories/model-feedback + about: Provide feedback for the driving or driver monitoring models - name: Discussions url: https://github.com/commaai/openpilot/discussions - about: For questions and discussion about openpilot + about: For questions and general discussion about openpilot - name: Community Wiki url: https://github.com/commaai/openpilot/wiki about: Check out our community wiki From bc7be114d838aafc54a5e480dc969c76552e7f26 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 3 Oct 2022 13:58:40 -0700 Subject: [PATCH 05/62] UI: remove unused tap detection constant (#25956) --- selfdrive/ui/ui.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 887b7ee841..d7e51ccfeb 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -155,9 +155,6 @@ public: Device(QObject *parent = 0); private: - // auto brightness - const float accel_samples = 5*UI_FREQ; - bool awake = false; int interactive_timeout = 0; bool ignition_on = false; From 1b8324af876e66630b5f4e50623e3136a39f6ecb Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 4 Oct 2022 06:19:42 +0800 Subject: [PATCH 06/62] c++ cabana: Initial version (#25946) * draft * continue * fix QChart unresponsive with large points * build with --extras * add filter * save DBC button * more buttons * add flag to use qcamera * stop replay in dctor * README * use getMsg * video control * edit signal * add colors * correct ts * add/edit signals * use bus:address as key --- SConstruct | 4 + selfdrive/ui/SConscript | 1 + tools/cabana/.gitignore | 4 + tools/cabana/README | 9 + tools/cabana/SConscript | 20 ++ tools/cabana/cabana | 4 + tools/cabana/cabana.cc | 34 +++ tools/cabana/chartswidget.cc | 102 ++++++++ tools/cabana/chartswidget.h | 34 +++ tools/cabana/detailwidget.cc | 458 +++++++++++++++++++++++++++++++++ tools/cabana/detailwidget.h | 102 ++++++++ tools/cabana/mainwin.cc | 38 +++ tools/cabana/mainwin.h | 22 ++ tools/cabana/messageswidget.cc | 94 +++++++ tools/cabana/messageswidget.h | 24 ++ tools/cabana/parser.cc | 98 +++++++ tools/cabana/parser.h | 66 +++++ tools/cabana/videowidget.cc | 80 ++++++ tools/cabana/videowidget.h | 17 ++ tools/replay/SConscript | 1 + tools/replay/replay.cc | 2 +- tools/replay/replay.h | 4 + tools/ubuntu_setup.sh | 1 + 23 files changed, 1218 insertions(+), 1 deletion(-) create mode 100644 tools/cabana/.gitignore create mode 100644 tools/cabana/README create mode 100644 tools/cabana/SConscript create mode 100755 tools/cabana/cabana create mode 100644 tools/cabana/cabana.cc create mode 100644 tools/cabana/chartswidget.cc create mode 100644 tools/cabana/chartswidget.h create mode 100644 tools/cabana/detailwidget.cc create mode 100644 tools/cabana/detailwidget.h create mode 100644 tools/cabana/mainwin.cc create mode 100644 tools/cabana/mainwin.h create mode 100644 tools/cabana/messageswidget.cc create mode 100644 tools/cabana/messageswidget.h create mode 100644 tools/cabana/parser.cc create mode 100644 tools/cabana/parser.h create mode 100644 tools/cabana/videowidget.cc create mode 100644 tools/cabana/videowidget.h diff --git a/SConstruct b/SConstruct index 178b0cc872..e015218f2a 100644 --- a/SConstruct +++ b/SConstruct @@ -433,6 +433,10 @@ SConscript(['selfdrive/navd/SConscript']) SConscript(['tools/replay/SConscript']) +opendbc = abspath([File('opendbc/can/libdbc.so')]) +Export('opendbc') +SConscript(['tools/cabana/SConscript']) + if GetOption('test'): SConscript('panda/tests/safety/SConscript') diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 92f6578dfc..84e055752a 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -32,6 +32,7 @@ if maps: qt_env['CPPDEFINES'] += ["ENABLE_MAPS"] widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) +Export('widgets') qt_libs = [widgets, qt_util] + base_libs # build assets diff --git a/tools/cabana/.gitignore b/tools/cabana/.gitignore new file mode 100644 index 0000000000..0c21d5530d --- /dev/null +++ b/tools/cabana/.gitignore @@ -0,0 +1,4 @@ +moc_* +*.moc + +_cabana diff --git a/tools/cabana/README b/tools/cabana/README new file mode 100644 index 0000000000..f64e6b2d2d --- /dev/null +++ b/tools/cabana/README @@ -0,0 +1,9 @@ +# Cabana + + + +Cabana is a tool developed to view raw CAN data. One use for this is creating and editing [CAN Dictionaries](http://socialledge.com/sjsu/index.php/DBC_Format) (DBC files), and the tool provides direct integration with [commaai/opendbc](https://github.com/commaai/opendbc) (a collection of DBC files), allowing you to load the DBC files direct from source, and save to your fork. In addition, you can load routes from [comma connect](https://connect.comma.ai). + +## Usage Instructions + +See [openpilot wiki](https://github.com/commaai/openpilot/wiki/Cabana) diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript new file mode 100644 index 0000000000..f32ee166b6 --- /dev/null +++ b/tools/cabana/SConscript @@ -0,0 +1,20 @@ +import os +Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', + 'cereal', 'transformations', 'widgets', 'replay_lib', 'opendbc') + +base_frameworks = qt_env['FRAMEWORKS'] +base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', + 'capnp', 'kj', 'm', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] + +if arch == "Darwin": + base_frameworks.append('OpenCL') +else: + base_libs.append('OpenCL') + +qt_libs = ['qt_util', 'Qt5Charts'] + base_libs +if arch in ['x86_64', 'Darwin'] and GetOption('extras'): + qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] + + # qt_env["LD_LIBRARY_PATH"] = [Dir(f"#opendbc/can").abspath] + cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs + qt_env.Program('_cabana', ['cabana.cc', 'mainwin.cc', 'chartswidget.cc', 'videowidget.cc', 'parser.cc', 'messageswidget.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) diff --git a/tools/cabana/cabana b/tools/cabana/cabana new file mode 100755 index 0000000000..b29dd66e3d --- /dev/null +++ b/tools/cabana/cabana @@ -0,0 +1,4 @@ +#!/bin/sh +cd "$(dirname "$0")" +export LD_LIBRARY_PATH="../../opendbc/can:$LD_LIBRARY_PATH" +exec ./_cabana "$1" diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc new file mode 100644 index 0000000000..0adc744b49 --- /dev/null +++ b/tools/cabana/cabana.cc @@ -0,0 +1,34 @@ +#include +#include + +#include "selfdrive/ui/qt/util.h" +#include "tools/cabana/mainwin.h" + +const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; +Parser *parser = nullptr; + +int main(int argc, char *argv[]) { + initApp(argc, argv); + QApplication app(argc, argv); + + QCommandLineParser cmd_parser; + cmd_parser.addHelpOption(); + cmd_parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai"); + cmd_parser.addOption({"demo", "use a demo route instead of providing your own"}); + cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); + cmd_parser.addOption({"qcam", "use qcamera"}); + cmd_parser.process(app); + const QStringList args = cmd_parser.positionalArguments(); + if (args.empty() && !cmd_parser.isSet("demo")) { + cmd_parser.showHelp(); + } + + const QString route = args.empty() ? DEMO_ROUTE : args.first(); + parser = new Parser(&app); + if (!parser->loadRoute(route, cmd_parser.value("data_dir"), cmd_parser.isSet("qcam"))) { + return 0; + } + MainWindow w; + w.showMaximized(); + return app.exec(); +} diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc new file mode 100644 index 0000000000..a8fe39968e --- /dev/null +++ b/tools/cabana/chartswidget.cc @@ -0,0 +1,102 @@ +#include "tools/cabana/chartswidget.h" + +#include + +using namespace QtCharts; + +int64_t get_raw_value(const QByteArray &msg, const Signal &sig) { + int64_t ret = 0; + + int i = sig.msb / 8; + int bits = sig.size; + while (i >= 0 && i < msg.size() && bits > 0) { + int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i * 8; + int msb = (int)(sig.msb / 8) == i ? sig.msb : (i + 1) * 8 - 1; + int size = msb - lsb + 1; + + uint64_t d = (msg[i] >> (lsb - (i * 8))) & ((1ULL << size) - 1); + ret |= d << (bits - size); + + bits -= size; + i = sig.is_little_endian ? i - 1 : i + 1; + } + return ret; +} + +ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { + main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + connect(parser, &Parser::updated, this, &ChartsWidget::updateState); + connect(parser, &Parser::showPlot, this, &ChartsWidget::addChart); + connect(parser, &Parser::hidePlot, this, &ChartsWidget::removeChart); +} + +void ChartsWidget::addChart(const QString &id, const QString &sig_name) { + const QString char_name = id + sig_name; + if (charts.find(char_name) == charts.end()) { + QLineSeries *series = new QLineSeries(); + series->setUseOpenGL(true); + auto chart = new QChart(); + chart->setTitle(id + ": " + sig_name); + chart->addSeries(series); + chart->createDefaultAxes(); + chart->legend()->hide(); + auto chart_view = new QChartView(chart); + chart_view->setMinimumSize({width(), 300}); + chart_view->setMaximumSize({width(), 300}); + chart_view->setRenderHint(QPainter::Antialiasing); + main_layout->addWidget(chart_view); + charts[char_name] = {.id = id, .sig_name = sig_name, .chart_view = chart_view}; + } +} + +void ChartsWidget::removeChart(const QString &id, const QString &sig_name) { + auto it = charts.find(id + sig_name); + if (it == charts.end()) return; + + delete it->second.chart_view; + charts.erase(it); +} + +void ChartsWidget::updateState() { + static double last_update = millis_since_boot(); + double current_ts = millis_since_boot(); + bool update = (current_ts - last_update) > 500; + if (update) { + last_update = current_ts; + } + + auto getSig = [=](const QString &id, const QString &name) -> const Signal * { + for (auto &sig : parser->getMsg(id)->sigs) { + if (name == sig.name.c_str()) return &sig; + } + return nullptr; + }; + + for (auto &[_, c] : charts) { + if (auto sig = getSig(c.id, c.sig_name)) { + const auto &can_data = parser->can_msgs[c.id].back(); + int64_t val = get_raw_value(can_data.dat, *sig); + if (sig->is_signed) { + val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0; + } + double value = val * sig->factor + sig->offset; + + if (value > c.max_y) c.max_y = value; + if (value < c.min_y) c.min_y = value; + + while (c.data.size() > DATA_LIST_SIZE) { + c.data.pop_front(); + } + c.data.push_back({can_data.ts / 1000., value}); + + if (update) { + QChart *chart = c.chart_view->chart(); + QLineSeries *series = (QLineSeries *)chart->series()[0]; + series->replace(c.data); + chart->axisX()->setRange(c.data.front().x(), c.data.back().x()); + chart->axisY()->setRange(c.min_y, c.max_y); + } + } + } +} diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h new file mode 100644 index 0000000000..7bc8335a32 --- /dev/null +++ b/tools/cabana/chartswidget.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "tools/cabana/parser.h" + +class ChartsWidget : public QWidget { + Q_OBJECT + + public: + ChartsWidget(QWidget *parent = nullptr); + inline bool hasChart(const QString &id, const QString &sig_name) { + return charts.find(id+sig_name) != charts.end(); + } + void addChart(const QString &id, const QString &sig_name); + void removeChart(const QString &id, const QString &sig_name); + void updateState(); + + protected: + QVBoxLayout *main_layout; + struct SignalChart { + QString id; + QString sig_name; + int max_y = 0; + int min_y = 0; + QList data; + QtCharts::QChartView *chart_view = nullptr; + }; + std::map charts; +}; diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc new file mode 100644 index 0000000000..6799376577 --- /dev/null +++ b/tools/cabana/detailwidget.cc @@ -0,0 +1,458 @@ +#include "tools/cabana/detailwidget.h" + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/scrollview.h" + +const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; + +static QVector BIG_ENDIAN_START_BITS = []() { + QVector ret; + for (int i = 0; i < 64; i++) { + for (int j = 7; j >= 0; j--) { + ret.push_back(j + i * 8); + } + } + return ret; +}(); + +static int bigEndianBitIndex(int index) { + // TODO: Add a helper function in dbc.h + return BIG_ENDIAN_START_BITS.indexOf(index); +} + +DetailWidget::DetailWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + QLabel *title = new QLabel(tr("SELECTED MESSAGE:"), this); + main_layout->addWidget(title); + + QHBoxLayout *name_layout = new QHBoxLayout(); + name_label = new QLabel(this); + name_label->setStyleSheet("font-weight:bold;"); + name_layout->addWidget(name_label); + name_layout->addStretch(); + edit_btn = new QPushButton(tr("Edit"), this); + edit_btn->setVisible(false); + QObject::connect(edit_btn, &QPushButton::clicked, [=]() { + EditMessageDialog dlg(msg_id, this); + int ret = dlg.exec(); + if (ret) { + setMsg(msg_id); + } + }); + name_layout->addWidget(edit_btn); + main_layout->addLayout(name_layout); + + binary_view = new BinaryView(this); + main_layout->addWidget(binary_view); + + QHBoxLayout *signals_layout = new QHBoxLayout(); + signals_layout->addWidget(new QLabel(tr("Signals"))); + signals_layout->addStretch(); + add_sig_btn = new QPushButton(tr("Add signal"), this); + add_sig_btn->setVisible(false); + QObject::connect(add_sig_btn, &QPushButton::clicked, [=]() { + AddSignalDialog dlg(msg_id, this); + int ret = dlg.exec(); + if (ret) { + setMsg(msg_id); + } + }); + signals_layout->addWidget(add_sig_btn); + main_layout->addLayout(signals_layout); + + QWidget *container = new QWidget(this); + QVBoxLayout *container_layout = new QVBoxLayout(container); + signal_edit_layout = new QVBoxLayout(); + signal_edit_layout->setSpacing(2); + container_layout->addLayout(signal_edit_layout); + + messages_view = new MessagesView(this); + container_layout->addWidget(messages_view); + + QScrollArea *scroll = new QScrollArea(this); + scroll->setWidget(container); + scroll->setWidgetResizable(true); + scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + + main_layout->addWidget(scroll); + setFixedWidth(600); + + connect(parser, &Parser::updated, this, &DetailWidget::updateState); +} + +void DetailWidget::updateState() { + if (msg_id.isEmpty()) return; + + auto &list = parser->can_msgs[msg_id]; + if (!list.empty()) { + binary_view->setData(list.back().dat); + messages_view->setMessages(list); + } +} + +SignalForm::SignalForm(const Signal &sig, QWidget *parent) : QWidget(parent) { + QVBoxLayout *v_layout = new QVBoxLayout(this); + + QHBoxLayout *h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Name"))); + name = new QLineEdit(sig.name.c_str()); + h->addWidget(name); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Size"))); + size = new QSpinBox(); + size->setValue(sig.size); + h->addWidget(size); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Most significant bit"))); + msb = new QSpinBox(); + msb->setValue(sig.msb); + h->addWidget(msb); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Endianness"))); + endianness = new QComboBox(); + endianness->addItems({"Little", "Big"}); + endianness->setCurrentIndex(sig.is_little_endian ? 0 : 1); + h->addWidget(endianness); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("sign"))); + sign = new QComboBox(); + sign->addItems({"Signed", "Unsigned"}); + sign->setCurrentIndex(sig.is_signed ? 0 : 1); + h->addWidget(sign); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Factor"))); + factor = new QSpinBox(); + factor->setValue(sig.factor); + h->addWidget(factor); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Offset"))); + offset = new QSpinBox(); + offset->setValue(sig.offset); + h->addWidget(offset); + v_layout->addLayout(h); + + // TODO: parse the following parameters in opendbc + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Unit"))); + unit = new QLineEdit(); + h->addWidget(unit); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Comment"))); + comment = new QLineEdit(); + h->addWidget(comment); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Minimum value"))); + min_val = new QSpinBox(); + h->addWidget(min_val); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Maximum value"))); + max_val = new QSpinBox(); + h->addWidget(max_val); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Value descriptions"))); + val_desc = new QLineEdit(); + h->addWidget(val_desc); + v_layout->addLayout(h); +} + +std::optional SignalForm::getSignal() { + Signal sig = {}; + sig.name = name->text().toStdString(); + sig.size = size->text().toInt(); + sig.offset = offset->text().toDouble(); + sig.factor = factor->text().toDouble(); + sig.msb = msb->text().toInt(); + sig.is_signed = sign->currentIndex() == 0; + sig.is_little_endian = endianness->currentIndex() == 0; + if (sig.is_little_endian) { + sig.lsb = sig.start_bit; + sig.msb = sig.start_bit + sig.size - 1; + } else { + sig.lsb = BIG_ENDIAN_START_BITS[bigEndianBitIndex(sig.start_bit) + sig.size - 1]; + sig.msb = sig.start_bit; + } + return (sig.name.empty() || sig.size <= 0) ? std::nullopt : std::optional(sig); +} + +void DetailWidget::setMsg(const QString &id) { + msg_id = id; + QString name = tr("untitled"); + + for (auto edit : signal_edit) { + delete edit; + } + signal_edit.clear(); + int i = 0; + auto msg = parser->getMsg(id); + if (msg) { + for (auto &s : msg->sigs) { + SignalEdit *edit = new SignalEdit(id, s, i++, this); + connect(edit, &SignalEdit::removed, [=]() { + QTimer::singleShot(0, [=]() { setMsg(id); }); + }); + signal_edit_layout->addWidget(edit); + signal_edit.push_back(edit); + } + name = msg->name.c_str(); + } + name_label->setText(name); + binary_view->setMsg(msg_id); + + edit_btn->setVisible(true); + add_sig_btn->setVisible(msg != nullptr); +} + +SignalEdit::SignalEdit(const QString &id, const Signal &sig, int idx, QWidget *parent) : id(id), name_(sig.name.c_str()), QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + + // title + QHBoxLayout *title_layout = new QHBoxLayout(); + QLabel *icon = new QLabel(">"); + icon->setStyleSheet("font-weight:bold"); + title_layout->addWidget(icon); + title = new ElidedLabel(this); + title->setText(sig.name.c_str()); + title->setStyleSheet(QString("font-weight:bold; color:%1").arg(SIGNAL_COLORS[idx % std::size(SIGNAL_COLORS)])); + connect(title, &ElidedLabel::clicked, [=]() { + edit_container->isVisible() ? edit_container->hide() : edit_container->show(); + icon->setText(edit_container->isVisible() ? "▼" : ">"); + }); + title_layout->addWidget(title); + title_layout->addStretch(); + QPushButton *show_plot = new QPushButton(tr("Show Plot")); + QObject::connect(show_plot, &QPushButton::clicked, [=]() { + if (show_plot->text() == tr("Show Plot")) { + emit parser->showPlot(id, name_); + show_plot->setText(tr("Hide Plot")); + } else { + emit parser->hidePlot(id, name_); + show_plot->setText(tr("Show Plot")); + } + }); + title_layout->addWidget(show_plot); + main_layout->addLayout(title_layout); + + edit_container = new QWidget(this); + QVBoxLayout *v_layout = new QVBoxLayout(edit_container); + form = new SignalForm(sig, this); + v_layout->addWidget(form); + + QHBoxLayout *h = new QHBoxLayout(); + remove_btn = new QPushButton(tr("Remove Signal")); + QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); + h->addWidget(remove_btn); + h->addStretch(); + QPushButton *save_btn = new QPushButton(tr("Save")); + QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); + h->addWidget(save_btn); + v_layout->addLayout(h); + + edit_container->setVisible(false); + main_layout->addWidget(edit_container); +} + +void SignalEdit::save() { + Msg *msg = const_cast(parser->getMsg(id)); + if (!msg) return; + + for (auto &sig : msg->sigs) { + if (name_ == sig.name.c_str()) { + if (auto s = form->getSignal()) { + sig = *s; + } + break; + } + } +} + +void SignalEdit::remove() { + QMessageBox msgbox; + msgbox.setText(tr("Remove signal")); + msgbox.setInformativeText(tr("Are you sure you want to remove signal '%1'").arg(name_)); + msgbox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); + msgbox.setDefaultButton(QMessageBox::Cancel); + if (msgbox.exec()) { + parser->removeSignal(id, name_); + emit removed(); + } +} + +BinaryView::BinaryView(QWidget *parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + table = new QTableWidget(this); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->horizontalHeader()->hide(); + table->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + table->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + main_layout->addWidget(table); + table->setColumnCount(9); + setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); +} + +void BinaryView::setMsg(const QString &id) { + auto msg = parser->getMsg(Parser::addressFromId(id)); + int row_count = msg ? msg->size : parser->can_msgs[id].back().dat.size(); + + table->setRowCount(row_count); + table->setColumnCount(9); + for (int i = 0; i < table->rowCount(); ++i) { + for (int j = 0; j < table->columnCount(); ++j) { + auto item = new QTableWidgetItem(); + item->setTextAlignment(Qt::AlignCenter); + if (j == 8) { + QFont font; + font.setBold(true); + item->setFont(font); + } + table->setItem(i, j, item); + } + } + + if (msg) { + for (int i = 0; i < msg->sigs.size(); ++i) { + const auto &sig = msg->sigs[i]; + int start = sig.is_little_endian ? sig.start_bit : bigEndianBitIndex(sig.start_bit); + for (int j = start; j <= start + sig.size - 1; ++j) { + table->item(j / 8, j % 8)->setBackground(QColor(SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)])); + } + } + } + + setFixedHeight(table->rowHeight(0) * table->rowCount() + 25); + if (!parser->can_msgs.empty()) { + setData(parser->can_msgs[id].back().dat); + } +} + +void BinaryView::setData(const QByteArray &binary) { + std::string s; + for (int j = 0; j < binary.size(); ++j) { + s += std::bitset<8>(binary[j]).to_string(); + } + + char hex[3] = {'\0'}; + for (int i = 0; i < binary.size(); ++i) { + for (int j = 0; j < 8; ++j) { + table->item(i, j)->setText(QChar(s[i * 8 + j])); + } + sprintf(&hex[0], "%02X", (unsigned char)binary[i]); + table->item(i, 8)->setText(hex); + } +} + +MessagesView::MessagesView(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + QLabel *title = new QLabel("MESSAGE TIME BYTES"); + main_layout->addWidget(title); + + message_layout = new QVBoxLayout(); + main_layout->addLayout(message_layout); + main_layout->addStretch(); +} + +void MessagesView::setMessages(const std::list &list) { + auto begin = list.begin(); + std::advance(begin, std::max(0, (int)(list.size() - 100))); + int j = 0; + for (auto it = begin; it != list.end(); ++it) { + QLabel *label; + if (j >= messages.size()) { + label = new QLabel(); + message_layout->addWidget(label); + messages.push_back(label); + } else { + label = messages[j]; + } + label->setText(it->hex_dat); + ++j; + } +} + +EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : QDialog(parent) { + setWindowTitle(tr("Edit message")); + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->addWidget(new QLabel(tr("ID: (%1)").arg(id))); + + auto msg = const_cast(parser->getMsg(Parser::addressFromId(id))); + QHBoxLayout *h_layout = new QHBoxLayout(); + h_layout->addWidget(new QLabel(tr("Name"))); + h_layout->addStretch(); + QLineEdit *name_edit = new QLineEdit(this); + name_edit->setText(msg ? msg->name.c_str() : "untitled"); + h_layout->addWidget(name_edit); + main_layout->addLayout(h_layout); + + h_layout = new QHBoxLayout(); + h_layout->addWidget(new QLabel(tr("Size"))); + h_layout->addStretch(); + QSpinBox *size_spin = new QSpinBox(this); + size_spin->setValue(msg ? msg->size : parser->can_msgs[id].back().dat.size()); + h_layout->addWidget(size_spin); + main_layout->addLayout(h_layout); + + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + main_layout->addWidget(buttonBox); + + connect(buttonBox, &QDialogButtonBox::accepted, [=]() { + if (size_spin->value() <= 0 || name_edit->text().isEmpty()) return; + + if (msg) { + msg->name = name_edit->text().toStdString(); + msg->size = size_spin->value(); + } else { + Msg m = {}; + m.address = Parser::addressFromId(id); + m.name = name_edit->text().toStdString(); + m.size = size_spin->value(); + parser->addNewMsg(m); + } + QDialog::accept(); + }); + connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); +} + +AddSignalDialog::AddSignalDialog(const QString &id, QWidget *parent) : QDialog(parent) { + setWindowTitle(tr("Add signal to %1").arg(parser->getMsg(id)->name.c_str())); + QVBoxLayout *main_layout = new QVBoxLayout(this); + Signal sig = {.name = "untitled"}; + auto form = new SignalForm(sig, this); + main_layout->addWidget(form); + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + main_layout->addWidget(buttonBox); + connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); + connect(buttonBox, &QDialogButtonBox::accepted, [=]() { + if (auto msg = const_cast(parser->getMsg(id))) { + if (auto signal = form->getSignal()) { + msg->sigs.push_back(*signal); + } + } + QDialog::accept(); + }); +} diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h new file mode 100644 index 0000000000..70f2804f70 --- /dev/null +++ b/tools/cabana/detailwidget.h @@ -0,0 +1,102 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "opendbc/can/common.h" +#include "opendbc/can/common_dbc.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/parser.h" + +class SignalForm : public QWidget { + Q_OBJECT + + public: + SignalForm(const Signal &sig, QWidget *parent); + std::optional getSignal(); + QLineEdit *name, *unit, *comment, *val_desc; + QSpinBox *size, *msb, *lsb, *factor, *offset, *min_val, *max_val; + QComboBox *sign, *endianness; +}; + +class MessagesView : public QWidget { + Q_OBJECT + + public: + MessagesView(QWidget *parent); + void setMessages(const std::list &data); + std::vector messages; + QVBoxLayout *message_layout; +}; + +class BinaryView : public QWidget { + Q_OBJECT + + public: + BinaryView(QWidget *parent); + void setMsg(const QString &id); + void setData(const QByteArray &binary); + + QTableWidget *table; +}; + +class SignalEdit : public QWidget { + Q_OBJECT + + public: + SignalEdit(const QString &id, const Signal &sig, int idx, QWidget *parent); + void save(); + +signals: + void removed(); + protected: + void remove(); + QString id; + QString name_; + ElidedLabel *title; + SignalForm *form; + QWidget *edit_container; + QPushButton *remove_btn; +}; + +class DetailWidget : public QWidget { + Q_OBJECT + public: + DetailWidget(QWidget *parent); + void setMsg(const QString &id); + + public slots: + void updateState(); + + protected: + QLabel *name_label = nullptr; + QPushButton *edit_btn, *add_sig_btn; + QVBoxLayout *signal_edit_layout; + Signal *sig = nullptr; + MessagesView *messages_view; + QString msg_id; + BinaryView *binary_view; + std::vector signal_edit; +}; + +class EditMessageDialog : public QDialog { + Q_OBJECT + + public: + EditMessageDialog(const QString &id, QWidget *parent); +}; + +class AddSignalDialog : public QDialog { + Q_OBJECT + + public: + AddSignalDialog(const QString &id, QWidget *parent); +}; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc new file mode 100644 index 0000000000..d1b0d98f5f --- /dev/null +++ b/tools/cabana/mainwin.cc @@ -0,0 +1,38 @@ +#include "tools/cabana/mainwin.h" + +#include +#include + +MainWindow::MainWindow() : QWidget() { + assert(parser != nullptr); + + QVBoxLayout *main_layout = new QVBoxLayout(this); + QHBoxLayout *h_layout = new QHBoxLayout(); + main_layout->addLayout(h_layout); + + messages_widget = new MessagesWidget(this); + QObject::connect(messages_widget, &MessagesWidget::msgChanged, [=](const QString &id) { + detail_widget->setMsg(id); + }); + h_layout->addWidget(messages_widget); + + detail_widget = new DetailWidget(this); + h_layout->addWidget(detail_widget); + + // right widget + QWidget *right_container = new QWidget(this); + right_container->setFixedWidth(640); + QVBoxLayout *r_layout = new QVBoxLayout(right_container); + video_widget = new VideoWidget(this); + r_layout->addWidget(video_widget); + + charts_widget = new ChartsWidget(this); + QScrollArea *scroll = new QScrollArea(this); + scroll->setWidget(charts_widget); + scroll->setWidgetResizable(true); + scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + r_layout->addWidget(scroll); + + h_layout->addWidget(right_container); +} diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h new file mode 100644 index 0000000000..f19c48297e --- /dev/null +++ b/tools/cabana/mainwin.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "tools/cabana/chartswidget.h" +#include "tools/cabana/detailwidget.h" +#include "tools/cabana/messageswidget.h" +#include "tools/cabana/parser.h" +#include "tools/cabana/videowidget.h" + +class MainWindow : public QWidget { + Q_OBJECT + + public: + MainWindow(); + + protected: + VideoWidget *video_widget; + MessagesWidget *messages_widget; + DetailWidget *detail_widget; + ChartsWidget *charts_widget; +}; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc new file mode 100644 index 0000000000..d81d3e9ede --- /dev/null +++ b/tools/cabana/messageswidget.cc @@ -0,0 +1,94 @@ +#include "tools/cabana/messageswidget.h" + +#include +#include +#include +#include +#include +#include + +MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + + QHBoxLayout *dbc_file_layout = new QHBoxLayout(); + QComboBox *combo = new QComboBox(this); + auto dbc_names = get_dbc_names(); + for (const auto &name : dbc_names) { + combo->addItem(QString::fromStdString(name)); + } + connect(combo, &QComboBox::currentTextChanged, [=](const QString &dbc) { + parser->openDBC(dbc); + }); + // For test purpose + combo->setCurrentText("toyota_nodsu_pt_generated"); + dbc_file_layout->addWidget(combo); + + dbc_file_layout->addStretch(); + QPushButton *save_btn = new QPushButton(tr("Save DBC"), this); + QObject::connect(save_btn, &QPushButton::clicked, [=]() { + // TODO: save DBC to file + }); + dbc_file_layout->addWidget(save_btn); + + main_layout->addLayout(dbc_file_layout); + + filter = new QLineEdit(this); + filter->setPlaceholderText(tr("filter messages")); + main_layout->addWidget(filter); + + table_widget = new QTableWidget(this); + table_widget->setSelectionBehavior(QAbstractItemView::SelectRows); + table_widget->setSelectionMode(QAbstractItemView::SingleSelection); + table_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); + table_widget->setColumnCount(4); + table_widget->setColumnWidth(0, 250); + table_widget->setColumnWidth(1, 80); + table_widget->setColumnWidth(2, 80); + table_widget->setHorizontalHeaderLabels({tr("Name"), tr("ID"), tr("Count"), tr("Bytes")}); + table_widget->horizontalHeader()->setStretchLastSection(true); + QObject::connect(table_widget, &QTableWidget::itemSelectionChanged, [=]() { + auto id = table_widget->selectedItems()[0]->data(Qt::UserRole); + emit msgChanged(id.toString()); + }); + main_layout->addWidget(table_widget); + + connect(parser, &Parser::updated, this, &MessagesWidget::updateState); +} + +void MessagesWidget::updateState() { + auto getTableItem = [=](int row, int col) -> QTableWidgetItem * { + auto item = table_widget->item(row, col); + if (!item) { + item = new QTableWidgetItem(); + table_widget->setItem(row, col, item); + } + return item; + }; + + table_widget->setRowCount(parser->can_msgs.size()); + int i = 0; + const QString filter_str = filter->text().toLower(); + for (const auto &[id, list] : parser->can_msgs) { + assert(!list.empty()); + + QString name; + if (auto msg = parser->getMsg(list.back().address)) { + name = msg->name.c_str(); + } else { + name = tr("untitled"); + } + if (!filter_str.isEmpty() && !name.toLower().contains(filter_str)) { + table_widget->hideRow(i++); + continue; + } + + auto item = getTableItem(i, 0); + item->setText(name); + item->setData(Qt::UserRole, id); + getTableItem(i, 1)->setText(id); + getTableItem(i, 2)->setText(QString("%1").arg(parser->counters[id])); + getTableItem(i, 3)->setText(list.back().hex_dat); + table_widget->showRow(i); + i++; + } +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h new file mode 100644 index 0000000000..dca725199d --- /dev/null +++ b/tools/cabana/messageswidget.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include "tools/cabana/parser.h" + +class MessagesWidget : public QWidget { + Q_OBJECT + + public: + MessagesWidget(QWidget *parent); + + public slots: + void updateState(); + + signals: + void msgChanged(const QString &id); + + protected: + QLineEdit *filter; + QTableWidget *table_widget; +}; diff --git a/tools/cabana/parser.cc b/tools/cabana/parser.cc new file mode 100644 index 0000000000..481f0dfbdf --- /dev/null +++ b/tools/cabana/parser.cc @@ -0,0 +1,98 @@ +#include "tools/cabana/parser.h" + +#include + +#include "cereal/messaging/messaging.h" + +Parser::Parser(QObject *parent) : QObject(parent) { + qRegisterMetaType>(); + QObject::connect(this, &Parser::received, this, &Parser::process, Qt::QueuedConnection); + + thread = new QThread(); + connect(thread, &QThread::started, [=]() { recvThread(); }); + QObject::connect(thread, &QThread::finished, thread, &QThread::deleteLater); + thread->start(); +} + +Parser::~Parser() { + replay->stop(); + exit = true; + thread->quit(); + thread->wait(); +} + +bool Parser::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) { + replay = new Replay(route, {"can", "roadEncodeIdx"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this); + if (!replay->load()) { + return false; + } + replay->start(); + return true; +} + +void Parser::openDBC(const QString &name) { + dbc_name = name; + dbc = const_cast(dbc_lookup(name.toStdString())); + msg_map.clear(); + for (auto &msg : dbc->msgs) { + msg_map[msg.address] = &msg; + } +} + +void Parser::process(std::vector can) { + for (auto &data : can) { + ++counters[data.id]; + auto &list = can_msgs[data.id]; + while (list.size() > DATA_LIST_SIZE) { + list.pop_front(); + } + list.push_back(data); + } + emit updated(); +} + +void Parser::recvThread() { + AlignedBuffer aligned_buf; + std::unique_ptr context(Context::create()); + std::unique_ptr subscriber(SubSocket::create(context.get(), "can")); + subscriber->setTimeout(100); + while (!exit) { + std::unique_ptr msg(subscriber->receive()); + if (!msg) continue; + + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); + cereal::Event::Reader event = cmsg.getRoot(); + std::vector can; + can.reserve(event.getCan().size()); + for (const auto &c : event.getCan()) { + CanData &data = can.emplace_back(); + data.address = c.getAddress(); + data.bus_time = c.getBusTime(); + data.source = c.getSrc(); + data.dat.append((char *)c.getDat().begin(), c.getDat().size()); + data.hex_dat = data.dat.toHex(' ').toUpper(); + data.id = QString("%1:%2").arg(data.source).arg(data.address, 1, 16); + data.ts = (event.getLogMonoTime() - replay->routeStartTime()) / (double)1e6; + } + emit received(can); + } +} + +void Parser::addNewMsg(const Msg &msg) { + dbc->msgs.push_back(msg); + msg_map[dbc->msgs.back().address] = &dbc->msgs.back(); +} + +void Parser::removeSignal(const QString &id, const QString &sig_name) { + Msg *msg = const_cast(getMsg(id)); + if (!msg) return; + + auto it = std::find_if(msg->sigs.begin(), msg->sigs.end(), [=](auto &sig) { return sig_name == sig.name.c_str(); }); + if (it != msg->sigs.end()) { + msg->sigs.erase(it); + } +} + +uint32_t Parser::addressFromId(const QString &id) { + return id.mid(id.indexOf(':') + 1).toUInt(nullptr, 16); +} diff --git a/tools/cabana/parser.h b/tools/cabana/parser.h new file mode 100644 index 0000000000..fd5ded7c5e --- /dev/null +++ b/tools/cabana/parser.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "opendbc/can/common.h" +#include "opendbc/can/common_dbc.h" +#include "tools/replay/replay.h" + +const int DATA_LIST_SIZE = 500; +// const int FPS = 20; + +struct CanData { + QString id; + double ts; + uint32_t address; + uint16_t bus_time; + uint8_t source; + QByteArray dat; + QString hex_dat; +}; + +class Parser : public QObject { + Q_OBJECT + + public: + Parser(QObject *parent); + ~Parser(); + static uint32_t addressFromId(const QString &id); + bool loadRoute(const QString &route, const QString &data_dir, bool use_qcam); + void openDBC(const QString &name); + void saveDBC(const QString &name) {} + void addNewMsg(const Msg &msg); + void removeSignal(const QString &id, const QString &sig_name); + const Msg *getMsg(const QString &id) { + return getMsg(addressFromId(id)); + } + const Msg *getMsg(uint32_t address) { + auto it = msg_map.find(address); + return it != msg_map.end() ? it->second : nullptr; + } + signals: + void showPlot(const QString &id, const QString &name); + void hidePlot(const QString &id, const QString &name); + void received(std::vector can); + void updated(); + + public: + void recvThread(); + void process(std::vector can); + QThread *thread; + QString dbc_name; + std::atomic exit = false; + std::map> can_msgs; + std::map counters; + Replay *replay = nullptr; + DBC *dbc = nullptr; + std::map msg_map; +}; + +Q_DECLARE_METATYPE(std::vector); + +extern Parser *parser; diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc new file mode 100644 index 0000000000..caa109eab0 --- /dev/null +++ b/tools/cabana/videowidget.cc @@ -0,0 +1,80 @@ +#include "tools/cabana/videowidget.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "tools/cabana/parser.h" + +inline QString formatTime(int seconds) { + return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh::mm::ss" : "mm::ss"); +} + +VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + + cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, true, this); + cam_widget->setFixedSize(640, 480); + main_layout->addWidget(cam_widget); + + // slider controls + QHBoxLayout *slider_layout = new QHBoxLayout(); + QLabel *time_label = new QLabel("00:00"); + slider_layout->addWidget(time_label); + + slider = new QSlider(Qt::Horizontal, this); + // slider->setFixedWidth(640); + slider->setSingleStep(1); + slider->setMaximum(parser->replay->totalSeconds()); + QObject::connect(slider, &QSlider::sliderReleased, [=]() { + time_label->setText(formatTime(slider->value())); + parser->replay->seekTo(slider->value(), false); + }); + slider_layout->addWidget(slider); + + QLabel *total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); + slider_layout->addWidget(total_time_label); + + main_layout->addLayout(slider_layout); + + // btn controls + QHBoxLayout *control_layout = new QHBoxLayout(); + QPushButton *play = new QPushButton("⏸"); + play->setStyleSheet("font-weight:bold"); + QObject::connect(play, &QPushButton::clicked, [=]() { + bool is_paused = parser->replay->isPaused(); + play->setText(is_paused ? "⏸" : "▶"); + parser->replay->pause(!is_paused); + }); + control_layout->addWidget(play); + + QButtonGroup *group = new QButtonGroup(this); + group->setExclusive(true); + for (float speed : {0.1, 0.5, 1., 2.}) { + QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); + btn->setCheckable(true); + QObject::connect(btn, &QPushButton::clicked, [=]() { + parser->replay->setSpeed(speed); + }); + control_layout->addWidget(btn); + group->addButton(btn); + if (speed == 1.0) btn->setChecked(true); + } + + main_layout->addLayout(control_layout); + + QTimer *timer = new QTimer(this); + timer->setInterval(1000); + timer->callOnTimeout([=]() { + int current_seconds = parser->replay->currentSeconds(); + time_label->setText(formatTime(current_seconds)); + if (!slider->isSliderDown()) { + slider->setValue(current_seconds); + } + }); + timer->start(); +} diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h new file mode 100644 index 0000000000..f0b9e458bd --- /dev/null +++ b/tools/cabana/videowidget.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +#include "selfdrive/ui/qt/widgets/cameraview.h" + +class VideoWidget : public QWidget { + Q_OBJECT + +public: + VideoWidget(QWidget *parnet = nullptr); + +protected: + CameraViewWidget *cam_widget; + QSlider *slider; +}; diff --git a/tools/replay/SConscript b/tools/replay/SConscript index d3967708fa..9985375688 100644 --- a/tools/replay/SConscript +++ b/tools/replay/SConscript @@ -18,6 +18,7 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): replay_lib_src = ["replay.cc", "consoleui.cc", "camera.cc", "filereader.cc", "logreader.cc", "framereader.cc", "route.cc", "util.cc"] replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=qt_libs, FRAMEWORKS=base_frameworks) + Export('replay_lib') replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'ncurses'] + qt_libs qt_env.Program("replay", ["main.cc"], LIBS=replay_libs, FRAMEWORKS=base_frameworks) diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index c6c78f47ae..1684dfaca9 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -382,7 +382,7 @@ void Replay::stream() { if (cur_which < sockets_.size() && sockets_[cur_which] != nullptr) { // keep time - long etime = cur_mono_time_ - evt_start_ts; + long etime = (cur_mono_time_ - evt_start_ts) / speed_; long rtime = nanos_since_boot() - loop_start_ts; long behind_ns = etime - rtime; // if behind_ns is greater than 1 second, it means that an invalid segemnt is skipped by seeking/replaying diff --git a/tools/replay/replay.h b/tools/replay/replay.h index e86c453f7e..3327362f97 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -52,8 +52,11 @@ public: inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } inline const Route* route() const { return route_.get(); } inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline uint64_t routeStartTime() const { return route_start_ts_; } inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } inline int totalSeconds() const { return segments_.size() * 60; } + inline void setSpeed(float speed) { speed_ = speed; } + inline float getSpeed() const { return speed_; } inline const std::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock); @@ -112,4 +115,5 @@ protected: QFuture timeline_future; std::vector> timeline; std::string car_fingerprint_; + float speed_ = 1.0; }; diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 863b853718..7e021bcc23 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -63,6 +63,7 @@ function install_ubuntu_common_requirements() { qttools5-dev-tools \ libqt5sql5-sqlite \ libqt5svg5-dev \ + libqt5charts5-dev \ libqt5x11extras5-dev \ libreadline-dev \ libdw1 \ From 3068c48224d62ea6c3070332668fedafe522f1d3 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Oct 2022 16:06:29 -0700 Subject: [PATCH 07/62] add fault for invalid safety RX checks (#25949) * add fault for invalid safety RX checks * just a bool * bump panda --- cereal | 2 +- panda | 2 +- selfdrive/boardd/boardd.cc | 1 + selfdrive/car/hyundai/carstate.py | 1 - selfdrive/controls/controlsd.py | 2 +- 5 files changed, 4 insertions(+), 4 deletions(-) diff --git a/cereal b/cereal index d4cf8728e2..5ba96b6ded 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d4cf8728e2fa2d87d90098efa7ddeaf8f98a03db +Subproject commit 5ba96b6ded57bcd91e60140ce0036f61370f8512 diff --git a/panda b/panda index 1910db8d4c..e987e6c639 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 1910db8d4c3f932fe85b186fba1d24795cb2b742 +Subproject commit e987e6c6393e1e1432ba6bb740eb2c1b80542043 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 09e7137b38..3e39985c29 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -381,6 +381,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); ps.setInterruptLoad(health.interrupt_load); ps.setFanPower(health.fan_power); + ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid)); std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 61da04d04b..b9c7327a93 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -31,7 +31,6 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] self.brake_error = False - self.park_brake = False self.buttons_counter = 0 # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b6479e5608..303e5f8df8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -313,7 +313,7 @@ class Controls: else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES - if safety_mismatch or self.mismatch_counter >= 200: + if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) if log.PandaState.FaultType.relayMalfunction in pandaState.faults: From 4404cb42b445cdb7c2a9583357582ba2f11f2067 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 3 Oct 2022 16:45:00 -0700 Subject: [PATCH 08/62] fix build without extras (#25957) --- tools/cabana/SConscript | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index f32ee166b6..0caae14e92 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -1,6 +1,6 @@ import os Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc', - 'cereal', 'transformations', 'widgets', 'replay_lib', 'opendbc') + 'cereal', 'transformations', 'widgets', 'opendbc') base_frameworks = qt_env['FRAMEWORKS'] base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', @@ -15,6 +15,7 @@ qt_libs = ['qt_util', 'Qt5Charts'] + base_libs if arch in ['x86_64', 'Darwin'] and GetOption('extras'): qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] + Import('replay_lib') # qt_env["LD_LIBRARY_PATH"] = [Dir(f"#opendbc/can").abspath] cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs qt_env.Program('_cabana', ['cabana.cc', 'mainwin.cc', 'chartswidget.cc', 'videowidget.cc', 'parser.cc', 'messageswidget.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) From 5352006cbd00dc84262a4789b66cba47669d6d75 Mon Sep 17 00:00:00 2001 From: Greg Hogan Date: Mon, 3 Oct 2022 19:19:39 -0700 Subject: [PATCH 09/62] hyundai: fix FCA11 checksum and counter (#25027) * hyundai: fix FCA11 checksum and counter * update refs and comment about alt DBC definition we do not support --- opendbc | 2 +- selfdrive/car/hyundai/hyundaican.py | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/opendbc b/opendbc index eaac172af9..9ddcdb22c4 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit eaac172af9cb342204e69ec52339cdf3c6a8ac4e +Subproject commit 9ddcdb22c4929baf310295e832668e6e7fcfa602 diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index b3e1aa6b66..139a14d5e8 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -132,18 +132,16 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s } commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) + # note that some vehicles most likely have an alternate checksum/counter definition + # https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602 fca11_values = { - # seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat... - # (where first value is aligned to Supplemental_Counter == 0) - # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)] - "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1, - "Supplemental_Counter": idx % 0xF, + "CR_FCA_Alive": idx % 0xF, "PAINT1_Status": 1, "FCA_DrvSetStatus": 1, "FCA_Status": 1, # AEB disabled } fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] - fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10 + fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7]) commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) return commands From dc63245b8955dc941e4c2370f6b953564f3f775b Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Mon, 3 Oct 2022 20:47:00 -0700 Subject: [PATCH 10/62] reset LaikadEphemeris after loggerd test --- selfdrive/loggerd/tests/test_loggerd.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index a2138b0aa6..9c3565d130 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -110,6 +110,8 @@ class TestLoggerd(unittest.TestCase): self.assertEqual(getattr(initData, initData_key), v) self.assertEqual(logged_params[param_key].decode(), v) + params.put("LaikadEphemeris", "") + def test_rotation(self): os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") From f35c234e9c23d9af49fcc6f82475931dc2d887db Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 3 Oct 2022 21:19:51 -0700 Subject: [PATCH 11/62] thermald: consider pmic temp while onroad (#25959) * thermald: consider pmic temp while onroad * this is better --- selfdrive/thermald/thermald.py | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 5c2fbd6825..64ca39d296 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -177,7 +177,8 @@ def thermald_thread(end_event, hw_queue): modem_temps=[], ) - temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) + all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) + offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) should_start_prev = False in_car = False engaged_prev = False @@ -239,24 +240,32 @@ def thermald_thread(end_event, hw_queue): msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() - max_comp_temp = temp_filter.update( - max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) - ) + # this one is only used for offroad + temp_sources = [ + msg.deviceState.memoryTempC, + max(msg.deviceState.cpuTempC), + max(msg.deviceState.gpuTempC), + ] + offroad_comp_temp = offroad_temp_filter.update(max(temp_sources)) + + # this drives the thermal status while onroad + temp_sources.append(max(msg.deviceState.pmicTempC)) + all_comp_temp = all_temp_filter.update(max(temp_sources)) if fan_controller is not None: - msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"]) + msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"]) is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) - if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: + if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP: # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 thermal_status = ThermalStatus.danger else: current_band = THERMAL_BANDS[thermal_status] band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) - if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: + if current_band.min_temp is not None and all_comp_temp < current_band.min_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] - elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: + elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp: thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] # **** starting logic **** From 84adb8d9259be92951c604b0780608de8c039f9f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 4 Oct 2022 00:39:19 -0700 Subject: [PATCH 12/62] GM: raise max brake (#25810) * -4 didn't make any sense? * comments * comments * update to 400 * bump panda * remove unused iso limit vars * update comments * bump panda * Update selfdrive/car/gm/values.py * Update ref_commit --- panda | 2 +- selfdrive/car/gm/values.py | 16 ++++++++-------- selfdrive/controls/lib/longcontrol.py | 3 --- selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 10 insertions(+), 13 deletions(-) diff --git a/panda b/panda index e987e6c639..9bcd9b9a24 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e987e6c6393e1e1432ba6bb740eb2c1b80542043 +Subproject commit 9bcd9b9a24149b241992f26caf9920d427ee609d diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index fec21d8d24..999dabfee3 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -23,11 +23,12 @@ class CarControllerParams: ADAS_KEEPALIVE_STEP = 100 CAMERA_KEEPALIVE_STEP = 100 - # Volt gasbrake lookups - # TODO: These values should be confirmed on non-Volt vehicles + # Volt gas/brake lookups + # TODO: These values should be confirmed on non-Volt vehicles. + # MAX_GAS should achieve 2 m/s^2 and MAX_BRAKE with regen should achieve -4.0 m/s^2 MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. ZERO_GAS = 2048 # Coasting - MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen + MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we @@ -38,15 +39,14 @@ class CarControllerParams: ACCEL_MAX = 2. # m/s^2 ACCEL_MIN = -4. # m/s^2 - EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] - EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] - # ICE has much less engine braking force compared to regen in EVs, # lower threshold removes some braking deadzone GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] - BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] - + EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] + + BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] + EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 43e1f9cc4b..db5bf4d3e6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -7,9 +7,6 @@ from selfdrive.modeld.constants import T_IDXS LongCtrlState = car.CarControl.Actuators.LongControlState -# As per ISO 15622:2018 for all speeds -ACCEL_MIN_ISO = -3.5 # m/s^2 -ACCEL_MAX_ISO = 2.0 # m/s^2 def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_1sec, brake_pressed, cruise_standstill): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1f459b1948..2769483c90 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -53079010a5db8105854212157b5ee90029df7b92 \ No newline at end of file +653fbfeac952c288ffd595ee13139b9547b998bf From ca746b0a18eebc9b0fa7a6810dba0b28a60548c2 Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Tue, 4 Oct 2022 13:34:31 -0700 Subject: [PATCH 13/62] Extend torqued (#25961) * extend live torque to all hyundai and toyota cars * update refs --- selfdrive/locationd/torqued.py | 5 ++--- selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 78c3029af4..66af234590 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -12,7 +12,6 @@ from common.realtime import config_realtime_process, DT_MDL from common.filter_simple import FirstOrderFilter from system.swaglog import cloudlog from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from selfdrive.car.toyota.values import CAR as TOYOTA HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -33,7 +32,7 @@ MAX_INVALID_THRESHOLD = 10 MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches -ALLOWED_FINGERPRINTS = [TOYOTA.COROLLA_TSS2, TOYOTA.COROLLA, TOYOTA.COROLLAH_TSS2] +ALLOWED_CARS = ['toyota', 'hyundai'] def slope2rot(slope): @@ -98,7 +97,7 @@ class TorqueEstimator: self.offline_friction = 0.0 self.offline_latAccelFactor = 0.0 self.resets = 0.0 - self.use_params = CP.carFingerprint in ALLOWED_FINGERPRINTS + self.use_params = CP.carName in ALLOWED_CARS if CP.lateralTuning.which() == 'torque': self.offline_friction = CP.lateralTuning.torque.friction diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2769483c90..d4d16b726b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -653fbfeac952c288ffd595ee13139b9547b998bf +1989d8c2a5de94faa3756b7d10fc94e6c063afa5 \ No newline at end of file From 0231c4ba851819963502d0068d574a055f865692 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 4 Oct 2022 17:23:41 -0700 Subject: [PATCH 14/62] cleanup stale longitudinal params (#25967) --- selfdrive/controls/controlsd.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 303e5f8df8..9cf079a2b3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -143,6 +143,11 @@ class Controls: put_nonblocking("CarParamsCache", cp_bytes) put_nonblocking("CarParamsPersistent", cp_bytes) + # cleanup old params + if not self.CP.experimentalLongitudinalAvailable: + params.remove("EndToEndLong") + params.remove("ExperimentalLongitudinalEnabled") + self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() self.AM = AlertManager() From 285fd56a1d9ceb4addc5f34c510817bb075af977 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 4 Oct 2022 17:47:37 -0700 Subject: [PATCH 15/62] bootlog: add helper to get a bootlog by segment id --- tools/lib/bootlog.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py index 3515370823..1e474e5dde 100644 --- a/tools/lib/bootlog.py +++ b/tools/lib/bootlog.py @@ -1,6 +1,7 @@ import datetime import functools import re +from typing import List, Optional from tools.lib.auth_config import get_token from tools.lib.api import CommaApi @@ -48,8 +49,15 @@ class Bootlog: return False return self.datetime < b.datetime +def get_bootlog_from_id(bootlog_id: str) -> Optional[Bootlog]: + # TODO: implement an API endpoint for this + bl = Bootlog(bootlog_id) + for b in get_bootlogs(bl.dongle_id): + if b == bl: + return b + return None -def get_bootlogs(dongle_id: str): +def get_bootlogs(dongle_id: str) -> List[Bootlog]: api = CommaApi(get_token()) r = api.get(f'v1/devices/{dongle_id}/bootlogs') return [Bootlog(b) for b in r] From de67a28f1bae5f3677c57ed179baf8bf9c1d971d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 4 Oct 2022 19:21:23 -0700 Subject: [PATCH 16/62] Ford: handle VIN (#25966) fix ford vin --- selfdrive/car/vin.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 50c2abde46..ee40cf298a 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -27,6 +27,9 @@ def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): for addr in valid_vin_addrs: vin = results.get((addr, None)) if vin is not None: + # Ford pads with null bytes + vin = vin.replace(b'\x00', b'') + # Honda Bosch response starts with a length, trim to correct length if vin.startswith(b'\x11'): vin = vin[1:18] From 7ed064f7baff36b08713729c9c99304ef0224a58 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 4 Oct 2022 20:38:39 -0700 Subject: [PATCH 17/62] VIN: make Ford exception more explicit (#25972) * Update vin.py * only replace from end --- selfdrive/car/vin.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index ee40cf298a..cf1c25e851 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -28,7 +28,8 @@ def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False): vin = results.get((addr, None)) if vin is not None: # Ford pads with null bytes - vin = vin.replace(b'\x00', b'') + if len(vin) == 24: + vin = re.sub(b'\x00*$', b'', vin) # Honda Bosch response starts with a length, trim to correct length if vin.startswith(b'\x11'): From f2859b3be55d95bc53ff90aa536a7d8ae14b212f Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 4 Oct 2022 20:58:07 -0700 Subject: [PATCH 18/62] thermald: prevent started cycling too quickly (#25971) * thermald: prevent started cycling too quickly * move that * fix logging and no wait after boot Co-authored-by: Comma Device --- selfdrive/thermald/thermald.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 64ca39d296..89b81f06ec 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -278,6 +278,7 @@ def thermald_thread(end_event, hw_queue): startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version + startup_conditions["offroad_min_time"] = (not started_seen) or ((off_ts is not None) and (sec_since_boot() - off_ts) > 5.) # with 2% left, we killall, otherwise the phone will take a long time to boot startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 @@ -338,7 +339,8 @@ def thermald_thread(end_event, hw_queue): started_seen = True else: if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): - cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions) + cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True) + startup_conditions_prev = startup_conditions.copy() started_ts = None if off_ts is None: @@ -372,7 +374,6 @@ def thermald_thread(end_event, hw_queue): pm.send("deviceState", msg) should_start_prev = should_start - startup_conditions_prev = startup_conditions.copy() # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) From e5d2c3ce7abd9fa4c82a774e765e30ba0a97461a Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 4 Oct 2022 21:19:04 -0700 Subject: [PATCH 19/62] Planner cleanup (#25969) --- selfdrive/controls/lib/lateral_planner.py | 3 --- .../lib/longitudinal_mpc_lib/long_mpc.py | 25 ------------------- .../controls/lib/longitudinal_planner.py | 4 +-- 3 files changed, 2 insertions(+), 30 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 3470754bc6..2ad5f784d7 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -22,7 +22,6 @@ class LateralPlanner: self.solution_invalid_cnt = 0 self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) - self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) @@ -45,8 +44,6 @@ class LateralPlanner: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = np.array(md.orientation.z) - if len(md.position.xStd) == TRAJECTORY_SIZE: - self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic desire_state = md.meta.desireState diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 01a69d6f87..695222ed4d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -255,9 +255,6 @@ class LongitudinalMpc: elif self.mode == 'blended': cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] - elif self.mode == 'e2e': - cost_weights = [0., 0.2, 0.25, 1., 0.0, .1] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) @@ -386,28 +383,6 @@ class LongitudinalMpc: (lead_1_obstacle[0] - lead_0_obstacle[0]): self.source = 'lead1' - - def update_with_xva(self, x, v, a): - self.params[:,0] = -10. - self.params[:,1] = 10. - self.params[:,2] = 1e5 - self.params[:,4] = T_FOLLOW - self.params[:,5] = LEAD_DANGER_FACTOR - - # v, and a are in local frame, but x is wrt the x[0] position - # In >90degree turns, x goes to 0 (and may even be -ve) - # So, we use integral(v) + x[0] to obtain the forward-distance - xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) - x = np.cumsum(np.insert(xforward, 0, x[0])) - self.yref[:,1] = x - self.yref[:,2] = v - self.yref[:,3] = a - for i in range(N): - self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - self.params[:,3] = np.copy(self.prev_a) - self.run() - def run(self): # t0 = sec_since_boot() # reset = 0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6ec3762a94..a0363536ca 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -87,8 +87,8 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm): - if self.param_read_counter % 50 == 0: + def update(self, sm, read=True): + if self.param_read_counter % 50 == 0 and read: self.read_param() self.param_read_counter += 1 From 300577f38f963bb2854a6139ed50544d9bb493b1 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Oct 2022 13:00:22 -0700 Subject: [PATCH 20/62] fixup toggle cleanup --- selfdrive/controls/controlsd.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9cf079a2b3..6ce1156baf 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -145,8 +145,9 @@ class Controls: # cleanup old params if not self.CP.experimentalLongitudinalAvailable: - params.remove("EndToEndLong") params.remove("ExperimentalLongitudinalEnabled") + if not self.CP.openpilotLongitudinalControl: + params.remove("EndToEndLong") self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() From c8b7d297b6645b82406b75f93c3ee37643575c50 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Wed, 5 Oct 2022 15:39:40 -0700 Subject: [PATCH 21/62] docs: specify Lexus Safety System+ package for RX 2016 (#25974) * docs: specify Lexus Safety System+ package for RX 2016 This package isn't standard on the 2016 MY. https://cdn.dealereprocess.org/cdn/brochures/lexus/2016-rx350.pdf * hybrid too Co-authored-by: Adeeb Shihadeh Co-authored-by: Adeeb Shihadeh --- docs/CARS.md | 8 +++++--- selfdrive/car/toyota/values.py | 10 ++++++++-- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index ecd6932e87..82ef37ae8d 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. -# 204 Supported Cars +# 206 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| @@ -110,9 +110,11 @@ A supported vehicle is one that just works when you install a comma three. All s |Lexus|NX Hybrid 2018-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Lexus|RX 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX 2016|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Lexus|RX Hybrid 2016-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX Hybrid 2016|Lexus Safety System+|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX Hybrid 2017-19|All|Stock[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 1f8ab2498d..0075a483f6 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -171,8 +171,14 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"), CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"), CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"), - CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]), - CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]), + CAR.LEXUS_RX: [ + ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Lexus RX 2017-19", footnotes=[Footnote.DSU]), + ], + CAR.LEXUS_RXH: [ + ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]), + ToyotaCarInfo("Lexus RX Hybrid 2017-19", footnotes=[Footnote.DSU]), + ], CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"), CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"), } From 06e283a7504b23de79f12ec2c0a7ab280eda57e4 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Oct 2022 16:10:14 -0700 Subject: [PATCH 22/62] UI: only show cell settings on non-prime connection (#25067) * fix indentation * add primeTypeChanged signal to uiState * hide advanced networking toggles on prime type change * switch between map settings on prime type change * cleanup * remove duplicate code, wait for signal Co-authored-by: Cameron Clough --- selfdrive/ui/qt/maps/map_settings.cc | 5 +++-- selfdrive/ui/qt/offroad/networking.cc | 15 ++++++++++++--- selfdrive/ui/qt/offroad/networking.h | 3 +++ selfdrive/ui/qt/offroad/wifiManager.cc | 20 ++++++++++---------- selfdrive/ui/qt/widgets/prime.cc | 6 +----- selfdrive/ui/ui.cc | 7 +++++++ selfdrive/ui/ui.h | 4 +++- 7 files changed, 39 insertions(+), 21 deletions(-) diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc index dd2ad04a7d..3205ca517d 100644 --- a/selfdrive/ui/qt/maps/map_settings.cc +++ b/selfdrive/ui/qt/maps/map_settings.cc @@ -115,7 +115,9 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) { stack->addWidget(main_widget); stack->addWidget(no_prime_widget); - stack->setCurrentIndex(uiState()->prime_type ? 0 : 1); + connect(uiState(), &UIState::primeTypeChanged, [=](int prime_type) { + stack->setCurrentIndex(prime_type ? 0 : 1); + }); QVBoxLayout *wrapper = new QVBoxLayout(this); wrapper->addWidget(stack); @@ -194,7 +196,6 @@ void MapPanel::parseResponse(const QString &response, bool success) { } void MapPanel::refresh() { - stack->setCurrentIndex(uiState()->prime_type ? 0 : 1); if (cur_destinations == prev_destinations) return; QJsonDocument doc = QJsonDocument::fromJson(cur_destinations.trimmed().toUtf8()); diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 0ed6317c3c..13697adfb5 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -8,9 +8,11 @@ #include #include +#include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/widgets/prime.h" #include "selfdrive/ui/qt/widgets/scrollview.h" @@ -150,7 +152,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Roaming toggle const bool roamingEnabled = params.getBool("GsmRoaming"); - ToggleControl *roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); + roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); QObject::connect(roamingToggle, &ToggleControl::toggleFlipped, [=](bool state) { params.putBool("GsmRoaming", state); wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn")), params.getBool("GsmMetered")); @@ -158,7 +160,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid list->addItem(roamingToggle); // APN settings - ButtonControl *editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); + editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); connect(editApnButton, &ButtonControl::clicked, [=]() { const QString cur_apn = QString::fromStdString(params.get("GsmApn")); QString apn = InputDialog::getText(tr("Enter APN"), this, tr("leave blank for automatic configuration"), false, -1, cur_apn).trimmed(); @@ -174,7 +176,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Metered toggle const bool metered = params.getBool("GsmMetered"); - ToggleControl *meteredToggle = new ToggleControl(tr("Cellular Metered"), tr("Prevent large data uploads when on a metered connection"), "", metered); + meteredToggle = new ToggleControl(tr("Cellular Metered"), tr("Prevent large data uploads when on a metered connection"), "", metered); QObject::connect(meteredToggle, &SshToggle::toggleFlipped, [=](bool state) { params.putBool("GsmMetered", state); wifi->updateGsmSettings(params.getBool("GsmRoaming"), QString::fromStdString(params.get("GsmApn")), state); @@ -184,6 +186,13 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid // Set initial config wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn")), metered); + connect(uiState(), &UIState::primeTypeChanged, this, [=](int prime_type) { + bool gsmVisible = prime_type == PrimeType::NONE || prime_type == PrimeType::LITE; + roamingToggle->setVisible(gsmVisible); + editApnButton->setVisible(gsmVisible); + meteredToggle->setVisible(gsmVisible); + }); + main_layout->addWidget(new ScrollView(list, this)); main_layout->addStretch(1); } diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h index 4fc9a53d93..79cbcc3493 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -40,6 +40,9 @@ public: private: LabelControl* ipLabel; ToggleControl* tetheringToggle; + ToggleControl* roamingToggle; + ButtonControl* editApnButton; + ToggleControl* meteredToggle; WifiManager* wifi = nullptr; Params params; diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 3a30456c93..62de3041b9 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -415,16 +415,16 @@ void WifiManager::addTetheringConnection() { } void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { - int prime_type = uiState()->prime_type; - int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); - - if (!ipv4_forward) { - QTimer::singleShot(5000, this, [=] { - qWarning() << "net.ipv4.ip_forward = 0"; - std::system("sudo sysctl net.ipv4.ip_forward=0"); - }); - } - call->deleteLater(); + int prime_type = uiState()->prime_type; + int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); + + if (!ipv4_forward) { + QTimer::singleShot(5000, this, [=] { + qWarning() << "net.ipv4.ip_forward = 0"; + std::system("sudo sysctl net.ipv4.ip_forward=0"); + }); + } + call->deleteLater(); } void WifiManager::setTetheringEnabled(bool enabled) { diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 04684fc765..da2f4e60d1 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -312,11 +312,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) { QJsonObject json = doc.object(); int prime_type = json["prime_type"].toInt(); - - if (uiState()->prime_type != prime_type) { - uiState()->prime_type = prime_type; - Params().put("PrimeType", std::to_string(prime_type)); - } + uiState()->prime_type = prime_type; if (!json["is_paired"].toBool()) { mainLayout->setCurrentIndex(0); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index fcee00d1c8..152c7fbfcd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -201,6 +201,13 @@ void UIState::updateStatus() { started_prev = scene.started; emit offroadTransition(!scene.started); } + + // Handle prime type change + if (prime_type != prime_type_prev) { + prime_type_prev = prime_type; + emit primeTypeChanged(prime_type); + Params().put("PrimeType", std::to_string(prime_type)); + } } UIState::UIState(QObject *parent) : QObject(parent) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index d7e51ccfeb..f60c26b59a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -126,7 +126,7 @@ public: UIScene scene = {}; bool awake; - int prime_type = 0; + int prime_type; QString language; QTransform car_space_transform; @@ -135,6 +135,7 @@ public: signals: void uiUpdate(const UIState &s); void offroadTransition(bool offroad); + void primeTypeChanged(int prime_type); private slots: void update(); @@ -142,6 +143,7 @@ private slots: private: QTimer *timer; bool started_prev = false; + int prime_type_prev = -1; }; UIState *uiState(); From ef24c0b2ce8a4cf1dbc82886b806079889adf97b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Oct 2022 16:40:39 -0700 Subject: [PATCH 23/62] agnos 6.1 (#25973) * agnos 6.1 * staging manifest * no casync * prod manifest --- launch_env.sh | 2 +- selfdrive/sensord/rawgps/modemdiag.py | 15 +-------------- system/hardware/tici/agnos.json | 6 +++--- 3 files changed, 5 insertions(+), 18 deletions(-) diff --git a/launch_env.sh b/launch_env.sh index 48c5696b94..88e1f2a9c5 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="6" + export AGNOS_VERSION="6.1" fi if [ -z "$PASSIVE" ]; then diff --git a/selfdrive/sensord/rawgps/modemdiag.py b/selfdrive/sensord/rawgps/modemdiag.py index cc2bc5b261..5d72aeba9e 100644 --- a/selfdrive/sensord/rawgps/modemdiag.py +++ b/selfdrive/sensord/rawgps/modemdiag.py @@ -1,5 +1,3 @@ -import os -import time import select from serial import Serial from crcmod import mkCrcFun @@ -11,18 +9,7 @@ class ModemDiag: self.pend = b'' def open_serial(self): - def op(): - return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0) - try: - serial = op() - except Exception: - # TODO: this is a hack to get around modemmanager's exclusive open - print("unlocking serial...") - os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'') - os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'') - time.sleep(0.5) - os.system("sudo chmod 666 /dev/ttyUSB0") - serial = op() + serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True) serial.flush() serial.reset_input_buffer() serial.reset_output_buffer() diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index dc0fa6f1c1..8534c8a978 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -41,9 +41,9 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-9db38e27c912005472f3ac02be336af4f82307295118b6db22921479d44a941d.img.xz", - "hash": "05e7ce440b33721b020a249043d9568a5898080e26411ca250fb330ad2e5ed8e", - "hash_raw": "9db38e27c912005472f3ac02be336af4f82307295118b6db22921479d44a941d", + "url": "https://commadist.azureedge.net/agnosupdate/system-b40b08912576bb972907acba7c201c1399395cbc0cba06ce6e5e3f70ab565cb5.img.xz", + "hash": "6e8fbcc21a265f7f58062abce7675dc05540e2b60cee2df56992a151ba64936f", + "hash_raw": "b40b08912576bb972907acba7c201c1399395cbc0cba06ce6e5e3f70ab565cb5", "size": 10737418240, "sparse": true, "full_check": false, From 8b41d817e401fcf577bc68dfc6654ee1e0c3cbb5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 5 Oct 2022 17:06:52 -0700 Subject: [PATCH 24/62] IsoTpParallelQuery: set separation time (#25978) * Specify separation time in openpilot * comment * Update selfdrive/car/isotp_parallel_query.py * Update selfdrive/car/isotp_parallel_query.py --- panda | 2 +- selfdrive/car/isotp_parallel_query.py | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/panda b/panda index 9bcd9b9a24..3334dc21f5 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 9bcd9b9a24149b241992f26caf9920d427ee609d +Subproject commit 3334dc21f5c55007c5a754dfd8ee5d642be3e2bb diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 94c8d052b3..4b4bdcc0ca 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -68,7 +68,10 @@ class IsoTpParallelQuery: self.bus, sub_addr=sub_addr, debug=self.debug) max_len = 8 if sub_addr is None else 7 - return IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug) + # uses iso-tp frame separation time of 10 ms + # TODO: use single_frame_mode so ECUs can send as fast as they want, + # as well as reduces chances we process messages from previous queries + return IsoTpMessage(can_client, timeout=0, separation_time=0.01, debug=self.debug, max_len=max_len) def get_data(self, timeout, total_timeout=60.): self._drain_rx() From 6db9f051f761506e4d91ef1879cfae4c8cb1ddc8 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 5 Oct 2022 17:09:46 -0700 Subject: [PATCH 25/62] Car docs: test no duplicate years (#25975) * unit test * clean up * revert test * clean up * like this like this * no model model --- selfdrive/car/tests/test_docs.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index 191b36b8f2..b7056df5b3 100755 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +from collections import defaultdict import re import unittest @@ -20,6 +21,15 @@ class TestCarDocs(unittest.TestCase): self.assertEqual(generated_cars_md, current_cars_md, "Run selfdrive/car/docs.py to update the compatibility documentation") + def test_duplicate_years(self): + make_model_years = defaultdict(list) + for car in self.all_cars: + with self.subTest(car_info_name=car.name): + make_model = (car.make, car.model) + for year in car.year_list: + self.assertNotIn(year, make_model_years[make_model], f"{car.name}: Duplicate model year") + make_model_years[make_model].append(year) + def test_missing_car_info(self): all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys() for platform in sorted(interfaces.keys()): From 6a8a38b1a13b678bddbfdbcf5ba438bd7516c9be Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Oct 2022 17:13:48 -0700 Subject: [PATCH 26/62] pigeond: prevent locking up a CPU core (#25979) --- selfdrive/sensord/pigeond.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index e1fa2f4cad..f47cefed6c 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -251,6 +251,9 @@ def main(): msg = messaging.new_message('ubloxRaw', len(dat)) msg.ubloxRaw = dat[:] pm.send('ubloxRaw', msg) + else: + # prevent locking up a CPU core if ublox disconnects + time.sleep(0.001) if __name__ == "__main__": main() From c9c46c1b36d5784ed4245979eee5cc5f042fba12 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Wed, 5 Oct 2022 19:07:59 -0700 Subject: [PATCH 27/62] Revert "updated: configure branch upstream (#25916)" This reverts commit 17ed8dd0e9a8ecd0dcf8b573176ea27355c6a1ee. --- selfdrive/updated.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/updated.py b/selfdrive/updated.py index f96abcb7d5..9568b28ae3 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -327,7 +327,7 @@ class Updater: self._has_internet = False setup_git_options(OVERLAY_MERGED) - output = run(["git", "ls-remote", "--heads", "origin"], OVERLAY_MERGED) + output = run(["git", "ls-remote", "--heads"], OVERLAY_MERGED) self.branches = defaultdict(lambda: None) for line in output.split('\n'): @@ -363,7 +363,6 @@ class Updater: cloudlog.info("git reset in progress") cmds = [ ["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"], - ["git", "branch", "--set-upstream-to", f"origin/{branch}"], ["git", "reset", "--hard"], ["git", "clean", "-xdff"], ["git", "submodule", "init"], From e59008bf9335fba195f1df8741954a9f34ae00d9 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 5 Oct 2022 20:59:04 -0700 Subject: [PATCH 28/62] rawgpsd: more robust + simple test (#25977) * rawgps cleanup * wait for modem manager * cleanup Co-authored-by: Comma Device --- selfdrive/sensord/rawgps/rawgpsd.py | 165 +++++++++++++++--------- selfdrive/sensord/rawgps/test_rawgps.py | 49 +++++++ 2 files changed, 150 insertions(+), 64 deletions(-) create mode 100755 selfdrive/sensord/rawgps/test_rawgps.py diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 7c4582902b..5149ab6473 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -5,23 +5,32 @@ import signal import itertools import math import time +import subprocess from typing import NoReturn from struct import unpack_from, calcsize, pack -import cereal.messaging as messaging + from cereal import log -from system.swaglog import cloudlog +import cereal.messaging as messaging from laika.gps_time import GPSTime - +from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv -from selfdrive.sensord.rawgps.structs import dict_unpacker -from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv -from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv -from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv -from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT -from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT +from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, + gps_measurement_report, gps_measurement_report_sv, + glonass_measurement_report, glonass_measurement_report_sv, + oemdre_measurement_report, oemdre_measurement_report_sv, + LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, + LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT) DEBUG = int(os.getenv("DEBUG", "0"))==1 +LOG_TYPES = [ + LOG_GNSS_GPS_MEASUREMENT_REPORT, + LOG_GNSS_GLONASS_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_POSITION_REPORT, +] + + miscStatusFields = { "multipathEstimateIsValid": 0, "directionIsValid": 1, @@ -65,59 +74,43 @@ measurementStatusGlonassFields = { "glonassTimeMarkValid": 17 } -def main() -> NoReturn: - unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) - unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) - unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) - unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) - - unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) - unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) - - log_types = [ - LOG_GNSS_GPS_MEASUREMENT_REPORT, - LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, - ] - pub_types = ['qcomGnss'] - unpack_position, _ = dict_unpacker(position_report) - log_types.append(LOG_GNSS_POSITION_REPORT) - pub_types.append("gpsLocation") - - # connect to modem - diag = ModemDiag() - - # NV enable OEMDRE +def try_setup_logs(diag, log_types): + for _ in range(5): + try: + setup_logs(diag, log_types) + break + except Exception: + cloudlog.exception("setup logs failed, trying again") + else: + raise Exception(f"setup logs failed, {log_types=}") + +def mmcli(cmd: str) -> None: + for _ in range(5): + try: + subprocess.check_call(f"mmcli -m 0 {cmd}", shell=True) + break + except subprocess.CalledProcessError: + cloudlog.exception("rawgps.mmcli_command_failed") + else: + raise Exception(f"failed to execute mmcli command {cmd=}") + +def setup_quectel(diag: ModemDiag): + # enable OEMDRE in the NV # TODO: it has to reboot for this to take effect DIAG_NV_READ_F = 38 DIAG_NV_WRITE_F = 39 NV_GNSS_OEM_FEATURE_MASK = 7165 + send_recv(diag, DIAG_NV_WRITE_F, pack(' NoReturn: GPSDIAG_OEM_DRE_ON = 1 # gpsdiag_OemControlReqType - opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack(' NoReturn: + unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) + unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) + + unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True) + unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True) + + unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) + unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + + unpack_position, _ = dict_unpacker(position_report) + + # wait for ModemManager to come up + cloudlog.warning("waiting for modem to come up") + while True: + ret = subprocess.call("mmcli -m 0 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + if ret == 0: + break + time.sleep(0.1) + + # connect to modem + diag = ModemDiag() + + def cleanup(sig, frame): + cloudlog.warning(f"caught sig {sig}, disabling quectel gps") + teardown_quectel(diag) + cloudlog.warning("quectel cleanup done") + sys.exit(0) + signal.signal(signal.SIGINT, cleanup) + signal.signal(signal.SIGTERM, cleanup) + + setup_quectel(diag) + cloudlog.warning("quectel setup done") + + pm = messaging.PubMaster(['qcomGnss', 'gpsLocation']) while 1: opcode, payload = diag.recv() assert opcode == DIAG_LOG_F + (pending_msgs, log_outer_length), inner_log_packet = unpack_from(' 0: cloudlog.debug("have %d pending messages" % pending_msgs) assert log_outer_length == len(inner_log_packet) + (log_inner_length, log_type, log_time), log_payload = unpack_from(' 0, "rawgpsd didn't start outputting messages in time" + + et = time.monotonic() - start_time + assert et < 5, f"rawgpsd took {et:.1f}s to start" + managed_processes['rawgpsd'].stop() + + def test_turns_off_gnss(self): + for s in (0.1, 0.5, 1, 5): + managed_processes['rawgpsd'].start() + time.sleep(s) + managed_processes['rawgpsd'].stop() + + ls = subprocess.check_output("mmcli -m 0 --location-status --output-json", shell=True, encoding='utf-8') + loc_status = json.loads(ls) + assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'} + + +if __name__ == "__main__": + unittest.main() From dc72438be57e8908b1946df2a0ee81202a70469c Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Wed, 5 Oct 2022 21:16:41 -0700 Subject: [PATCH 29/62] regen: add arg for migrating sensorEvents with old timestamps (#25980) * add event logMonoTime * add arg for old logtime --- selfdrive/test/process_replay/regen.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 54c84978c2..eee3745f8e 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -190,7 +190,7 @@ def migrate_carparams(lr): return all_msgs -def migrate_sensorEvents(lr): +def migrate_sensorEvents(lr, old_logtime=False): all_msgs = [] for msg in lr: if msg.which() != 'sensorEventsDEPRECATED': @@ -214,6 +214,8 @@ def migrate_sensorEvents(lr): m = messaging.new_message(sensor_service) m.valid = True + if old_logtime: + m.logMonoTime = msg.logMonoTime m_dat = getattr(m, sensor_service) m_dat.version = evt.version From a6ba073231761e06ac6f070a01b434243d9d0693 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Thu, 6 Oct 2022 12:17:22 +0800 Subject: [PATCH 30/62] canbana: complete basic functions (#25965) * add chart header * get all signal val from logs * loop in selected range * clear list before append * automatically zoom on yaxis * cleanup * sync charts * fix event_begin_sec * set the color of rubber * add TODO * sync slider with charts * keep video aspect ratio * sync plot buttons * reduce flickers * cleanup * refactor detail view * clear counters * more use qcamera --- tools/cabana/SConscript | 3 +- tools/cabana/cabana.cc | 6 +- tools/cabana/chartswidget.cc | 206 +++++++++++----- tools/cabana/chartswidget.h | 52 +++- tools/cabana/detailwidget.cc | 421 +++++++++------------------------ tools/cabana/detailwidget.h | 86 +++---- tools/cabana/mainwin.cc | 9 +- tools/cabana/mainwin.h | 4 +- tools/cabana/messageswidget.cc | 35 ++- tools/cabana/messageswidget.h | 2 +- tools/cabana/parser.cc | 114 +++++++-- tools/cabana/parser.h | 58 +++-- tools/cabana/signaledit.cc | 203 ++++++++++++++++ tools/cabana/signaledit.h | 50 ++++ tools/cabana/videowidget.cc | 49 ++-- tools/cabana/videowidget.h | 5 + tools/replay/logreader.cc | 18 +- tools/replay/logreader.h | 7 +- tools/replay/replay.cc | 21 +- tools/replay/replay.h | 9 +- tools/replay/route.cc | 6 +- tools/replay/route.h | 3 +- 22 files changed, 833 insertions(+), 534 deletions(-) create mode 100644 tools/cabana/signaledit.cc create mode 100644 tools/cabana/signaledit.h diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 0caae14e92..c87e2cdd94 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -16,6 +16,5 @@ if arch in ['x86_64', 'Darwin'] and GetOption('extras'): qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] Import('replay_lib') - # qt_env["LD_LIBRARY_PATH"] = [Dir(f"#opendbc/can").abspath] cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs - qt_env.Program('_cabana', ['cabana.cc', 'mainwin.cc', 'chartswidget.cc', 'videowidget.cc', 'parser.cc', 'messageswidget.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) + qt_env.Program('_cabana', ['cabana.cc', 'mainwin.cc', 'chartswidget.cc', 'videowidget.cc', 'signaledit.cc', 'parser.cc', 'messageswidget.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 0adc744b49..20cd889023 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -5,7 +5,6 @@ #include "tools/cabana/mainwin.h" const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; -Parser *parser = nullptr; int main(int argc, char *argv[]) { initApp(argc, argv); @@ -16,7 +15,6 @@ int main(int argc, char *argv[]) { cmd_parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai"); cmd_parser.addOption({"demo", "use a demo route instead of providing your own"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); - cmd_parser.addOption({"qcam", "use qcamera"}); cmd_parser.process(app); const QStringList args = cmd_parser.positionalArguments(); if (args.empty() && !cmd_parser.isSet("demo")) { @@ -24,8 +22,8 @@ int main(int argc, char *argv[]) { } const QString route = args.empty() ? DEMO_ROUTE : args.first(); - parser = new Parser(&app); - if (!parser->loadRoute(route, cmd_parser.value("data_dir"), cmd_parser.isSet("qcam"))) { + Parser p(&app); + if (!p.loadRoute(route, cmd_parser.value("data_dir"), true)) { return 0; } MainWindow w; diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index a8fe39968e..836eb34946 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -1,20 +1,24 @@ #include "tools/cabana/chartswidget.h" +#include +#include +#include +#include +#include #include +#include -using namespace QtCharts; - -int64_t get_raw_value(const QByteArray &msg, const Signal &sig) { +int64_t get_raw_value(uint8_t *data, size_t data_size, const Signal &sig) { int64_t ret = 0; int i = sig.msb / 8; int bits = sig.size; - while (i >= 0 && i < msg.size() && bits > 0) { + while (i >= 0 && i < data_size && bits > 0) { int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i * 8; int msb = (int)(sig.msb / 8) == i ? sig.msb : (i + 1) * 8 - 1; int size = msb - lsb + 1; - uint64_t d = (msg[i] >> (lsb - (i * 8))) & ((1ULL << size) - 1); + uint64_t d = (data[i] >> (lsb - (i * 8))) & ((1ULL << size) - 1); ret |= d << (bits - size); bits -= size; @@ -26,77 +30,163 @@ int64_t get_raw_value(const QByteArray &msg, const Signal &sig) { ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(0, 0, 0, 0); - connect(parser, &Parser::updated, this, &ChartsWidget::updateState); connect(parser, &Parser::showPlot, this, &ChartsWidget::addChart); connect(parser, &Parser::hidePlot, this, &ChartsWidget::removeChart); + connect(parser, &Parser::signalRemoved, this, &ChartsWidget::removeChart); } void ChartsWidget::addChart(const QString &id, const QString &sig_name) { const QString char_name = id + sig_name; if (charts.find(char_name) == charts.end()) { - QLineSeries *series = new QLineSeries(); - series->setUseOpenGL(true); - auto chart = new QChart(); - chart->setTitle(id + ": " + sig_name); - chart->addSeries(series); - chart->createDefaultAxes(); - chart->legend()->hide(); - auto chart_view = new QChartView(chart); - chart_view->setMinimumSize({width(), 300}); - chart_view->setMaximumSize({width(), 300}); - chart_view->setRenderHint(QPainter::Antialiasing); - main_layout->addWidget(chart_view); - charts[char_name] = {.id = id, .sig_name = sig_name, .chart_view = chart_view}; + auto chart = new ChartWidget(id, sig_name, this); + main_layout->addWidget(chart); + charts[char_name] = chart; } } void ChartsWidget::removeChart(const QString &id, const QString &sig_name) { - auto it = charts.find(id + sig_name); - if (it == charts.end()) return; - - delete it->second.chart_view; - charts.erase(it); + if (auto it = charts.find(id + sig_name); it != charts.end()) { + it->second->deleteLater(); + charts.erase(it); + } } -void ChartsWidget::updateState() { - static double last_update = millis_since_boot(); - double current_ts = millis_since_boot(); - bool update = (current_ts - last_update) > 500; - if (update) { - last_update = current_ts; +ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *parent) : id(id), sig_name(sig_name), QWidget(parent) { + QStackedLayout *stacked = new QStackedLayout(this); + stacked->setStackingMode(QStackedLayout::StackAll); + + QWidget *chart_widget = new QWidget(this); + QVBoxLayout *chart_layout = new QVBoxLayout(chart_widget); + chart_layout->setSpacing(0); + chart_layout->setContentsMargins(0, 0, 0, 0); + + QWidget *header = new QWidget(this); + header->setStyleSheet("background-color:white"); + QHBoxLayout *header_layout = new QHBoxLayout(header); + header_layout->setContentsMargins(11, 11, 11, 0); + auto title = new QLabel(tr("%1 %2").arg(parser->getMsg(id)->name.c_str()).arg(id)); + header_layout->addWidget(title); + header_layout->addStretch(); + zoom_label = new QLabel("", this); + header_layout->addWidget(zoom_label); + QPushButton *zoom_in = new QPushButton("↺", this); + zoom_in->setToolTip(tr("reset zoom")); + QObject::connect(zoom_in, &QPushButton::clicked, []() { parser->resetRange(); }); + header_layout->addWidget(zoom_in); + + QPushButton *remove_btn = new QPushButton("✖", this); + QObject::connect(remove_btn, &QPushButton::clicked, [=]() { + emit parser->hidePlot(id, sig_name); + }); + header_layout->addWidget(remove_btn); + chart_layout->addWidget(header); + + QLineSeries *series = new QLineSeries(); + series->setUseOpenGL(true); + auto chart = new QChart(); + chart->setTitle(sig_name); + chart->addSeries(series); + chart->createDefaultAxes(); + chart->legend()->hide(); + QFont font; + font.setBold(true); + chart->setTitleFont(font); + chart->setMargins({0, 0, 0, 0}); + chart->layout()->setContentsMargins(0, 0, 0, 0); + QObject::connect(dynamic_cast(chart->axisX()), &QValueAxis::rangeChanged, parser, &Parser::setRange); + + chart_view = new QChartView(chart); + chart_view->setFixedHeight(300); + chart_view->setRenderHint(QPainter::Antialiasing); + chart_view->setRubberBand(QChartView::HorizontalRubberBand); + if (auto rubber = chart_view->findChild()) { + QPalette pal; + pal.setBrush(QPalette::Base, QColor(0, 0, 0, 80)); + rubber->setPalette(pal); } + chart_layout->addWidget(chart_view); + chart_layout->addStretch(); - auto getSig = [=](const QString &id, const QString &name) -> const Signal * { - for (auto &sig : parser->getMsg(id)->sigs) { - if (name == sig.name.c_str()) return &sig; - } - return nullptr; - }; - - for (auto &[_, c] : charts) { - if (auto sig = getSig(c.id, c.sig_name)) { - const auto &can_data = parser->can_msgs[c.id].back(); - int64_t val = get_raw_value(can_data.dat, *sig); - if (sig->is_signed) { - val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0; - } - double value = val * sig->factor + sig->offset; + stacked->addWidget(chart_widget); + line_marker = new LineMarker(chart, this); + stacked->addWidget(line_marker); + line_marker->setAttribute(Qt::WA_TransparentForMouseEvents, true); + line_marker->raise(); - if (value > c.max_y) c.max_y = value; - if (value < c.min_y) c.min_y = value; + setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + QObject::connect(parser, &Parser::updated, this, &ChartWidget::updateState); + QObject::connect(parser, &Parser::rangeChanged, this, &ChartWidget::rangeChanged); + QObject::connect(parser, &Parser::eventsMerged, this, &ChartWidget::updateSeries); - while (c.data.size() > DATA_LIST_SIZE) { - c.data.pop_front(); - } - c.data.push_back({can_data.ts / 1000., value}); - - if (update) { - QChart *chart = c.chart_view->chart(); - QLineSeries *series = (QLineSeries *)chart->series()[0]; - series->replace(c.data); - chart->axisX()->setRange(c.data.front().x(), c.data.back().x()); - chart->axisY()->setRange(c.min_y, c.max_y); + updateSeries(); +} + +void ChartWidget::updateState() { + line_marker->update(); +} + +void ChartWidget::updateSeries() { + const Signal *sig = parser->getSig(id, sig_name); + auto events = parser->replay->events(); + if (!sig || !events) return; + + auto l = id.split(':'); + int bus = l[0].toInt(); + uint32_t address = l[1].toUInt(nullptr, 16); + + vals.clear(); + vals.reserve(3 * 60 * 100); + uint64_t route_start_time = parser->replay->routeStartTime(); + for (auto &evt : *events) { + if (evt->which == cereal::Event::Which::CAN) { + for (auto c : evt->event.getCan()) { + if (bus == c.getSrc() && address == c.getAddress()) { + auto dat = c.getDat(); + int64_t val = get_raw_value((uint8_t *)dat.begin(), dat.size(), *sig); + if (sig->is_signed) { + val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0; + } + double value = val * sig->factor + sig->offset; + double ts = (evt->mono_time - route_start_time) / (double)1e9; // seconds + vals.push_back({ts, value}); + } } } } + QLineSeries *series = (QLineSeries *)chart_view->chart()->series()[0]; + series->replace(vals); + auto [begin, end] = parser->range(); + chart_view->chart()->axisX()->setRange(begin, end); +} + +void ChartWidget::rangeChanged(qreal min, qreal max) { + auto axis_x = dynamic_cast(chart_view->chart()->axisX()); + if (axis_x->min() != min || axis_x->max() != max) { + axis_x->setRange(min, max); + } + // auto zoom on yaxis + double min_y = 0, max_y = 0; + for (auto &p : vals) { + if (p.x() > max) break; + + if (p.x() >= min) { + if (p.y() < min_y) min_y = p.y(); + if (p.y() > max_y) max_y = p.y(); + } + } + chart_view->chart()->axisY()->setRange(min_y * 0.95, max_y * 1.05); +} + +LineMarker::LineMarker(QChart *chart, QWidget *parent) : chart(chart), QWidget(parent) {} + +void LineMarker::paintEvent(QPaintEvent *event) { + auto axis_x = dynamic_cast(chart->axisX()); + if (axis_x->max() <= axis_x->min()) return; + + double x = chart->plotArea().left() + chart->plotArea().width() * (parser->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min()); + QPainter p(this); + QPen pen = QPen(Qt::black); + pen.setWidth(2); + p.setPen(pen); + p.drawLine(QPointF{x, 50.}, QPointF{x, (qreal)height() - 11}); } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 7bc8335a32..1be5fdeecb 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -1,17 +1,53 @@ #pragma once +#include + +#include #include #include #include #include -#include #include "tools/cabana/parser.h" +using namespace QtCharts; + +class LineMarker : public QWidget { +Q_OBJECT + +public: + LineMarker(QChart *chart, QWidget *parent); + void paintEvent(QPaintEvent *event) override; + +private: + QChart *chart; +}; + +class ChartWidget : public QWidget { +Q_OBJECT + +public: + ChartWidget(const QString &id, const QString &sig_name, QWidget *parent); + inline QChart *chart() const { return chart_view->chart(); } + +protected: + void updateState(); + void addData(const CanData &can_data, const Signal &sig); + void updateSeries(); + void rangeChanged(qreal min, qreal max); + + QString id; + QString sig_name; + QLabel *zoom_label; + QChartView *chart_view = nullptr; + LineMarker *line_marker = nullptr; + QList vals; +}; + class ChartsWidget : public QWidget { Q_OBJECT - public: +public: ChartsWidget(QWidget *parent = nullptr); inline bool hasChart(const QString &id, const QString &sig_name) { return charts.find(id+sig_name) != charts.end(); @@ -20,15 +56,7 @@ class ChartsWidget : public QWidget { void removeChart(const QString &id, const QString &sig_name); void updateState(); - protected: +protected: QVBoxLayout *main_layout; - struct SignalChart { - QString id; - QString sig_name; - int max_y = 0; - int min_y = 0; - QList data; - QtCharts::QChartView *chart_view = nullptr; - }; - std::map charts; + std::map charts; }; diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 6799376577..e573a01970 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -1,68 +1,53 @@ + #include "tools/cabana/detailwidget.h" #include +#include #include -#include #include #include #include +#include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/widgets/scrollview.h" -const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; - -static QVector BIG_ENDIAN_START_BITS = []() { - QVector ret; - for (int i = 0; i < 64; i++) { - for (int j = 7; j >= 0; j--) { - ret.push_back(j + i * 8); - } - } - return ret; -}(); - -static int bigEndianBitIndex(int index) { - // TODO: Add a helper function in dbc.h - return BIG_ENDIAN_START_BITS.indexOf(index); +inline const QString &getColor(int i) { + static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"}; + return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)]; } +// DetailWidget + DetailWidget::DetailWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - QLabel *title = new QLabel(tr("SELECTED MESSAGE:"), this); - main_layout->addWidget(title); - QHBoxLayout *name_layout = new QHBoxLayout(); name_label = new QLabel(this); name_label->setStyleSheet("font-weight:bold;"); - name_layout->addWidget(name_label); - name_layout->addStretch(); + name_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + name_label->setAlignment(Qt::AlignCenter); + main_layout->addWidget(name_label); + + // title + QHBoxLayout *title_layout = new QHBoxLayout(); + time_label = new QLabel(this); + title_layout->addWidget(time_label); + title_layout->addStretch(); + edit_btn = new QPushButton(tr("Edit"), this); edit_btn->setVisible(false); - QObject::connect(edit_btn, &QPushButton::clicked, [=]() { - EditMessageDialog dlg(msg_id, this); - int ret = dlg.exec(); - if (ret) { - setMsg(msg_id); - } - }); - name_layout->addWidget(edit_btn); - main_layout->addLayout(name_layout); + title_layout->addWidget(edit_btn); + main_layout->addLayout(title_layout); + // binary view binary_view = new BinaryView(this); main_layout->addWidget(binary_view); + // scroll area QHBoxLayout *signals_layout = new QHBoxLayout(); signals_layout->addWidget(new QLabel(tr("Signals"))); signals_layout->addStretch(); add_sig_btn = new QPushButton(tr("Add signal"), this); add_sig_btn->setVisible(false); - QObject::connect(add_sig_btn, &QPushButton::clicked, [=]() { - AddSignalDialog dlg(msg_id, this); - int ret = dlg.exec(); - if (ret) { - setMsg(msg_id); - } - }); signals_layout->addWidget(add_sig_btn); main_layout->addLayout(signals_layout); @@ -72,238 +57,67 @@ DetailWidget::DetailWidget(QWidget *parent) : QWidget(parent) { signal_edit_layout->setSpacing(2); container_layout->addLayout(signal_edit_layout); - messages_view = new MessagesView(this); - container_layout->addWidget(messages_view); + history_log = new HistoryLog(this); + container_layout->addWidget(history_log); QScrollArea *scroll = new QScrollArea(this); scroll->setWidget(container); scroll->setWidgetResizable(true); + scroll->setFrameShape(QFrame::NoFrame); + scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); main_layout->addWidget(scroll); - setFixedWidth(600); - connect(parser, &Parser::updated, this, &DetailWidget::updateState); + QObject::connect(add_sig_btn, &QPushButton::clicked, this, &DetailWidget::addSignal); + QObject::connect(edit_btn, &QPushButton::clicked, this, &DetailWidget::editMsg); + QObject::connect(parser, &Parser::updated, this, &DetailWidget::updateState); } -void DetailWidget::updateState() { - if (msg_id.isEmpty()) return; - - auto &list = parser->can_msgs[msg_id]; - if (!list.empty()) { - binary_view->setData(list.back().dat); - messages_view->setMessages(list); - } -} - -SignalForm::SignalForm(const Signal &sig, QWidget *parent) : QWidget(parent) { - QVBoxLayout *v_layout = new QVBoxLayout(this); - - QHBoxLayout *h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Name"))); - name = new QLineEdit(sig.name.c_str()); - h->addWidget(name); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Size"))); - size = new QSpinBox(); - size->setValue(sig.size); - h->addWidget(size); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Most significant bit"))); - msb = new QSpinBox(); - msb->setValue(sig.msb); - h->addWidget(msb); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Endianness"))); - endianness = new QComboBox(); - endianness->addItems({"Little", "Big"}); - endianness->setCurrentIndex(sig.is_little_endian ? 0 : 1); - h->addWidget(endianness); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("sign"))); - sign = new QComboBox(); - sign->addItems({"Signed", "Unsigned"}); - sign->setCurrentIndex(sig.is_signed ? 0 : 1); - h->addWidget(sign); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Factor"))); - factor = new QSpinBox(); - factor->setValue(sig.factor); - h->addWidget(factor); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Offset"))); - offset = new QSpinBox(); - offset->setValue(sig.offset); - h->addWidget(offset); - v_layout->addLayout(h); - - // TODO: parse the following parameters in opendbc - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Unit"))); - unit = new QLineEdit(); - h->addWidget(unit); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Comment"))); - comment = new QLineEdit(); - h->addWidget(comment); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Minimum value"))); - min_val = new QSpinBox(); - h->addWidget(min_val); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Maximum value"))); - max_val = new QSpinBox(); - h->addWidget(max_val); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Value descriptions"))); - val_desc = new QLineEdit(); - h->addWidget(val_desc); - v_layout->addLayout(h); -} - -std::optional SignalForm::getSignal() { - Signal sig = {}; - sig.name = name->text().toStdString(); - sig.size = size->text().toInt(); - sig.offset = offset->text().toDouble(); - sig.factor = factor->text().toDouble(); - sig.msb = msb->text().toInt(); - sig.is_signed = sign->currentIndex() == 0; - sig.is_little_endian = endianness->currentIndex() == 0; - if (sig.is_little_endian) { - sig.lsb = sig.start_bit; - sig.msb = sig.start_bit + sig.size - 1; - } else { - sig.lsb = BIG_ENDIAN_START_BITS[bigEndianBitIndex(sig.start_bit) + sig.size - 1]; - sig.msb = sig.start_bit; - } - return (sig.name.empty() || sig.size <= 0) ? std::nullopt : std::optional(sig); -} - -void DetailWidget::setMsg(const QString &id) { - msg_id = id; - QString name = tr("untitled"); +void DetailWidget::setMsg(const CanData *c) { + can_data = c; + clearLayout(signal_edit_layout); + edit_btn->setVisible(true); - for (auto edit : signal_edit) { - delete edit; - } - signal_edit.clear(); - int i = 0; - auto msg = parser->getMsg(id); - if (msg) { - for (auto &s : msg->sigs) { - SignalEdit *edit = new SignalEdit(id, s, i++, this); - connect(edit, &SignalEdit::removed, [=]() { - QTimer::singleShot(0, [=]() { setMsg(id); }); - }); - signal_edit_layout->addWidget(edit); - signal_edit.push_back(edit); + if (auto msg = parser->getMsg(can_data->address)) { + name_label->setText(msg->name.c_str()); + add_sig_btn->setVisible(true); + for (int i = 0; i < msg->sigs.size(); ++i) { + signal_edit_layout->addWidget(new SignalEdit(can_data->id, msg->sigs[i], getColor(i))); } - name = msg->name.c_str(); + } else { + name_label->setText(tr("untitled")); + add_sig_btn->setVisible(false); } - name_label->setText(name); - binary_view->setMsg(msg_id); - edit_btn->setVisible(true); - add_sig_btn->setVisible(msg != nullptr); + binary_view->setMsg(can_data); + history_log->clear(); } -SignalEdit::SignalEdit(const QString &id, const Signal &sig, int idx, QWidget *parent) : id(id), name_(sig.name.c_str()), QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setContentsMargins(0, 0, 0, 0); - - // title - QHBoxLayout *title_layout = new QHBoxLayout(); - QLabel *icon = new QLabel(">"); - icon->setStyleSheet("font-weight:bold"); - title_layout->addWidget(icon); - title = new ElidedLabel(this); - title->setText(sig.name.c_str()); - title->setStyleSheet(QString("font-weight:bold; color:%1").arg(SIGNAL_COLORS[idx % std::size(SIGNAL_COLORS)])); - connect(title, &ElidedLabel::clicked, [=]() { - edit_container->isVisible() ? edit_container->hide() : edit_container->show(); - icon->setText(edit_container->isVisible() ? "▼" : ">"); - }); - title_layout->addWidget(title); - title_layout->addStretch(); - QPushButton *show_plot = new QPushButton(tr("Show Plot")); - QObject::connect(show_plot, &QPushButton::clicked, [=]() { - if (show_plot->text() == tr("Show Plot")) { - emit parser->showPlot(id, name_); - show_plot->setText(tr("Hide Plot")); - } else { - emit parser->hidePlot(id, name_); - show_plot->setText(tr("Show Plot")); - } - }); - title_layout->addWidget(show_plot); - main_layout->addLayout(title_layout); +void DetailWidget::updateState() { + if (!can_data) return; - edit_container = new QWidget(this); - QVBoxLayout *v_layout = new QVBoxLayout(edit_container); - form = new SignalForm(sig, this); - v_layout->addWidget(form); - - QHBoxLayout *h = new QHBoxLayout(); - remove_btn = new QPushButton(tr("Remove Signal")); - QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); - h->addWidget(remove_btn); - h->addStretch(); - QPushButton *save_btn = new QPushButton(tr("Save")); - QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); - h->addWidget(save_btn); - v_layout->addLayout(h); - - edit_container->setVisible(false); - main_layout->addWidget(edit_container); + time_label->setText(QString("time: %1").arg(can_data->ts, 0, 'f', 3)); + binary_view->setData(can_data->dat); + history_log->updateState(); } -void SignalEdit::save() { - Msg *msg = const_cast(parser->getMsg(id)); - if (!msg) return; - - for (auto &sig : msg->sigs) { - if (name_ == sig.name.c_str()) { - if (auto s = form->getSignal()) { - sig = *s; - } - break; - } +void DetailWidget::editMsg() { + EditMessageDialog dlg(can_data->id, this); + if (dlg.exec()) { + setMsg(can_data); } } -void SignalEdit::remove() { - QMessageBox msgbox; - msgbox.setText(tr("Remove signal")); - msgbox.setInformativeText(tr("Are you sure you want to remove signal '%1'").arg(name_)); - msgbox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); - msgbox.setDefaultButton(QMessageBox::Cancel); - if (msgbox.exec()) { - parser->removeSignal(id, name_); - emit removed(); +void DetailWidget::addSignal() { + AddSignalDialog dlg(can_data->id, this); + if (dlg.exec()) { + setMsg(can_data); } } +// BinaryView + BinaryView::BinaryView(QWidget *parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); table = new QTableWidget(this); @@ -316,15 +130,16 @@ BinaryView::BinaryView(QWidget *parent) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); } -void BinaryView::setMsg(const QString &id) { - auto msg = parser->getMsg(Parser::addressFromId(id)); - int row_count = msg ? msg->size : parser->can_msgs[id].back().dat.size(); +void BinaryView::setMsg(const CanData *can_data) { + const Msg *msg = parser->getMsg(can_data->address); + int row_count = msg ? msg->size : can_data->dat.size(); table->setRowCount(row_count); table->setColumnCount(9); for (int i = 0; i < table->rowCount(); ++i) { for (int j = 0; j < table->columnCount(); ++j) { auto item = new QTableWidgetItem(); + item->setFlags(item->flags() ^ Qt::ItemIsEditable); item->setTextAlignment(Qt::AlignCenter); if (j == 8) { QFont font; @@ -336,19 +151,17 @@ void BinaryView::setMsg(const QString &id) { } if (msg) { + // set background color for (int i = 0; i < msg->sigs.size(); ++i) { const auto &sig = msg->sigs[i]; int start = sig.is_little_endian ? sig.start_bit : bigEndianBitIndex(sig.start_bit); for (int j = start; j <= start + sig.size - 1; ++j) { - table->item(j / 8, j % 8)->setBackground(QColor(SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)])); + table->item(j / 8, j % 8)->setBackground(QColor(getColor(i))); } } } setFixedHeight(table->rowHeight(0) * table->rowCount() + 25); - if (!parser->can_msgs.empty()) { - setData(parser->can_msgs[id].back().dat); - } } void BinaryView::setData(const QByteArray &binary) { @@ -357,6 +170,7 @@ void BinaryView::setData(const QByteArray &binary) { s += std::bitset<8>(binary[j]).to_string(); } + setUpdatesEnabled(false); char hex[3] = {'\0'}; for (int i = 0; i < binary.size(); ++i) { for (int j = 0; j < 8; ++j) { @@ -365,46 +179,58 @@ void BinaryView::setData(const QByteArray &binary) { sprintf(&hex[0], "%02X", (unsigned char)binary[i]); table->item(i, 8)->setText(hex); } + setUpdatesEnabled(true); } -MessagesView::MessagesView(QWidget *parent) : QWidget(parent) { +// HistoryLog + +HistoryLog::HistoryLog(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - QLabel *title = new QLabel("MESSAGE TIME BYTES"); + QLabel *title = new QLabel("TIME BYTES"); main_layout->addWidget(title); - message_layout = new QVBoxLayout(); + QVBoxLayout *message_layout = new QVBoxLayout(); + for (int i = 0; i < std::size(labels); ++i) { + labels[i] = new QLabel(); + labels[i]->setVisible(false); + message_layout->addWidget(labels[i]); + } main_layout->addLayout(message_layout); main_layout->addStretch(); } -void MessagesView::setMessages(const std::list &list) { - auto begin = list.begin(); - std::advance(begin, std::max(0, (int)(list.size() - 100))); - int j = 0; - for (auto it = begin; it != list.end(); ++it) { - QLabel *label; - if (j >= messages.size()) { - label = new QLabel(); - message_layout->addWidget(label); - messages.push_back(label); - } else { - label = messages[j]; - } - label->setText(it->hex_dat); - ++j; +void HistoryLog::updateState() { + int i = 0; + for (; i < parser->history_log.size(); ++i) { + const auto &c = parser->history_log[i]; + auto label = labels[i]; + label->setVisible(true); + label->setText(QString("%1 %2").arg(c.ts, 0, 'f', 3).arg(c.hex_dat)); + } + + for (; i < std::size(labels); ++i) { + labels[i]->setVisible(false); } } -EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : QDialog(parent) { +void HistoryLog::clear() { + setUpdatesEnabled(false); + for (auto l : labels) l->setVisible(false); + setUpdatesEnabled(true); +} + +// EditMessageDialog + +EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : id(id), QDialog(parent) { setWindowTitle(tr("Edit message")); QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->addWidget(new QLabel(tr("ID: (%1)").arg(id))); - auto msg = const_cast(parser->getMsg(Parser::addressFromId(id))); + auto msg = const_cast(parser->getMsg(id)); QHBoxLayout *h_layout = new QHBoxLayout(); h_layout->addWidget(new QLabel(tr("Name"))); h_layout->addStretch(); - QLineEdit *name_edit = new QLineEdit(this); + name_edit = new QLineEdit(this); name_edit->setText(msg ? msg->name.c_str() : "untitled"); h_layout->addWidget(name_edit); main_layout->addLayout(h_layout); @@ -412,47 +238,30 @@ EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : QDial h_layout = new QHBoxLayout(); h_layout->addWidget(new QLabel(tr("Size"))); h_layout->addStretch(); - QSpinBox *size_spin = new QSpinBox(this); - size_spin->setValue(msg ? msg->size : parser->can_msgs[id].back().dat.size()); + size_spin = new QSpinBox(this); + size_spin->setValue(msg ? msg->size : parser->can_msgs[id].dat.size()); h_layout->addWidget(size_spin); main_layout->addLayout(h_layout); auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); main_layout->addWidget(buttonBox); - connect(buttonBox, &QDialogButtonBox::accepted, [=]() { - if (size_spin->value() <= 0 || name_edit->text().isEmpty()) return; - - if (msg) { - msg->name = name_edit->text().toStdString(); - msg->size = size_spin->value(); - } else { - Msg m = {}; - m.address = Parser::addressFromId(id); - m.name = name_edit->text().toStdString(); - m.size = size_spin->value(); - parser->addNewMsg(m); - } - QDialog::accept(); - }); + connect(buttonBox, &QDialogButtonBox::accepted, this, &EditMessageDialog::save); connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); } -AddSignalDialog::AddSignalDialog(const QString &id, QWidget *parent) : QDialog(parent) { - setWindowTitle(tr("Add signal to %1").arg(parser->getMsg(id)->name.c_str())); - QVBoxLayout *main_layout = new QVBoxLayout(this); - Signal sig = {.name = "untitled"}; - auto form = new SignalForm(sig, this); - main_layout->addWidget(form); - auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); - main_layout->addWidget(buttonBox); - connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); - connect(buttonBox, &QDialogButtonBox::accepted, [=]() { - if (auto msg = const_cast(parser->getMsg(id))) { - if (auto signal = form->getSignal()) { - msg->sigs.push_back(*signal); - } - } - QDialog::accept(); - }); +void EditMessageDialog::save() { + if (size_spin->value() <= 0 || name_edit->text().isEmpty()) return; + + if (auto msg = const_cast(parser->getMsg(id))) { + msg->name = name_edit->text().toStdString(); + msg->size = size_spin->value(); + } else { + Msg m = {}; + m.address = Parser::addressFromId(id); + m.name = name_edit->text().toStdString(); + m.size = size_spin->value(); + parser->addNewMsg(m); + } + QDialog::accept(); } diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index 70f2804f70..b2e7cbf3b7 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -1,102 +1,70 @@ #pragma once -#include + #include -#include #include -#include #include -#include #include #include #include -#include #include "opendbc/can/common.h" #include "opendbc/can/common_dbc.h" -#include "selfdrive/ui/qt/widgets/controls.h" #include "tools/cabana/parser.h" +#include "tools/cabana/signaledit.h" -class SignalForm : public QWidget { +class HistoryLog : public QWidget { Q_OBJECT - public: - SignalForm(const Signal &sig, QWidget *parent); - std::optional getSignal(); - QLineEdit *name, *unit, *comment, *val_desc; - QSpinBox *size, *msb, *lsb, *factor, *offset, *min_val, *max_val; - QComboBox *sign, *endianness; -}; - -class MessagesView : public QWidget { - Q_OBJECT +public: + HistoryLog(QWidget *parent); + void clear(); + void updateState(); - public: - MessagesView(QWidget *parent); - void setMessages(const std::list &data); - std::vector messages; - QVBoxLayout *message_layout; +private: + QLabel *labels[LOG_SIZE] = {}; }; class BinaryView : public QWidget { Q_OBJECT - public: +public: BinaryView(QWidget *parent); - void setMsg(const QString &id); + void setMsg(const CanData *can_data); void setData(const QByteArray &binary); QTableWidget *table; }; -class SignalEdit : public QWidget { +class EditMessageDialog : public QDialog { Q_OBJECT - public: - SignalEdit(const QString &id, const Signal &sig, int idx, QWidget *parent); +public: + EditMessageDialog(const QString &id, QWidget *parent); + +protected: void save(); -signals: - void removed(); - protected: - void remove(); + QLineEdit *name_edit; + QSpinBox *size_spin; QString id; - QString name_; - ElidedLabel *title; - SignalForm *form; - QWidget *edit_container; - QPushButton *remove_btn; }; class DetailWidget : public QWidget { Q_OBJECT - public: + +public: DetailWidget(QWidget *parent); - void setMsg(const QString &id); + void setMsg(const CanData *c); - public slots: +private: void updateState(); + void addSignal(); + void editMsg(); - protected: - QLabel *name_label = nullptr; + const CanData *can_data = nullptr; + QLabel *name_label, *time_label; QPushButton *edit_btn, *add_sig_btn; QVBoxLayout *signal_edit_layout; - Signal *sig = nullptr; - MessagesView *messages_view; - QString msg_id; + HistoryLog *history_log; BinaryView *binary_view; - std::vector signal_edit; -}; - -class EditMessageDialog : public QDialog { - Q_OBJECT - - public: - EditMessageDialog(const QString &id, QWidget *parent); -}; - -class AddSignalDialog : public QDialog { - Q_OBJECT - - public: - AddSignalDialog(const QString &id, QWidget *parent); }; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index d1b0d98f5f..8852987fbe 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -4,19 +4,15 @@ #include MainWindow::MainWindow() : QWidget() { - assert(parser != nullptr); - QVBoxLayout *main_layout = new QVBoxLayout(this); QHBoxLayout *h_layout = new QHBoxLayout(); main_layout->addLayout(h_layout); messages_widget = new MessagesWidget(this); - QObject::connect(messages_widget, &MessagesWidget::msgChanged, [=](const QString &id) { - detail_widget->setMsg(id); - }); h_layout->addWidget(messages_widget); detail_widget = new DetailWidget(this); + detail_widget->setFixedWidth(600); h_layout->addWidget(detail_widget); // right widget @@ -30,9 +26,12 @@ MainWindow::MainWindow() : QWidget() { QScrollArea *scroll = new QScrollArea(this); scroll->setWidget(charts_widget); scroll->setWidgetResizable(true); + scroll->setFrameShape(QFrame::NoFrame); scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); r_layout->addWidget(scroll); h_layout->addWidget(right_container); + + QObject::connect(messages_widget, &MessagesWidget::msgChanged, detail_widget, &DetailWidget::setMsg); } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index f19c48297e..82ecceb02b 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -11,10 +11,10 @@ class MainWindow : public QWidget { Q_OBJECT - public: +public: MainWindow(); - protected: +protected: VideoWidget *video_widget; MessagesWidget *messages_widget; DetailWidget *detail_widget; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index d81d3e9ede..840ea25810 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -1,11 +1,9 @@ #include "tools/cabana/messageswidget.h" #include -#include #include #include #include -#include MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); @@ -47,8 +45,9 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget->setHorizontalHeaderLabels({tr("Name"), tr("ID"), tr("Count"), tr("Bytes")}); table_widget->horizontalHeader()->setStretchLastSection(true); QObject::connect(table_widget, &QTableWidget::itemSelectionChanged, [=]() { - auto id = table_widget->selectedItems()[0]->data(Qt::UserRole); - emit msgChanged(id.toString()); + const CanData *c = &(parser->can_msgs[table_widget->selectedItems()[1]->text()]); + parser->setCurrentMsg(c->id); + emit msgChanged(c); }); main_layout->addWidget(table_widget); @@ -60,6 +59,7 @@ void MessagesWidget::updateState() { auto item = table_widget->item(row, col); if (!item) { item = new QTableWidgetItem(); + item->setFlags(item->flags() ^ Qt::ItemIsEditable); table_widget->setItem(row, col, item); } return item; @@ -67,28 +67,27 @@ void MessagesWidget::updateState() { table_widget->setRowCount(parser->can_msgs.size()); int i = 0; - const QString filter_str = filter->text().toLower(); - for (const auto &[id, list] : parser->can_msgs) { - assert(!list.empty()); - - QString name; - if (auto msg = parser->getMsg(list.back().address)) { + QString name, untitled = tr("untitled"); + const QString filter_str = filter->text(); + for (const auto &[_, c] : parser->can_msgs) { + if (auto msg = parser->getMsg(c.address)) { name = msg->name.c_str(); } else { - name = tr("untitled"); + name = untitled; } - if (!filter_str.isEmpty() && !name.toLower().contains(filter_str)) { + if (!filter_str.isEmpty() && !name.contains(filter_str, Qt::CaseInsensitive)) { table_widget->hideRow(i++); continue; } - auto item = getTableItem(i, 0); - item->setText(name); - item->setData(Qt::UserRole, id); - getTableItem(i, 1)->setText(id); - getTableItem(i, 2)->setText(QString("%1").arg(parser->counters[id])); - getTableItem(i, 3)->setText(list.back().hex_dat); + getTableItem(i, 0)->setText(name); + getTableItem(i, 1)->setText(c.id); + getTableItem(i, 2)->setText(QString::number(parser->counters[c.id])); + getTableItem(i, 3)->setText(c.hex_dat); table_widget->showRow(i); i++; } + if (table_widget->currentRow() == -1) { + table_widget->selectRow(0); + } } diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index dca725199d..1dbb4a1af3 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -16,7 +16,7 @@ class MessagesWidget : public QWidget { void updateState(); signals: - void msgChanged(const QString &id); + void msgChanged(const CanData *id); protected: QLineEdit *filter; diff --git a/tools/cabana/parser.cc b/tools/cabana/parser.cc index 481f0dfbdf..3950d8bd57 100644 --- a/tools/cabana/parser.cc +++ b/tools/cabana/parser.cc @@ -4,7 +4,11 @@ #include "cereal/messaging/messaging.h" +Parser *parser = nullptr; + Parser::Parser(QObject *parent) : QObject(parent) { + parser = this; + qRegisterMetaType>(); QObject::connect(this, &Parser::received, this, &Parser::process, Qt::QueuedConnection); @@ -23,32 +27,48 @@ Parser::~Parser() { bool Parser::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) { replay = new Replay(route, {"can", "roadEncodeIdx"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this); - if (!replay->load()) { - return false; + QObject::connect(replay, &Replay::segmentsMerged, this, &Parser::segmentsMerged); + if (replay->load()) { + replay->start(); + return true; } - replay->start(); - return true; + return false; } void Parser::openDBC(const QString &name) { dbc_name = name; dbc = const_cast(dbc_lookup(name.toStdString())); + counters.clear(); msg_map.clear(); for (auto &msg : dbc->msgs) { msg_map[msg.address] = &msg; } } -void Parser::process(std::vector can) { - for (auto &data : can) { - ++counters[data.id]; - auto &list = can_msgs[data.id]; - while (list.size() > DATA_LIST_SIZE) { - list.pop_front(); +void Parser::process(std::vector msgs) { + static double prev_update_ts = 0; + for (const auto &can_data : msgs) { + can_msgs[can_data.id] = can_data; + current_sec = can_data.ts; + ++counters[can_data.id]; + + if (can_data.id == current_msg_id) { + while (history_log.size() >= LOG_SIZE) { + history_log.pop_back(); + } + history_log.push_front(can_data); } - list.push_back(data); } - emit updated(); + double current_ts = millis_since_boot(); + if ((current_ts - prev_update_ts) > 1000.0 / FPS) { + prev_update_ts = current_ts; + emit updated(); + } + + if (current_sec < begin_sec || current_sec > end_sec) { + // loop replay in selected range. + replay->seekTo(begin_sec, false); + } } void Parser::recvThread() { @@ -56,13 +76,16 @@ void Parser::recvThread() { std::unique_ptr context(Context::create()); std::unique_ptr subscriber(SubSocket::create(context.get(), "can")); subscriber->setTimeout(100); + + std::vector can; while (!exit) { std::unique_ptr msg(subscriber->receive()); if (!msg) continue; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); cereal::Event::Reader event = cmsg.getRoot(); - std::vector can; + + can.clear(); can.reserve(event.getCan().size()); for (const auto &c : event.getCan()) { CanData &data = can.emplace_back(); @@ -72,7 +95,7 @@ void Parser::recvThread() { data.dat.append((char *)c.getDat().begin(), c.getDat().size()); data.hex_dat = data.dat.toHex(' ').toUpper(); data.id = QString("%1:%2").arg(data.source).arg(data.address, 1, 16); - data.ts = (event.getLogMonoTime() - replay->routeStartTime()) / (double)1e6; + data.ts = (event.getLogMonoTime() - replay->routeStartTime()) / (double)1e9; // seconds } emit received(can); } @@ -90,9 +113,72 @@ void Parser::removeSignal(const QString &id, const QString &sig_name) { auto it = std::find_if(msg->sigs.begin(), msg->sigs.end(), [=](auto &sig) { return sig_name == sig.name.c_str(); }); if (it != msg->sigs.end()) { msg->sigs.erase(it); + emit signalRemoved(id, sig_name); } } uint32_t Parser::addressFromId(const QString &id) { return id.mid(id.indexOf(':') + 1).toUInt(nullptr, 16); } + +const Signal *Parser::getSig(const QString &id, const QString &sig_name) { + if (auto msg = getMsg(id)) { + auto it = std::find_if(msg->sigs.begin(), msg->sigs.end(), [&](auto &s) { return sig_name == s.name.c_str(); }); + if (it != msg->sigs.end()) { + return &(*it); + } + } + return nullptr; +} + +void Parser::setRange(double min, double max) { + if (begin_sec != min || end_sec != max) { + begin_sec = min; + end_sec = max; + is_zoomed = begin_sec != event_begin_sec || end_sec != event_end_sec; + emit rangeChanged(min, max); + } +} + +void Parser::segmentsMerged() { + auto events = replay->events(); + if (!events || events->empty()) return; + + auto it = std::find_if(events->begin(), events->end(), [=](const Event *e) { return e->which == cereal::Event::Which::CAN; }); + event_begin_sec = it == events->end() ? 0 : ((*it)->mono_time - replay->routeStartTime()) / (double)1e9; + event_end_sec = double(events->back()->mono_time - replay->routeStartTime()) / 1e9; + if (!is_zoomed) { + begin_sec = event_begin_sec; + end_sec = event_end_sec; + } + emit eventsMerged(); +} + +void Parser::resetRange() { + setRange(event_begin_sec, event_end_sec); +} + +void Parser::setCurrentMsg(const QString &id) { + current_msg_id = id; + history_log.clear(); +} + +// helper functions + +static QVector BIG_ENDIAN_START_BITS = []() { + QVector ret; + for (int i = 0; i < 64; i++) { + for (int j = 7; j >= 0; j--) { + ret.push_back(j + i * 8); + } + } + return ret; +}(); + +int bigEndianStartBitsIndex(int start_bit) { + return BIG_ENDIAN_START_BITS[start_bit]; +} + +int bigEndianBitIndex(int index) { + return BIG_ENDIAN_START_BITS.indexOf(index); +} diff --git a/tools/cabana/parser.h b/tools/cabana/parser.h index fd5ded7c5e..2f8c441059 100644 --- a/tools/cabana/parser.h +++ b/tools/cabana/parser.h @@ -1,17 +1,20 @@ #pragma once +#include +#include + #include +#include #include #include -#include -#include #include "opendbc/can/common.h" #include "opendbc/can/common_dbc.h" #include "tools/replay/replay.h" -const int DATA_LIST_SIZE = 500; -// const int FPS = 20; +const int DATA_LIST_SIZE = 50; +const int FPS = 20; +const static int LOG_SIZE = 25; struct CanData { QString id; @@ -26,7 +29,7 @@ struct CanData { class Parser : public QObject { Q_OBJECT - public: +public: Parser(QObject *parent); ~Parser(); static uint32_t addressFromId(const QString &id); @@ -35,32 +38,57 @@ class Parser : public QObject { void saveDBC(const QString &name) {} void addNewMsg(const Msg &msg); void removeSignal(const QString &id, const QString &sig_name); - const Msg *getMsg(const QString &id) { - return getMsg(addressFromId(id)); - } - const Msg *getMsg(uint32_t address) { + const Signal *getSig(const QString &id, const QString &sig_name); + void setRange(double min, double max); + void resetRange(); + void setCurrentMsg(const QString &id); + inline std::pair range() const { return {begin_sec, end_sec}; } + inline double currentSec() const { return current_sec; } + inline bool isZoomed() const { return is_zoomed; } + inline const Msg *getMsg(const QString &id) { return getMsg(addressFromId(id)); } + inline const Msg *getMsg(uint32_t address) { auto it = msg_map.find(address); return it != msg_map.end() ? it->second : nullptr; } - signals: + +signals: void showPlot(const QString &id, const QString &name); void hidePlot(const QString &id, const QString &name); + void signalRemoved(const QString &id, const QString &sig_name); + void eventsMerged(); + void rangeChanged(double min, double max); void received(std::vector can); void updated(); - public: +public: + Replay *replay = nullptr; + QHash counters; + std::map can_msgs; + QList history_log; + +protected: void recvThread(); void process(std::vector can); + void segmentsMerged(); + + double current_sec = 0.; + std::atomic exit = false; QThread *thread; QString dbc_name; - std::atomic exit = false; - std::map> can_msgs; - std::map counters; - Replay *replay = nullptr; + double begin_sec = 0; + double end_sec = 0; + double event_begin_sec = 0; + double event_end_sec = 0; + bool is_zoomed = false; DBC *dbc = nullptr; std::map msg_map; + QString current_msg_id; }; Q_DECLARE_METATYPE(std::vector); +// TODO: Add helper function in dbc.h +int bigEndianStartBitsIndex(int start_bit); +int bigEndianBitIndex(int index); + extern Parser *parser; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc new file mode 100644 index 0000000000..080b7920de --- /dev/null +++ b/tools/cabana/signaledit.cc @@ -0,0 +1,203 @@ +#include "tools/cabana/signaledit.h" + +#include +#include +#include +#include +#include + +// SignalForm + +SignalForm::SignalForm(const Signal &sig, QWidget *parent) : QWidget(parent) { + QVBoxLayout *v_layout = new QVBoxLayout(this); + + QHBoxLayout *h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Name"))); + name = new QLineEdit(sig.name.c_str()); + h->addWidget(name); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Size"))); + size = new QSpinBox(); + size->setValue(sig.size); + h->addWidget(size); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Most significant bit"))); + msb = new QSpinBox(); + msb->setValue(sig.msb); + h->addWidget(msb); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Endianness"))); + endianness = new QComboBox(); + endianness->addItems({"Little", "Big"}); + endianness->setCurrentIndex(sig.is_little_endian ? 0 : 1); + h->addWidget(endianness); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("sign"))); + sign = new QComboBox(); + sign->addItems({"Signed", "Unsigned"}); + sign->setCurrentIndex(sig.is_signed ? 0 : 1); + h->addWidget(sign); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Factor"))); + factor = new QSpinBox(); + factor->setValue(sig.factor); + h->addWidget(factor); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Offset"))); + offset = new QSpinBox(); + offset->setValue(sig.offset); + h->addWidget(offset); + v_layout->addLayout(h); + + // TODO: parse the following parameters in opendbc + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Unit"))); + unit = new QLineEdit(); + h->addWidget(unit); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Comment"))); + comment = new QLineEdit(); + h->addWidget(comment); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Minimum value"))); + min_val = new QSpinBox(); + h->addWidget(min_val); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Maximum value"))); + max_val = new QSpinBox(); + h->addWidget(max_val); + v_layout->addLayout(h); + + h = new QHBoxLayout(); + h->addWidget(new QLabel(tr("Value descriptions"))); + val_desc = new QLineEdit(); + h->addWidget(val_desc); + v_layout->addLayout(h); +} + +std::optional SignalForm::getSignal() { + Signal sig = {}; + sig.name = name->text().toStdString(); + sig.size = size->text().toInt(); + sig.offset = offset->text().toDouble(); + sig.factor = factor->text().toDouble(); + sig.msb = msb->text().toInt(); + sig.is_signed = sign->currentIndex() == 0; + sig.is_little_endian = endianness->currentIndex() == 0; + if (sig.is_little_endian) { + sig.lsb = sig.start_bit; + sig.msb = sig.start_bit + sig.size - 1; + } else { + sig.lsb = bigEndianStartBitsIndex(bigEndianBitIndex(sig.start_bit) + sig.size - 1); + sig.msb = sig.start_bit; + } + return (sig.name.empty() || sig.size <= 0) ? std::nullopt : std::optional(sig); +} + +// SignalEdit + +SignalEdit::SignalEdit(const QString &id, const Signal &sig, const QString &color, QWidget *parent) : id(id), name_(sig.name.c_str()), QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + + // title + QHBoxLayout *title_layout = new QHBoxLayout(); + QLabel *icon = new QLabel(">"); + icon->setStyleSheet("font-weight:bold"); + title_layout->addWidget(icon); + title = new ElidedLabel(this); + title->setText(sig.name.c_str()); + title->setStyleSheet(QString("font-weight:bold; color:%1").arg(color)); + connect(title, &ElidedLabel::clicked, [=]() { + edit_container->isVisible() ? edit_container->hide() : edit_container->show(); + icon->setText(edit_container->isVisible() ? "▼" : ">"); + }); + title_layout->addWidget(title); + title_layout->addStretch(); + plot_btn = new QPushButton("📈"); + plot_btn->setStyleSheet("font-size:16px"); + plot_btn->setToolTip(tr("Show Plot")); + plot_btn->setContentsMargins(5, 5, 5, 5); + plot_btn->setFixedSize(30, 30); + QObject::connect(plot_btn, &QPushButton::clicked, [=]() { emit parser->showPlot(id, name_); }); + title_layout->addWidget(plot_btn); + main_layout->addLayout(title_layout); + + edit_container = new QWidget(this); + QVBoxLayout *v_layout = new QVBoxLayout(edit_container); + form = new SignalForm(sig, this); + v_layout->addWidget(form); + + QHBoxLayout *h = new QHBoxLayout(); + remove_btn = new QPushButton(tr("Remove Signal")); + QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); + h->addWidget(remove_btn); + h->addStretch(); + QPushButton *save_btn = new QPushButton(tr("Save")); + QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); + h->addWidget(save_btn); + v_layout->addLayout(h); + + edit_container->setVisible(false); + main_layout->addWidget(edit_container); +} + +void SignalEdit::save() { + if (auto sig = const_cast(parser->getSig(id, name_))) { + if (auto s = form->getSignal()) { + *sig = *s; + // TODO: reset the chart for sig + } + } +} + +void SignalEdit::remove() { + QMessageBox msgbox; + msgbox.setText(tr("Remove signal")); + msgbox.setInformativeText(tr("Are you sure you want to remove signal '%1'").arg(name_)); + msgbox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); + msgbox.setDefaultButton(QMessageBox::Cancel); + if (msgbox.exec()) { + parser->removeSignal(id, name_); + deleteLater(); + } +} + +// AddSignalDialog + +AddSignalDialog::AddSignalDialog(const QString &id, QWidget *parent) : QDialog(parent) { + setWindowTitle(tr("Add signal to %1").arg(parser->getMsg(id)->name.c_str())); + QVBoxLayout *main_layout = new QVBoxLayout(this); + Signal sig = {.name = "untitled"}; + auto form = new SignalForm(sig, this); + main_layout->addWidget(form); + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + main_layout->addWidget(buttonBox); + connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); + connect(buttonBox, &QDialogButtonBox::accepted, [=]() { + if (auto msg = const_cast(parser->getMsg(id))) { + if (auto signal = form->getSignal()) { + msg->sigs.push_back(*signal); + } + } + QDialog::accept(); + }); +} diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h new file mode 100644 index 0000000000..b8140cc93b --- /dev/null +++ b/tools/cabana/signaledit.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/parser.h" + +class SignalForm : public QWidget { + Q_OBJECT + +public: + SignalForm(const Signal &sig, QWidget *parent); + std::optional getSignal(); + + QLineEdit *name, *unit, *comment, *val_desc; + QSpinBox *size, *msb, *lsb, *factor, *offset, *min_val, *max_val; + QComboBox *sign, *endianness; +}; + +class SignalEdit : public QWidget { + Q_OBJECT + +public: + SignalEdit(const QString &id, const Signal &sig, const QString &color, QWidget *parent = nullptr); + void save(); + +protected: + void remove(); + + QString id; + QString name_; + QPushButton *plot_btn; + ElidedLabel *title; + SignalForm *form; + QWidget *edit_container; + QPushButton *remove_btn; +}; + +class AddSignalDialog : public QDialog { + Q_OBJECT + +public: + AddSignalDialog(const QString &id, QWidget *parent); +}; diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index caa109eab0..dbf988d8f2 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -3,9 +3,7 @@ #include #include #include -#include #include -#include #include #include "tools/cabana/parser.h" @@ -17,17 +15,19 @@ inline QString formatTime(int seconds) { VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); - cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, true, this); - cam_widget->setFixedSize(640, 480); + cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, false, this); + cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); main_layout->addWidget(cam_widget); // slider controls QHBoxLayout *slider_layout = new QHBoxLayout(); - QLabel *time_label = new QLabel("00:00"); + time_label = new QLabel("00:00"); slider_layout->addWidget(time_label); slider = new QSlider(Qt::Horizontal, this); - // slider->setFixedWidth(640); + QObject::connect(slider, &QSlider::sliderMoved, [=]() { + time_label->setText(formatTime(slider->value())); + }); slider->setSingleStep(1); slider->setMaximum(parser->replay->totalSeconds()); QObject::connect(slider, &QSlider::sliderReleased, [=]() { @@ -36,7 +36,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { }); slider_layout->addWidget(slider); - QLabel *total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); + total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); slider_layout->addWidget(total_time_label); main_layout->addLayout(slider_layout); @@ -57,9 +57,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { for (float speed : {0.1, 0.5, 1., 2.}) { QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); btn->setCheckable(true); - QObject::connect(btn, &QPushButton::clicked, [=]() { - parser->replay->setSpeed(speed); - }); + QObject::connect(btn, &QPushButton::clicked, [=]() { parser->replay->setSpeed(speed); }); control_layout->addWidget(btn); group->addButton(btn); if (speed == 1.0) btn->setChecked(true); @@ -67,14 +65,25 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { main_layout->addLayout(control_layout); - QTimer *timer = new QTimer(this); - timer->setInterval(1000); - timer->callOnTimeout([=]() { - int current_seconds = parser->replay->currentSeconds(); - time_label->setText(formatTime(current_seconds)); - if (!slider->isSliderDown()) { - slider->setValue(current_seconds); - } - }); - timer->start(); + QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged); + QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState); +} + +void VideoWidget::rangeChanged(double min, double max) { + if (!parser->isZoomed()) { + min = 0; + max = parser->replay->totalSeconds(); + } + time_label->setText(formatTime(min)); + total_time_label->setText(formatTime(max)); + slider->setMaximum(max); + slider->setValue(parser->currentSec()); +} + +void VideoWidget::updateState() { + if (!slider->isSliderDown()) { + int current_sec = parser->currentSec(); + time_label->setText(formatTime(current_sec)); + slider->setValue(current_sec); + } } diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index f0b9e458bd..813516e78f 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -12,6 +13,10 @@ public: VideoWidget(QWidget *parnet = nullptr); protected: + void rangeChanged(double min, double max); + void updateState(); + CameraViewWidget *cam_widget; + QLabel *time_label, *total_time_label; QSlider *slider; }; diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc index 9b7a07a83f..e3d5071412 100644 --- a/tools/replay/logreader.cc +++ b/tools/replay/logreader.cc @@ -46,7 +46,9 @@ LogReader::~LogReader() { #endif } -bool LogReader::load(const std::string &url, std::atomic *abort, bool local_cache, int chunk_size, int retries) { +bool LogReader::load(const std::string &url, std::atomic *abort, + const std::set &allow, + bool local_cache, int chunk_size, int retries) { raw_ = FileReader(local_cache, chunk_size, retries).read(url, abort); if (raw_.empty()) return false; @@ -54,18 +56,26 @@ bool LogReader::load(const std::string &url, std::atomic *abort, bool loca raw_ = decompressBZ2(raw_, abort); if (raw_.empty()) return false; } - return parse(abort); + return parse(allow, abort); } bool LogReader::load(const std::byte *data, size_t size, std::atomic *abort) { raw_.assign((const char *)data, size); - return parse(abort); + return parse({}, abort); } -bool LogReader::parse(std::atomic *abort) { +bool LogReader::parse(const std::set &allow, std::atomic *abort) { try { kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word)); while (words.size() > 0 && !(abort && *abort)) { + if (!allow.empty()) { + capnp::FlatArrayMessageReader reader(words); + auto which = reader.getRoot().which(); + if (allow.find(which) == allow.end()) { + words = kj::arrayPtr(reader.getEnd(), words.end()); + continue; + } + } #ifdef HAS_MEMORY_RESOURCE Event *evt = new (mbr_) Event(words); diff --git a/tools/replay/logreader.h b/tools/replay/logreader.h index bd666d0a74..010839af22 100644 --- a/tools/replay/logreader.h +++ b/tools/replay/logreader.h @@ -5,6 +5,8 @@ #include #endif +#include + #include "cereal/gen/cpp/log.capnp.h" #include "system/camerad/cameras/camera_common.h" #include "tools/replay/filereader.h" @@ -50,12 +52,13 @@ class LogReader { public: LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE); ~LogReader(); - bool load(const std::string &url, std::atomic *abort = nullptr, bool local_cache = false, int chunk_size = -1, int retries = 0); + bool load(const std::string &url, std::atomic *abort = nullptr, const std::set &allow = {}, + bool local_cache = false, int chunk_size = -1, int retries = 0); bool load(const std::byte *data, size_t size, std::atomic *abort = nullptr); std::vector events; private: - bool parse(std::atomic *abort); + bool parse(const std::set &allow, std::atomic *abort); std::string raw_; #ifdef HAS_MEMORY_RESOURCE std::pmr::monotonic_buffer_resource *mbr_ = nullptr; diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index 1684dfaca9..b64e87a03e 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -19,6 +19,9 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s if ((allow.empty() || allow.contains(it.name)) && !block.contains(it.name)) { uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue(); sockets_[which] = it.name; + if (!allow.empty() || !block.empty()) { + allow_list.insert((cereal::Event::Which)which); + } s.push_back(it.name); } } @@ -91,17 +94,17 @@ void Replay::updateEvents(const std::function &lambda) { stream_cv_.notify_one(); } -void Replay::seekTo(int seconds, bool relative) { +void Replay::seekTo(double seconds, bool relative) { seconds = relative ? seconds + currentSeconds() : seconds; updateEvents([&]() { - seconds = std::max(0, seconds); - int seg = seconds / 60; + seconds = std::max(double(0.0), seconds); + int seg = (int)seconds / 60; if (segments_.find(seg) == segments_.end()) { rWarning("can't seek to %d s segment %d is invalid", seconds, seg); return true; } - rInfo("seeking to %d s, segment %d", seconds, seg); + rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); @@ -122,7 +125,9 @@ void Replay::buildTimeline() { for (int i = 0; i < segments_.size() && !exit_; ++i) { LogReader log; - if (!log.load(route_->at(i).qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; + if (!log.load(route_->at(i).qlog.toStdString(), &exit_, + {cereal::Event::Which::CONTROLS_STATE, cereal::Event::Which::USER_FLAG}, + !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; for (const Event *e : log.events) { if (e->which == cereal::Event::Which::CONTROLS_STATE) { @@ -215,7 +220,7 @@ void Replay::queueSegment() { if ((seg && !seg->isLoaded()) || !seg) { if (!seg) { rDebug("loading segment %d...", n); - seg = std::make_unique(n, route_->at(n), flags_); + seg = std::make_unique(n, route_->at(n), flags_, allow_list); QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); } break; @@ -270,6 +275,9 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: segments_merged_ = segments_need_merge; return true; }); + if (stream_thread_) { + emit segmentsMerged(); + } } } @@ -306,6 +314,7 @@ void Replay::startStream(const Segment *cur_segment) { camera_server_ = std::make_unique(camera_size); } + emit segmentsMerged(); // start stream thread stream_thread_ = new QThread(); QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 3327362f97..aa0bbc33e7 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -45,18 +45,19 @@ public: void stop(); void pause(bool pause); void seekToFlag(FindFlag flag); - void seekTo(int seconds, bool relative); + void seekTo(double seconds, bool relative); inline bool isPaused() const { return paused_; } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } inline const Route* route() const { return route_.get(); } - inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; } inline uint64_t routeStartTime() const { return route_start_ts_; } inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } inline int totalSeconds() const { return segments_.size() * 60; } inline void setSpeed(float speed) { speed_ = speed; } inline float getSpeed() const { return speed_; } + inline const std::vector *events() const { return events_.get(); } inline const std::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock); @@ -65,6 +66,7 @@ public: signals: void streamStarted(); + void segmentsMerged(); protected slots: void segmentLoadFinished(bool success); @@ -98,7 +100,7 @@ protected: bool paused_ = false; bool events_updated_ = false; uint64_t route_start_ts_ = 0; - uint64_t cur_mono_time_ = 0; + std::atomic cur_mono_time_ = 0; std::unique_ptr> events_; std::unique_ptr> new_events_; std::vector segments_merged_; @@ -114,6 +116,7 @@ protected: std::mutex timeline_lock; QFuture timeline_future; std::vector> timeline; + std::set allow_list; std::string car_fingerprint_; float speed_ = 1.0; }; diff --git a/tools/replay/route.cc b/tools/replay/route.cc index c91b27ae81..f0d6ec5a12 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -99,7 +99,9 @@ void Route::addFileToSegment(int n, const QString &file) { // class Segment -Segment::Segment(int n, const SegmentFile &files, uint32_t flags) : seg_num(n), flags(flags) { +Segment::Segment(int n, const SegmentFile &files, uint32_t flags, + const std::set &allow) + : seg_num(n), flags(flags), allow(allow) { // [RoadCam, DriverCam, WideRoadCam, log]. fallback to qcamera/qlog const std::array file_list = { (flags & REPLAY_FLAG_QCAMERA) || files.road_cam.isEmpty() ? files.qcamera : files.road_cam, @@ -130,7 +132,7 @@ void Segment::loadFile(int id, const std::string file) { success = frames[id]->load(file, flags & REPLAY_FLAG_NO_HW_DECODER, &abort_, local_cache, 20 * 1024 * 1024, 3); } else { log = std::make_unique(); - success = log->load(file, &abort_, local_cache, 0, 3); + success = log->load(file, &abort_, allow, local_cache, 0, 3); } if (!success) { diff --git a/tools/replay/route.h b/tools/replay/route.h index 6ca9c3b883..6b78ebad87 100644 --- a/tools/replay/route.h +++ b/tools/replay/route.h @@ -47,7 +47,7 @@ class Segment : public QObject { Q_OBJECT public: - Segment(int n, const SegmentFile &files, uint32_t flags); + Segment(int n, const SegmentFile &files, uint32_t flags, const std::set &allow = {}); ~Segment(); inline bool isLoaded() const { return !loading_ && !abort_; } @@ -65,4 +65,5 @@ protected: std::atomic loading_ = 0; QFutureSynchronizer synchronizer_; uint32_t flags; + std::set allow; }; From cb0b7375b728d1b6e92db68c9ba55f0f54c09a3f Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 5 Oct 2022 21:43:38 -0700 Subject: [PATCH 31/62] Rocket Launcher Model (#25963) * 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/300 * compute only 9 tokens: 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/300 * tinygrad: cleanup gather * 1456d261-d232-4654-8885-4d9fde883894/440 6b7d7cec-ead8-40f3-86cc-86d52c9b03fe/700 * empty commit for tests * bump tinygrad * dont use tinygrad matmul for now * bump tinygrad * 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * float16 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * increase steer rate cost * Revert "increase steer rate cost" This reverts commit 74ce9ab9be7ef17ecfec931f96851b12f37f2336. * fork tinygrad * empty commit for tests * basics * Kinda works * new lat * new tuning * Move LATMPCN so scons compiles * Update long weights * Add tinygrad optim * Update model ref * update weights * Update ref * Try * Error message for field ignore * update model regf * ref commit * Fix onnx test Co-authored-by: Yassine Yousfi --- .gitmodules | 2 +- release/files_common | 1 + selfdrive/controls/lib/drive_helpers.py | 6 -- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 63 ++++++++++--------- selfdrive/controls/lib/lateral_planner.py | 18 +++--- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 6 +- selfdrive/controls/tests/test_lateral_mpc.py | 5 +- selfdrive/modeld/SConscript | 4 +- selfdrive/modeld/models/driving.cc | 16 +++-- selfdrive/modeld/models/driving.h | 15 ++++- selfdrive/modeld/models/supercombo.onnx | 4 +- selfdrive/modeld/runners/onnx_runner.py | 13 ++-- selfdrive/modeld/thneed/thneed_common.cc | 2 +- selfdrive/test/process_replay/compare_logs.py | 2 +- selfdrive/test/process_replay/model_replay.py | 4 +- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- tinygrad_repo | 2 +- 19 files changed, 93 insertions(+), 76 deletions(-) diff --git a/.gitmodules b/.gitmodules index 26f93ef164..544c95c943 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,4 +18,4 @@ url = ../../commaai/body.git [submodule "tinygrad"] path = tinygrad_repo - url = https://github.com/geohot/tinygrad.git + url = ../../commaai/tinygrad.git diff --git a/release/files_common b/release/files_common index 0c341eb3f6..07ffaf8501 100644 --- a/release/files_common +++ b/release/files_common @@ -574,3 +574,4 @@ tinygrad_repo/tinygrad/ops.py tinygrad_repo/tinygrad/shapetracker.py tinygrad_repo/tinygrad/tensor.py tinygrad_repo/tinygrad/nn/__init__.py +tinygrad_repo/tinygrad/nn/optim.py diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e8f9585a6f..f81cd0c40c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -33,12 +33,6 @@ CRUISE_INTERVAL_SIGN = { } -class MPC_COST_LAT: - PATH = 1.0 - HEADING = 1.0 - STEER_RATE = 1.0 - - def apply_deadzone(error, deadzone): if error > deadzone: error -= deadzone diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index c0e7358160..07efad73c9 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -5,7 +5,6 @@ import numpy as np from casadi import SX, vertcat, sin, cos from common.realtime import sec_since_boot -from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N from selfdrive.modeld.constants import T_IDXS if __name__ == '__main__': # generating code @@ -18,6 +17,9 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json") X_DIM = 4 P_DIM = 2 +N = 16 +COST_E_DIM = 3 +COST_DIM = COST_E_DIM + 1 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -29,8 +31,8 @@ def gen_lat_model(): x_ego = SX.sym('x_ego') y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') - curv_ego = SX.sym('curv_ego') - model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) + psi_rate_ego = SX.sym('psi_rate_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego) # parameters v_ego = SX.sym('v_ego') @@ -38,22 +40,22 @@ def gen_lat_model(): model.p = vertcat(v_ego, rotation_radius) # controls - curv_rate = SX.sym('curv_rate') - model.u = vertcat(curv_rate) + psi_accel_ego = SX.sym('psi_accel_ego') + model.u = vertcat(psi_accel_ego) # xdot x_ego_dot = SX.sym('x_ego_dot') y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') - curv_ego_dot = SX.sym('curv_ego_dot') + psi_rate_ego_dot = SX.sym('psi_rate_ego_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot) # dynamics model - f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), - v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), - v_ego * curv_ego, - curv_rate) + f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego, + v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego, + psi_rate_ego, + psi_accel_ego) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -72,26 +74,28 @@ def gen_lat_ocp(): ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' - Q = np.diag([0.0, 0.0]) - QR = np.diag([0.0, 0.0, 0.0]) + Q = np.diag(np.zeros(COST_E_DIM)) + QR = np.diag(np.zeros(COST_DIM)) ocp.cost.W = QR ocp.cost.W_e = Q - y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] - curv_rate = ocp.model.u[0] + y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3] + psi_rate_ego_dot = ocp.model.u[0] v_ego = ocp.model.p[0] ocp.parameter_values = np.zeros((P_DIM, )) - ocp.cost.yref = np.zeros((3, )) - ocp.cost.yref_e = np.zeros((2, )) + ocp.cost.yref = np.zeros((COST_DIM, )) + ocp.cost.yref_e = np.zeros((COST_E_DIM, )) # TODO hacky weights to keep behavior the same ocp.model.cost_y_expr = vertcat(y_ego, - ((v_ego +5.0) * psi_ego), - ((v_ego + 5.0) * 4.0 * curv_rate)) + ((v_ego + 5.0) * psi_ego), + ((v_ego + 5.0) * psi_rate_ego), + ((v_ego + 5.0) * psi_rate_ego_dot)) ocp.model.cost_y_expr_e = vertcat(y_ego, - ((v_ego +5.0) * psi_ego)) + ((v_ego + 5.0) * psi_ego), + ((v_ego + 5.0) * psi_rate_ego)) # set constraints ocp.constraints.constr_type = 'BGH' @@ -124,10 +128,10 @@ class LateralMpc(): def reset(self, x0=np.zeros(X_DIM)): self.x_sol = np.zeros((N+1, X_DIM)) self.u_sol = np.zeros((N, 1)) - self.yref = np.zeros((N+1, 3)) + self.yref = np.zeros((N+1, COST_DIM)) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) - self.solver.cost_set(N, "yref", self.yref[N][:2]) + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) # Somehow needed for stable init for i in range(N+1): @@ -140,14 +144,13 @@ class LateralMpc(): self.solve_time = 0.0 self.cost = 0 - def set_weights(self, path_weight, heading_weight, steer_rate_weight): - W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight])) + def set_weights(self, path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost): + W = np.asfortranarray(np.diag([path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost])) for i in range(N): self.solver.cost_set(i, 'W', W) - #TODO hacky weights to keep behavior the same - self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) + self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM]) - def run(self, x0, p, y_pts, heading_pts, curv_rate_pts): + def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts): x0_cp = np.copy(x0) p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) @@ -155,13 +158,13 @@ class LateralMpc(): self.yref[:,0] = y_pts v_ego = p_cp[0] # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts*(v_ego+5.0) - self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0 + self.yref[:,1] = heading_pts * (v_ego+5.0) + self.yref[:,2] = yaw_rate_pts * (v_ego+5.0) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.set(i, "p", p_cp) self.solver.set(N, "p", p_cp) - self.solver.cost_set(N, "yref", self.yref[N][:2]) + self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) t = sec_since_boot() self.solution_status = self.solver.solve() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 2ad5f784d7..48c98e7350 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -3,7 +3,8 @@ from common.realtime import sec_since_boot, DT_MDL from common.numpy_fast import interp from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc -from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N +from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N +from selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log @@ -23,7 +24,7 @@ class LateralPlanner: self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) - self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,)) + self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE) @@ -44,6 +45,7 @@ class LateralPlanner: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) self.plan_yaw = np.array(md.orientation.z) + self.plan_yaw_rate = np.array(md.orientationRate.z) # Lane change logic desire_state = md.meta.desireState @@ -55,24 +57,24 @@ class LateralPlanner: d_path_xyz = self.path_xyz # Heading cost is useful at low speed, otherwise end of plan can be off-heading - heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) - self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE) + heading_cost = interp(v_ego, [5.0, 10.0], [1.0, 0.15]) + self.lat_mpc.set_weights(1.0, heading_cost, 0.0, .075) y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) - curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate) + yaw_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 - assert len(curv_rate_pts) == LAT_MPC_N + 1 + assert len(yaw_rate_pts) == LAT_MPC_N + 1 lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) p = np.array([v_ego, lateral_factor]) self.lat_mpc.run(self.x0, p, y_pts, heading_pts, - curv_rate_pts) + yaw_rate_pts) # init state for next # mpc.u_sol is the desired curvature rate given x0 curv state. # with x0[3] = measured_curvature, this would be the actual desired rate. @@ -103,7 +105,7 @@ class LateralPlanner: lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist() + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/sm['carState'].vEgo).tolist() lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 695222ed4d..2aab4b71af 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -253,7 +253,7 @@ class LongitudinalMpc: cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] + cost_weights = [0., 0.1, 0.2, 5.0, 0.0, 1.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a0363536ca..018136e6f2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -58,7 +58,6 @@ class LongitudinalPlanner: self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) - self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -76,10 +75,7 @@ class LongitudinalPlanner: x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) - # Uniform interp so gradient is less noisy - a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x) - j_sparse = np.gradient(a_sparse, self.t_uniform) - j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse) + j = np.zeros(len(T_IDXS_MPC)) else: x = np.zeros(len(T_IDXS_MPC)) v = np.zeros(len(T_IDXS_MPC)) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 4864dbdc06..9b986c053d 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -1,7 +1,8 @@ import unittest import numpy as np from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc -from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS +from selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS +from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., @@ -9,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur if lat_mpc is None: lat_mpc = LateralMpc() - lat_mpc.set_weights(1., 1., 1.) + lat_mpc.set_weights(1., 1., 0.0, 1.) y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 246f8c2941..5c02e2b15f 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -71,9 +71,9 @@ if use_thneed and arch == "larch64" or GetOption('pc_thneed'): fn = File("models/supercombo").abspath if GetOption('pc_thneed'): - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && GPU=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" else: - cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 PYOPENCL_NO_CACHE=1 MATMUL=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" + cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 MATMUL=1 PYOPENCL_NO_CACHE=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" # is there a better way then listing all of tinygrad? lenv.Command(fn + ".thneed", [fn + ".onnx", diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 8d02eb6b2f..3316e6d114 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -41,11 +41,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true, false, context); #ifdef TEMPORAL - s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); + s->m->addRecurrent(&s->feature_buffer[0], TEMPORAL_SIZE); #endif #ifdef DESIRE - s->m->addDesire(s->pulse_desire, DESIRE_LEN); + s->m->addDesire(s->pulse_desire, DESIRE_LEN*(HISTORY_BUFFER_LEN+1)); #endif #ifdef TRAFFIC_CONVENTION @@ -56,18 +56,20 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { #ifdef DESIRE + std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN); if (desire_in != NULL) { for (int i = 1; i < DESIRE_LEN; i++) { // Model decides when action is completed // so desire input is just a pulse triggered on rising edge if (desire_in[i] - s->prev_desire[i] > .99) { - s->pulse_desire[i] = desire_in[i]; + s->pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN-1)+i] = desire_in[i]; } else { - s->pulse_desire[i] = 0.0; + s->pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN-1)+i] = 0.0; } s->prev_desire[i] = desire_in[i]; } } +LOGT("Desire enqueued"); #endif int rhd_idx = is_rhd; @@ -92,6 +94,12 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, s->m->execute(); LOGT("Execution finished"); + #ifdef TEMPORAL + std::memmove(&s->feature_buffer[0], &s->feature_buffer[FEATURE_LEN], sizeof(float) * FEATURE_LEN*(HISTORY_BUFFER_LEN-1)); + std::memcpy(&s->feature_buffer[FEATURE_LEN*(HISTORY_BUFFER_LEN-1)], &s->output[OUTPUT_SIZE], sizeof(float) * FEATURE_LEN); + LOGT("Features enqueued"); + #endif + return (ModelOutput*)&s->output; } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index e2ee812e44..90767384eb 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -16,6 +16,8 @@ #include "selfdrive/modeld/models/commonmodel.h" #include "selfdrive/modeld/runners/run.h" +constexpr int FEATURE_LEN = 2048; +constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; constexpr int TRAFFIC_CONVENTION_LEN = 2; @@ -233,6 +235,11 @@ struct ModelOutputMeta { }; static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); +struct ModelOutputFeatures { + std::array feature; +}; +static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN)); + struct ModelOutput { const ModelOutputPlans plans; const ModelOutputLaneLines lane_lines; @@ -244,22 +251,24 @@ struct ModelOutput { }; constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); + #ifdef TEMPORAL - constexpr int TEMPORAL_SIZE = 512; + constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN; #else constexpr int TEMPORAL_SIZE = 0; #endif -constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE; +constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN; // TODO: convert remaining arrays to std::array and update model runners struct ModelState { ModelFrame *frame = nullptr; ModelFrame *wide_frame = nullptr; + std::array feature_buffer = {}; std::array output = {}; std::unique_ptr m; #ifdef DESIRE float prev_desire[DESIRE_LEN] = {}; - float pulse_desire[DESIRE_LEN] = {}; + float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {}; #endif #ifdef TRAFFIC_CONVENTION float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 7b11edbe08..37edc36c02 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:50c7fc8565ac69a4b9a0de122e961326820e78bf13659255a89d0ed04be030d5 -size 95167481 +oid sha256:30f30bc1251c03db135564ecbf7dc0bc96cbb07be0ebd3691edd8d555dc087fa +size 58539693 diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index ac7cc68814..d4a11a7c0b 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -9,6 +9,8 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE" import onnxruntime as ort # pylint: disable=import-error +ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8} + def read(sz, tf8=False): dd = [] gt = 0 @@ -18,7 +20,7 @@ def read(sz, tf8=False): assert(len(st) > 0) dd.append(st) gt += len(st) - r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) + r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32) if tf8: r = r / 255. return r @@ -29,22 +31,23 @@ def write(d): def run_loop(m, tf8_input=False): ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] keys = [x.name for x in m.get_inputs()] + itypes = [ORT_TYPES_TO_NP_TYPES[x.type] for x in m.get_inputs()] # run once to initialize CUDA provider if "CUDAExecutionProvider" in m.get_providers(): - m.run(None, dict(zip(keys, [np.zeros(shp, dtype=np.float32) for shp in ishapes]))) + m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes)]))) print("ready to run onnx model", keys, ishapes, file=sys.stderr) while 1: inputs = [] - for k, shp in zip(keys, ishapes): + for k, shp, itp in zip(keys, ishapes, itypes): ts = np.product(shp) #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) - inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) + inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp).astype(itp)) ret = m.run(None, dict(zip(keys, inputs))) #print(ret, file=sys.stderr) for r in ret: - write(r) + write(r.astype(np.float32)) if __name__ == "__main__": diff --git a/selfdrive/modeld/thneed/thneed_common.cc b/selfdrive/modeld/thneed/thneed_common.cc index 21170b13a6..ecdf1237e3 100644 --- a/selfdrive/modeld/thneed/thneed_common.cc +++ b/selfdrive/modeld/thneed/thneed_common.cc @@ -12,7 +12,7 @@ map, int> g_args_size; map g_program_source; void Thneed::stop() { - printf("Thneed::stop: recorded %lu commands\n", cmds.size()); + //printf("Thneed::stop: recorded %lu commands\n", cmds.size()); record = false; } diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index bf6daf5fed..c14956b1b2 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -41,7 +41,7 @@ def remove_ignored_fields(msg, ignore): elif isinstance(v, numbers.Number): val = 0 else: - raise NotImplementedError + raise NotImplementedError('Error ignoring field') setattr(attr, keys[-1], val) return msg.as_reader() diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index ccd89bea9a..ecfacbf5d2 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" SEGMENT = 0 -MAX_FRAMES = 10 if PC else 1300 +MAX_FRAMES = 100 if PC else 1300 SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) @@ -174,7 +174,7 @@ if __name__ == "__main__": 'driverStateV2.dspExecutionTime' ] # TODO this tolerance is absurdly large - tolerance = 5e-1 if PC else None + tolerance = 2.0 if PC else None results: Any = {TEST_ROUTE: {}} log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}} results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 958d3da14d..9727ef3d17 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -c40319a454840d8a2196ec1227d27b536ee14375 +008c0a622b7471c6234690d668f76bcb5dc8d999 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d4d16b726b..0e3cdf797f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1989d8c2a5de94faa3756b7d10fc94e6c063afa5 \ No newline at end of file +e0ffcae8def2fd9c82c547d1f257d4f06a48a3c3 diff --git a/tinygrad_repo b/tinygrad_repo index 2e9b7637b3..82ca9c6666 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2e9b7637b3c3c8895fda9f964215db3a35fe3441 +Subproject commit 82ca9c66664b5f3739e1881f62cb94b72cf0b1fa From 8b33ee97502cf673d769fb7c2283773dd7c3dd86 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Wed, 5 Oct 2022 23:02:35 -0700 Subject: [PATCH 32/62] Bump tinygrad to master --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 82ca9c6666..2993dfe921 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 82ca9c66664b5f3739e1881f62cb94b72cf0b1fa +Subproject commit 2993dfe9219089bd1464741757f0c2ad67fe6a2e From 4cd3753d98bea907929ce09d9f5c218d5fce7653 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 7 Oct 2022 02:20:49 +0800 Subject: [PATCH 33/62] cabana: insert new chart at the top (#25981) * small cleanup * new chart insert at the top --- tools/cabana/chartswidget.cc | 2 +- tools/cabana/mainwin.cc | 1 + tools/cabana/messageswidget.cc | 22 +++++++++++----------- tools/cabana/parser.h | 1 - 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 836eb34946..a504250e6d 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -39,7 +39,7 @@ void ChartsWidget::addChart(const QString &id, const QString &sig_name) { const QString char_name = id + sig_name; if (charts.find(char_name) == charts.end()) { auto chart = new ChartWidget(id, sig_name, this); - main_layout->addWidget(chart); + main_layout->insertWidget(0, chart); charts[char_name] = chart; } } diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 8852987fbe..079f592362 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -5,6 +5,7 @@ MainWindow::MainWindow() : QWidget() { QVBoxLayout *main_layout = new QVBoxLayout(this); + QHBoxLayout *h_layout = new QHBoxLayout(); main_layout->addLayout(h_layout); diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index 840ea25810..daeb37eb9f 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -14,20 +14,11 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { for (const auto &name : dbc_names) { combo->addItem(QString::fromStdString(name)); } - connect(combo, &QComboBox::currentTextChanged, [=](const QString &dbc) { - parser->openDBC(dbc); - }); - // For test purpose - combo->setCurrentText("toyota_nodsu_pt_generated"); dbc_file_layout->addWidget(combo); dbc_file_layout->addStretch(); QPushButton *save_btn = new QPushButton(tr("Save DBC"), this); - QObject::connect(save_btn, &QPushButton::clicked, [=]() { - // TODO: save DBC to file - }); dbc_file_layout->addWidget(save_btn); - main_layout->addLayout(dbc_file_layout); filter = new QLineEdit(this); @@ -44,14 +35,23 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { table_widget->setColumnWidth(2, 80); table_widget->setHorizontalHeaderLabels({tr("Name"), tr("ID"), tr("Count"), tr("Bytes")}); table_widget->horizontalHeader()->setStretchLastSection(true); + main_layout->addWidget(table_widget); + + QObject::connect(parser, &Parser::updated, this, &MessagesWidget::updateState); + QObject::connect(save_btn, &QPushButton::clicked, [=]() { + // TODO: save DBC to file + }); + QObject::connect(combo, &QComboBox::currentTextChanged, [=](const QString &dbc) { + parser->openDBC(dbc); + }); QObject::connect(table_widget, &QTableWidget::itemSelectionChanged, [=]() { const CanData *c = &(parser->can_msgs[table_widget->selectedItems()[1]->text()]); parser->setCurrentMsg(c->id); emit msgChanged(c); }); - main_layout->addWidget(table_widget); - connect(parser, &Parser::updated, this, &MessagesWidget::updateState); + // For test purpose + combo->setCurrentText("toyota_nodsu_pt_generated"); } void MessagesWidget::updateState() { diff --git a/tools/cabana/parser.h b/tools/cabana/parser.h index 2f8c441059..4ed64a90c5 100644 --- a/tools/cabana/parser.h +++ b/tools/cabana/parser.h @@ -12,7 +12,6 @@ #include "opendbc/can/common_dbc.h" #include "tools/replay/replay.h" -const int DATA_LIST_SIZE = 50; const int FPS = 20; const static int LOG_SIZE = 25; From 7156633034ac0232b60f79b2d47bf3e4bbaad1c9 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 7 Oct 2022 02:21:04 +0800 Subject: [PATCH 34/62] cabana: use QFormLayout (#25982) --- tools/cabana/detailwidget.cc | 19 ++++----- tools/cabana/signaledit.cc | 81 ++++++++++-------------------------- 2 files changed, 30 insertions(+), 70 deletions(-) diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index e573a01970..74044cd173 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -224,24 +225,20 @@ void HistoryLog::clear() { EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : id(id), QDialog(parent) { setWindowTitle(tr("Edit message")); QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->addWidget(new QLabel(tr("ID: (%1)").arg(id))); + + QFormLayout *form_layout = new QFormLayout(); + form_layout->addRow("ID", new QLabel(id)); auto msg = const_cast(parser->getMsg(id)); - QHBoxLayout *h_layout = new QHBoxLayout(); - h_layout->addWidget(new QLabel(tr("Name"))); - h_layout->addStretch(); name_edit = new QLineEdit(this); name_edit->setText(msg ? msg->name.c_str() : "untitled"); - h_layout->addWidget(name_edit); - main_layout->addLayout(h_layout); + form_layout->addRow(tr("Name"), name_edit); - h_layout = new QHBoxLayout(); - h_layout->addWidget(new QLabel(tr("Size"))); - h_layout->addStretch(); size_spin = new QSpinBox(this); size_spin->setValue(msg ? msg->size : parser->can_msgs[id].dat.size()); - h_layout->addWidget(size_spin); - main_layout->addLayout(h_layout); + form_layout->addRow(tr("Size"), size_spin); + + main_layout->addLayout(form_layout); auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); main_layout->addWidget(buttonBox); diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index 080b7920de..e233b7b3c2 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -1,6 +1,7 @@ #include "tools/cabana/signaledit.h" #include +#include #include #include #include @@ -9,88 +10,48 @@ // SignalForm SignalForm::SignalForm(const Signal &sig, QWidget *parent) : QWidget(parent) { - QVBoxLayout *v_layout = new QVBoxLayout(this); + QFormLayout *form_layout = new QFormLayout(this); - QHBoxLayout *h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Name"))); name = new QLineEdit(sig.name.c_str()); - h->addWidget(name); - v_layout->addLayout(h); + form_layout->addRow(tr("Name"), name); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Size"))); size = new QSpinBox(); size->setValue(sig.size); - h->addWidget(size); - v_layout->addLayout(h); + form_layout->addRow(tr("Size"), size); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Most significant bit"))); msb = new QSpinBox(); msb->setValue(sig.msb); - h->addWidget(msb); - v_layout->addLayout(h); + form_layout->addRow(tr("Most significant bit"), msb); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Endianness"))); endianness = new QComboBox(); endianness->addItems({"Little", "Big"}); endianness->setCurrentIndex(sig.is_little_endian ? 0 : 1); - h->addWidget(endianness); - v_layout->addLayout(h); + form_layout->addRow(tr("Endianness"), endianness); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("sign"))); sign = new QComboBox(); sign->addItems({"Signed", "Unsigned"}); sign->setCurrentIndex(sig.is_signed ? 0 : 1); - h->addWidget(sign); - v_layout->addLayout(h); + form_layout->addRow(tr("sign"), sign); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Factor"))); factor = new QSpinBox(); factor->setValue(sig.factor); - h->addWidget(factor); - v_layout->addLayout(h); + form_layout->addRow(tr("Factor"), factor); - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Offset"))); offset = new QSpinBox(); offset->setValue(sig.offset); - h->addWidget(offset); - v_layout->addLayout(h); + form_layout->addRow(tr("Offset"), offset); // TODO: parse the following parameters in opendbc - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Unit"))); unit = new QLineEdit(); - h->addWidget(unit); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Comment"))); + form_layout->addRow(tr("Unit"), unit); comment = new QLineEdit(); - h->addWidget(comment); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Minimum value"))); + form_layout->addRow(tr("Comment"), comment); min_val = new QSpinBox(); - h->addWidget(min_val); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Maximum value"))); + form_layout->addRow(tr("Minimum value"), min_val); max_val = new QSpinBox(); - h->addWidget(max_val); - v_layout->addLayout(h); - - h = new QHBoxLayout(); - h->addWidget(new QLabel(tr("Value descriptions"))); + form_layout->addRow(tr("Maximum value"), max_val); val_desc = new QLineEdit(); - h->addWidget(val_desc); - v_layout->addLayout(h); + form_layout->addRow(tr("Value descriptions"), val_desc); } std::optional SignalForm::getSignal() { @@ -126,12 +87,9 @@ SignalEdit::SignalEdit(const QString &id, const Signal &sig, const QString &colo title = new ElidedLabel(this); title->setText(sig.name.c_str()); title->setStyleSheet(QString("font-weight:bold; color:%1").arg(color)); - connect(title, &ElidedLabel::clicked, [=]() { - edit_container->isVisible() ? edit_container->hide() : edit_container->show(); - icon->setText(edit_container->isVisible() ? "▼" : ">"); - }); title_layout->addWidget(title); title_layout->addStretch(); + plot_btn = new QPushButton("📈"); plot_btn->setStyleSheet("font-size:16px"); plot_btn->setToolTip(tr("Show Plot")); @@ -148,16 +106,21 @@ SignalEdit::SignalEdit(const QString &id, const Signal &sig, const QString &colo QHBoxLayout *h = new QHBoxLayout(); remove_btn = new QPushButton(tr("Remove Signal")); - QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); h->addWidget(remove_btn); h->addStretch(); QPushButton *save_btn = new QPushButton(tr("Save")); - QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); h->addWidget(save_btn); v_layout->addLayout(h); edit_container->setVisible(false); main_layout->addWidget(edit_container); + + QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); + QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); + QObject::connect(title, &ElidedLabel::clicked, [=]() { + edit_container->isVisible() ? edit_container->hide() : edit_container->show(); + icon->setText(edit_container->isVisible() ? "▼" : ">"); + }); } void SignalEdit::save() { From 2d9e7972598918a72de736e12edbc2967d77e841 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 7 Oct 2022 04:35:56 +0800 Subject: [PATCH 35/62] cabana: Fix the incorrect Y axis (#25984) --- tools/cabana/chartswidget.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index a504250e6d..f964e5606a 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -136,6 +136,7 @@ void ChartWidget::updateSeries() { vals.clear(); vals.reserve(3 * 60 * 100); + double min_y = 0, max_y = 0; uint64_t route_start_time = parser->replay->routeStartTime(); for (auto &evt : *events) { if (evt->which == cereal::Event::Which::CAN) { @@ -147,6 +148,9 @@ void ChartWidget::updateSeries() { val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0; } double value = val * sig->factor + sig->offset; + if (value < min_y) min_y = value; + if (value > max_y) max_y = value; + double ts = (evt->mono_time - route_start_time) / (double)1e9; // seconds vals.push_back({ts, value}); } @@ -157,6 +161,7 @@ void ChartWidget::updateSeries() { series->replace(vals); auto [begin, end] = parser->range(); chart_view->chart()->axisX()->setRange(begin, end); + chart_view->chart()->axisY()->setRange(min_y * 0.95, max_y * 1.05); } void ChartWidget::rangeChanged(qreal min, qreal max) { From 9935a651157c512c37926b43fdcda3566de11261 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 6 Oct 2022 13:47:09 -0700 Subject: [PATCH 36/62] always enable rawgps on tici (#25987) --- selfdrive/manager/process_config.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 06efdbb960..3f63fbb959 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -43,6 +43,7 @@ procs = [ PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), PythonProcess("laikad", "selfdrive.locationd.laikad"), + PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=TICI), PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("paramsd", "selfdrive.locationd.paramsd"), @@ -55,11 +56,9 @@ procs = [ PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), PythonProcess("statsd", "selfdrive.statsd", offroad=True), + # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar), - - # Experimental - PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=(TICI and os.path.isfile("/persist/comma/use-quectel-rawgps"))), ] managed_processes = {p.name: p for p in procs} From 9ec262bbfd09c7cd12773cabab0bb6d933b4d13e Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 7 Oct 2022 04:57:11 +0800 Subject: [PATCH 37/62] cabana: Docking and undocking charts (#25983) * floating dock charts * more button * setMinimumSize * move reset zoom button to title bar * show chart count * cleanup * reduce flicker * dont update linemarker if pos not changed * cleanup * remove blank line * always show dock/undock button --- tools/cabana/chartswidget.cc | 125 ++++++++++++++++++++++++++++------- tools/cabana/chartswidget.h | 34 +++++++--- tools/cabana/mainwin.cc | 40 ++++++++--- tools/cabana/mainwin.h | 7 +- tools/cabana/signaledit.cc | 2 - tools/cabana/videowidget.cc | 1 + 6 files changed, 161 insertions(+), 48 deletions(-) diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index f964e5606a..3e1a8b8410 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -2,7 +2,6 @@ #include #include -#include #include #include #include @@ -27,30 +26,105 @@ int64_t get_raw_value(uint8_t *data, size_t data_size, const Signal &sig) { return ret; } +// ChartsWidget + ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) { - main_layout = new QVBoxLayout(this); + QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(0, 0, 0, 0); - connect(parser, &Parser::showPlot, this, &ChartsWidget::addChart); - connect(parser, &Parser::hidePlot, this, &ChartsWidget::removeChart); - connect(parser, &Parser::signalRemoved, this, &ChartsWidget::removeChart); + + // title bar + title_bar = new QWidget(this); + QHBoxLayout *title_layout = new QHBoxLayout(title_bar); + title_label = new QLabel(tr("Charts")); + + title_layout->addWidget(title_label); + title_layout->addStretch(); + + reset_zoom_btn = new QPushButton("⟲", this); + reset_zoom_btn->setVisible(false); + reset_zoom_btn->setFixedSize(30, 30); + reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)")); + title_layout->addWidget(reset_zoom_btn); + + remove_all_btn = new QPushButton(tr("✖")); + remove_all_btn->setVisible(false); + remove_all_btn->setToolTip(tr("Remove all charts")); + remove_all_btn->setFixedSize(30, 30); + title_layout->addWidget(remove_all_btn); + + dock_btn = new QPushButton(); + dock_btn->setFixedSize(30, 30); + updateDockButton(); + title_layout->addWidget(dock_btn); + + main_layout->addWidget(title_bar, 0, Qt::AlignTop); + + // charts + QWidget *charts_container = new QWidget(this); + QVBoxLayout *charts_main = new QVBoxLayout(charts_container); + charts_layout = new QVBoxLayout(); + charts_main->addLayout(charts_layout); + charts_main->addStretch(); + + QScrollArea *charts_scroll = new QScrollArea(this); + charts_scroll->setWidgetResizable(true); + charts_scroll->setWidget(charts_container); + charts_scroll->setFrameShape(QFrame::NoFrame); + charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + main_layout->addWidget(charts_scroll); + + QObject::connect(parser, &Parser::showPlot, this, &ChartsWidget::addChart); + QObject::connect(parser, &Parser::hidePlot, this, &ChartsWidget::removeChart); + QObject::connect(parser, &Parser::signalRemoved, this, &ChartsWidget::removeChart); + QObject::connect(reset_zoom_btn, &QPushButton::clicked, parser, &Parser::resetRange); + QObject::connect(remove_all_btn, &QPushButton::clicked, this, &ChartsWidget::removeAll); + QObject::connect(dock_btn, &QPushButton::clicked, [=]() { + emit dock(!docking); + docking = !docking; + updateDockButton(); + }); +} + +void ChartsWidget::updateDockButton() { + dock_btn->setText(docking ? "⬈" : "⬋"); + dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts")); } void ChartsWidget::addChart(const QString &id, const QString &sig_name) { const QString char_name = id + sig_name; if (charts.find(char_name) == charts.end()) { auto chart = new ChartWidget(id, sig_name, this); - main_layout->insertWidget(0, chart); + charts_layout->insertWidget(0, chart); charts[char_name] = chart; } + remove_all_btn->setVisible(true); + reset_zoom_btn->setVisible(true); + title_label->setText(tr("Charts (%1)").arg(charts.size())); } void ChartsWidget::removeChart(const QString &id, const QString &sig_name) { if (auto it = charts.find(id + sig_name); it != charts.end()) { it->second->deleteLater(); charts.erase(it); + if (charts.empty()) { + remove_all_btn->setVisible(false); + reset_zoom_btn->setVisible(false); + } } + title_label->setText(tr("Charts (%1)").arg(charts.size())); +} + +void ChartsWidget::removeAll() { + for (auto [_, chart] : charts) + chart->deleteLater(); + charts.clear(); + remove_all_btn->setVisible(false); + reset_zoom_btn->setVisible(false); } +// ChartWidget + ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *parent) : id(id), sig_name(sig_name), QWidget(parent) { QStackedLayout *stacked = new QStackedLayout(this); stacked->setStackingMode(QStackedLayout::StackAll); @@ -64,17 +138,13 @@ ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *pa header->setStyleSheet("background-color:white"); QHBoxLayout *header_layout = new QHBoxLayout(header); header_layout->setContentsMargins(11, 11, 11, 0); - auto title = new QLabel(tr("%1 %2").arg(parser->getMsg(id)->name.c_str()).arg(id)); + QLabel *title = new QLabel(tr("%1 %2").arg(parser->getMsg(id)->name.c_str()).arg(id)); header_layout->addWidget(title); header_layout->addStretch(); - zoom_label = new QLabel("", this); - header_layout->addWidget(zoom_label); - QPushButton *zoom_in = new QPushButton("↺", this); - zoom_in->setToolTip(tr("reset zoom")); - QObject::connect(zoom_in, &QPushButton::clicked, []() { parser->resetRange(); }); - header_layout->addWidget(zoom_in); QPushButton *remove_btn = new QPushButton("✖", this); + remove_btn->setFixedSize(30, 30); + remove_btn->setToolTip(tr("Remove chart")); QObject::connect(remove_btn, &QPushButton::clicked, [=]() { emit parser->hidePlot(id, sig_name); }); @@ -108,7 +178,7 @@ ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *pa chart_layout->addStretch(); stacked->addWidget(chart_widget); - line_marker = new LineMarker(chart, this); + line_marker = new LineMarker(this); stacked->addWidget(line_marker); line_marker->setAttribute(Qt::WA_TransparentForMouseEvents, true); line_marker->raise(); @@ -122,7 +192,15 @@ ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *pa } void ChartWidget::updateState() { - line_marker->update(); + auto chart = chart_view->chart(); + auto axis_x = dynamic_cast(chart->axisX()); + if (axis_x->max() <= axis_x->min()) return; + + int x = chart->plotArea().left() + chart->plotArea().width() * (parser->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min()); + if (line_marker_x != x) { + line_marker->setX(x); + line_marker_x = x; + } } void ChartWidget::updateSeries() { @@ -182,16 +260,15 @@ void ChartWidget::rangeChanged(qreal min, qreal max) { chart_view->chart()->axisY()->setRange(min_y * 0.95, max_y * 1.05); } -LineMarker::LineMarker(QChart *chart, QWidget *parent) : chart(chart), QWidget(parent) {} +// LineMarker -void LineMarker::paintEvent(QPaintEvent *event) { - auto axis_x = dynamic_cast(chart->axisX()); - if (axis_x->max() <= axis_x->min()) return; +void LineMarker::setX(double x) { + x_pos = x; + update(); +} - double x = chart->plotArea().left() + chart->plotArea().width() * (parser->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min()); +void LineMarker::paintEvent(QPaintEvent *event) { QPainter p(this); - QPen pen = QPen(Qt::black); - pen.setWidth(2); - p.setPen(pen); - p.drawLine(QPointF{x, 50.}, QPointF{x, (qreal)height() - 11}); + p.setPen(QPen(Qt::black, 2)); + p.drawLine(QPointF{x_pos, 50.}, QPointF{x_pos, (qreal)height() - 11}); } diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 1be5fdeecb..0413d65e09 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -16,11 +17,12 @@ class LineMarker : public QWidget { Q_OBJECT public: - LineMarker(QChart *chart, QWidget *parent); - void paintEvent(QPaintEvent *event) override; + LineMarker(QWidget *parent) : QWidget(parent) {} + void setX(double x); private: - QChart *chart; + void paintEvent(QPaintEvent *event) override; + double x_pos = 0.0; }; class ChartWidget : public QWidget { @@ -30,7 +32,7 @@ public: ChartWidget(const QString &id, const QString &sig_name, QWidget *parent); inline QChart *chart() const { return chart_view->chart(); } -protected: +private: void updateState(); void addData(const CanData &can_data, const Signal &sig); void updateSeries(); @@ -38,9 +40,9 @@ protected: QString id; QString sig_name; - QLabel *zoom_label; QChartView *chart_view = nullptr; LineMarker *line_marker = nullptr; + double line_marker_x = 0.0; QList vals; }; @@ -49,14 +51,26 @@ class ChartsWidget : public QWidget { public: ChartsWidget(QWidget *parent = nullptr); - inline bool hasChart(const QString &id, const QString &sig_name) { - return charts.find(id+sig_name) != charts.end(); - } void addChart(const QString &id, const QString &sig_name); void removeChart(const QString &id, const QString &sig_name); + void removeAll(); + inline bool hasChart(const QString &id, const QString &sig_name) { + return charts.find(id + sig_name) != charts.end(); + } + +signals: + void dock(bool floating); + +private: void updateState(); + void updateDockButton(); -protected: - QVBoxLayout *main_layout; + QWidget *title_bar; + QLabel *title_label; + bool docking = true; + QPushButton *dock_btn; + QPushButton *reset_zoom_btn; + QPushButton *remove_all_btn; + QVBoxLayout *charts_layout; std::map charts; }; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 079f592362..49e6cd2cca 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -1,6 +1,7 @@ #include "tools/cabana/mainwin.h" #include +#include #include MainWindow::MainWindow() : QWidget() { @@ -16,23 +17,42 @@ MainWindow::MainWindow() : QWidget() { detail_widget->setFixedWidth(600); h_layout->addWidget(detail_widget); - // right widget + // right widgets QWidget *right_container = new QWidget(this); right_container->setFixedWidth(640); - QVBoxLayout *r_layout = new QVBoxLayout(right_container); + r_layout = new QVBoxLayout(right_container); + video_widget = new VideoWidget(this); - r_layout->addWidget(video_widget); + r_layout->addWidget(video_widget, 0, Qt::AlignTop); charts_widget = new ChartsWidget(this); - QScrollArea *scroll = new QScrollArea(this); - scroll->setWidget(charts_widget); - scroll->setWidgetResizable(true); - scroll->setFrameShape(QFrame::NoFrame); - scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding); - r_layout->addWidget(scroll); + r_layout->addWidget(charts_widget); h_layout->addWidget(right_container); QObject::connect(messages_widget, &MessagesWidget::msgChanged, detail_widget, &DetailWidget::setMsg); + QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts); +} + +void MainWindow::dockCharts(bool dock) { + charts_widget->setUpdatesEnabled(false); + if (dock && floating_window) { + r_layout->addWidget(charts_widget); + floating_window->deleteLater(); + floating_window = nullptr; + } else if (!dock && !floating_window) { + floating_window = new QWidget(nullptr); + floating_window->setLayout(new QVBoxLayout()); + floating_window->layout()->addWidget(charts_widget); + floating_window->setWindowFlags(Qt::WindowTitleHint | Qt::WindowMaximizeButtonHint | Qt::WindowMinimizeButtonHint); + floating_window->setMinimumSize(QGuiApplication::primaryScreen()->size() / 2); + floating_window->showMaximized(); + } + charts_widget->setUpdatesEnabled(true); +} + +void MainWindow::closeEvent(QCloseEvent *event) { + if (floating_window) + floating_window->deleteLater(); + QWidget::closeEvent(event); } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index 82ecceb02b..bcd15e4d8e 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -1,7 +1,5 @@ #pragma once -#include - #include "tools/cabana/chartswidget.h" #include "tools/cabana/detailwidget.h" #include "tools/cabana/messageswidget.h" @@ -13,10 +11,15 @@ class MainWindow : public QWidget { public: MainWindow(); + void dockCharts(bool dock); protected: + void closeEvent(QCloseEvent *event) override; + VideoWidget *video_widget; MessagesWidget *messages_widget; DetailWidget *detail_widget; ChartsWidget *charts_widget; + QWidget *floating_window = nullptr; + QVBoxLayout *r_layout; }; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc index e233b7b3c2..c214adab09 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signaledit.cc @@ -91,9 +91,7 @@ SignalEdit::SignalEdit(const QString &id, const Signal &sig, const QString &colo title_layout->addStretch(); plot_btn = new QPushButton("📈"); - plot_btn->setStyleSheet("font-size:16px"); plot_btn->setToolTip(tr("Show Plot")); - plot_btn->setContentsMargins(5, 5, 5, 5); plot_btn->setFixedSize(30, 30); QObject::connect(plot_btn, &QPushButton::clicked, [=]() { emit parser->showPlot(id, name_); }); title_layout->addWidget(plot_btn); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index dbf988d8f2..c8e1ad680f 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -64,6 +64,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { } main_layout->addLayout(control_layout); + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged); QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState); From e66a70b9e988ae34acff117a686818e913b797f7 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Thu, 6 Oct 2022 14:15:36 -0700 Subject: [PATCH 38/62] bump opendbc --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index 9ddcdb22c4..ae2fd934ce 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 9ddcdb22c4929baf310295e832668e6e7fcfa602 +Subproject commit ae2fd934ceb8501c56a0802564c14963dbb201ac From 9e6265ce2188ac48442b07a77e86ca4fd3eba06b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 6 Oct 2022 15:01:57 -0700 Subject: [PATCH 39/62] CI: re-enable power draw test (#25988) * CI: re-enable power draw test * adjust for ngrl --- Jenkinsfile | 2 +- system/hardware/tici/test_power_draw.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index c4038090e1..b9b9eda667 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -127,7 +127,7 @@ pipeline { steps { phone_steps("tici2", [ ["build", "cd selfdrive/manager && ./build.py"], - //["test power draw", "python system/hardware/tici/test_power_draw.py"], + ["test power draw", "python system/hardware/tici/test_power_draw.py"], ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], diff --git a/system/hardware/tici/test_power_draw.py b/system/hardware/tici/test_power_draw.py index 4b380372b9..4830975917 100755 --- a/system/hardware/tici/test_power_draw.py +++ b/system/hardware/tici/test_power_draw.py @@ -20,7 +20,7 @@ class Proc: PROCS = [ Proc('camerad', 2.15), - Proc('modeld', 1.0, atol=0.15), + Proc('modeld', 1.15, atol=0.2), Proc('dmonitoringmodeld', 0.35), Proc('encoderd', 0.23), ] From 750b96aaedb88defd522a60a4bb5fbfeb46332db Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 7 Oct 2022 06:02:22 +0800 Subject: [PATCH 40/62] cabana: improve time control (#25985) * group signals/slots together * slider:fix wrong minimum * add TODO * moveing to mouse click position * show tickmark * fix seek back to the old pos after sliderReleased * reduce data copied in queued connection * drop packets while seeking * install event filter in streaming * stop replay in dctor --- tools/cabana/detailwidget.cc | 2 +- tools/cabana/messageswidget.cc | 2 +- tools/cabana/parser.cc | 61 ++++++++++++++++------------------ tools/cabana/parser.h | 14 ++++---- tools/cabana/videowidget.cc | 50 ++++++++++++++++++++-------- tools/cabana/videowidget.h | 14 +++++++- tools/replay/replay.cc | 2 ++ tools/replay/replay.h | 10 ++++++ 8 files changed, 100 insertions(+), 55 deletions(-) diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc index 74044cd173..1b6552804e 100644 --- a/tools/cabana/detailwidget.cc +++ b/tools/cabana/detailwidget.cc @@ -206,7 +206,7 @@ void HistoryLog::updateState() { const auto &c = parser->history_log[i]; auto label = labels[i]; label->setVisible(true); - label->setText(QString("%1 %2").arg(c.ts, 0, 'f', 3).arg(c.hex_dat)); + label->setText(QString("%1 %2").arg(c.ts, 0, 'f', 3).arg(toHex(c.dat))); } for (; i < std::size(labels); ++i) { diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index daeb37eb9f..7de3507b3d 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -83,7 +83,7 @@ void MessagesWidget::updateState() { getTableItem(i, 0)->setText(name); getTableItem(i, 1)->setText(c.id); getTableItem(i, 2)->setText(QString::number(parser->counters[c.id])); - getTableItem(i, 3)->setText(c.hex_dat); + getTableItem(i, 3)->setText(toHex(c.dat)); table_widget->showRow(i); i++; } diff --git a/tools/cabana/parser.cc b/tools/cabana/parser.cc index 3950d8bd57..f4bacbb86d 100644 --- a/tools/cabana/parser.cc +++ b/tools/cabana/parser.cc @@ -6,27 +6,25 @@ Parser *parser = nullptr; +static bool event_filter(const Event *e, void *opaque) { + Parser *p = (Parser*)opaque; + return p->eventFilter(e); +} + Parser::Parser(QObject *parent) : QObject(parent) { parser = this; qRegisterMetaType>(); QObject::connect(this, &Parser::received, this, &Parser::process, Qt::QueuedConnection); - - thread = new QThread(); - connect(thread, &QThread::started, [=]() { recvThread(); }); - QObject::connect(thread, &QThread::finished, thread, &QThread::deleteLater); - thread->start(); } Parser::~Parser() { replay->stop(); - exit = true; - thread->quit(); - thread->wait(); } bool Parser::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) { replay = new Replay(route, {"can", "roadEncodeIdx"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this); + replay->installEventFilter(event_filter, this); QObject::connect(replay, &Replay::segmentsMerged, this, &Parser::segmentsMerged); if (replay->load()) { replay->start(); @@ -47,9 +45,9 @@ void Parser::openDBC(const QString &name) { void Parser::process(std::vector msgs) { static double prev_update_ts = 0; + for (const auto &can_data : msgs) { can_msgs[can_data.id] = can_data; - current_sec = can_data.ts; ++counters[can_data.id]; if (can_data.id == current_msg_id) { @@ -59,46 +57,45 @@ void Parser::process(std::vector msgs) { history_log.push_front(can_data); } } - double current_ts = millis_since_boot(); - if ((current_ts - prev_update_ts) > 1000.0 / FPS) { - prev_update_ts = current_ts; + double now = millis_since_boot(); + if ((now - prev_update_ts) > 1000.0 / FPS) { + prev_update_ts = now; emit updated(); } if (current_sec < begin_sec || current_sec > end_sec) { // loop replay in selected range. - replay->seekTo(begin_sec, false); + seekTo(begin_sec); } } -void Parser::recvThread() { - AlignedBuffer aligned_buf; - std::unique_ptr context(Context::create()); - std::unique_ptr subscriber(SubSocket::create(context.get(), "can")); - subscriber->setTimeout(100); +bool Parser::eventFilter(const Event *event) { + // drop packets when the GUI thread is calling seekTo. to make sure the current_sec is accurate. + if (!seeking && event->which == cereal::Event::Which::CAN) { + current_sec = (event->mono_time - replay->routeStartTime()) / (double)1e9; - std::vector can; - while (!exit) { - std::unique_ptr msg(subscriber->receive()); - if (!msg) continue; + auto can = event->event.getCan(); + msgs_buf.clear(); + msgs_buf.reserve(can.size()); - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); - cereal::Event::Reader event = cmsg.getRoot(); - - can.clear(); - can.reserve(event.getCan().size()); - for (const auto &c : event.getCan()) { - CanData &data = can.emplace_back(); + for (const auto &c : can) { + CanData &data = msgs_buf.emplace_back(); data.address = c.getAddress(); data.bus_time = c.getBusTime(); data.source = c.getSrc(); data.dat.append((char *)c.getDat().begin(), c.getDat().size()); - data.hex_dat = data.dat.toHex(' ').toUpper(); data.id = QString("%1:%2").arg(data.source).arg(data.address, 1, 16); - data.ts = (event.getLogMonoTime() - replay->routeStartTime()) / (double)1e9; // seconds + data.ts = current_sec; } - emit received(can); + emit received(msgs_buf); } + return true; +} + +void Parser::seekTo(double ts) { + seeking = true; + replay->seekTo(ts, false); + seeking = false; } void Parser::addNewMsg(const Msg &msg) { diff --git a/tools/cabana/parser.h b/tools/cabana/parser.h index 4ed64a90c5..1632fcf6a6 100644 --- a/tools/cabana/parser.h +++ b/tools/cabana/parser.h @@ -6,7 +6,6 @@ #include #include #include -#include #include "opendbc/can/common.h" #include "opendbc/can/common_dbc.h" @@ -22,7 +21,6 @@ struct CanData { uint16_t bus_time; uint8_t source; QByteArray dat; - QString hex_dat; }; class Parser : public QObject { @@ -32,11 +30,13 @@ public: Parser(QObject *parent); ~Parser(); static uint32_t addressFromId(const QString &id); + bool eventFilter(const Event *event); bool loadRoute(const QString &route, const QString &data_dir, bool use_qcam); void openDBC(const QString &name); void saveDBC(const QString &name) {} void addNewMsg(const Msg &msg); void removeSignal(const QString &id, const QString &sig_name); + void seekTo(double ts); const Signal *getSig(const QString &id, const QString &sig_name); void setRange(double min, double max); void resetRange(); @@ -66,13 +66,11 @@ public: QList history_log; protected: - void recvThread(); void process(std::vector can); void segmentsMerged(); - double current_sec = 0.; - std::atomic exit = false; - QThread *thread; + std::atomic current_sec = 0.; + std::atomic seeking = false; QString dbc_name; double begin_sec = 0; double end_sec = 0; @@ -82,6 +80,7 @@ protected: DBC *dbc = nullptr; std::map msg_map; QString current_msg_id; + std::vector msgs_buf; }; Q_DECLARE_METATYPE(std::vector); @@ -89,5 +88,8 @@ Q_DECLARE_METATYPE(std::vector); // TODO: Add helper function in dbc.h int bigEndianStartBitsIndex(int start_bit); int bigEndianBitIndex(int index); +inline QString toHex(const QByteArray &dat) { + return dat.toHex(' ').toUpper(); +} extern Parser *parser; diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index c8e1ad680f..26fb845340 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -15,6 +16,7 @@ inline QString formatTime(int seconds) { VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); + // TODO: figure out why the CameraViewWidget crashed occasionally. cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, false, this); cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); main_layout->addWidget(cam_widget); @@ -24,16 +26,12 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { time_label = new QLabel("00:00"); slider_layout->addWidget(time_label); - slider = new QSlider(Qt::Horizontal, this); - QObject::connect(slider, &QSlider::sliderMoved, [=]() { - time_label->setText(formatTime(slider->value())); - }); - slider->setSingleStep(1); + slider = new Slider(this); + slider->setSingleStep(0); + slider->setMinimum(0); + slider->setTickInterval(60); + slider->setTickPosition(QSlider::TicksBelow); slider->setMaximum(parser->replay->totalSeconds()); - QObject::connect(slider, &QSlider::sliderReleased, [=]() { - time_label->setText(formatTime(slider->value())); - parser->replay->seekTo(slider->value(), false); - }); slider_layout->addWidget(slider); total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); @@ -45,11 +43,6 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QHBoxLayout *control_layout = new QHBoxLayout(); QPushButton *play = new QPushButton("⏸"); play->setStyleSheet("font-weight:bold"); - QObject::connect(play, &QPushButton::clicked, [=]() { - bool is_paused = parser->replay->isPaused(); - play->setText(is_paused ? "⏸" : "▶"); - parser->replay->pause(!is_paused); - }); control_layout->addWidget(play); QButtonGroup *group = new QButtonGroup(this); @@ -68,6 +61,20 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged); QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState); + QObject::connect(slider, &QSlider::sliderMoved, [=]() { time_label->setText(formatTime(slider->value())); }); + QObject::connect(slider, &QSlider::sliderReleased, [this]() { setPosition(slider->value()); }); + QObject::connect(slider, &Slider::setPosition, this, &VideoWidget::setPosition); + + QObject::connect(play, &QPushButton::clicked, [=]() { + bool is_paused = parser->replay->isPaused(); + play->setText(is_paused ? "⏸" : "▶"); + parser->replay->pause(!is_paused); + }); +} + +void VideoWidget::setPosition(int value) { + time_label->setText(formatTime(value)); + parser->seekTo(value); } void VideoWidget::rangeChanged(double min, double max) { @@ -77,6 +84,7 @@ void VideoWidget::rangeChanged(double min, double max) { } time_label->setText(formatTime(min)); total_time_label->setText(formatTime(max)); + slider->setMinimum(min); slider->setMaximum(max); slider->setValue(parser->currentSec()); } @@ -88,3 +96,17 @@ void VideoWidget::updateState() { slider->setValue(current_sec); } } + +// Slider +// TODO: show timeline bar like what replay did. +Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { +} + +void Slider::mousePressEvent(QMouseEvent *e) { + QSlider::mousePressEvent(e); + if (e->button() == Qt::LeftButton && !isSliderDown()) { + int value = minimum() + ((maximum() - minimum()) * e->x()) / width(); + setValue(value); + emit setPosition(value); + } +} diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index 813516e78f..a3605459e0 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -6,6 +6,17 @@ #include "selfdrive/ui/qt/widgets/cameraview.h" +class Slider : public QSlider { + Q_OBJECT + +public: + Slider(QWidget *parent); + void mousePressEvent(QMouseEvent* e) override; + +signals: + void setPosition(int value); +}; + class VideoWidget : public QWidget { Q_OBJECT @@ -15,8 +26,9 @@ public: protected: void rangeChanged(double min, double max); void updateState(); + void setPosition(int value); CameraViewWidget *cam_widget; QLabel *time_label, *total_time_label; - QSlider *slider; + Slider *slider; }; diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index b64e87a03e..80e58b47a3 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -325,6 +325,8 @@ void Replay::startStream(const Segment *cur_segment) { } void Replay::publishMessage(const Event *e) { + if (event_filter && event_filter(e, filter_opaque)) return; + if (sm == nullptr) { auto bytes = e->bytes(); int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); diff --git a/tools/replay/replay.h b/tools/replay/replay.h index aa0bbc33e7..725bd1a27e 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -32,6 +32,7 @@ enum class FindFlag { }; enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag }; +typedef bool (*replayEventFilter)(const Event *, void *); class Replay : public QObject { Q_OBJECT @@ -47,6 +48,13 @@ public: void seekToFlag(FindFlag flag); void seekTo(double seconds, bool relative); inline bool isPaused() const { return paused_; } + // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming. + // the filter function must return true if the event should be filtered. + // otherwise it must return false. + inline void installEventFilter(replayEventFilter filter, void *opaque) { + filter_opaque = opaque; + event_filter = filter; + } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } @@ -119,4 +127,6 @@ protected: std::set allow_list; std::string car_fingerprint_; float speed_ = 1.0; + replayEventFilter event_filter = nullptr; + void *filter_opaque = nullptr; }; From e7805eb5c54d336a93af730594ef1ccc725dad82 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 6 Oct 2022 15:24:52 -0700 Subject: [PATCH 41/62] FPv2: fixed fingerprint overrides query result (#25990) * query FW versions if fixed (override after) * skip here * also skip here * and here --- scripts/launch_corolla.sh | 1 + selfdrive/car/car_helpers.py | 2 +- selfdrive/test/profiling/profiler.py | 1 + tools/sim/launch_openpilot.sh | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/scripts/launch_corolla.sh b/scripts/launch_corolla.sh index 0801938e71..146fbacf0a 100755 --- a/scripts/launch_corolla.sh +++ b/scripts/launch_corolla.sh @@ -3,4 +3,5 @@ DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" export FINGERPRINT="TOYOTA COROLLA TSS2 2019" +export SKIP_FW_QUERY="1" $DIR/../launch_openpilot.sh diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 273364071b..e1c299d5c3 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -81,7 +81,7 @@ def fingerprint(logcan, sendcan): skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) ecu_rx_addrs = set() - if not fixed_fingerprint and not skip_fw_query: + if not skip_fw_query: # Vin query only reliably works through OBDII bus = 1 diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py index 91226fc577..732a69eebd 100755 --- a/selfdrive/test/profiling/profiler.py +++ b/selfdrive/test/profiling/profiler.py @@ -53,6 +53,7 @@ def profile(proc, func, car='toyota'): msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1")) os.environ['FINGERPRINT'] = fingerprint + os.environ['SKIP_FW_QUERY'] = "1" os.environ['REPLAY'] = "1" def run(sm, pm, can_sock): diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index 15f45b4cc2..adabc40c2e 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -3,6 +3,7 @@ export PASSIVE="0" export NOBOARD="1" export SIMULATION="1" +export SKIP_FW_QUERY="1" export FINGERPRINT="HONDA CIVIC 2016" export BLOCK="camerad,loggerd,encoderd" From cf1e978ad1e1bd339a3d0b43b809918036df6703 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 6 Oct 2022 15:56:41 -0700 Subject: [PATCH 42/62] Fingerprinting: log if using cache (#25989) * Log if using cache or not * log in the same log message --- selfdrive/car/car_helpers.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index e1c299d5c3..4a8fd5fbd9 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -95,16 +95,19 @@ def fingerprint(logcan, sendcan): cloudlog.warning("Using cached CarParams") vin, vin_rx_addr = cached_params.carVin, 0 car_fw = list(cached_params.carFw) + cached = True else: cloudlog.warning("Getting VIN & FW versions") vin_rx_addr, vin = get_vin(logcan, sendcan, bus) ecu_rx_addrs = get_present_ecus(logcan, sendcan) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs) + cached = False exact_fw_match, fw_candidates = match_fw_to_car(car_fw) else: vin, vin_rx_addr = VIN_UNKNOWN, 0 exact_fw_match, fw_candidates, car_fw = True, set(), [] + cached = False if not is_valid_vin(vin): cloudlog.event("Malformed VIN", vin=vin, error=True) @@ -165,7 +168,7 @@ def fingerprint(logcan, sendcan): car_fingerprint = fixed_fingerprint source = car.CarParams.FingerprintSource.fixed - cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, + cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached, fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True) return car_fingerprint, finger, vin, car_fw, source, exact_match From 01d05f66fe5a189209538650dce319b2f7e192ee Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 6 Oct 2022 16:46:15 -0700 Subject: [PATCH 43/62] auto-detect pigeon or quectel (#25991) * auto-detect pigeon or quectel * persistent * fix sim * fix process replay * fix locationd unit tests * fix that Co-authored-by: Comma Device --- common/params.cc | 1 + common/params.h | 4 ++-- common/params_pyx.pyx | 6 +++--- selfdrive/locationd/laikad.py | 2 +- selfdrive/locationd/locationd.cc | 6 +++--- selfdrive/locationd/test/test_locationd.py | 1 + selfdrive/sensord/pigeond.py | 9 +++++++-- selfdrive/test/process_replay/process_replay.py | 1 + tools/sim/bridge.py | 7 +++++-- 9 files changed, 24 insertions(+), 13 deletions(-) diff --git a/common/params.cc b/common/params.cc index d4c5cd7caf..a23c0e003e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -166,6 +166,7 @@ std::unordered_map keys = { {"TermsVersion", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, + {"UbloxAvailable", PERSISTENT}, {"UpdateAvailable", CLEAR_ON_MANAGER_START}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START}, diff --git a/common/params.h b/common/params.h index 7758a015f6..aecb3ee471 100644 --- a/common/params.h +++ b/common/params.h @@ -29,8 +29,8 @@ public: // helpers for reading values std::string get(const std::string &key, bool block = false); - inline bool getBool(const std::string &key) { - return get(key) == "1"; + inline bool getBool(const std::string &key, bool block = false) { + return get(key, block) == "1"; } std::map readAll(); diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index bbddda46ea..9d8933609f 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -16,7 +16,7 @@ cdef extern from "common/params.h": cdef cppclass c_Params "Params": c_Params(string) nogil string get(string, bool) nogil - bool getBool(string) nogil + bool getBool(string, bool) nogil int remove(string) nogil int put(string, string) nogil int putBool(string, bool) nogil @@ -68,11 +68,11 @@ cdef class Params: return val if encoding is None else val.decode(encoding) - def get_bool(self, key): + def get_bool(self, key, bool block=False): cdef string k = self.check_key(key) cdef bool r with nogil: - r = self.p.getBool(k) + r = self.p.getBool(k, block) return r def put(self, key, dat): diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index c7f2d2ceac..c4fdfb9871 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -328,7 +328,7 @@ class EphemerisSourceType(IntEnum): def main(sm=None, pm=None): - use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps") + use_qcom = not Params().get_bool("UbloxAvailable", block=True) if use_qcom: raw_gnss_socket = "qcomGnss" else: diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index ac340fb4aa..9608f4003b 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -492,10 +492,10 @@ void Localizer::determine_gps_mode(double current_time) { int Localizer::locationd_thread() { const char* gps_location_socket; - if (util::file_exists("/persist/comma/use-quectel-rawgps")) { - gps_location_socket = "gpsLocation"; - } else { + if (Params().getBool("UbloxAvailable", true)) { gps_location_socket = "gpsLocationExternal"; + } else { + gps_location_socket = "gpsLocation"; } const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 8841b3e67c..e32861cfae 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -22,6 +22,7 @@ class TestLocationdProc(unittest.TestCase): self.pm = messaging.PubMaster(self.LLD_MSGS) + Params().put_bool("UbloxAvailable", True) managed_processes['locationd'].prepare() managed_processes['locationd'].start() diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index f47cefed6c..5fe120c061 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -116,7 +116,7 @@ class TTYPigeon(): time.sleep(0.001) -def initialize_pigeon(pigeon: TTYPigeon) -> None: +def initialize_pigeon(pigeon: TTYPigeon) -> bool: # try initializing a few times for _ in range(10): try: @@ -198,6 +198,10 @@ def initialize_pigeon(pigeon: TTYPigeon) -> None: break except TimeoutError: cloudlog.warning("Initialization failed, trying again!") + else: + cloudlog.warning("Failed to initialize pigeon") + return False + return True def deinitialize_and_exit(pigeon: Optional[TTYPigeon]): cloudlog.warning("Storing almanac in ublox flash") @@ -236,7 +240,8 @@ def main(): time.sleep(0.5) pigeon = TTYPigeon() - initialize_pigeon(pigeon) + r = initialize_pigeon(pigeon) + Params().put_bool("UbloxAvailable", r) # start receiving data while True: diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 10084bff9f..9d37be4a56 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -417,6 +417,7 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None): params.put_bool("DisengageOnAccelerator", True) params.put_bool("WideCameraOnly", False) params.put_bool("DisableLogging", False) + params.put_bool("UbloxAvailable", True) os.environ["NO_RADAR_SLEEP"] = "1" os.environ["REPLAY"] = "1" diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index c400eb93f5..e436e92292 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -244,11 +244,13 @@ class CarlaBridge: def __init__(self, arguments): set_params_enabled() + self.params = Params() + msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - Params().put("CalibrationParams", msg.to_bytes()) - Params().put_bool("WideCameraOnly", not arguments.dual_camera) + self.params.put("CalibrationParams", msg.to_bytes()) + self.params.put_bool("WideCameraOnly", not arguments.dual_camera) self._args = arguments self._carla_objects = [] @@ -363,6 +365,7 @@ class CarlaBridge: gps_bp = blueprint_library.find('sensor.other.gnss') gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) gps.listen(lambda gps: gps_callback(gps, vehicle_state)) + self.params.put_bool("UbloxAvailable", True) self._carla_objects.extend([imu, gps]) # launch fake car threads From 7c49c44c4ee2e846a275dab1cb5141009d55318e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 6 Oct 2022 16:55:14 -0700 Subject: [PATCH 44/62] fix speed of mock car with qcom gps (#25993) * fix speed of mock car with qcom gps * typo --- selfdrive/car/mock/interface.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 36062da1d1..a3194cd79e 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase): cloudlog.debug("Using Mock Car Interface") - self.gyro = messaging.sub_sock('gyroscope') - self.gps = messaging.sub_sock('gpsLocationExternal') + self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) self.speed = 0. self.prev_speed = 0. @@ -45,15 +44,16 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): + self.sm.update(0) + # get basic data from phone and gps since CAN isn't connected - gyro_sensor = messaging.recv_sock(self.gyro) - if gyro_sensor is not None: - self.yaw_rate_meas = -gyro_sensor.gyroscope.gyroUncalibrated.v[0] + if self.sm.updated['gyroscope']: + self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0] - gps = messaging.recv_sock(self.gps) - if gps is not None: + gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' + if self.sm.updated[gps_sock]: self.prev_speed = self.speed - self.speed = gps.gpsLocationExternal.speed + self.speed = self.sm[gps_sock].speed # create message ret = car.CarState.new_message() From 1ecf6f351c35de24affbb1e2cb5675aea1a36f10 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 00:16:18 -0700 Subject: [PATCH 45/62] Divide by 0 bug fix lateral planner (#25995) * Divide by speed correctly * Update * Update lateral_planner.py * Update ref_commit --- selfdrive/controls/lib/lateral_planner.py | 15 +++++++++------ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 48c98e7350..10e3edbebe 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -75,10 +75,10 @@ class LateralPlanner: y_pts, heading_pts, yaw_rate_pts) - # init state for next - # mpc.u_sol is the desired curvature rate given x0 curv state. - # with x0[3] = measured_curvature, this would be the actual desired rate. - # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. + # init state for next iteration + # mpc.u_sol is the desired second derivative of psi given x0 curv state. + # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate. + # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control. self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) # Check for infeasible MPC solution @@ -105,8 +105,11 @@ class LateralPlanner: lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/sm['carState'].vEgo).tolist() - lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] + + # clip speed for curv calculation at 1m/s, to prevent low speed extremes + clipped_speed = max(1.0, sm['carState'].vEgo) + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist() + lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0e3cdf797f..78367235e6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e0ffcae8def2fd9c82c547d1f257d4f06a48a3c3 +f9536e41a6a160bdaa29d42bb164b0e4033857e5 From cfabae28e7554c12c0fcb4beab8b7b3353140316 Mon Sep 17 00:00:00 2001 From: pbkompasz <47194071+pbkompasz@users.noreply.github.com> Date: Fri, 7 Oct 2022 18:41:14 +0300 Subject: [PATCH 46/62] README -> README.md (#26005) --- tools/cabana/{README => README.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tools/cabana/{README => README.md} (100%) diff --git a/tools/cabana/README b/tools/cabana/README.md similarity index 100% rename from tools/cabana/README rename to tools/cabana/README.md From 1f5187892f0854bbc6a7d7799fa0d9ca2284714a Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 7 Oct 2022 10:39:11 -0700 Subject: [PATCH 47/62] safer modem manager commands (#25999) Co-authored-by: Comma Device --- selfdrive/sensord/rawgps/rawgpsd.py | 4 ++-- selfdrive/sensord/rawgps/test_rawgps.py | 2 +- system/hardware/tici/hardware.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 5149ab6473..c430acc5e5 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -88,7 +88,7 @@ def try_setup_logs(diag, log_types): def mmcli(cmd: str) -> None: for _ in range(5): try: - subprocess.check_call(f"mmcli -m 0 {cmd}", shell=True) + subprocess.check_call(f"mmcli -m any --timeout 30 {cmd}", shell=True) break except subprocess.CalledProcessError: cloudlog.exception("rawgps.mmcli_command_failed") @@ -151,7 +151,7 @@ def main() -> NoReturn: # wait for ModemManager to come up cloudlog.warning("waiting for modem to come up") while True: - ret = subprocess.call("mmcli -m 0 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + ret = subprocess.call("mmcli -m any --timeout 10 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) if ret == 0: break time.sleep(0.1) diff --git a/selfdrive/sensord/rawgps/test_rawgps.py b/selfdrive/sensord/rawgps/test_rawgps.py index f8dd3ad1e5..5bd0833955 100755 --- a/selfdrive/sensord/rawgps/test_rawgps.py +++ b/selfdrive/sensord/rawgps/test_rawgps.py @@ -40,7 +40,7 @@ class TestRawgpsd(unittest.TestCase): time.sleep(s) managed_processes['rawgpsd'].stop() - ls = subprocess.check_output("mmcli -m 0 --location-status --output-json", shell=True, encoding='utf-8') + ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8') loc_status = json.loads(ls) assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'} diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 340093b604..e2fd20c1be 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -480,7 +480,7 @@ class Tici(HardwareBase): # blue prime config if sim_id.startswith('8901410'): - os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') + os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') def get_networks(self): r = {} From 8dbb25e683039e2cd393b574e0fdaa0c894b1104 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 8 Oct 2022 02:28:09 +0800 Subject: [PATCH 48/62] params: remove all files if call clearAll with the ParamKeyType.ALL key. (#26000) * remove all files in dir * fix test case name --- common/params.cc | 11 +++++++---- common/tests/test_util.cc | 17 +++++++++++++++++ common/util.cc | 16 ++++++++++++++++ common/util.h | 1 + 4 files changed, 41 insertions(+), 4 deletions(-) diff --git a/common/params.cc b/common/params.cc index a23c0e003e..155bc88487 100644 --- a/common/params.cc +++ b/common/params.cc @@ -299,10 +299,13 @@ std::map Params::readAll() { void Params::clearAll(ParamKeyType key_type) { FileLock file_lock(params_path + "/.lock"); - std::string path; - for (auto &[key, type] : keys) { - if (type & key_type) { - unlink(getParamPath(key).c_str()); + if (key_type == ALL) { + util::remove_files_in_dir(getParamPath()); + } else { + for (auto &[key, type] : keys) { + if (type & key_type) { + unlink(getParamPath(key).c_str()); + } } } diff --git a/common/tests/test_util.cc b/common/tests/test_util.cc index 25ecf09aa9..b70cc9044a 100644 --- a/common/tests/test_util.cc +++ b/common/tests/test_util.cc @@ -143,3 +143,20 @@ TEST_CASE("util::create_directories") { REQUIRE(util::create_directories("", 0755) == false); } } + +TEST_CASE("util::remove_files_in_dir") { + std::string tmp_dir = "/tmp/test_remove_all_in_dir"; + system("rm /tmp/test_remove_all_in_dir -rf"); + REQUIRE(util::create_directories(tmp_dir, 0755)); + const int tmp_file_cnt = 10; + for (int i = 0; i < tmp_file_cnt; ++i) { + std::string tmp_file = tmp_dir + "/test_XXXXXX"; + int fd = mkstemp((char*)tmp_file.c_str()); + close(fd); + REQUIRE(util::file_exists(tmp_file.c_str())); + } + + REQUIRE(util::read_files_in_dir(tmp_dir).size() == tmp_file_cnt); + util::remove_files_in_dir(tmp_dir); + REQUIRE(util::read_files_in_dir(tmp_dir).empty()); +} diff --git a/common/util.cc b/common/util.cc index 92add63997..b6a8322a27 100644 --- a/common/util.cc +++ b/common/util.cc @@ -97,6 +97,22 @@ std::map read_files_in_dir(const std::string &path) { return ret; } +void remove_files_in_dir(const std::string &path) { + DIR *d = opendir(path.c_str()); + if (!d) return; + + std::string fn; + struct dirent *de = NULL; + while ((de = readdir(d))) { + if (de->d_type != DT_DIR) { + fn = path + "/" + de->d_name; + unlink(fn.c_str()); + } + } + + closedir(d); +} + int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { int fd = HANDLE_EINTR(open(path, flags, mode)); if (fd == -1) { diff --git a/common/util.h b/common/util.h index f3a24723b4..e13f4dc130 100644 --- a/common/util.h +++ b/common/util.h @@ -80,6 +80,7 @@ std::string dir_name(std::string const& path); // **** file fhelpers ***** std::string read_file(const std::string& fn); std::map read_files_in_dir(const std::string& path); +void remove_files_in_dir(const std::string& path); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); FILE* safe_fopen(const char* filename, const char* mode); From beb0f8ac234d28b72c6267d0b4c55fdf81c817b9 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 8 Oct 2022 02:28:24 +0800 Subject: [PATCH 49/62] cabana: custom slider to indicate the status of engaged/disengaged/alerts (#26003) * timeline * remove todo * cleanup --- tools/cabana/videowidget.cc | 46 ++++++++++++++++++++++++++++++++++++- tools/cabana/videowidget.h | 5 ++++ 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 26fb845340..7edbe5e38f 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -4,7 +4,9 @@ #include #include #include +#include #include +#include #include #include "tools/cabana/parser.h" @@ -98,8 +100,50 @@ void VideoWidget::updateState() { } // Slider -// TODO: show timeline bar like what replay did. Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { + QTimer *timer = new QTimer(this); + timer->setInterval(2000); + timer->callOnTimeout([this]() { timeline = parser->replay->getTimeline(); }); + timer->start(); +} + +void Slider::paintEvent(QPaintEvent *ev) { + auto getPaintRange = [this](double begin, double end) -> std::pair { + double total_sec = maximum() - minimum(); + int start_pos = ((std::max((double)minimum(), (double)begin) - minimum()) / total_sec) * width(); + int end_pos = ((std::min((double)maximum(), (double)end) - minimum()) / total_sec) * width(); + return {start_pos, end_pos}; + }; + + QPainter p(this); + const int v_margin = 2; + p.fillRect(rect().adjusted(0, v_margin, 0, -v_margin), QColor(0, 0, 128)); + + for (auto [begin, end, type] : timeline) { + if (begin > maximum() || end < minimum()) continue; + + if (type == TimelineType::Engaged) { + auto [start_pos, end_pos] = getPaintRange(begin, end); + p.fillRect(QRect(start_pos, v_margin, end_pos - start_pos, height() - v_margin * 2), QColor(0, 135, 0)); + } + } + for (auto [begin, end, type] : timeline) { + if (type == TimelineType::Engaged || begin > maximum() || end < minimum()) continue; + + auto [start_pos, end_pos] = getPaintRange(begin, end); + if (type == TimelineType::UserFlag) { + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), Qt::white); + } else { + QColor color(Qt::green); + if (type != TimelineType::AlertInfo) + color = type == TimelineType::AlertWarning ? Qt::yellow : Qt::red; + + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), color); + } + } + p.setPen(QPen(Qt::black, 2)); + qreal x = width() * ((value() - minimum()) / double(maximum() - minimum())); + p.drawLine(QPointF{x, 0.}, QPointF{x, (qreal)height()}); } void Slider::mousePressEvent(QMouseEvent *e) { diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index a3605459e0..188456ecbe 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -5,6 +5,7 @@ #include #include "selfdrive/ui/qt/widgets/cameraview.h" +#include "tools/replay/replay.h" class Slider : public QSlider { Q_OBJECT @@ -15,6 +16,10 @@ public: signals: void setPosition(int value); + +private: + void paintEvent(QPaintEvent *ev) override; + std::vector> timeline; }; class VideoWidget : public QWidget { From 46e741c7cc652a9b1b266c0881ef6f9c30f5a226 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Fri, 7 Oct 2022 15:50:35 -0400 Subject: [PATCH 50/62] VW MNB: Add EPS config script support (#26007) --- selfdrive/debug/vw_mqb_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index c1068bf067..8c4dbc55ee 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -70,8 +70,8 @@ if __name__ == "__main__": coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_length = len(current_coding) - # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) - if odx_file == "EV_SteerAssisMQB\x00": + # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS) + if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"): coding_variant = "ZF" coding_byte = 0 coding_bit = 4 @@ -111,7 +111,7 @@ if __name__ == "__main__": if args.action in ["enable", "disable"]: print("\nAttempting configuration update") - assert(coding_variant in ("ZF", "APA")) + assert(coding_variant in ("ZF", "APA")) # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the # bit we care about is always in the same place in the first byte if args.action == "enable": From 8cee561dbfcb6e45f7ecd51adb0df67f4ebba908 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 7 Oct 2022 13:51:55 -0700 Subject: [PATCH 51/62] tombstoned: remove android code --- selfdrive/tombstoned.py | 43 +---------------------------------------- 1 file changed, 1 insertion(+), 42 deletions(-) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 0045e0766c..61a575f141 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -62,47 +62,6 @@ def get_tombstones(): return files -def report_tombstone_android(fn): - f_size = os.path.getsize(fn) - if f_size > MAX_SIZE: - cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") - return - - with open(fn, encoding='ISO-8859-1') as f: - contents = f.read() - - message = " ".join(contents.split('\n')[5:7]) - - # Cut off pid/tid, since that varies per run - name_idx = message.find('name') - if name_idx >= 0: - message = message[name_idx:] - - executable = "" - start_exe_idx = message.find('>>> ') - end_exe_idx = message.find(' <<<') - if start_exe_idx >= 0 and end_exe_idx >= 0: - executable = message[start_exe_idx + 4:end_exe_idx] - - # Cut off fault addr - fault_idx = message.find(', fault addr') - if fault_idx >= 0: - message = message[:fault_idx] - - sentry.report_tombstone(fn, message, contents) - - # Copy crashlog to upload folder - clean_path = executable.replace('./', '').replace('/', '_') - date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") - - new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] - - crashlog_dir = os.path.join(ROOT, "crash") - mkdirs_exists_ok(crashlog_dir) - - shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) - - def report_tombstone_apport(fn): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: @@ -199,7 +158,7 @@ def main() -> NoReturn: if fn.endswith(".crash"): report_tombstone_apport(fn) else: - report_tombstone_android(fn) + cloudlog.error(f"unknown crash type: {fn}") except Exception: cloudlog.exception(f"Error reporting tombstone {fn}") From 6ce511cc605782c25d45c66b55f859caaf7ce516 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 7 Oct 2022 16:12:58 -0700 Subject: [PATCH 52/62] GM: use ECM brake-pressed threshold (#25970) * This brake position value disengages stock ACC, use it to avoid controls mismatch. 2016-2017 Volt will hit this threshold and disengage, must install new design of brake pedal retaining clip, TSB 16-NA-147. * 80 hz * comment * bump panda * update refs * bump panda * bump panda * bump panda * bump panda to master Co-authored-by: qadmus <42746943+qadmus@users.noreply.github.com> --- panda | 2 +- selfdrive/car/gm/carstate.py | 13 ++++++++----- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/panda b/panda index 3334dc21f5..1303af2db2 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 3334dc21f5c55007c5a754dfd8ee5d642be3e2bb +Subproject commit 1303af2db29a72eee180b10c6097fa5b19c29207 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 0bba1d29b8..f96a234dbd 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -40,9 +40,12 @@ class CarState(CarStateBase): else: ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) - # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. - ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 - ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 + # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe + # that the brake is being intermittently pressed without user interaction. + # To avoid a cruise fault we need to match the ECM's brake pressed signal and threshold + # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf + ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] / 0xd0 + ret.brakePressed = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] >= 8 # Regen braking is braking if self.CP.transmissionType == TransmissionType.direct: @@ -100,7 +103,7 @@ class CarState(CarStateBase): def get_can_parser(CP): signals = [ # sig_name, sig_address - ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("BrakePedalPos", "ECMAcceleratorPos"), ("FrontLeftDoor", "BCMDoorBeltStatus"), ("FrontRightDoor", "BCMDoorBeltStatus"), ("RearLeftDoor", "BCMDoorBeltStatus"), @@ -141,7 +144,7 @@ class CarState(CarStateBase): ("ASCMSteeringButton", 33), ("ECMEngineStatus", 100), ("PSCMSteeringAngle", 100), - ("EBCMBrakePedalPosition", 100), + ("ECMAcceleratorPos", 80), ] if CP.transmissionType == TransmissionType.direct: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 78367235e6..a89b4c2d71 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f9536e41a6a160bdaa29d42bb164b0e4033857e5 +338c24158bb28952dcd1554ea91734b4281e2fed \ No newline at end of file From 6c5693e965b9c63f8678f52b9e9b5abe35f23feb Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Fri, 7 Oct 2022 17:22:42 -0700 Subject: [PATCH 53/62] faster rocket launcher model (#26009) * cache tokens 1456d261-d232-4654-8885-4d9fde883894/440 e63ab895-2222-4abd-a9a5-af86bb70e260/700 * udpate ref commit * bump tinygrad to master --- selfdrive/modeld/models/driving.h | 2 +- selfdrive/modeld/models/supercombo.onnx | 4 ++-- selfdrive/test/process_replay/model_replay_ref_commit | 2 +- tinygrad_repo | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 90767384eb..8bb84d0245 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -16,7 +16,7 @@ #include "selfdrive/modeld/models/commonmodel.h" #include "selfdrive/modeld/runners/run.h" -constexpr int FEATURE_LEN = 2048; +constexpr int FEATURE_LEN = 128; constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 37edc36c02..aee0ac37ff 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:30f30bc1251c03db135564ecbf7dc0bc96cbb07be0ebd3691edd8d555dc087fa -size 58539693 +oid sha256:c4d37af666344af6bb218e0b939b1152ad3784c15ac79e37bcf0124643c8286a +size 58539563 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 9727ef3d17..2446ec061d 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -008c0a622b7471c6234690d668f76bcb5dc8d999 +c171250d2cc013b3eca1cda4fb62f3d0dda28d4d diff --git a/tinygrad_repo b/tinygrad_repo index 2993dfe921..46f6db7522 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2993dfe9219089bd1464741757f0c2ad67fe6a2e +Subproject commit 46f6db75228cb7744a3a0a87616bfffeffa93658 From 57a82ced285518cad129ec61814aab48bf51910e Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 17:27:09 -0700 Subject: [PATCH 54/62] rawgpsd: publish ephemerides (#25931) * add svpoly parsing * Publish poly * add source check * add safety check for invalid gpsWeek values * address PR comments * add qcom ephemeris source type * bump cereal and laika Co-authored-by: Kurt Nistelberger --- cereal | 2 +- laika_repo | 2 +- selfdrive/locationd/laikad.py | 52 +++++++++++++++++++++++------ selfdrive/sensord/rawgps/rawgpsd.py | 30 ++++++++++++++--- selfdrive/sensord/rawgps/structs.py | 41 +++++++++++++++++++++++ 5 files changed, 111 insertions(+), 16 deletions(-) diff --git a/cereal b/cereal index 5ba96b6ded..b29717c4c3 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5ba96b6ded57bcd91e60140ce0036f61370f8512 +Subproject commit b29717c4c328d5cf34d46f682f25267150f82637 diff --git a/laika_repo b/laika_repo index c8bc1fa01b..e1049cde0a 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 +Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index c4fdfb9871..2769f394c5 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -61,6 +61,7 @@ class Laikad: self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None + self.gps_week = None self.use_qcom = use_qcom def load_cache(self): @@ -107,11 +108,11 @@ class Laikad: return self.last_pos_fix def is_good_report(self, gnss_msg): - if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: + if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) # TODO support GLONASS return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] - elif gnss_msg.which == 'measurementReport' and not self.use_qcom: + elif gnss_msg.which() == 'measurementReport' and not self.use_qcom: return True else: return False @@ -129,9 +130,28 @@ class Laikad: new_meas = read_raw_ublox(report) return week, tow, new_meas + def is_ephemeris(self, gnss_msg): + if self.use_qcom: + return gnss_msg.which() == 'drSvPoly' + else: + return gnss_msg.which() == 'ephemeris' + + def read_ephemeris(self, gnss_msg): + # TODO this only works on GLONASS + if self.use_qcom: + # TODO this is not robust to gps week rollover + if self.gps_week is None: + return + ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + else: + ephem = convert_ublox_ephem(gnss_msg.ephemeris) + self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.cache_ephemeris(t=ephem.epoch) + def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) + self.gps_week = week t = gnss_mono_time * 1e-9 if week > 0: @@ -172,12 +192,10 @@ class Laikad: "correctedMeasurements": meas_msgs } return dat - # TODO this only works on GLONASS, qcom needs live ephemeris parsing too - elif gnss_msg.which == 'ephemeris': - ephem = convert_ublox_ephem(gnss_msg.ephemeris) - self.astro_dog.add_navs({ephem.prn: [ephem]}) - self.cache_ephemeris(t=ephem.epoch) - #elif gnss_msg.which == 'ionoData': + elif self.is_ephemeris(gnss_msg): + self.read_ephemeris(gnss_msg) + + #elif gnss_msg.which() == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): @@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement): c.satVel = meas.sat_vel.tolist() ephem = meas.sat_ephemeris assert ephem is not None + week, time_of_week = -1, -1 if ephem.eph_type == EphemerisType.NAV: source_type = EphemerisSourceType.nav - week, time_of_week = -1, -1 + elif ephem.eph_type == EphemerisType.QCOM_POLY: + source_type = EphemerisSourceType.qcom else: assert ephem.file_epoch is not None week = ephem.file_epoch.week @@ -325,6 +345,7 @@ class EphemerisSourceType(IntEnum): nav = 0 nasaUltraRapid = 1 glonassIacUltraRapid = 2 + qcom = 3 def main(sm=None, pm=None): @@ -348,6 +369,17 @@ def main(sm=None, pm=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] + + # TODO: Understand and use remaining unknown constellations + if gnss_msg.which() == "drMeasurementReport": + if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']: + continue + + if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max: + # gpsWeek 65535 is received rarely from quectel, this cannot be + # passed to GnssMeasurements's gpsWeek (Int16) + continue + msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) if msg is not None: pm.send('gnssMeasurements', msg) diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index c430acc5e5..c68e7aff99 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -14,12 +14,13 @@ import cereal.messaging as messaging from laika.gps_time import GPSTime from system.swaglog import cloudlog from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv -from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, +from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, glonass_measurement_report, glonass_measurement_report_sv, - oemdre_measurement_report, oemdre_measurement_report_sv, + oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report, LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT, - LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT) + LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT) DEBUG = int(os.getenv("DEBUG", "0"))==1 @@ -28,6 +29,7 @@ LOG_TYPES = [ LOG_GNSS_GLONASS_MEASUREMENT_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, LOG_GNSS_POSITION_REPORT, + LOG_GNSS_OEMDRE_SVPOLY_REPORT, ] @@ -146,6 +148,9 @@ def main() -> NoReturn: unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) + unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True) + unpack_position, _ = dict_unpacker(position_report) + unpack_position, _ = dict_unpacker(position_report) # wait for ModemManager to come up @@ -258,7 +263,24 @@ def main() -> NoReturn: pm.send('gpsLocation', msg) - if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: + elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: + msg = messaging.new_message('qcomGnss') + dat = unpack_svpoly(log_payload) + dat = relist(dat) + gnss = msg.qcomGnss + gnss.logTs = log_time + gnss.init('drSvPoly') + poly = gnss.drSvPoly + for k,v in dat.items(): + if k == "version": + assert v == 2 + elif k == "flags": + pass + else: + setattr(poly, k, v) + pm.send('qcomGnss', msg) + + elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: msg = messaging.new_message('qcomGnss') gnss = msg.qcomGnss diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py index 4bc9eca875..97e3d3d605 100644 --- a/selfdrive/sensord/rawgps/structs.py +++ b/selfdrive/sensord/rawgps/structs.py @@ -56,6 +56,29 @@ oemdre_measurement_report = """ uint8_t source; """ +oemdre_svpoly_report = """ + uint8_t version; + uint16_t sv_id; + int8_t frequency_index; + uint8_t flags; + uint16_t iode; + double t0; + double xyz0[3]; + double xyzN[9]; + float other[4]; + float position_uncertainty; + float iono_delay; + float iono_dot; + float sbas_iono_delay; + float sbas_iono_dot; + float tropo_delay; + float elevation; + float elevation_dot; + float elevation_uncertainty; + double velocity_coeff[12]; +""" + + oemdre_measurement_report_sv = """ uint8_t sv_id; uint8_t unkn; @@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False): nams = [name_to_camelcase(x) for x in nams] sz = calcsize(st) return lambda x: dict(zip(nams, unpack_from(st, x))), sz + +def relist(dat): + list_keys = set() + for key in dat.keys(): + if '[' in key: + list_keys.add(key.split('[')[0]) + list_dict = {} + for list_key in list_keys: + list_dict[list_key] = [] + i = 0 + while True: + key = list_key + f'[{i}]' + if key not in dat: + break + list_dict[list_key].append(dat[key]) + del dat[key] + i += 1 + return {**dat, **list_dict} From fb074378194db28067dbb77e8cd15db6ab5fd882 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 7 Oct 2022 19:15:04 -0700 Subject: [PATCH 55/62] Increase low speed jerk cost (#26008) * Increase low speed jerk cost * Update planner weight * Update ref_commit * Update lateral_planner.py * cleanup and refactor * Update ref_commit --- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 16 ++++----- selfdrive/controls/lib/lateral_planner.py | 35 +++++++++++-------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 30 insertions(+), 23 deletions(-) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 07efad73c9..0cbd576341 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -20,6 +20,7 @@ P_DIM = 2 N = 16 COST_E_DIM = 3 COST_DIM = COST_E_DIM + 1 +SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -88,14 +89,13 @@ def gen_lat_ocp(): ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) - # TODO hacky weights to keep behavior the same ocp.model.cost_y_expr = vertcat(y_ego, - ((v_ego + 5.0) * psi_ego), - ((v_ego + 5.0) * psi_rate_ego), - ((v_ego + 5.0) * psi_rate_ego_dot)) + ((v_ego + SPEED_OFFSET) * psi_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego_dot)) ocp.model.cost_y_expr_e = vertcat(y_ego, - ((v_ego + 5.0) * psi_ego), - ((v_ego + 5.0) * psi_rate_ego)) + ((v_ego + SPEED_OFFSET) * psi_ego), + ((v_ego + SPEED_OFFSET) * psi_rate_ego)) # set constraints ocp.constraints.constr_type = 'BGH' @@ -158,8 +158,8 @@ class LateralMpc(): self.yref[:,0] = y_pts v_ego = p_cp[0] # rotation_radius = p_cp[1] - self.yref[:,1] = heading_pts * (v_ego+5.0) - self.yref[:,2] = yaw_rate_pts * (v_ego+5.0) + self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET) + self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.set(i, "p", p_cp) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 10e3edbebe..d4c832b53b 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -12,6 +12,15 @@ from cereal import log TRAJECTORY_SIZE = 33 CAMERA_OFFSET = 0.04 + +PATH_COST = 1.0 +LATERAL_MOTION_COST = 0.11 +LATERAL_ACCEL_COST = 0.0 +LATERAL_JERK_COST = 0.05 + +MIN_SPEED = 1.5 + + class LateralPlanner: def __init__(self, CP): self.DH = DesireHelper() @@ -36,7 +45,8 @@ class LateralPlanner: self.lat_mpc.reset(x0=self.x0) def update(self, sm): - v_ego = sm['carState'].vEgo + # clip speed , lateral planning is not possible at 0 speed + self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) measured_curvature = sm['controlsState'].curvature # Parse model predictions @@ -56,20 +66,19 @@ class LateralPlanner: self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) d_path_xyz = self.path_xyz - # Heading cost is useful at low speed, otherwise end of plan can be off-heading - heading_cost = interp(v_ego, [5.0, 10.0], [1.0, 0.15]) - self.lat_mpc.set_weights(1.0, heading_cost, 0.0, .075) + self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, + LATERAL_ACCEL_COST, LATERAL_JERK_COST) - y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) - heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) - yaw_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) + y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) + heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) + yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 assert len(yaw_rate_pts) == LAT_MPC_N + 1 - lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) - p = np.array([v_ego, lateral_factor]) + lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2)) + p = np.array([self.v_ego, lateral_factor]) self.lat_mpc.run(self.x0, p, y_pts, @@ -86,7 +95,7 @@ class LateralPlanner: t = sec_since_boot() if mpc_nans or self.lat_mpc.solution_status != 0: self.reset_mpc() - self.x0[3] = measured_curvature + self.x0[3] = measured_curvature * self.v_ego if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") @@ -106,10 +115,8 @@ class LateralPlanner: lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() - # clip speed for curv calculation at 1m/s, to prevent low speed extremes - clipped_speed = max(1.0, sm['carState'].vEgo) - lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/clipped_speed).tolist() - lateralPlan.curvatureRates = [float(x/clipped_speed) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] + lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist() + lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.solverExecutionTime = self.lat_mpc.solve_time diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a89b4c2d71..a4a24ca603 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -338c24158bb28952dcd1554ea91734b4281e2fed \ No newline at end of file +f636c68e2b4ed88d3731930cf15b6dee984eb6dd From 36701a82a344447fe33476f42cd83034dbc373a4 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 8 Oct 2022 00:02:57 -0700 Subject: [PATCH 56/62] Kia: update Optima platform name (#25852) * https://en.wikipedia.org/wiki/Kia_K5 * it's actually the same generation, but 2019+ is a facelift * g4 * fix * rename --- selfdrive/car/hyundai/hyundaican.py | 2 +- selfdrive/car/hyundai/interface.py | 4 ++-- selfdrive/car/hyundai/values.py | 20 ++++++++++---------- selfdrive/car/tests/routes.py | 4 ++-- selfdrive/car/torque_data/substitute.yaml | 4 ++-- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 139a14d5e8..7f3f6a72c5 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -38,7 +38,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 # Likely cars lacking the ability to show individual lane lines in the dash - elif car_fingerprint in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019): + elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): # SysWarning 4 = keep hands on wheel + beep values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 97c1f7a5dc..2d960ed17f 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -202,12 +202,12 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] - elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.KIA_OPTIMA_H): + elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H): ret.mass = 3558. * CV.LB_TO_KG ret.wheelbase = 2.80 ret.steerRatio = 13.75 tire_stiffness_factor = 0.5 - if candidate == CAR.KIA_OPTIMA: + if candidate == CAR.KIA_OPTIMA_G4: ret.minSteerSpeed = 32 * CV.MPH_TO_MS CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.KIA_STINGER: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 52a5088319..a43f17ac00 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -85,8 +85,8 @@ class CAR: KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" - KIA_OPTIMA = "KIA OPTIMA 2016" - KIA_OPTIMA_2019 = "KIA OPTIMA 2019" + KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN" + KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT" KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" KIA_SELTOS = "KIA SELTOS 2021" KIA_SORENTO = "KIA SORENTO GT LINE 2018" @@ -155,8 +155,8 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), ], - CAR.KIA_OPTIMA: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 - CAR.KIA_OPTIMA_2019: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), + CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 + CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), CAR.KIA_OPTIMA_H: [ HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years HyundaiCarInfo("Kia Optima Hybrid 2019"), @@ -1136,7 +1136,7 @@ FW_VERSIONS = { b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b', ], }, - CAR.KIA_OPTIMA: { + CAR.KIA_OPTIMA_G4: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4100 ', ], @@ -1150,7 +1150,7 @@ FW_VERSIONS = { b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00', ], }, - CAR.KIA_OPTIMA_2019: { + CAR.KIA_OPTIMA_G4_FL: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ', ], @@ -1367,7 +1367,7 @@ CHECKSUM = { FEATURES = { # which message has the gear "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, - "use_tcu_gears": {CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, + "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022}, # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 @@ -1383,7 +1383,7 @@ HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_N EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022} # these cars require a special panda safety mode due to missing counters and checksums in the messages -LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} +LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py @@ -1408,8 +1408,8 @@ DBC = { CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_2019: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None), + CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 3ae3a357ce..82c0614493 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -88,8 +88,8 @@ routes = [ CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022), CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022, segment=1), CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED), - CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA), - CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_2019, segment=0), + CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4), + CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_G4_FL, segment=0), CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA), CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF), diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 92361d37f4..f5e3d1d61d 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -17,8 +17,8 @@ LEXUS RC 2020: LEXUS NX 2020 TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019 TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022 -KIA OPTIMA 2016: HYUNDAI SONATA 2020 -KIA OPTIMA 2019: HYUNDAI SONATA 2020 +KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020 +KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020 KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020 KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020 KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020 From 2e7fa330b346b6435d0b15524f3b5ce878d213de Mon Sep 17 00:00:00 2001 From: Greg Hogan Date: Sat, 8 Oct 2022 04:07:35 -0700 Subject: [PATCH 57/62] hyundai: fix logging stock AEB events (#25152) --- selfdrive/car/hyundai/carstate.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index b9c7327a93..1f2de3286e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -126,12 +126,12 @@ class CarState(CarStateBase): ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) if not self.CP.openpilotLongitudinalControl: - if self.CP.carFingerprint in FEATURES["use_fca"]: - ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0 - ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2 - else: - ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0 - ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2 + aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12" + aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct" + aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0 + aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0 + ret.stockFcw = aeb_warning and not aeb_braking + ret.stockAeb = aeb_warning and aeb_braking if self.CP.enableBsm: ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 @@ -294,12 +294,14 @@ class CarState(CarStateBase): signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), + ("CF_VSM_DecCmdAct", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), + ("CF_VSM_DecCmdAct", "SCC12"), ] if CP.enableBsm: @@ -383,12 +385,14 @@ class CarState(CarStateBase): signals += [ ("FCA_CmdAct", "FCA11"), ("CF_VSM_Warn", "FCA11"), + ("CF_VSM_DecCmdAct", "FCA11"), ] checks.append(("FCA11", 50)) else: signals += [ ("AEB_CmdAct", "SCC12"), ("CF_VSM_Warn", "SCC12"), + ("CF_VSM_DecCmdAct", "SCC12"), ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) From 2ed82387a5df5b86113d23fc4cc7d7aeadabdb52 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sun, 9 Oct 2022 05:10:00 +0800 Subject: [PATCH 58/62] cabana: fix Incorrect Y-Axis Scale (#26018) --- tools/cabana/chartswidget.cc | 33 +++++++++++++++------------------ tools/cabana/chartswidget.h | 1 + 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 3e1a8b8410..5caa8d5a43 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -194,8 +194,6 @@ ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *pa void ChartWidget::updateState() { auto chart = chart_view->chart(); auto axis_x = dynamic_cast(chart->axisX()); - if (axis_x->max() <= axis_x->min()) return; - int x = chart->plotArea().left() + chart->plotArea().width() * (parser->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min()); if (line_marker_x != x) { line_marker->setX(x); @@ -214,7 +212,6 @@ void ChartWidget::updateSeries() { vals.clear(); vals.reserve(3 * 60 * 100); - double min_y = 0, max_y = 0; uint64_t route_start_time = parser->replay->routeStartTime(); for (auto &evt : *events) { if (evt->which == cereal::Event::Which::CAN) { @@ -226,9 +223,6 @@ void ChartWidget::updateSeries() { val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0; } double value = val * sig->factor + sig->offset; - if (value < min_y) min_y = value; - if (value > max_y) max_y = value; - double ts = (evt->mono_time - route_start_time) / (double)1e9; // seconds vals.push_back({ts, value}); } @@ -239,7 +233,7 @@ void ChartWidget::updateSeries() { series->replace(vals); auto [begin, end] = parser->range(); chart_view->chart()->axisX()->setRange(begin, end); - chart_view->chart()->axisY()->setRange(min_y * 0.95, max_y * 1.05); + updateAxisY(); } void ChartWidget::rangeChanged(qreal min, qreal max) { @@ -247,17 +241,20 @@ void ChartWidget::rangeChanged(qreal min, qreal max) { if (axis_x->min() != min || axis_x->max() != max) { axis_x->setRange(min, max); } - // auto zoom on yaxis - double min_y = 0, max_y = 0; - for (auto &p : vals) { - if (p.x() > max) break; - - if (p.x() >= min) { - if (p.y() < min_y) min_y = p.y(); - if (p.y() > max_y) max_y = p.y(); - } - } - chart_view->chart()->axisY()->setRange(min_y * 0.95, max_y * 1.05); + updateAxisY(); +} + +// auto zoom on yaxis +void ChartWidget::updateAxisY() { + const auto axis_x = dynamic_cast(chart_view->chart()->axisX()); + // vals is a sorted list + auto begin = std::lower_bound(vals.begin(), vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; }); + if (begin == vals.end()) + return; + + auto end = std::upper_bound(vals.begin(), vals.end(), axis_x->max(), [](double x, auto &p) { return x < p.x(); }); + const auto [min, max] = std::minmax_element(begin, end, [](auto &p1, auto &p2) { return p1.y() < p2.y(); }); + chart_view->chart()->axisY()->setRange(min->y(), max->y()); } // LineMarker diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 0413d65e09..81df2237bc 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -37,6 +37,7 @@ private: void addData(const CanData &can_data, const Signal &sig); void updateSeries(); void rangeChanged(qreal min, qreal max); + void updateAxisY(); QString id; QString sig_name; From f31aa6853354226baada15f6a12b57f16bc3853b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 8 Oct 2022 23:28:33 -0700 Subject: [PATCH 59/62] Subaru: log stock FCW (#26012) * log stock FCW * Update selfdrive/car/subaru/carstate.py * never seen 1 (could be aeb?) --- selfdrive/car/subaru/carstate.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 128e4245b2..ba873c48d7 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -58,9 +58,6 @@ class CarState(CarStateBase): ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS - if self.car_fingerprint not in PREGLOBAL_CARS: - ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 - if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): ret.cruiseState.speed *= CV.MPH_TO_KPH @@ -78,6 +75,8 @@ class CarState(CarStateBase): else: ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 + ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3 + ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2 self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam From 754b44e7ceb2b6b029cf511b821f4328b90364f9 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sun, 9 Oct 2022 23:44:19 +0800 Subject: [PATCH 60/62] Cabana: increase slider precision to milliseconds (#26025) --- tools/cabana/videowidget.cc | 52 ++++++++++++++++++++++++------------- tools/cabana/videowidget.h | 3 +++ 2 files changed, 37 insertions(+), 18 deletions(-) diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 7edbe5e38f..957747584c 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -31,9 +31,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { slider = new Slider(this); slider->setSingleStep(0); slider->setMinimum(0); - slider->setTickInterval(60); - slider->setTickPosition(QSlider::TicksBelow); - slider->setMaximum(parser->replay->totalSeconds()); + slider->setMaximum(parser->replay->totalSeconds() * 1000); slider_layout->addWidget(slider); total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); @@ -63,7 +61,7 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged); QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState); - QObject::connect(slider, &QSlider::sliderMoved, [=]() { time_label->setText(formatTime(slider->value())); }); + QObject::connect(slider, &QSlider::sliderMoved, [=]() { time_label->setText(formatTime(slider->value() / 1000)); }); QObject::connect(slider, &QSlider::sliderReleased, [this]() { setPosition(slider->value()); }); QObject::connect(slider, &Slider::setPosition, this, &VideoWidget::setPosition); @@ -75,8 +73,8 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { } void VideoWidget::setPosition(int value) { - time_label->setText(formatTime(value)); - parser->seekTo(value); + time_label->setText(formatTime(value / 1000.0)); + parser->seekTo(value / 1000.0); } void VideoWidget::rangeChanged(double min, double max) { @@ -86,16 +84,16 @@ void VideoWidget::rangeChanged(double min, double max) { } time_label->setText(formatTime(min)); total_time_label->setText(formatTime(max)); - slider->setMinimum(min); - slider->setMaximum(max); - slider->setValue(parser->currentSec()); + slider->setMinimum(min * 1000); + slider->setMaximum(max * 1000); + slider->setValue(parser->currentSec() * 1000); } void VideoWidget::updateState() { if (!slider->isSliderDown()) { - int current_sec = parser->currentSec(); + double current_sec = parser->currentSec(); time_label->setText(formatTime(current_sec)); - slider->setValue(current_sec); + slider->setValue(current_sec * 1000); } } @@ -103,10 +101,25 @@ void VideoWidget::updateState() { Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { QTimer *timer = new QTimer(this); timer->setInterval(2000); - timer->callOnTimeout([this]() { timeline = parser->replay->getTimeline(); }); + timer->callOnTimeout([this]() { + timeline = parser->replay->getTimeline(); + update(); + }); timer->start(); } +void Slider::sliderChange(QAbstractSlider::SliderChange change) { + if (change == QAbstractSlider::SliderValueChange) { + qreal x = width() * ((value() - minimum()) / double(maximum() - minimum())); + if (x != slider_x) { + slider_x = x; + update(); + } + } else { + QAbstractSlider::sliderChange(change); + } +} + void Slider::paintEvent(QPaintEvent *ev) { auto getPaintRange = [this](double begin, double end) -> std::pair { double total_sec = maximum() - minimum(); @@ -117,17 +130,21 @@ void Slider::paintEvent(QPaintEvent *ev) { QPainter p(this); const int v_margin = 2; - p.fillRect(rect().adjusted(0, v_margin, 0, -v_margin), QColor(0, 0, 128)); + p.fillRect(rect().adjusted(0, v_margin, 0, -v_margin), QColor(111, 143, 175)); for (auto [begin, end, type] : timeline) { + begin *= 1000; + end *= 1000; if (begin > maximum() || end < minimum()) continue; if (type == TimelineType::Engaged) { auto [start_pos, end_pos] = getPaintRange(begin, end); - p.fillRect(QRect(start_pos, v_margin, end_pos - start_pos, height() - v_margin * 2), QColor(0, 135, 0)); + p.fillRect(QRect(start_pos, v_margin, end_pos - start_pos, height() - v_margin * 2), QColor(0, 163, 108)); } } for (auto [begin, end, type] : timeline) { + begin *= 1000; + end *= 1000; if (type == TimelineType::Engaged || begin > maximum() || end < minimum()) continue; auto [start_pos, end_pos] = getPaintRange(begin, end); @@ -136,14 +153,13 @@ void Slider::paintEvent(QPaintEvent *ev) { } else { QColor color(Qt::green); if (type != TimelineType::AlertInfo) - color = type == TimelineType::AlertWarning ? Qt::yellow : Qt::red; + color = type == TimelineType::AlertWarning ? QColor(255, 195, 0) : QColor(199, 0, 57); p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), color); } } - p.setPen(QPen(Qt::black, 2)); - qreal x = width() * ((value() - minimum()) / double(maximum() - minimum())); - p.drawLine(QPointF{x, 0.}, QPointF{x, (qreal)height()}); + p.setPen(QPen(QColor(88, 24, 69), 3)); + p.drawLine(QPoint{slider_x, 0}, QPoint{slider_x, height()}); } void Slider::mousePressEvent(QMouseEvent *e) { diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index 188456ecbe..060565d322 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -13,6 +13,7 @@ class Slider : public QSlider { public: Slider(QWidget *parent); void mousePressEvent(QMouseEvent* e) override; + void sliderChange(QAbstractSlider::SliderChange change) override; signals: void setPosition(int value); @@ -20,6 +21,8 @@ signals: private: void paintEvent(QPaintEvent *ev) override; std::vector> timeline; + + int slider_x = -1; }; class VideoWidget : public QWidget { From c719b3b7ddd05d8369aab13e11f99af7990c73ea Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 10 Oct 2022 23:24:14 -0700 Subject: [PATCH 61/62] HKG CAN-FD: log temporary steering faults (#26031) * log hkg can-fd temporary steering faults * bump to master --- opendbc | 2 +- selfdrive/car/hyundai/carstate.py | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/opendbc b/opendbc index ae2fd934ce..c35e8139bf 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit ae2fd934ceb8501c56a0802564c14963dbb201ac +Subproject commit c35e8139bf9e9d87b9efb6764ab7e65983e8d33e diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 1f2de3286e..4d60afe1fd 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -180,6 +180,7 @@ class CarState(CarStateBase): ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], cp.vl["BLINKERS"]["RIGHT_LAMP"]) @@ -414,6 +415,7 @@ class CarState(CarStateBase): ("STEERING_ANGLE", "STEERING_SENSORS"), ("STEERING_COL_TORQUE", "MDPS"), ("STEERING_OUT_TORQUE", "MDPS"), + ("LKA_FAULT", "MDPS"), ("CRUISE_ACTIVE", "SCC1"), ("COUNTER", cruise_btn_msg), From 478693771bf2570da0b0370c65c6f4113766eaca Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Tue, 11 Oct 2022 10:27:00 -0700 Subject: [PATCH 62/62] bump tinygrad (#26029) --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 46f6db7522..870ea766ee 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 46f6db75228cb7744a3a0a87616bfffeffa93658 +Subproject commit 870ea766eec7a38d7d590c81436f15271ba2667e