Multipanda support (#22402)
* squash all PR commits for easy rebase * merged all panda rx can into one message * fix buffers in can_send * more cleanup and minor fixes * fix even more stuff * fix non-allocated send buffer * make connecting more robust * fix bus offset * fix controls_mismatch? * simplify mismatch check * C++ style struct * fix connect loop * update ref Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>pull/22740/head
parent
25e4e94691
commit
585c16cd2a
|
@ -31,6 +31,25 @@
|
||||||
#include "selfdrive/boardd/panda.h"
|
#include "selfdrive/boardd/panda.h"
|
||||||
#include "selfdrive/boardd/pigeon.h"
|
#include "selfdrive/boardd/pigeon.h"
|
||||||
|
|
||||||
|
// -- Multi-panda conventions --
|
||||||
|
// Ordering:
|
||||||
|
// - The internal panda will always be the first panda
|
||||||
|
// - Consecutive pandas will be sorted based on panda type, and then serial number
|
||||||
|
// Connecting:
|
||||||
|
// - If a panda connection is dropped, boardd wil reconnect to all pandas
|
||||||
|
// - If a panda is added, we will only reconnect when we are offroad
|
||||||
|
// CAN buses:
|
||||||
|
// - Each panda will have it's block of 4 buses. E.g.: the second panda will use
|
||||||
|
// bus numbers 4, 5, 6 and 7
|
||||||
|
// - The internal panda will always be used for accessing the OBD2 port,
|
||||||
|
// and thus firmware queries
|
||||||
|
// Safety:
|
||||||
|
// - SafetyConfig is a list, which is mapped to the connected pandas
|
||||||
|
// - If there are more pandas connected than there are SafetyConfigs,
|
||||||
|
// the excess pandas will remain in "silent" ot "noOutput" mode
|
||||||
|
// Ignition:
|
||||||
|
// - If any of the ignition sources in any panda is high, ignition is high
|
||||||
|
|
||||||
#define MAX_IR_POWER 0.5f
|
#define MAX_IR_POWER 0.5f
|
||||||
#define MIN_IR_POWER 0.0f
|
#define MIN_IR_POWER 0.0f
|
||||||
#define CUTOFF_IL 200
|
#define CUTOFF_IL 200
|
||||||
|
@ -49,18 +68,30 @@ std::string get_time_str(const struct tm &time) {
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool safety_setter_thread(Panda *panda) {
|
bool check_all_connected(std::vector<Panda *> pandas) {
|
||||||
|
for (const auto& panda : pandas) {
|
||||||
|
if (!panda->connected) return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool safety_setter_thread(std::vector<Panda *> pandas) {
|
||||||
LOGD("Starting safety setter thread");
|
LOGD("Starting safety setter thread");
|
||||||
// diagnostic only is the default, needed for VIN query
|
|
||||||
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
|
// there should be at least one panda connected
|
||||||
|
if (pandas.size() == 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
|
||||||
|
|
||||||
Params p = Params();
|
Params p = Params();
|
||||||
|
|
||||||
// switch to SILENT when CarVin param is read
|
// switch to SILENT when CarVin param is read
|
||||||
while (true) {
|
while (true) {
|
||||||
if (do_exit || !panda->connected || !ignition) {
|
if (do_exit || !check_all_connected(pandas) || !ignition) {
|
||||||
return false;
|
return false;
|
||||||
};
|
}
|
||||||
|
|
||||||
std::string value_vin = p.get("CarVin");
|
std::string value_vin = p.get("CarVin");
|
||||||
if (value_vin.size() > 0) {
|
if (value_vin.size() > 0) {
|
||||||
|
@ -72,15 +103,16 @@ bool safety_setter_thread(Panda *panda) {
|
||||||
util::sleep_for(20);
|
util::sleep_for(20);
|
||||||
}
|
}
|
||||||
|
|
||||||
// VIN query done, stop listening to OBDII
|
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
|
||||||
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
|
|
||||||
|
|
||||||
std::string params;
|
std::string params;
|
||||||
LOGW("waiting for params to set safety model");
|
LOGW("waiting for params to set safety model");
|
||||||
while (true) {
|
while (true) {
|
||||||
if (do_exit || !panda->connected || !ignition) {
|
for (const auto& panda : pandas) {
|
||||||
return false;
|
if (do_exit || !panda->connected || !ignition) {
|
||||||
};
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (p.getBool("ControlsReady")) {
|
if (p.getBool("ControlsReady")) {
|
||||||
params = p.get("CarParams");
|
params = p.get("CarParams");
|
||||||
|
@ -97,27 +129,31 @@ bool safety_setter_thread(Panda *panda) {
|
||||||
int safety_param;
|
int safety_param;
|
||||||
|
|
||||||
auto safety_configs = car_params.getSafetyConfigs();
|
auto safety_configs = car_params.getSafetyConfigs();
|
||||||
if (safety_configs.size() > 0) {
|
for (uint32_t i=0; i<pandas.size(); i++) {
|
||||||
safety_model = safety_configs[0].getSafetyModel();
|
auto panda = pandas[i];
|
||||||
safety_param = safety_configs[0].getSafetyParam();
|
|
||||||
} else {
|
if (safety_configs.size() > i) {
|
||||||
// If no safety mode is set, default to silent
|
safety_model = safety_configs[i].getSafetyModel();
|
||||||
safety_model = cereal::CarParams::SafetyModel::SILENT;
|
safety_param = safety_configs[i].getSafetyParam();
|
||||||
safety_param = 0;
|
} else {
|
||||||
|
// If no safety mode is specified, default to silent
|
||||||
|
safety_model = cereal::CarParams::SafetyModel::SILENT;
|
||||||
|
safety_param = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param);
|
||||||
|
|
||||||
|
panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values
|
||||||
|
panda->set_safety_model(safety_model, safety_param);
|
||||||
}
|
}
|
||||||
|
|
||||||
panda->set_unsafe_mode(0); // see safety_declarations.h for allowed values
|
|
||||||
|
|
||||||
LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param);
|
|
||||||
panda->set_safety_model(safety_model, safety_param);
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Panda *usb_connect(std::string serial="", uint32_t index=0) {
|
||||||
Panda *usb_connect() {
|
|
||||||
std::unique_ptr<Panda> panda;
|
std::unique_ptr<Panda> panda;
|
||||||
try {
|
try {
|
||||||
panda = std::make_unique<Panda>();
|
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
|
||||||
} catch (std::exception &e) {
|
} catch (std::exception &e) {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
@ -128,27 +164,6 @@ Panda *usb_connect() {
|
||||||
panda->set_loopback(true);
|
panda->set_loopback(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (auto fw_sig = panda->get_firmware_version(); fw_sig) {
|
|
||||||
params.put("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size());
|
|
||||||
|
|
||||||
// Convert to hex for offroad
|
|
||||||
char fw_sig_hex_buf[16] = {0};
|
|
||||||
const uint8_t *fw_sig_buf = fw_sig->data();
|
|
||||||
for (size_t i = 0; i < 8; i++) {
|
|
||||||
fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4);
|
|
||||||
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF);
|
|
||||||
}
|
|
||||||
|
|
||||||
params.put("PandaFirmwareHex", fw_sig_hex_buf, 16);
|
|
||||||
LOGW("fw signature: %.*s", 16, fw_sig_hex_buf);
|
|
||||||
} else { return nullptr; }
|
|
||||||
|
|
||||||
// get panda serial
|
|
||||||
if (auto serial = panda->get_serial(); serial) {
|
|
||||||
params.put("PandaDongleId", serial->c_str(), serial->length());
|
|
||||||
LOGW("panda serial: %s", serial->c_str());
|
|
||||||
} else { return nullptr; }
|
|
||||||
|
|
||||||
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
|
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
|
||||||
#ifndef __x86_64__
|
#ifndef __x86_64__
|
||||||
static std::once_flag connected_once;
|
static std::once_flag connected_once;
|
||||||
|
@ -171,14 +186,7 @@ Panda *usb_connect() {
|
||||||
return panda.release();
|
return panda.release();
|
||||||
}
|
}
|
||||||
|
|
||||||
void can_recv(Panda *panda, PubMaster &pm) {
|
void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
|
||||||
kj::Array<capnp::word> can_data;
|
|
||||||
panda->can_receive(can_data);
|
|
||||||
auto bytes = can_data.asBytes();
|
|
||||||
pm.send("can", bytes.begin(), bytes.size());
|
|
||||||
}
|
|
||||||
|
|
||||||
void can_send_thread(Panda *panda, bool fake_send) {
|
|
||||||
LOGD("start send thread");
|
LOGD("start send thread");
|
||||||
|
|
||||||
AlignedBuffer aligned_buf;
|
AlignedBuffer aligned_buf;
|
||||||
|
@ -188,7 +196,12 @@ void can_send_thread(Panda *panda, bool fake_send) {
|
||||||
subscriber->setTimeout(100);
|
subscriber->setTimeout(100);
|
||||||
|
|
||||||
// run as fast as messages come in
|
// run as fast as messages come in
|
||||||
while (!do_exit && panda->connected) {
|
while (!do_exit) {
|
||||||
|
if (!check_all_connected(pandas)) {
|
||||||
|
do_exit = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
Message * msg = subscriber->receive();
|
Message * msg = subscriber->receive();
|
||||||
|
|
||||||
if (!msg) {
|
if (!msg) {
|
||||||
|
@ -204,7 +217,9 @@ void can_send_thread(Panda *panda, bool fake_send) {
|
||||||
//Dont send if older than 1 second
|
//Dont send if older than 1 second
|
||||||
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
|
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
|
||||||
if (!fake_send) {
|
if (!fake_send) {
|
||||||
panda->can_send(event.getSendcan());
|
for (const auto& panda : pandas) {
|
||||||
|
panda->can_send(event.getSendcan());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -215,7 +230,7 @@ void can_send_thread(Panda *panda, bool fake_send) {
|
||||||
delete context;
|
delete context;
|
||||||
}
|
}
|
||||||
|
|
||||||
void can_recv_thread(Panda *panda) {
|
void can_recv_thread(std::vector<Panda *> pandas) {
|
||||||
LOGD("start recv thread");
|
LOGD("start recv thread");
|
||||||
|
|
||||||
// can = 8006
|
// can = 8006
|
||||||
|
@ -225,8 +240,29 @@ void can_recv_thread(Panda *panda) {
|
||||||
const uint64_t dt = 10000000ULL;
|
const uint64_t dt = 10000000ULL;
|
||||||
uint64_t next_frame_time = nanos_since_boot() + dt;
|
uint64_t next_frame_time = nanos_since_boot() + dt;
|
||||||
|
|
||||||
while (!do_exit && panda->connected) {
|
while (!do_exit) {
|
||||||
can_recv(panda, pm);
|
if (!check_all_connected(pandas)){
|
||||||
|
do_exit = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<can_frame> raw_can_data;
|
||||||
|
bool comms_healthy = true;
|
||||||
|
for (const auto& panda : pandas) {
|
||||||
|
comms_healthy &= panda->can_receive(raw_can_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
MessageBuilder msg;
|
||||||
|
auto evt = msg.initEvent();
|
||||||
|
evt.setValid(comms_healthy);
|
||||||
|
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].setBusTime(raw_can_data[i].busTime);
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
pm.send("can", msg);
|
||||||
|
|
||||||
uint64_t cur_time = nanos_since_boot();
|
uint64_t cur_time = nanos_since_boot();
|
||||||
int64_t remaining = next_frame_time - cur_time;
|
int64_t remaining = next_frame_time - cur_time;
|
||||||
|
@ -257,70 +293,86 @@ void send_empty_panda_state(PubMaster *pm) {
|
||||||
pm->send("pandaStates", msg);
|
pm->send("pandaStates", msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) {
|
bool send_panda_states(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
|
||||||
health_t pandaState = panda->get_state();
|
bool ignition_local = false;
|
||||||
|
|
||||||
if (spoofing_started) {
|
|
||||||
pandaState.ignition_line = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
|
|
||||||
if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
|
|
||||||
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ignition_local = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
|
|
||||||
|
|
||||||
#ifndef __x86_64__
|
|
||||||
bool power_save_desired = !ignition_local && !pigeon_active;
|
|
||||||
if (pandaState.power_save_enabled != power_save_desired) {
|
|
||||||
panda->set_power_saving(power_save_desired);
|
|
||||||
}
|
|
||||||
|
|
||||||
// set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
|
|
||||||
if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
|
|
||||||
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// build msg
|
// build msg
|
||||||
MessageBuilder msg;
|
MessageBuilder msg;
|
||||||
auto evt = msg.initEvent();
|
auto evt = msg.initEvent();
|
||||||
evt.setValid(panda->comms_healthy);
|
auto pss = evt.initPandaStates(pandas.size());
|
||||||
|
|
||||||
// TODO: this has to be adapted to merge in multipanda support
|
std::vector<health_t> pandaStates;
|
||||||
auto ps = evt.initPandaStates(1);
|
for (const auto& panda : pandas){
|
||||||
ps[0].setUptime(pandaState.uptime);
|
health_t pandaState = panda->get_state();
|
||||||
ps[0].setIgnitionLine(pandaState.ignition_line);
|
|
||||||
ps[0].setIgnitionCan(pandaState.ignition_can);
|
|
||||||
ps[0].setControlsAllowed(pandaState.controls_allowed);
|
|
||||||
ps[0].setGasInterceptorDetected(pandaState.gas_interceptor_detected);
|
|
||||||
ps[0].setCanRxErrs(pandaState.can_rx_errs);
|
|
||||||
ps[0].setCanSendErrs(pandaState.can_send_errs);
|
|
||||||
ps[0].setCanFwdErrs(pandaState.can_fwd_errs);
|
|
||||||
ps[0].setGmlanSendErrs(pandaState.gmlan_send_errs);
|
|
||||||
ps[0].setPandaType(panda->hw_type);
|
|
||||||
ps[0].setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
|
|
||||||
ps[0].setSafetyParam(pandaState.safety_param);
|
|
||||||
ps[0].setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
|
|
||||||
ps[0].setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
|
|
||||||
ps[0].setHeartbeatLost((bool)(pandaState.heartbeat_lost));
|
|
||||||
ps[0].setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
|
|
||||||
|
|
||||||
// Convert faults bitset to capnp list
|
if (spoofing_started) {
|
||||||
std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
|
pandaState.ignition_line = 1;
|
||||||
auto faults = ps[0].initFaults(fault_bits.count());
|
}
|
||||||
|
|
||||||
size_t i = 0;
|
ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
|
||||||
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
|
|
||||||
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
|
pandaStates.push_back(pandaState);
|
||||||
if (fault_bits.test(f)) {
|
}
|
||||||
faults.set(i, cereal::PandaState::FaultType(f));
|
|
||||||
i++;
|
for (uint32_t i=0; i<pandas.size(); i++) {
|
||||||
|
auto panda = pandas[i];
|
||||||
|
auto pandaState = pandaStates[i];
|
||||||
|
|
||||||
|
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
|
||||||
|
if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
|
||||||
|
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifndef __x86_64__
|
||||||
|
bool power_save_desired = !ignition_local && !pigeon_active;
|
||||||
|
if (pandaState.power_save_enabled != power_save_desired) {
|
||||||
|
panda->set_power_saving(power_save_desired);
|
||||||
|
}
|
||||||
|
|
||||||
|
// set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
|
||||||
|
if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
|
||||||
|
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// TODO: do we still need this?
|
||||||
|
if (!panda->comms_healthy) {
|
||||||
|
evt.setValid(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
auto ps = pss[i];
|
||||||
|
ps.setUptime(pandaState.uptime);
|
||||||
|
ps.setIgnitionLine(pandaState.ignition_line);
|
||||||
|
ps.setIgnitionCan(pandaState.ignition_can);
|
||||||
|
ps.setControlsAllowed(pandaState.controls_allowed);
|
||||||
|
ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
|
||||||
|
ps.setCanRxErrs(pandaState.can_rx_errs);
|
||||||
|
ps.setCanSendErrs(pandaState.can_send_errs);
|
||||||
|
ps.setCanFwdErrs(pandaState.can_fwd_errs);
|
||||||
|
ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
|
||||||
|
ps.setPandaType(panda->hw_type);
|
||||||
|
ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
|
||||||
|
ps.setSafetyParam(pandaState.safety_param);
|
||||||
|
ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
|
||||||
|
ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
|
||||||
|
ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
|
||||||
|
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
|
||||||
|
|
||||||
|
// Convert faults bitset to capnp list
|
||||||
|
std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
|
||||||
|
auto faults = ps.initFaults(fault_bits.count());
|
||||||
|
|
||||||
|
size_t j = 0;
|
||||||
|
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
|
||||||
|
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
|
||||||
|
if (fault_bits.test(f)) {
|
||||||
|
faults.set(j, cereal::PandaState::FaultType(f));
|
||||||
|
j++;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
pm->send("pandaStates", msg);
|
|
||||||
|
|
||||||
|
pm->send("pandaStates", msg);
|
||||||
return ignition_local;
|
return ignition_local;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -355,23 +407,36 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
|
||||||
pm->send("peripheralState", msg);
|
pm->send("peripheralState", msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, bool spoofing_started) {
|
void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
|
||||||
Params params;
|
Params params;
|
||||||
|
Panda *peripheral_panda = pandas[0];
|
||||||
bool ignition_last = false;
|
bool ignition_last = false;
|
||||||
std::future<bool> safety_future;
|
std::future<bool> safety_future;
|
||||||
|
|
||||||
LOGD("start panda state thread");
|
LOGD("start panda state thread");
|
||||||
|
|
||||||
// run at 2hz
|
// run at 2hz
|
||||||
while (!do_exit && panda->connected) {
|
while (!do_exit) {
|
||||||
|
if(!check_all_connected(pandas)) {
|
||||||
|
do_exit = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
send_peripheral_state(pm, peripheral_panda);
|
send_peripheral_state(pm, peripheral_panda);
|
||||||
ignition = send_panda_state(pm, panda, spoofing_started);
|
ignition = send_panda_states(pm, pandas, spoofing_started);
|
||||||
|
|
||||||
|
// check if we have new pandas and are offroad
|
||||||
|
if (!ignition && (pandas.size() != Panda::list().size())) {
|
||||||
|
LOGW("Reconnecting to changed amount of pandas!");
|
||||||
|
do_exit = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
// clear VIN, CarParams, and set new safety on car start
|
// clear VIN, CarParams, and set new safety on car start
|
||||||
if (ignition && !ignition_last) {
|
if (ignition && !ignition_last) {
|
||||||
params.clearAll(CLEAR_ON_IGNITION_ON);
|
params.clearAll(CLEAR_ON_IGNITION_ON);
|
||||||
if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
|
if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
|
||||||
safety_future = std::async(std::launch::async, safety_setter_thread, panda);
|
safety_future = std::async(std::launch::async, safety_setter_thread, pandas);
|
||||||
} else {
|
} else {
|
||||||
LOGW("Safety setter thread already running");
|
LOGW("Safety setter thread already running");
|
||||||
}
|
}
|
||||||
|
@ -381,7 +446,9 @@ void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, b
|
||||||
|
|
||||||
ignition_last = ignition;
|
ignition_last = ignition;
|
||||||
|
|
||||||
panda->send_heartbeat();
|
for (const auto &panda : pandas) {
|
||||||
|
panda->send_heartbeat();
|
||||||
|
}
|
||||||
util::sleep_for(500);
|
util::sleep_for(500);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -561,7 +628,11 @@ void pigeon_thread(Panda *panda) {
|
||||||
delete pigeon;
|
delete pigeon;
|
||||||
}
|
}
|
||||||
|
|
||||||
int main() {
|
int main(int argc, char* argv[]) {
|
||||||
|
std::vector<std::thread> threads;
|
||||||
|
std::vector<Panda *> pandas;
|
||||||
|
Panda *peripheral_panda;
|
||||||
|
|
||||||
LOGW("starting boardd");
|
LOGW("starting boardd");
|
||||||
|
|
||||||
if (!Hardware::PC()) {
|
if (!Hardware::PC()) {
|
||||||
|
@ -575,31 +646,44 @@ int main() {
|
||||||
LOGW("attempting to connect");
|
LOGW("attempting to connect");
|
||||||
PubMaster pm({"pandaStates", "peripheralState"});
|
PubMaster pm({"pandaStates", "peripheralState"});
|
||||||
|
|
||||||
|
// connect loop
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
Panda *panda = usb_connect();
|
std::vector<std::string> serials(argv + 1, argv + argc);
|
||||||
Panda *peripheral_panda = panda;
|
if (serials.size() == 0) serials.push_back("");
|
||||||
|
|
||||||
// Send empty pandaState & peripheralState and try again
|
// connect to all provided serials
|
||||||
if (panda == nullptr || peripheral_panda == nullptr) {
|
for (int i=0; i<serials.size(); i++) {
|
||||||
|
Panda *p = usb_connect(serials[i], i);
|
||||||
|
if (p != NULL) {
|
||||||
|
pandas.push_back(p);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// send empty pandaState & peripheralState and try again
|
||||||
|
if (pandas.size() != serials.size()) {
|
||||||
send_empty_panda_state(&pm);
|
send_empty_panda_state(&pm);
|
||||||
send_empty_peripheral_state(&pm);
|
send_empty_peripheral_state(&pm);
|
||||||
util::sleep_for(500);
|
util::sleep_for(500);
|
||||||
continue;
|
} else {
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
LOGW("connected to board");
|
peripheral_panda = pandas[0];
|
||||||
|
|
||||||
std::vector<std::thread> threads;
|
LOGW("connected to board");
|
||||||
threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, getenv("STARTED") != nullptr);
|
|
||||||
threads.emplace_back(peripheral_control_thread, peripheral_panda);
|
|
||||||
threads.emplace_back(pigeon_thread, peripheral_panda);
|
|
||||||
|
|
||||||
threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
|
threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
|
||||||
threads.emplace_back(can_recv_thread, panda);
|
threads.emplace_back(peripheral_control_thread, peripheral_panda);
|
||||||
|
threads.emplace_back(pigeon_thread, peripheral_panda);
|
||||||
|
|
||||||
for (auto &t : threads) t.join();
|
threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
|
||||||
|
threads.emplace_back(can_recv_thread, pandas);
|
||||||
|
|
||||||
|
for (auto &t : threads) t.join();
|
||||||
|
|
||||||
|
// we have exited, clean up pandas
|
||||||
|
for (const auto& panda : pandas){
|
||||||
delete panda;
|
delete panda;
|
||||||
panda = nullptr;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,11 +1,5 @@
|
||||||
#include "cereal/messaging/messaging.h"
|
#include "cereal/messaging/messaging.h"
|
||||||
|
#include "panda.h"
|
||||||
typedef struct {
|
|
||||||
long address;
|
|
||||||
std::string dat;
|
|
||||||
long busTime;
|
|
||||||
long src;
|
|
||||||
} can_frame;
|
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@ static int init_usb_ctx(libusb_context **context) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
Panda::Panda(std::string serial) {
|
Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
|
||||||
// init libusb
|
// init libusb
|
||||||
ssize_t num_devices;
|
ssize_t num_devices;
|
||||||
libusb_device **dev_list = NULL;
|
libusb_device **dev_list = NULL;
|
||||||
|
@ -344,30 +344,35 @@ void Panda::send_heartbeat() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
|
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
|
||||||
const int msg_count = can_data_list.size();
|
send.resize(4 * can_data_list.size());
|
||||||
const int buf_size = msg_count*0x10;
|
|
||||||
|
|
||||||
if (send.size() < buf_size) {
|
uint32_t msg_cnt = 0;
|
||||||
send.resize(buf_size);
|
for (int i = 0; i < can_data_list.size(); i++) {
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < msg_count; i++) {
|
|
||||||
auto cmsg = can_data_list[i];
|
auto cmsg = can_data_list[i];
|
||||||
|
|
||||||
|
// check if the message is intended for this panda
|
||||||
|
uint8_t bus = cmsg.getSrc();
|
||||||
|
if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
if (cmsg.getAddress() >= 0x800) { // extended
|
if (cmsg.getAddress() >= 0x800) { // extended
|
||||||
send[i*4] = (cmsg.getAddress() << 3) | 5;
|
send[msg_cnt*4] = (cmsg.getAddress() << 3) | 5;
|
||||||
} else { // normal
|
} else { // normal
|
||||||
send[i*4] = (cmsg.getAddress() << 21) | 1;
|
send[msg_cnt*4] = (cmsg.getAddress() << 21) | 1;
|
||||||
}
|
}
|
||||||
auto can_data = cmsg.getDat();
|
auto can_data = cmsg.getDat();
|
||||||
assert(can_data.size() <= 8);
|
assert(can_data.size() <= 8);
|
||||||
send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
|
send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4);
|
||||||
memcpy(&send[i*4+2], can_data.begin(), can_data.size());
|
memcpy(&send[msg_cnt*4+2], can_data.begin(), can_data.size());
|
||||||
|
|
||||||
|
msg_cnt++;
|
||||||
}
|
}
|
||||||
|
|
||||||
usb_bulk_write(3, (unsigned char*)send.data(), buf_size, 5);
|
usb_bulk_write(3, (unsigned char*)send.data(), msg_cnt * 0x10, 5);
|
||||||
}
|
}
|
||||||
|
|
||||||
int Panda::can_receive(kj::Array<capnp::word>& out_buf) {
|
bool Panda::can_receive(std::vector<can_frame>& out_vec) {
|
||||||
uint32_t data[RECV_SIZE/4];
|
uint32_t data[RECV_SIZE/4];
|
||||||
int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
|
int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
|
||||||
|
|
||||||
|
@ -378,27 +383,34 @@ int Panda::can_receive(kj::Array<capnp::word>& out_buf) {
|
||||||
LOGW("Receive buffer full");
|
LOGW("Receive buffer full");
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t num_msg = recv / 0x10;
|
if (!comms_healthy) {
|
||||||
MessageBuilder msg;
|
return false;
|
||||||
auto evt = msg.initEvent();
|
}
|
||||||
evt.setValid(comms_healthy);
|
|
||||||
|
|
||||||
// populate message
|
// Append to the end of the out_vec, such that we can pass it to multiple pandas
|
||||||
auto canData = evt.initCan(num_msg);
|
// We already insert space for all the messages here for speed
|
||||||
|
size_t num_msg = recv / 0x10;
|
||||||
|
out_vec.reserve(out_vec.size() + num_msg);
|
||||||
|
|
||||||
|
// Populate messages
|
||||||
for (int i = 0; i < num_msg; i++) {
|
for (int i = 0; i < num_msg; i++) {
|
||||||
|
can_frame canData;
|
||||||
if (data[i*4] & 4) {
|
if (data[i*4] & 4) {
|
||||||
// extended
|
// extended
|
||||||
canData[i].setAddress(data[i*4] >> 3);
|
canData.address = data[i*4] >> 3;
|
||||||
//printf("got extended: %x\n", data[i*4] >> 3);
|
//printf("got extended: %x\n", data[i*4] >> 3);
|
||||||
} else {
|
} else {
|
||||||
// normal
|
// normal
|
||||||
canData[i].setAddress(data[i*4] >> 21);
|
canData.address = data[i*4] >> 21;
|
||||||
}
|
}
|
||||||
canData[i].setBusTime(data[i*4+1] >> 16);
|
canData.busTime = data[i*4+1] >> 16;
|
||||||
int len = data[i*4+1]&0xF;
|
int len = data[i*4+1] & 0xF;
|
||||||
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
|
canData.dat.assign((char *)&data[i*4+2], len);
|
||||||
canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
|
canData.src = ((data[i*4+1] >> 4) & 0xff) + bus_offset;
|
||||||
|
|
||||||
|
// add to vector
|
||||||
|
out_vec.push_back(canData);
|
||||||
}
|
}
|
||||||
out_buf = capnp::messageToFlatArray(msg);
|
|
||||||
return recv;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -3,6 +3,7 @@
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <ctime>
|
#include <ctime>
|
||||||
|
#include <list>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -15,6 +16,7 @@
|
||||||
// double the FIFO size
|
// double the FIFO size
|
||||||
#define RECV_SIZE (0x1000)
|
#define RECV_SIZE (0x1000)
|
||||||
#define TIMEOUT 0
|
#define TIMEOUT 0
|
||||||
|
#define PANDA_BUS_CNT 4
|
||||||
|
|
||||||
// copied from panda/board/main.c
|
// copied from panda/board/main.c
|
||||||
struct __attribute__((packed)) health_t {
|
struct __attribute__((packed)) health_t {
|
||||||
|
@ -39,6 +41,12 @@ struct __attribute__((packed)) health_t {
|
||||||
uint8_t heartbeat_lost;
|
uint8_t heartbeat_lost;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct can_frame {
|
||||||
|
long address;
|
||||||
|
std::string dat;
|
||||||
|
long busTime;
|
||||||
|
long src;
|
||||||
|
};
|
||||||
|
|
||||||
class Panda {
|
class Panda {
|
||||||
private:
|
private:
|
||||||
|
@ -50,7 +58,7 @@ class Panda {
|
||||||
void cleanup();
|
void cleanup();
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Panda(std::string serial="");
|
Panda(std::string serial="", uint32_t bus_offset=0);
|
||||||
~Panda();
|
~Panda();
|
||||||
|
|
||||||
std::string usb_serial;
|
std::string usb_serial;
|
||||||
|
@ -58,6 +66,7 @@ class Panda {
|
||||||
std::atomic<bool> comms_healthy = true;
|
std::atomic<bool> comms_healthy = true;
|
||||||
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
|
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
|
||||||
bool has_rtc = false;
|
bool has_rtc = false;
|
||||||
|
const uint32_t bus_offset;
|
||||||
|
|
||||||
// Static functions
|
// Static functions
|
||||||
static std::vector<std::string> list();
|
static std::vector<std::string> list();
|
||||||
|
@ -85,5 +94,5 @@ class Panda {
|
||||||
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
|
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
|
||||||
void send_heartbeat();
|
void send_heartbeat();
|
||||||
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
|
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
|
||||||
int can_receive(kj::Array<capnp::word>& out_buf);
|
bool can_receive(std::vector<can_frame>& out_vec);
|
||||||
};
|
};
|
||||||
|
|
|
@ -155,9 +155,6 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||||
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
|
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
|
||||||
{"NavSettingTime24h", PERSISTENT},
|
{"NavSettingTime24h", PERSISTENT},
|
||||||
{"OpenpilotEnabledToggle", PERSISTENT},
|
{"OpenpilotEnabledToggle", PERSISTENT},
|
||||||
{"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
|
|
||||||
{"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
|
|
||||||
{"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
|
|
||||||
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
|
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
|
||||||
{"Passive", PERSISTENT},
|
{"Passive", PERSISTENT},
|
||||||
{"PrimeRedirected", PERSISTENT},
|
{"PrimeRedirected", PERSISTENT},
|
||||||
|
|
|
@ -51,6 +51,8 @@ EventName = car.CarEvent.EventName
|
||||||
ButtonEvent = car.CarState.ButtonEvent
|
ButtonEvent = car.CarState.ButtonEvent
|
||||||
SafetyModel = car.CarParams.SafetyModel
|
SafetyModel = car.CarParams.SafetyModel
|
||||||
|
|
||||||
|
IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput]
|
||||||
|
|
||||||
|
|
||||||
class Controls:
|
class Controls:
|
||||||
def __init__(self, sm=None, pm=None, can_sock=None):
|
def __init__(self, sm=None, pm=None, can_sock=None):
|
||||||
|
@ -247,14 +249,17 @@ class Controls:
|
||||||
self.events.add(EventName.canError)
|
self.events.add(EventName.canError)
|
||||||
|
|
||||||
for i, pandaState in enumerate(self.sm['pandaStates']):
|
for i, pandaState in enumerate(self.sm['pandaStates']):
|
||||||
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent
|
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
|
||||||
if i < len(self.CP.safetyConfigs):
|
if i < len(self.CP.safetyConfigs):
|
||||||
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
|
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
|
||||||
else:
|
else:
|
||||||
safety_mismatch = pandaState.safetyModel != SafetyModel.silent
|
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
|
||||||
if safety_mismatch or self.mismatch_counter >= 200:
|
if safety_mismatch or self.mismatch_counter >= 200:
|
||||||
self.events.add(EventName.controlsMismatch)
|
self.events.add(EventName.controlsMismatch)
|
||||||
|
|
||||||
|
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
|
||||||
|
self.events.add(EventName.relayMalfunction)
|
||||||
|
|
||||||
if not self.sm['liveParameters'].valid:
|
if not self.sm['liveParameters'].valid:
|
||||||
self.events.add(EventName.vehicleModelInvalid)
|
self.events.add(EventName.vehicleModelInvalid)
|
||||||
|
|
||||||
|
@ -370,9 +375,9 @@ class Controls:
|
||||||
self.mismatch_counter = 0
|
self.mismatch_counter = 0
|
||||||
|
|
||||||
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
|
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
|
||||||
for pandaState in self.sm['pandaStates']:
|
if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
|
||||||
if pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled:
|
if ps.safetyModel not in IGNORED_SAFETY_MODES):
|
||||||
self.mismatch_counter += 1
|
self.mismatch_counter += 1
|
||||||
|
|
||||||
self.distance_traveled += CS.vEgo * DT_CTRL
|
self.distance_traveled += CS.vEgo * DT_CTRL
|
||||||
|
|
||||||
|
|
|
@ -21,10 +21,6 @@
|
||||||
"severity": 1,
|
"severity": 1,
|
||||||
"_comment": "Append the command and error to the text."
|
"_comment": "Append the command and error to the text."
|
||||||
},
|
},
|
||||||
"Offroad_PandaFirmwareMismatch": {
|
|
||||||
"text": "Unexpected panda firmware version. System won't start. Reboot your device to reflash panda.",
|
|
||||||
"severity": 1
|
|
||||||
},
|
|
||||||
"Offroad_InvalidTime": {
|
"Offroad_InvalidTime": {
|
||||||
"text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
|
"text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
|
||||||
"severity": 1
|
"severity": 1
|
||||||
|
|
|
@ -1,61 +1,40 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
# simple boardd wrapper that updates the panda first
|
# simple boardd wrapper that updates the panda first
|
||||||
import os
|
import os
|
||||||
|
import usb1
|
||||||
import time
|
import time
|
||||||
|
import subprocess
|
||||||
|
from typing import List
|
||||||
|
from functools import cmp_to_key
|
||||||
|
|
||||||
from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU
|
from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU
|
||||||
from common.basedir import BASEDIR
|
from common.basedir import BASEDIR
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
|
|
||||||
PANDA_FW_FN = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed")
|
|
||||||
|
|
||||||
|
def get_expected_signature(panda : Panda) -> bytes:
|
||||||
|
fn = DEFAULT_H7_FW_FN if (panda.get_mcu_type() == MCU_TYPE_H7) else DEFAULT_FW_FN
|
||||||
|
|
||||||
def get_expected_signature() -> bytes:
|
|
||||||
try:
|
try:
|
||||||
return Panda.get_signature_from_firmware(PANDA_FW_FN)
|
return Panda.get_signature_from_firmware(fn)
|
||||||
except Exception:
|
except Exception:
|
||||||
cloudlog.exception("Error computing expected signature")
|
cloudlog.exception("Error computing expected signature")
|
||||||
return b""
|
return b""
|
||||||
|
|
||||||
|
|
||||||
def update_panda() -> Panda:
|
def flash_panda(panda_serial : str) -> Panda:
|
||||||
panda = None
|
panda = Panda(panda_serial)
|
||||||
panda_dfu = None
|
|
||||||
|
|
||||||
cloudlog.info("Connecting to panda")
|
fw_signature = get_expected_signature(panda)
|
||||||
|
|
||||||
while True:
|
|
||||||
# break on normal mode Panda
|
|
||||||
panda_list = Panda.list()
|
|
||||||
if len(panda_list) > 0:
|
|
||||||
cloudlog.info("Panda found, connecting")
|
|
||||||
panda = Panda(panda_list[0])
|
|
||||||
break
|
|
||||||
|
|
||||||
# flash on DFU mode Panda
|
|
||||||
panda_dfu = PandaDFU.list()
|
|
||||||
if len(panda_dfu) > 0:
|
|
||||||
cloudlog.info("Panda in DFU mode found, flashing recovery")
|
|
||||||
panda_dfu = PandaDFU(panda_dfu[0])
|
|
||||||
panda_dfu.recover()
|
|
||||||
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
fw_signature = get_expected_signature()
|
|
||||||
|
|
||||||
try:
|
|
||||||
serial = panda.get_serial()[0].decode("utf-8")
|
|
||||||
except Exception:
|
|
||||||
serial = None
|
|
||||||
|
|
||||||
panda_version = "bootstub" if panda.bootstub else panda.get_version()
|
panda_version = "bootstub" if panda.bootstub else panda.get_version()
|
||||||
panda_signature = b"" if panda.bootstub else panda.get_signature()
|
panda_signature = b"" if panda.bootstub else panda.get_signature()
|
||||||
cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
|
cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
|
||||||
serial,
|
panda_serial,
|
||||||
panda_version,
|
panda_version,
|
||||||
panda_signature.hex(),
|
panda_signature.hex()[:16],
|
||||||
fw_signature.hex(),
|
fw_signature.hex()[:16],
|
||||||
))
|
))
|
||||||
|
|
||||||
if panda.bootstub or panda_signature != fw_signature:
|
if panda.bootstub or panda_signature != fw_signature:
|
||||||
|
@ -80,22 +59,68 @@ def update_panda() -> Panda:
|
||||||
|
|
||||||
return panda
|
return panda
|
||||||
|
|
||||||
|
def panda_sort_cmp(a : Panda, b : Panda):
|
||||||
|
a_type = a.get_type()
|
||||||
|
b_type = b.get_type()
|
||||||
|
|
||||||
|
# make sure the internal one is always first
|
||||||
|
if a.is_internal() and not b.is_internal():
|
||||||
|
return -1
|
||||||
|
if not a.is_internal() and b.is_internal():
|
||||||
|
return 1
|
||||||
|
|
||||||
|
# sort by hardware type
|
||||||
|
if a_type != b_type:
|
||||||
|
return a_type < b_type
|
||||||
|
|
||||||
|
# last resort: sort by serial number
|
||||||
|
return a.get_usb_serial() < b.get_usb_serial()
|
||||||
|
|
||||||
def main() -> None:
|
def main() -> None:
|
||||||
panda = update_panda()
|
while True:
|
||||||
|
try:
|
||||||
|
# Flash all Pandas in DFU mode
|
||||||
|
for p in PandaDFU.list():
|
||||||
|
cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}")
|
||||||
|
PandaDFU(p).recover()
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
# check health for lost heartbeat
|
panda_serials = Panda.list()
|
||||||
health = panda.health()
|
if len(panda_serials) == 0:
|
||||||
if health["heartbeat_lost"]:
|
continue
|
||||||
Params().put_bool("PandaHeartbeatLost", True)
|
|
||||||
cloudlog.event("heartbeat lost", deviceState=health)
|
|
||||||
|
|
||||||
cloudlog.info("Resetting panda")
|
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
|
||||||
panda.reset()
|
|
||||||
|
|
||||||
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
|
# Flash pandas
|
||||||
os.execvp("./boardd", ["./boardd"])
|
pandas = []
|
||||||
|
for serial in panda_serials:
|
||||||
|
pandas.append(flash_panda(serial))
|
||||||
|
|
||||||
|
# check health for lost heartbeat
|
||||||
|
for panda in pandas:
|
||||||
|
health = panda.health()
|
||||||
|
if health["heartbeat_lost"]:
|
||||||
|
Params().put_bool("PandaHeartbeatLost", True)
|
||||||
|
cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
|
||||||
|
|
||||||
|
cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
|
||||||
|
panda.reset()
|
||||||
|
|
||||||
|
# sort pandas to have deterministic order
|
||||||
|
pandas.sort(key=cmp_to_key(panda_sort_cmp))
|
||||||
|
panda_serials = list(map(lambda p: p.get_usb_serial(), pandas))
|
||||||
|
|
||||||
|
# close all pandas
|
||||||
|
for p in pandas:
|
||||||
|
p.close()
|
||||||
|
except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
|
||||||
|
# a panda was disconnected while setting everything up. let's try again
|
||||||
|
cloudlog.exception("Panda USB exception while setting up")
|
||||||
|
continue
|
||||||
|
|
||||||
|
# run boardd with all connected serials as arguments
|
||||||
|
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
|
||||||
|
subprocess.run(["./boardd", *panda_serials], check=True)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
4ad66f0fea4c42e79ba347a73ff839e5369c0411
|
f1f065451899f9b8e1cec379b32b970c1c16f849
|
|
@ -20,13 +20,10 @@ from selfdrive.controls.lib.alertmanager import set_offroad_alert
|
||||||
from selfdrive.controls.lib.pid import PIController
|
from selfdrive.controls.lib.pid import PIController
|
||||||
from selfdrive.hardware import EON, TICI, PC, HARDWARE
|
from selfdrive.hardware import EON, TICI, PC, HARDWARE
|
||||||
from selfdrive.loggerd.config import get_available_percent
|
from selfdrive.loggerd.config import get_available_percent
|
||||||
from selfdrive.pandad import get_expected_signature
|
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
from selfdrive.thermald.power_monitoring import PowerMonitoring
|
from selfdrive.thermald.power_monitoring import PowerMonitoring
|
||||||
from selfdrive.version import tested_branch, terms_version, training_version
|
from selfdrive.version import tested_branch, terms_version, training_version
|
||||||
|
|
||||||
FW_SIGNATURE = get_expected_signature()
|
|
||||||
|
|
||||||
ThermalStatus = log.DeviceState.ThermalStatus
|
ThermalStatus = log.DeviceState.ThermalStatus
|
||||||
NetworkType = log.DeviceState.NetworkType
|
NetworkType = log.DeviceState.NetworkType
|
||||||
NetworkStrength = log.DeviceState.NetworkStrength
|
NetworkStrength = log.DeviceState.NetworkStrength
|
||||||
|
@ -364,10 +361,6 @@ def thermald_thread():
|
||||||
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
|
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
|
||||||
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
|
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
|
||||||
|
|
||||||
panda_signature = params.get("PandaFirmware")
|
|
||||||
startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
|
|
||||||
set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"]))
|
|
||||||
|
|
||||||
# with 2% left, we killall, otherwise the phone will take a long time to boot
|
# with 2% left, we killall, otherwise the phone will take a long time to boot
|
||||||
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
|
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
|
||||||
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
|
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
|
||||||
|
|
Loading…
Reference in New Issue