From 2a981f553162ff41dd50ed6921b90ac512efa3d2 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 18 Apr 2023 00:37:26 +0200 Subject: [PATCH] cabana: support direct streaming from panda over USB (#27936) * refactor livestream into devicestream * add panda stream * unused * whitespace * move logging to base class * add cmdline args * Update selfdrive/boardd/boardd.cc --------- Co-authored-by: Adeeb Shihadeh --- selfdrive/boardd/SConscript | 4 +- selfdrive/boardd/panda.cc | 3 - tools/cabana/README.md | 26 +++++---- tools/cabana/SConscript | 4 +- tools/cabana/cabana.cc | 13 ++++- tools/cabana/streams/devicestream.cc | 27 +++++++++ tools/cabana/streams/devicestream.h | 17 ++++++ tools/cabana/streams/livestream.cc | 43 ++++---------- tools/cabana/streams/livestream.h | 13 +++-- tools/cabana/streams/pandastream.cc | 85 ++++++++++++++++++++++++++++ tools/cabana/streams/pandastream.h | 33 +++++++++++ 11 files changed, 211 insertions(+), 57 deletions(-) create mode 100644 tools/cabana/streams/devicestream.cc create mode 100644 tools/cabana/streams/devicestream.h create mode 100644 tools/cabana/streams/pandastream.cc create mode 100644 tools/cabana/streams/pandastream.h diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index d99e67a9f0..ac92d7d710 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,7 +1,9 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs) +panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) + +env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 4ad4b5e652..4873040f37 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -21,9 +21,6 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { hw_type = get_hw_type(); - assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) && - (hw_type != cereal::PandaState::PandaType::GREY_PANDA)); - has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || (hw_type == cereal::PandaState::PandaType::DOS) || (hw_type == cereal::PandaState::PandaType::TRES); diff --git a/tools/cabana/README.md b/tools/cabana/README.md index 4faa7c61d0..921decff3c 100644 --- a/tools/cabana/README.md +++ b/tools/cabana/README.md @@ -11,19 +11,23 @@ $ ./cabana -h Usage: ./cabana [options] route Options: - -h, --help Displays this help. - --demo use a demo route instead of providing your own - --qcam load qcamera - --ecam load wide road camera - --stream read can messages from live streaming - --zmq the ip address on which to receive zmq messages - --data_dir local directory with routes - --no-vipc do not output video - --dbc dbc file to open + -h, --help Displays help on commandline options. + --help-all Displays help including Qt specific options. + --demo use a demo route instead of providing your own + --qcam load qcamera + --ecam load wide road camera + --stream read can messages from live streaming + --panda read can messages from panda + --panda-serial read can messages from panda with given serial + --zmq the ip address on which to receive zmq + messages + --data_dir local directory with routes + --no-vipc do not output video + --dbc dbc file to open Arguments: - route the drive to replay. find your drives at - connect.comma.ai + route the drive to replay. find your drives at + connect.comma.ai ``` See [openpilot wiki](https://github.com/commaai/openpilot/wiki/Cabana) diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 975ff64057..4acbc4fa66 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -17,7 +17,7 @@ qt_libs = ['qt_util'] + base_libs cabana_env = qt_env.Clone() cabana_env["LIBPATH"] += ['../../opendbc/can'] -cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'libdbc_static', 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs +cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'panda', 'libdbc_static', 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'usb-1.0'] + qt_libs opendbc_path = '-DOPENDBC_FILE_PATH=\'"%s"\'' % (cabana_env.Dir("../../opendbc").abspath) cabana_env['CXXFLAGS'] += [opendbc_path] @@ -29,7 +29,7 @@ cabana_env.Depends(assets, Glob('/assets/*', exclude=[assets, assets_src, "asset prev_moc_path = cabana_env['QT_MOCHPREFIX'] cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_' -cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc', +cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/pandastream.cc', 'streams/devicestream.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc', 'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc', 'chart/chartswidget.cc', 'chart/chart.cc', 'chart/signalselector.cc', 'chart/tiplabel.cc', 'chart/sparkline.cc', 'commands.cc', 'messageswidget.cc', 'route.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc index 10b394cec5..2f7a560600 100644 --- a/tools/cabana/cabana.cc +++ b/tools/cabana/cabana.cc @@ -5,7 +5,8 @@ #include "selfdrive/ui/qt/util.h" #include "tools/cabana/mainwin.h" #include "tools/cabana/route.h" -#include "tools/cabana/streams/livestream.h" +#include "tools/cabana/streams/devicestream.h" +#include "tools/cabana/streams/pandastream.h" #include "tools/cabana/streams/replaystream.h" int main(int argc, char *argv[]) { @@ -24,6 +25,8 @@ int main(int argc, char *argv[]) { cmd_parser.addOption({"qcam", "load qcamera"}); cmd_parser.addOption({"ecam", "load wide road camera"}); cmd_parser.addOption({"stream", "read can messages from live streaming"}); + cmd_parser.addOption({"panda", "read can messages from panda"}); + cmd_parser.addOption({"panda-serial", "read can messages from panda with given serial", "panda-serial"}); cmd_parser.addOption({"zmq", "the ip address on which to receive zmq messages", "zmq"}); cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"}); cmd_parser.addOption({"no-vipc", "do not output video"}); @@ -34,7 +37,13 @@ int main(int argc, char *argv[]) { std::unique_ptr stream; if (cmd_parser.isSet("stream")) { - stream.reset(new LiveStream(&app, cmd_parser.value("zmq"))); + stream.reset(new DeviceStream(&app, cmd_parser.value("zmq"))); + } else if (cmd_parser.isSet("panda") || cmd_parser.isSet("panda-serial")) { + PandaStreamConfig config = {}; + if (cmd_parser.isSet("panda-serial")) { + config.serial = cmd_parser.value("panda-serial"); + } + stream.reset(new PandaStream(&app, config)); } else { // TODO: Remove when OpenpilotPrefix supports ZMQ #ifndef __APPLE__ diff --git a/tools/cabana/streams/devicestream.cc b/tools/cabana/streams/devicestream.cc new file mode 100644 index 0000000000..137fe0cee6 --- /dev/null +++ b/tools/cabana/streams/devicestream.cc @@ -0,0 +1,27 @@ +#include "tools/cabana/streams/devicestream.h" + +DeviceStream::DeviceStream(QObject *parent, QString address) : zmq_address(address), LiveStream(parent) { +} + +void DeviceStream::streamThread() { + if (!zmq_address.isEmpty()) { + setenv("ZMQ", "1", 1); + } + + std::unique_ptr context(Context::create()); + std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString(); + std::unique_ptr sock(SubSocket::create(context.get(), "can", address)); + assert(sock != NULL); + sock->setTimeout(50); + // run as fast as messages come in + while (!QThread::currentThread()->isInterruptionRequested()) { + Message *msg = sock->receive(true); + if (!msg) { + QThread::msleep(50); + continue; + } + + std::lock_guard lk(lock); + handleEvent(messages.emplace_back(msg).event); + } +} diff --git a/tools/cabana/streams/devicestream.h b/tools/cabana/streams/devicestream.h new file mode 100644 index 0000000000..715dcd17e0 --- /dev/null +++ b/tools/cabana/streams/devicestream.h @@ -0,0 +1,17 @@ +#pragma once + +#include "tools/cabana/streams/livestream.h" + +class DeviceStream : public LiveStream { + Q_OBJECT +public: + DeviceStream(QObject *parent, QString address = {}); + + inline QString routeName() const override { + return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address); + } + +protected: + void streamThread() override; + const QString zmq_address; +}; diff --git a/tools/cabana/streams/livestream.cc b/tools/cabana/streams/livestream.cc index 0fcee4455a..a7aab1ab6d 100644 --- a/tools/cabana/streams/livestream.cc +++ b/tools/cabana/streams/livestream.cc @@ -2,7 +2,13 @@ #include -LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) { +LiveStream::LiveStream(QObject *parent) : AbstractStream(parent, true) { + if (settings.log_livestream) { + std::string path = (settings.log_path + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd--hh-mm-ss") + "--0").toStdString(); + util::create_directories(path, 0755); + fs.reset(new std::ofstream(path + "/rlog" , std::ios::binary | std::ios::out)); + } + stream_thread = new QThread(this); QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); }); QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater); @@ -15,39 +21,12 @@ LiveStream::~LiveStream() { stream_thread->wait(); } -void LiveStream::streamThread() { - if (!zmq_address.isEmpty()) { - setenv("ZMQ", "1", 1); - } - - std::unique_ptr fs; - if (settings.log_livestream) { - std::string path = (settings.log_path + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd--hh-mm-ss") + "--0").toStdString(); - util::create_directories(path, 0755); - fs.reset(new std::ofstream(path + "/rlog" , std::ios::binary | std::ios::out)); - } - std::unique_ptr context(Context::create()); - std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString(); - std::unique_ptr sock(SubSocket::create(context.get(), "can", address)); - assert(sock != NULL); - sock->setTimeout(50); - // run as fast as messages come in - while (!QThread::currentThread()->isInterruptionRequested()) { - Message *msg = sock->receive(true); - if (!msg) { - QThread::msleep(50); - continue; - } - - if (fs) { - fs->write(msg->getData(), msg->getSize()); - } - std::lock_guard lk(lock); - handleEvent(messages.emplace_back(msg).event); +void LiveStream::handleEvent(Event *evt) { + if (fs) { + auto bytes = evt->words.asChars(); + fs->write(bytes.begin(), bytes.size()); } -} -void LiveStream::handleEvent(Event *evt) { if (start_ts == 0 || evt->mono_time < start_ts) { if (evt->mono_time < start_ts) { qDebug() << "stream is looping back to old time stamp"; diff --git a/tools/cabana/streams/livestream.h b/tools/cabana/streams/livestream.h index 598c0b9365..880e5b95ab 100644 --- a/tools/cabana/streams/livestream.h +++ b/tools/cabana/streams/livestream.h @@ -6,11 +6,8 @@ class LiveStream : public AbstractStream { Q_OBJECT public: - LiveStream(QObject *parent, QString address = {}); + LiveStream(QObject *parent); virtual ~LiveStream(); - inline QString routeName() const override { - return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address); - } inline double routeStartTime() const override { return start_ts / (double)1e9; } inline double currentSec() const override { return (current_ts - start_ts) / (double)1e9; } void setSpeed(float speed) override { speed_ = std::min(1.0, speed); } @@ -19,7 +16,7 @@ public: protected: virtual void handleEvent(Event *evt); - virtual void streamThread(); + virtual void streamThread() = 0; void process(QHash *) override; struct Msg { @@ -27,6 +24,9 @@ protected: event = ::new Event(aligned_buf.align(m)); delete m; } + Msg(const char *data, const size_t size) { + event = ::new Event(aligned_buf.align(data, size)); + } ~Msg() { ::delete event; } Event *event; AlignedBuffer aligned_buf; @@ -41,6 +41,7 @@ protected: std::atomic pause_ = false; uint64_t last_update_ts = 0; - const QString zmq_address; + std::unique_ptr fs; + QThread *stream_thread; }; diff --git a/tools/cabana/streams/pandastream.cc b/tools/cabana/streams/pandastream.cc new file mode 100644 index 0000000000..7b1bc28354 --- /dev/null +++ b/tools/cabana/streams/pandastream.cc @@ -0,0 +1,85 @@ +#include "tools/cabana/streams/pandastream.h" + +PandaStream::PandaStream(QObject *parent, PandaStreamConfig config_) : config(config_), LiveStream(parent) { + if (config.serial.isEmpty()) { + auto serials = Panda::list(); + if (serials.size() == 0) { + throw std::runtime_error("No panda found"); + } + config.serial = QString::fromStdString(serials[0]); + } + + qDebug() << "Connecting to panda with serial" << config.serial; + if (!connect()) { + throw std::runtime_error("Failed to connect to panda"); + } +} + +bool PandaStream::connect() { + try { + panda.reset(new Panda(config.serial.toStdString())); + config.bus_config.resize(3); + qDebug() << "Connected"; + } catch (const std::exception& e) { + return false; + } + + panda->set_safety_model(cereal::CarParams::SafetyModel::SILENT); + + for (int bus = 0; bus < config.bus_config.size(); bus++) { + panda->set_can_speed_kbps(bus, config.bus_config[bus].can_speed_kbps); + + // CAN-FD + if (panda->hw_type == cereal::PandaState::PandaType::RED_PANDA || panda->hw_type == cereal::PandaState::PandaType::RED_PANDA_V2) { + if (config.bus_config[bus].can_fd) { + panda->set_data_speed_kbps(bus, config.bus_config[bus].data_speed_kbps); + } else { + // Hack to disable can-fd by setting data speed to a low value + panda->set_data_speed_kbps(bus, 10); + } + } + + } + return true; +} + +void PandaStream::streamThread() { + std::vector raw_can_data; + + while (!QThread::currentThread()->isInterruptionRequested()) { + QThread::msleep(1); + + if (!panda->connected()) { + qDebug() << "Connection to panda lost. Attempting reconnect."; + if (!connect()){ + QThread::msleep(1000); + continue; + } + } + + raw_can_data.clear(); + if (!panda->can_receive(raw_can_data)) { + qDebug() << "failed to receive"; + continue; + } + + MessageBuilder msg; + auto evt = msg.initEvent(); + auto canData = evt.initCan(raw_can_data.size()); + + for (uint i = 0; isend_heartbeat(false); + } +} diff --git a/tools/cabana/streams/pandastream.h b/tools/cabana/streams/pandastream.h new file mode 100644 index 0000000000..fe90de2068 --- /dev/null +++ b/tools/cabana/streams/pandastream.h @@ -0,0 +1,33 @@ +#pragma once + +#include "tools/cabana/streams/livestream.h" +#include "selfdrive/boardd/panda.h" + +struct BusConfig { + int can_speed_kbps = 500; + int data_speed_kbps = 2000; + bool can_fd = false; +}; + +struct PandaStreamConfig { + QString serial = ""; + std::vector bus_config; +}; + +class PandaStream : public LiveStream { + Q_OBJECT +public: + PandaStream(QObject *parent, PandaStreamConfig config_ = {}); + + inline QString routeName() const override { + return QString("Live Streaming From Panda %1").arg(config.serial); + } + +protected: + void streamThread() override; + bool connect(); + + std::unique_ptr panda; + PandaStreamConfig config = {}; +}; +