74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
#include "tools/cabana/streams/pandastream.h"
|
|
|
|
#include <chrono>
|
|
#include <cstdio>
|
|
#include <thread>
|
|
|
|
PandaStream::PandaStream(PandaStreamConfig config_) : config(config_) {
|
|
if (!connect()) {
|
|
throw std::runtime_error("Failed to connect to panda");
|
|
}
|
|
}
|
|
|
|
bool PandaStream::connect() {
|
|
try {
|
|
fprintf(stderr, "Connecting to panda %s\n", config.serial.c_str());
|
|
panda.reset(new Panda(config.serial));
|
|
config.bus_config.resize(3);
|
|
fprintf(stderr, "Connected\n");
|
|
} catch (const std::exception& e) {
|
|
return false;
|
|
}
|
|
|
|
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
|
for (int bus = 0; bus < config.bus_config.size(); bus++) {
|
|
panda->set_can_speed_kbps(bus, config.bus_config[bus].can_speed_kbps);
|
|
|
|
|
|
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 {
|
|
|
|
panda->set_data_speed_kbps(bus, 10);
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void PandaStream::streamThread() {
|
|
std::vector<can_frame> raw_can_data;
|
|
|
|
while (!exit_) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
|
if (!panda->connected()) {
|
|
fprintf(stderr, "Connection to panda lost. Attempting reconnect.\n");
|
|
if (!connect()){
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
continue;
|
|
}
|
|
}
|
|
|
|
raw_can_data.clear();
|
|
if (!panda->can_receive(raw_can_data)) {
|
|
fprintf(stderr, "failed to receive\n");
|
|
continue;
|
|
}
|
|
|
|
MessageBuilder msg;
|
|
auto evt = msg.initEvent();
|
|
auto canData = evt.initCan(raw_can_data.size());
|
|
for (uint i = 0; i<raw_can_data.size(); i++) {
|
|
canData[i].setAddress(raw_can_data[i].address);
|
|
canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
|
|
canData[i].setSrc(raw_can_data[i].src);
|
|
}
|
|
|
|
handleEvent(capnp::messageToFlatArray(msg));
|
|
|
|
panda->send_heartbeat(false);
|
|
}
|
|
}
|