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
Robbe Derks
GitHub
parent
25e4e94691
commit
585c16cd2a
|
|
@ -31,6 +31,25 @@
|
|||
#include "selfdrive/boardd/panda.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 MIN_IR_POWER 0.0f
|
||||
#define CUTOFF_IL 200
|
||||
|
|
@ -49,18 +68,30 @@ std::string get_time_str(const struct tm &time) {
|
|||
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");
|
||||
// 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();
|
||||
|
||||
// switch to SILENT when CarVin param is read
|
||||
while (true) {
|
||||
if (do_exit || !panda->connected || !ignition) {
|
||||
if (do_exit || !check_all_connected(pandas) || !ignition) {
|
||||
return false;
|
||||
};
|
||||
}
|
||||
|
||||
std::string value_vin = p.get("CarVin");
|
||||
if (value_vin.size() > 0) {
|
||||
|
|
@ -72,15 +103,16 @@ bool safety_setter_thread(Panda *panda) {
|
|||
util::sleep_for(20);
|
||||
}
|
||||
|
||||
// VIN query done, stop listening to OBDII
|
||||
panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
|
||||
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
|
||||
|
||||
std::string params;
|
||||
LOGW("waiting for params to set safety model");
|
||||
while (true) {
|
||||
if (do_exit || !panda->connected || !ignition) {
|
||||
return false;
|
||||
};
|
||||
for (const auto& panda : pandas) {
|
||||
if (do_exit || !panda->connected || !ignition) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (p.getBool("ControlsReady")) {
|
||||
params = p.get("CarParams");
|
||||
|
|
@ -97,27 +129,31 @@ bool safety_setter_thread(Panda *panda) {
|
|||
int safety_param;
|
||||
|
||||
auto safety_configs = car_params.getSafetyConfigs();
|
||||
if (safety_configs.size() > 0) {
|
||||
safety_model = safety_configs[0].getSafetyModel();
|
||||
safety_param = safety_configs[0].getSafetyParam();
|
||||
} else {
|
||||
// If no safety mode is set, default to silent
|
||||
safety_model = cereal::CarParams::SafetyModel::SILENT;
|
||||
safety_param = 0;
|
||||
for (uint32_t i=0; i<pandas.size(); i++) {
|
||||
auto panda = pandas[i];
|
||||
|
||||
if (safety_configs.size() > i) {
|
||||
safety_model = safety_configs[i].getSafetyModel();
|
||||
safety_param = safety_configs[i].getSafetyParam();
|
||||
} 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;
|
||||
}
|
||||
|
||||
|
||||
Panda *usb_connect() {
|
||||
Panda *usb_connect(std::string serial="", uint32_t index=0) {
|
||||
std::unique_ptr<Panda> panda;
|
||||
try {
|
||||
panda = std::make_unique<Panda>();
|
||||
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
|
||||
} catch (std::exception &e) {
|
||||
return nullptr;
|
||||
}
|
||||
|
|
@ -128,27 +164,6 @@ Panda *usb_connect() {
|
|||
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
|
||||
#ifndef __x86_64__
|
||||
static std::once_flag connected_once;
|
||||
|
|
@ -171,14 +186,7 @@ Panda *usb_connect() {
|
|||
return panda.release();
|
||||
}
|
||||
|
||||
void can_recv(Panda *panda, PubMaster &pm) {
|
||||
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) {
|
||||
void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
|
||||
LOGD("start send thread");
|
||||
|
||||
AlignedBuffer aligned_buf;
|
||||
|
|
@ -188,7 +196,12 @@ void can_send_thread(Panda *panda, bool fake_send) {
|
|||
subscriber->setTimeout(100);
|
||||
|
||||
// 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();
|
||||
|
||||
if (!msg) {
|
||||
|
|
@ -204,7 +217,9 @@ void can_send_thread(Panda *panda, bool fake_send) {
|
|||
//Dont send if older than 1 second
|
||||
if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
|
||||
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;
|
||||
}
|
||||
|
||||
void can_recv_thread(Panda *panda) {
|
||||
void can_recv_thread(std::vector<Panda *> pandas) {
|
||||
LOGD("start recv thread");
|
||||
|
||||
// can = 8006
|
||||
|
|
@ -225,8 +240,29 @@ void can_recv_thread(Panda *panda) {
|
|||
const uint64_t dt = 10000000ULL;
|
||||
uint64_t next_frame_time = nanos_since_boot() + dt;
|
||||
|
||||
while (!do_exit && panda->connected) {
|
||||
can_recv(panda, pm);
|
||||
while (!do_exit) {
|
||||
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();
|
||||
int64_t remaining = next_frame_time - cur_time;
|
||||
|
|
@ -257,70 +293,86 @@ void send_empty_panda_state(PubMaster *pm) {
|
|||
pm->send("pandaStates", msg);
|
||||
}
|
||||
|
||||
bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) {
|
||||
health_t pandaState = panda->get_state();
|
||||
|
||||
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
|
||||
bool send_panda_states(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
|
||||
bool ignition_local = false;
|
||||
|
||||
// build msg
|
||||
MessageBuilder msg;
|
||||
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
|
||||
auto ps = evt.initPandaStates(1);
|
||||
ps[0].setUptime(pandaState.uptime);
|
||||
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));
|
||||
std::vector<health_t> pandaStates;
|
||||
for (const auto& panda : pandas){
|
||||
health_t pandaState = panda->get_state();
|
||||
|
||||
// Convert faults bitset to capnp list
|
||||
std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
|
||||
auto faults = ps[0].initFaults(fault_bits.count());
|
||||
if (spoofing_started) {
|
||||
pandaState.ignition_line = 1;
|
||||
}
|
||||
|
||||
size_t i = 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(i, cereal::PandaState::FaultType(f));
|
||||
i++;
|
||||
ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
|
||||
|
||||
pandaStates.push_back(pandaState);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -355,23 +407,36 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
|
|||
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;
|
||||
Panda *peripheral_panda = pandas[0];
|
||||
bool ignition_last = false;
|
||||
std::future<bool> safety_future;
|
||||
|
||||
LOGD("start panda state thread");
|
||||
|
||||
// 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);
|
||||
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
|
||||
if (ignition && !ignition_last) {
|
||||
params.clearAll(CLEAR_ON_IGNITION_ON);
|
||||
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 {
|
||||
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;
|
||||
|
||||
panda->send_heartbeat();
|
||||
for (const auto &panda : pandas) {
|
||||
panda->send_heartbeat();
|
||||
}
|
||||
util::sleep_for(500);
|
||||
}
|
||||
}
|
||||
|
|
@ -561,7 +628,11 @@ void pigeon_thread(Panda *panda) {
|
|||
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");
|
||||
|
||||
if (!Hardware::PC()) {
|
||||
|
|
@ -575,31 +646,44 @@ int main() {
|
|||
LOGW("attempting to connect");
|
||||
PubMaster pm({"pandaStates", "peripheralState"});
|
||||
|
||||
// connect loop
|
||||
while (!do_exit) {
|
||||
Panda *panda = usb_connect();
|
||||
Panda *peripheral_panda = panda;
|
||||
std::vector<std::string> serials(argv + 1, argv + argc);
|
||||
if (serials.size() == 0) serials.push_back("");
|
||||
|
||||
// Send empty pandaState & peripheralState and try again
|
||||
if (panda == nullptr || peripheral_panda == nullptr) {
|
||||
// connect to all provided serials
|
||||
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_peripheral_state(&pm);
|
||||
util::sleep_for(500);
|
||||
continue;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
LOGW("connected to board");
|
||||
peripheral_panda = pandas[0];
|
||||
|
||||
std::vector<std::thread> threads;
|
||||
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);
|
||||
LOGW("connected to board");
|
||||
|
||||
threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
|
||||
threads.emplace_back(can_recv_thread, panda);
|
||||
threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
|
||||
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;
|
||||
panda = nullptr;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,11 +1,5 @@
|
|||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
typedef struct {
|
||||
long address;
|
||||
std::string dat;
|
||||
long busTime;
|
||||
long src;
|
||||
} can_frame;
|
||||
#include "panda.h"
|
||||
|
||||
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
|
||||
ssize_t num_devices;
|
||||
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) {
|
||||
const int msg_count = can_data_list.size();
|
||||
const int buf_size = msg_count*0x10;
|
||||
send.resize(4 * can_data_list.size());
|
||||
|
||||
if (send.size() < buf_size) {
|
||||
send.resize(buf_size);
|
||||
}
|
||||
|
||||
for (int i = 0; i < msg_count; i++) {
|
||||
uint32_t msg_cnt = 0;
|
||||
for (int i = 0; i < can_data_list.size(); 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
|
||||
send[i*4] = (cmsg.getAddress() << 3) | 5;
|
||||
send[msg_cnt*4] = (cmsg.getAddress() << 3) | 5;
|
||||
} else { // normal
|
||||
send[i*4] = (cmsg.getAddress() << 21) | 1;
|
||||
send[msg_cnt*4] = (cmsg.getAddress() << 21) | 1;
|
||||
}
|
||||
auto can_data = cmsg.getDat();
|
||||
assert(can_data.size() <= 8);
|
||||
send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
|
||||
memcpy(&send[i*4+2], can_data.begin(), can_data.size());
|
||||
send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4);
|
||||
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];
|
||||
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");
|
||||
}
|
||||
|
||||
size_t num_msg = recv / 0x10;
|
||||
MessageBuilder msg;
|
||||
auto evt = msg.initEvent();
|
||||
evt.setValid(comms_healthy);
|
||||
if (!comms_healthy) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// populate message
|
||||
auto canData = evt.initCan(num_msg);
|
||||
// Append to the end of the out_vec, such that we can pass it to multiple pandas
|
||||
// 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++) {
|
||||
can_frame canData;
|
||||
if (data[i*4] & 4) {
|
||||
// extended
|
||||
canData[i].setAddress(data[i*4] >> 3);
|
||||
canData.address = data[i*4] >> 3;
|
||||
//printf("got extended: %x\n", data[i*4] >> 3);
|
||||
} else {
|
||||
// normal
|
||||
canData[i].setAddress(data[i*4] >> 21);
|
||||
canData.address = data[i*4] >> 21;
|
||||
}
|
||||
canData[i].setBusTime(data[i*4+1] >> 16);
|
||||
int len = data[i*4+1]&0xF;
|
||||
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
|
||||
canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
|
||||
canData.busTime = data[i*4+1] >> 16;
|
||||
int len = data[i*4+1] & 0xF;
|
||||
canData.dat.assign((char *)&data[i*4+2], len);
|
||||
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 <cstdint>
|
||||
#include <ctime>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
|
@ -15,6 +16,7 @@
|
|||
// double the FIFO size
|
||||
#define RECV_SIZE (0x1000)
|
||||
#define TIMEOUT 0
|
||||
#define PANDA_BUS_CNT 4
|
||||
|
||||
// copied from panda/board/main.c
|
||||
struct __attribute__((packed)) health_t {
|
||||
|
|
@ -39,6 +41,12 @@ struct __attribute__((packed)) health_t {
|
|||
uint8_t heartbeat_lost;
|
||||
};
|
||||
|
||||
struct can_frame {
|
||||
long address;
|
||||
std::string dat;
|
||||
long busTime;
|
||||
long src;
|
||||
};
|
||||
|
||||
class Panda {
|
||||
private:
|
||||
|
|
@ -50,7 +58,7 @@ class Panda {
|
|||
void cleanup();
|
||||
|
||||
public:
|
||||
Panda(std::string serial="");
|
||||
Panda(std::string serial="", uint32_t bus_offset=0);
|
||||
~Panda();
|
||||
|
||||
std::string usb_serial;
|
||||
|
|
@ -58,6 +66,7 @@ class Panda {
|
|||
std::atomic<bool> comms_healthy = true;
|
||||
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
|
||||
bool has_rtc = false;
|
||||
const uint32_t bus_offset;
|
||||
|
||||
// Static functions
|
||||
static std::vector<std::string> list();
|
||||
|
|
@ -85,5 +94,5 @@ class Panda {
|
|||
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
|
||||
void send_heartbeat();
|
||||
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},
|
||||
{"NavSettingTime24h", 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},
|
||||
{"Passive", PERSISTENT},
|
||||
{"PrimeRedirected", PERSISTENT},
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@ EventName = car.CarEvent.EventName
|
|||
ButtonEvent = car.CarState.ButtonEvent
|
||||
SafetyModel = car.CarParams.SafetyModel
|
||||
|
||||
IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput]
|
||||
|
||||
|
||||
class Controls:
|
||||
def __init__(self, sm=None, pm=None, can_sock=None):
|
||||
|
|
@ -247,14 +249,17 @@ class Controls:
|
|||
self.events.add(EventName.canError)
|
||||
|
||||
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):
|
||||
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
|
||||
else:
|
||||
safety_mismatch = pandaState.safetyModel != SafetyModel.silent
|
||||
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
|
||||
if safety_mismatch or self.mismatch_counter >= 200:
|
||||
self.events.add(EventName.controlsMismatch)
|
||||
|
||||
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
|
||||
self.events.add(EventName.relayMalfunction)
|
||||
|
||||
if not self.sm['liveParameters'].valid:
|
||||
self.events.add(EventName.vehicleModelInvalid)
|
||||
|
||||
|
|
@ -370,9 +375,9 @@ class Controls:
|
|||
self.mismatch_counter = 0
|
||||
|
||||
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
|
||||
for pandaState in self.sm['pandaStates']:
|
||||
if pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled:
|
||||
self.mismatch_counter += 1
|
||||
if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
|
||||
if ps.safetyModel not in IGNORED_SAFETY_MODES):
|
||||
self.mismatch_counter += 1
|
||||
|
||||
self.distance_traveled += CS.vEgo * DT_CTRL
|
||||
|
||||
|
|
|
|||
|
|
@ -21,10 +21,6 @@
|
|||
"severity": 1,
|
||||
"_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": {
|
||||
"text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
|
||||
"severity": 1
|
||||
|
|
|
|||
|
|
@ -1,61 +1,40 @@
|
|||
#!/usr/bin/env python3
|
||||
# simple boardd wrapper that updates the panda first
|
||||
import os
|
||||
import usb1
|
||||
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.params import Params
|
||||
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:
|
||||
return Panda.get_signature_from_firmware(PANDA_FW_FN)
|
||||
return Panda.get_signature_from_firmware(fn)
|
||||
except Exception:
|
||||
cloudlog.exception("Error computing expected signature")
|
||||
return b""
|
||||
|
||||
|
||||
def update_panda() -> Panda:
|
||||
panda = None
|
||||
panda_dfu = None
|
||||
def flash_panda(panda_serial : str) -> Panda:
|
||||
panda = Panda(panda_serial)
|
||||
|
||||
cloudlog.info("Connecting to 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
|
||||
fw_signature = get_expected_signature(panda)
|
||||
|
||||
panda_version = "bootstub" if panda.bootstub else panda.get_version()
|
||||
panda_signature = b"" if panda.bootstub else panda.get_signature()
|
||||
cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
|
||||
serial,
|
||||
panda_serial,
|
||||
panda_version,
|
||||
panda_signature.hex(),
|
||||
fw_signature.hex(),
|
||||
panda_signature.hex()[:16],
|
||||
fw_signature.hex()[:16],
|
||||
))
|
||||
|
||||
if panda.bootstub or panda_signature != fw_signature:
|
||||
|
|
@ -80,22 +59,68 @@ def update_panda() -> 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:
|
||||
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
|
||||
health = panda.health()
|
||||
if health["heartbeat_lost"]:
|
||||
Params().put_bool("PandaHeartbeatLost", True)
|
||||
cloudlog.event("heartbeat lost", deviceState=health)
|
||||
panda_serials = Panda.list()
|
||||
if len(panda_serials) == 0:
|
||||
continue
|
||||
|
||||
cloudlog.info("Resetting panda")
|
||||
panda.reset()
|
||||
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
|
||||
|
||||
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
|
||||
os.execvp("./boardd", ["./boardd"])
|
||||
# Flash pandas
|
||||
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__":
|
||||
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.hardware import EON, TICI, PC, HARDWARE
|
||||
from selfdrive.loggerd.config import get_available_percent
|
||||
from selfdrive.pandad import get_expected_signature
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.thermald.power_monitoring import PowerMonitoring
|
||||
from selfdrive.version import tested_branch, terms_version, training_version
|
||||
|
||||
FW_SIGNATURE = get_expected_signature()
|
||||
|
||||
ThermalStatus = log.DeviceState.ThermalStatus
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
|
|
@ -364,10 +361,6 @@ def thermald_thread():
|
|||
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
|
||||
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
|
||||
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
|
||||
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
|
||||
|
|
|
|||
Loading…
Reference in New Issue