wip
This commit is contained in:
Your Name
11
selfdrive/boardd/SConscript
Normal file
11
selfdrive/boardd/SConscript
Normal file
@@ -0,0 +1,11 @@
|
|||||||
|
Import('env', 'envCython', 'common', 'cereal', 'messaging')
|
||||||
|
|
||||||
|
libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
|
||||||
|
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"])
|
||||||
|
if GetOption('extras'):
|
||||||
|
env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc'], LIBS=[panda] + libs)
|
||||||
Binary file not shown.
599
selfdrive/boardd/boardd.cc
Normal file
599
selfdrive/boardd/boardd.cc
Normal file
@@ -0,0 +1,599 @@
|
|||||||
|
#include "selfdrive/boardd/boardd.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <array>
|
||||||
|
#include <atomic>
|
||||||
|
#include <bitset>
|
||||||
|
#include <cassert>
|
||||||
|
#include <cerrno>
|
||||||
|
#include <chrono>
|
||||||
|
#include <future>
|
||||||
|
#include <memory>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
#include "cereal/gen/cpp/car.capnp.h"
|
||||||
|
#include "cereal/messaging/messaging.h"
|
||||||
|
#include "common/params.h"
|
||||||
|
#include "common/ratekeeper.h"
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
#include "common/timing.h"
|
||||||
|
#include "common/util.h"
|
||||||
|
#include "system/hardware/hw.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 will 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" or "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 400
|
||||||
|
#define SATURATE_IL 1000
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
|
||||||
|
std::atomic<bool> ignition(false);
|
||||||
|
|
||||||
|
ExitHandler do_exit;
|
||||||
|
|
||||||
|
bool check_all_connected(const std::vector<Panda *> &pandas) {
|
||||||
|
for (const auto& panda : pandas) {
|
||||||
|
if (!panda->connected()) {
|
||||||
|
do_exit = true;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool safety_setter_thread(std::vector<Panda *> pandas) {
|
||||||
|
LOGD("Starting safety setter thread");
|
||||||
|
|
||||||
|
Params p;
|
||||||
|
|
||||||
|
// there should be at least one panda connected
|
||||||
|
if (pandas.size() == 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// initialize to ELM327 without OBD multiplexing for fingerprinting
|
||||||
|
bool obd_multiplexing_enabled = false;
|
||||||
|
for (int i = 0; i < pandas.size(); i++) {
|
||||||
|
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
|
||||||
|
}
|
||||||
|
|
||||||
|
// openpilot can switch between multiplexing modes for different FW queries
|
||||||
|
while (true) {
|
||||||
|
if (do_exit || !check_all_connected(pandas) || !ignition) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled");
|
||||||
|
if (obd_multiplexing_requested != obd_multiplexing_enabled) {
|
||||||
|
for (int i = 0; i < pandas.size(); i++) {
|
||||||
|
const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
|
||||||
|
pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
|
||||||
|
}
|
||||||
|
obd_multiplexing_enabled = obd_multiplexing_requested;
|
||||||
|
p.putBool("ObdMultiplexingChanged", true);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (p.getBool("FirmwareQueryDone")) {
|
||||||
|
LOGW("finished FW query");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
util::sleep_for(20);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string params;
|
||||||
|
LOGW("waiting for params to set safety model");
|
||||||
|
while (true) {
|
||||||
|
if (do_exit || !check_all_connected(pandas) || !ignition) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (p.getBool("ControlsReady")) {
|
||||||
|
params = p.get("CarParams");
|
||||||
|
if (params.size() > 0) break;
|
||||||
|
}
|
||||||
|
util::sleep_for(100);
|
||||||
|
}
|
||||||
|
LOGW("got %lu bytes CarParams", params.size());
|
||||||
|
|
||||||
|
AlignedBuffer aligned_buf;
|
||||||
|
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
|
||||||
|
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||||
|
cereal::CarParams::SafetyModel safety_model;
|
||||||
|
uint16_t safety_param;
|
||||||
|
|
||||||
|
auto safety_configs = car_params.getSafetyConfigs();
|
||||||
|
uint16_t alternative_experience = car_params.getAlternativeExperience();
|
||||||
|
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 = 0U;
|
||||||
|
}
|
||||||
|
|
||||||
|
LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
|
||||||
|
panda->set_alternative_experience(alternative_experience);
|
||||||
|
panda->set_safety_model(safety_model, safety_param);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Panda *connect(std::string serial="", uint32_t index=0) {
|
||||||
|
std::unique_ptr<Panda> panda;
|
||||||
|
try {
|
||||||
|
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
|
||||||
|
} catch (std::exception &e) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// common panda config
|
||||||
|
if (getenv("BOARDD_LOOPBACK")) {
|
||||||
|
panda->set_loopback(true);
|
||||||
|
}
|
||||||
|
//panda->enable_deepsleep();
|
||||||
|
|
||||||
|
if (!panda->up_to_date() && !getenv("BOARDD_SKIP_FW_CHECK")) {
|
||||||
|
throw std::runtime_error("Panda firmware out of date. Run pandad.py to update.");
|
||||||
|
}
|
||||||
|
|
||||||
|
return panda.release();
|
||||||
|
}
|
||||||
|
|
||||||
|
void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
|
||||||
|
util::set_thread_name("boardd_can_send");
|
||||||
|
|
||||||
|
AlignedBuffer aligned_buf;
|
||||||
|
std::unique_ptr<Context> context(Context::create());
|
||||||
|
std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan"));
|
||||||
|
assert(subscriber != NULL);
|
||||||
|
subscriber->setTimeout(100);
|
||||||
|
|
||||||
|
// run as fast as messages come in
|
||||||
|
while (!do_exit && check_all_connected(pandas)) {
|
||||||
|
std::unique_ptr<Message> msg(subscriber->receive());
|
||||||
|
if (!msg) {
|
||||||
|
if (errno == EINTR) {
|
||||||
|
do_exit = true;
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
|
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||||
|
|
||||||
|
// Don't send if older than 1 second
|
||||||
|
if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
|
||||||
|
for (const auto& panda : pandas) {
|
||||||
|
LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str());
|
||||||
|
panda->can_send(event.getSendcan());
|
||||||
|
LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str());
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void can_recv_thread(std::vector<Panda *> pandas) {
|
||||||
|
util::set_thread_name("boardd_can_recv");
|
||||||
|
|
||||||
|
PubMaster pm({"can"});
|
||||||
|
|
||||||
|
// run at 100Hz
|
||||||
|
RateKeeper rk("boardd_can_recv", 100);
|
||||||
|
std::vector<can_frame> raw_can_data;
|
||||||
|
|
||||||
|
while (!do_exit && check_all_connected(pandas)) {
|
||||||
|
bool comms_healthy = true;
|
||||||
|
raw_can_data.clear();
|
||||||
|
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);
|
||||||
|
|
||||||
|
rk.keepTime();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
|
||||||
|
bool ignition_local = false;
|
||||||
|
const uint32_t pandas_cnt = pandas.size();
|
||||||
|
|
||||||
|
// build msg
|
||||||
|
MessageBuilder msg;
|
||||||
|
auto evt = msg.initEvent();
|
||||||
|
auto pss = evt.initPandaStates(pandas_cnt);
|
||||||
|
|
||||||
|
std::vector<health_t> pandaStates;
|
||||||
|
pandaStates.reserve(pandas_cnt);
|
||||||
|
|
||||||
|
std::vector<std::array<can_health_t, PANDA_CAN_CNT>> pandaCanStates;
|
||||||
|
pandaCanStates.reserve(pandas_cnt);
|
||||||
|
|
||||||
|
const bool red_panda_comma_three = (pandas.size() == 2) &&
|
||||||
|
(pandas[0]->hw_type == cereal::PandaState::PandaType::DOS) &&
|
||||||
|
(pandas[1]->hw_type == cereal::PandaState::PandaType::RED_PANDA);
|
||||||
|
|
||||||
|
for (const auto& panda : pandas){
|
||||||
|
auto health_opt = panda->get_state();
|
||||||
|
if (!health_opt) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
health_t health = *health_opt;
|
||||||
|
|
||||||
|
std::array<can_health_t, PANDA_CAN_CNT> can_health{};
|
||||||
|
for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
|
||||||
|
auto can_health_opt = panda->get_can_state(i);
|
||||||
|
if (!can_health_opt) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
can_health[i] = *can_health_opt;
|
||||||
|
}
|
||||||
|
pandaCanStates.push_back(can_health);
|
||||||
|
|
||||||
|
if (spoofing_started) {
|
||||||
|
health.ignition_line_pkt = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// on comma three setups with a red panda, the dos can
|
||||||
|
// get false positive ignitions due to the harness box
|
||||||
|
// without a harness connector, so ignore it
|
||||||
|
if (red_panda_comma_three && (panda->hw_type == cereal::PandaState::PandaType::DOS)) {
|
||||||
|
health.ignition_line_pkt = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0));
|
||||||
|
|
||||||
|
pandaStates.push_back(health);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < pandas_cnt; i++) {
|
||||||
|
auto panda = pandas[i];
|
||||||
|
const auto &health = 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 (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
|
||||||
|
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool power_save_desired = !ignition_local;
|
||||||
|
if (health.power_save_enabled_pkt != 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 && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
|
||||||
|
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!panda->comms_healthy()) {
|
||||||
|
evt.setValid(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
auto ps = pss[i];
|
||||||
|
ps.setVoltage(health.voltage_pkt);
|
||||||
|
ps.setCurrent(health.current_pkt);
|
||||||
|
ps.setUptime(health.uptime_pkt);
|
||||||
|
ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt);
|
||||||
|
ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt);
|
||||||
|
ps.setIgnitionLine(health.ignition_line_pkt);
|
||||||
|
ps.setIgnitionCan(health.ignition_can_pkt);
|
||||||
|
ps.setControlsAllowed(health.controls_allowed_pkt);
|
||||||
|
ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
|
||||||
|
ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
|
||||||
|
ps.setGmlanSendErrs(health.gmlan_send_errs_pkt);
|
||||||
|
ps.setPandaType(panda->hw_type);
|
||||||
|
ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt));
|
||||||
|
ps.setSafetyParam(health.safety_param_pkt);
|
||||||
|
ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
|
||||||
|
ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
|
||||||
|
ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
|
||||||
|
ps.setAlternativeExperience(health.alternative_experience_pkt);
|
||||||
|
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
|
||||||
|
ps.setInterruptLoad(health.interrupt_load_pkt);
|
||||||
|
ps.setFanPower(health.fan_power);
|
||||||
|
ps.setFanStallCount(health.fan_stall_count);
|
||||||
|
ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt));
|
||||||
|
ps.setSpiChecksumErrorCount(health.spi_checksum_error_count_pkt);
|
||||||
|
ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
|
||||||
|
ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
|
||||||
|
|
||||||
|
std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
|
||||||
|
|
||||||
|
for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
|
||||||
|
const auto &can_health = pandaCanStates[i][j];
|
||||||
|
cs[j].setBusOff((bool)can_health.bus_off);
|
||||||
|
cs[j].setBusOffCnt(can_health.bus_off_cnt);
|
||||||
|
cs[j].setErrorWarning((bool)can_health.error_warning);
|
||||||
|
cs[j].setErrorPassive((bool)can_health.error_passive);
|
||||||
|
cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
|
||||||
|
cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
|
||||||
|
cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
|
||||||
|
cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
|
||||||
|
cs[j].setReceiveErrorCnt(can_health.receive_error_cnt);
|
||||||
|
cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt);
|
||||||
|
cs[j].setTotalErrorCnt(can_health.total_error_cnt);
|
||||||
|
cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt);
|
||||||
|
cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt);
|
||||||
|
cs[j].setTotalTxCnt(can_health.total_tx_cnt);
|
||||||
|
cs[j].setTotalRxCnt(can_health.total_rx_cnt);
|
||||||
|
cs[j].setTotalFwdCnt(can_health.total_fwd_cnt);
|
||||||
|
cs[j].setCanSpeed(can_health.can_speed);
|
||||||
|
cs[j].setCanDataSpeed(can_health.can_data_speed);
|
||||||
|
cs[j].setCanfdEnabled(can_health.canfd_enabled);
|
||||||
|
cs[j].setBrsEnabled(can_health.brs_enabled);
|
||||||
|
cs[j].setCanfdNonIso(can_health.canfd_non_iso);
|
||||||
|
cs[j].setIrq0CallRate(can_health.irq0_call_rate);
|
||||||
|
cs[j].setIrq1CallRate(can_health.irq1_call_rate);
|
||||||
|
cs[j].setIrq2CallRate(can_health.irq2_call_rate);
|
||||||
|
cs[j].setCanCoreResetCnt(can_health.can_core_reset_cnt);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert faults bitset to capnp list
|
||||||
|
std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
|
||||||
|
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::HEARTBEAT_LOOP_WATCHDOG); f++) {
|
||||||
|
if (fault_bits.test(f)) {
|
||||||
|
faults.set(j, cereal::PandaState::FaultType(f));
|
||||||
|
j++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pm->send("pandaStates", msg);
|
||||||
|
return ignition_local;
|
||||||
|
}
|
||||||
|
|
||||||
|
void send_peripheral_state(PubMaster *pm, Panda *panda) {
|
||||||
|
// build msg
|
||||||
|
MessageBuilder msg;
|
||||||
|
auto evt = msg.initEvent();
|
||||||
|
evt.setValid(panda->comms_healthy());
|
||||||
|
|
||||||
|
auto ps = evt.initPeripheralState();
|
||||||
|
ps.setPandaType(panda->hw_type);
|
||||||
|
|
||||||
|
double read_time = millis_since_boot();
|
||||||
|
ps.setVoltage(Hardware::get_voltage());
|
||||||
|
ps.setCurrent(Hardware::get_current());
|
||||||
|
read_time = millis_since_boot() - read_time;
|
||||||
|
if (read_time > 50) {
|
||||||
|
LOGW("reading hwmon took %lfms", read_time);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t fan_speed_rpm = panda->get_fan_speed();
|
||||||
|
ps.setFanSpeedRpm(fan_speed_rpm);
|
||||||
|
|
||||||
|
pm->send("peripheralState", msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
|
||||||
|
util::set_thread_name("boardd_panda_state");
|
||||||
|
|
||||||
|
Params params;
|
||||||
|
SubMaster sm({"controlsState"});
|
||||||
|
PubMaster pm({"pandaStates", "peripheralState"});
|
||||||
|
|
||||||
|
Panda *peripheral_panda = pandas[0];
|
||||||
|
bool is_onroad = false;
|
||||||
|
bool is_onroad_last = false;
|
||||||
|
std::future<bool> safety_future;
|
||||||
|
|
||||||
|
std::vector<std::string> connected_serials;
|
||||||
|
for (Panda *p : pandas) {
|
||||||
|
connected_serials.push_back(p->hw_serial());
|
||||||
|
}
|
||||||
|
|
||||||
|
LOGD("start panda state thread");
|
||||||
|
|
||||||
|
// run at 10hz
|
||||||
|
RateKeeper rk("panda_state_thread", 10);
|
||||||
|
|
||||||
|
while (!do_exit && check_all_connected(pandas)) {
|
||||||
|
// send out peripheralState at 2Hz
|
||||||
|
if (sm.frame % 5 == 0) {
|
||||||
|
send_peripheral_state(&pm, peripheral_panda);
|
||||||
|
}
|
||||||
|
|
||||||
|
auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started);
|
||||||
|
|
||||||
|
if (!ignition_opt) {
|
||||||
|
LOGE("Failed to get ignition_opt");
|
||||||
|
rk.keepTime();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
ignition = *ignition_opt;
|
||||||
|
|
||||||
|
// check if we should have pandad reconnect
|
||||||
|
if (!ignition) {
|
||||||
|
bool comms_healthy = true;
|
||||||
|
for (const auto &panda : pandas) {
|
||||||
|
comms_healthy &= panda->comms_healthy();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!comms_healthy) {
|
||||||
|
LOGE("Reconnecting, communication to pandas not healthy");
|
||||||
|
do_exit = true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// check for new pandas
|
||||||
|
for (std::string &s : Panda::list(true)) {
|
||||||
|
if (!std::count(connected_serials.begin(), connected_serials.end(), s)) {
|
||||||
|
LOGW("Reconnecting to new panda: %s", s.c_str());
|
||||||
|
do_exit = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (do_exit) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
is_onroad = params.getBool("IsOnroad");
|
||||||
|
|
||||||
|
// set new safety on onroad transition, after params are cleared
|
||||||
|
if (is_onroad && !is_onroad_last) {
|
||||||
|
if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
|
||||||
|
safety_future = std::async(std::launch::async, safety_setter_thread, pandas);
|
||||||
|
} else {
|
||||||
|
LOGW("Safety setter thread already running");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
is_onroad_last = is_onroad;
|
||||||
|
|
||||||
|
sm.update(0);
|
||||||
|
const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
|
||||||
|
|
||||||
|
for (const auto &panda : pandas) {
|
||||||
|
panda->send_heartbeat(engaged);
|
||||||
|
}
|
||||||
|
|
||||||
|
rk.keepTime();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void peripheral_control_thread(Panda *panda, bool no_fan_control) {
|
||||||
|
util::set_thread_name("boardd_peripheral_control");
|
||||||
|
|
||||||
|
SubMaster sm({"deviceState", "driverCameraState"});
|
||||||
|
|
||||||
|
uint64_t last_driver_camera_t = 0;
|
||||||
|
uint16_t prev_fan_speed = 999;
|
||||||
|
uint16_t ir_pwr = 0;
|
||||||
|
uint16_t prev_ir_pwr = 999;
|
||||||
|
|
||||||
|
FirstOrderFilter integ_lines_filter(0, 30.0, 0.05);
|
||||||
|
|
||||||
|
while (!do_exit && panda->connected()) {
|
||||||
|
sm.update(1000);
|
||||||
|
|
||||||
|
if (sm.updated("deviceState") && !no_fan_control) {
|
||||||
|
// Fan speed
|
||||||
|
uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired();
|
||||||
|
if (fan_speed != prev_fan_speed || sm.frame % 100 == 0) {
|
||||||
|
panda->set_fan_speed(fan_speed);
|
||||||
|
prev_fan_speed = fan_speed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (sm.updated("driverCameraState")) {
|
||||||
|
auto event = sm["driverCameraState"];
|
||||||
|
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
|
||||||
|
|
||||||
|
cur_integ_lines = integ_lines_filter.update(cur_integ_lines);
|
||||||
|
last_driver_camera_t = event.getLogMonoTime();
|
||||||
|
|
||||||
|
if (cur_integ_lines <= CUTOFF_IL) {
|
||||||
|
ir_pwr = 100.0 * MIN_IR_POWER;
|
||||||
|
} else if (cur_integ_lines > SATURATE_IL) {
|
||||||
|
ir_pwr = 100.0 * MAX_IR_POWER;
|
||||||
|
} else {
|
||||||
|
ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_integ_lines - CUTOFF_IL) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_IL - CUTOFF_IL)));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Disable IR on input timeout
|
||||||
|
if (nanos_since_boot() - last_driver_camera_t > 1e9) {
|
||||||
|
ir_pwr = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0 || ir_pwr >= 50.0) {
|
||||||
|
panda->set_ir_pwr(ir_pwr);
|
||||||
|
prev_ir_pwr = ir_pwr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void boardd_main_thread(std::vector<std::string> serials) {
|
||||||
|
LOGW("launching boardd");
|
||||||
|
|
||||||
|
if (serials.size() == 0) {
|
||||||
|
serials = Panda::list();
|
||||||
|
|
||||||
|
if (serials.size() == 0) {
|
||||||
|
LOGW("no pandas found, exiting");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string serials_str;
|
||||||
|
for (int i = 0; i < serials.size(); i++) {
|
||||||
|
serials_str += serials[i];
|
||||||
|
if (i < serials.size() - 1) serials_str += ", ";
|
||||||
|
}
|
||||||
|
LOGW("connecting to pandas: %s", serials_str.c_str());
|
||||||
|
|
||||||
|
// connect to all provided serials
|
||||||
|
std::vector<Panda *> pandas;
|
||||||
|
for (int i = 0; i < serials.size() && !do_exit; /**/) {
|
||||||
|
Panda *p = connect(serials[i], i);
|
||||||
|
if (!p) {
|
||||||
|
util::sleep_for(100);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
pandas.push_back(p);
|
||||||
|
++i;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!do_exit) {
|
||||||
|
LOGW("connected to all pandas");
|
||||||
|
|
||||||
|
std::vector<std::thread> threads;
|
||||||
|
|
||||||
|
threads.emplace_back(panda_state_thread, pandas, getenv("STARTED") != nullptr);
|
||||||
|
threads.emplace_back(peripheral_control_thread, pandas[0], getenv("NO_FAN_CONTROL") != nullptr);
|
||||||
|
|
||||||
|
threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
|
||||||
|
threads.emplace_back(can_recv_thread, pandas);
|
||||||
|
|
||||||
|
for (auto &t : threads) t.join();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (Panda *panda : pandas) {
|
||||||
|
delete panda;
|
||||||
|
}
|
||||||
|
}
|
||||||
9
selfdrive/boardd/boardd.h
Normal file
9
selfdrive/boardd/boardd.h
Normal file
@@ -0,0 +1,9 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "selfdrive/boardd/panda.h"
|
||||||
|
|
||||||
|
bool safety_setter_thread(std::vector<Panda *> pandas);
|
||||||
|
void boardd_main_thread(std::vector<std::string> serials);
|
||||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
21
selfdrive/boardd/can_list_to_can_capnp.cc
Normal file
21
selfdrive/boardd/can_list_to_can_capnp.cc
Normal file
@@ -0,0 +1,21 @@
|
|||||||
|
#include "cereal/messaging/messaging.h"
|
||||||
|
#include "selfdrive/boardd/panda.h"
|
||||||
|
|
||||||
|
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
|
||||||
|
MessageBuilder msg;
|
||||||
|
auto event = msg.initEvent(valid);
|
||||||
|
|
||||||
|
auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
|
||||||
|
int j = 0;
|
||||||
|
for (auto it = can_list.begin(); it != can_list.end(); it++, j++) {
|
||||||
|
auto c = canData[j];
|
||||||
|
c.setAddress(it->address);
|
||||||
|
c.setBusTime(it->busTime);
|
||||||
|
c.setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
|
||||||
|
c.setSrc(it->src);
|
||||||
|
}
|
||||||
|
const uint64_t msg_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
|
||||||
|
out.resize(msg_size);
|
||||||
|
kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>((unsigned char *)out.data(), msg_size));
|
||||||
|
capnp::writeMessage(output_stream, msg);
|
||||||
|
}
|
||||||
22
selfdrive/boardd/main.cc
Normal file
22
selfdrive/boardd/main.cc
Normal file
@@ -0,0 +1,22 @@
|
|||||||
|
#include <cassert>
|
||||||
|
|
||||||
|
#include "selfdrive/boardd/boardd.h"
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
#include "common/util.h"
|
||||||
|
#include "system/hardware/hw.h"
|
||||||
|
|
||||||
|
int main(int argc, char *argv[]) {
|
||||||
|
LOGW("starting boardd");
|
||||||
|
|
||||||
|
if (!Hardware::PC()) {
|
||||||
|
int err;
|
||||||
|
err = util::set_realtime_priority(54);
|
||||||
|
assert(err == 0);
|
||||||
|
err = util::set_core_affinity({4});
|
||||||
|
assert(err == 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::string> serials(argv + 1, argv + argc);
|
||||||
|
boardd_main_thread(serials);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
284
selfdrive/boardd/panda.cc
Normal file
284
selfdrive/boardd/panda.cc
Normal file
@@ -0,0 +1,284 @@
|
|||||||
|
#include "selfdrive/boardd/panda.h"
|
||||||
|
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cereal/messaging/messaging.h"
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
#include "common/util.h"
|
||||||
|
|
||||||
|
Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
|
||||||
|
// try USB first, then SPI
|
||||||
|
try {
|
||||||
|
handle = std::make_unique<PandaUsbHandle>(serial);
|
||||||
|
LOGW("connected to %s over USB", serial.c_str());
|
||||||
|
} catch (std::exception &e) {
|
||||||
|
#ifndef __APPLE__
|
||||||
|
handle = std::make_unique<PandaSpiHandle>(serial);
|
||||||
|
LOGW("connected to %s over SPI", serial.c_str());
|
||||||
|
#else
|
||||||
|
throw e;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
hw_type = get_hw_type();
|
||||||
|
can_reset_communications();
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Panda::connected() {
|
||||||
|
return handle->connected;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Panda::comms_healthy() {
|
||||||
|
return handle->comms_healthy;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string Panda::hw_serial() {
|
||||||
|
return handle->hw_serial;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::string> Panda::list(bool usb_only) {
|
||||||
|
std::vector<std::string> serials = PandaUsbHandle::list();
|
||||||
|
|
||||||
|
#ifndef __APPLE__
|
||||||
|
if (!usb_only) {
|
||||||
|
for (auto s : PandaSpiHandle::list()) {
|
||||||
|
if (std::find(serials.begin(), serials.end(), s) == serials.end()) {
|
||||||
|
serials.push_back(s);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return serials;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) {
|
||||||
|
handle->control_write(0xdc, (uint16_t)safety_model, safety_param);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_alternative_experience(uint16_t alternative_experience) {
|
||||||
|
handle->control_write(0xdf, alternative_experience, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
cereal::PandaState::PandaType Panda::get_hw_type() {
|
||||||
|
unsigned char hw_query[1] = {0};
|
||||||
|
|
||||||
|
handle->control_read(0xc1, 0, 0, hw_query, 1);
|
||||||
|
return (cereal::PandaState::PandaType)(hw_query[0]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_fan_speed(uint16_t fan_speed) {
|
||||||
|
handle->control_write(0xb1, fan_speed, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t Panda::get_fan_speed() {
|
||||||
|
uint16_t fan_speed_rpm = 0;
|
||||||
|
handle->control_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm));
|
||||||
|
return fan_speed_rpm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_ir_pwr(uint16_t ir_pwr) {
|
||||||
|
handle->control_write(0xb0, ir_pwr, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<health_t> Panda::get_state() {
|
||||||
|
health_t health {0};
|
||||||
|
int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
|
||||||
|
return err >= 0 ? std::make_optional(health) : std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<can_health_t> Panda::get_can_state(uint16_t can_number) {
|
||||||
|
can_health_t can_health {0};
|
||||||
|
int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health));
|
||||||
|
return err >= 0 ? std::make_optional(can_health) : std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_loopback(bool loopback) {
|
||||||
|
handle->control_write(0xe5, loopback, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<std::vector<uint8_t>> Panda::get_firmware_version() {
|
||||||
|
std::vector<uint8_t> fw_sig_buf(128);
|
||||||
|
int read_1 = handle->control_read(0xd3, 0, 0, &fw_sig_buf[0], 64);
|
||||||
|
int read_2 = handle->control_read(0xd4, 0, 0, &fw_sig_buf[64], 64);
|
||||||
|
return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<std::string> Panda::get_serial() {
|
||||||
|
char serial_buf[17] = {'\0'};
|
||||||
|
int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16);
|
||||||
|
return err >= 0 ? std::make_optional(serial_buf) : std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Panda::up_to_date() {
|
||||||
|
if (auto fw_sig = get_firmware_version()) {
|
||||||
|
for (auto fn : { "panda.bin.signed", "panda_h7.bin.signed" }) {
|
||||||
|
auto content = util::read_file(std::string("../../panda/board/obj/") + fn);
|
||||||
|
if (content.size() >= fw_sig->size() &&
|
||||||
|
memcmp(content.data() + content.size() - fw_sig->size(), fw_sig->data(), fw_sig->size()) == 0) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_power_saving(bool power_saving) {
|
||||||
|
handle->control_write(0xe7, power_saving, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::enable_deepsleep() {
|
||||||
|
handle->control_write(0xfb, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::send_heartbeat(bool engaged) {
|
||||||
|
handle->control_write(0xf3, engaged, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
|
||||||
|
handle->control_write(0xde, bus, (speed * 10));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) {
|
||||||
|
handle->control_write(0xf9, bus, (speed * 10));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) {
|
||||||
|
handle->control_write(0xfc, bus, non_iso);
|
||||||
|
}
|
||||||
|
|
||||||
|
static uint8_t len_to_dlc(uint8_t len) {
|
||||||
|
if (len <= 8) {
|
||||||
|
return len;
|
||||||
|
}
|
||||||
|
if (len <= 24) {
|
||||||
|
return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0);
|
||||||
|
} else {
|
||||||
|
return 11 + (len / 16) + ((len % 16) ? 1 : 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
|
||||||
|
std::function<void(uint8_t *, size_t)> write_func) {
|
||||||
|
int32_t pos = 0;
|
||||||
|
uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
|
||||||
|
|
||||||
|
for (auto cmsg : can_data_list) {
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
auto can_data = cmsg.getDat();
|
||||||
|
uint8_t data_len_code = len_to_dlc(can_data.size());
|
||||||
|
assert(can_data.size() <= 64);
|
||||||
|
assert(can_data.size() == dlc_to_len[data_len_code]);
|
||||||
|
|
||||||
|
can_header header = {};
|
||||||
|
header.addr = cmsg.getAddress();
|
||||||
|
header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
|
||||||
|
header.data_len_code = data_len_code;
|
||||||
|
header.bus = bus - bus_offset;
|
||||||
|
header.checksum = 0;
|
||||||
|
|
||||||
|
memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
|
||||||
|
memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size());
|
||||||
|
uint32_t msg_size = sizeof(can_header) + can_data.size();
|
||||||
|
|
||||||
|
// set checksum
|
||||||
|
((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size);
|
||||||
|
|
||||||
|
pos += msg_size;
|
||||||
|
|
||||||
|
if (pos >= USB_TX_SOFT_LIMIT) {
|
||||||
|
write_func(send_buf, pos);
|
||||||
|
pos = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// send remaining packets
|
||||||
|
if (pos > 0) write_func(send_buf, pos);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
|
||||||
|
pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
|
||||||
|
handle->bulk_write(3, data, size, 5);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Panda::can_receive(std::vector<can_frame>& out_vec) {
|
||||||
|
// Check if enough space left in buffer to store RECV_SIZE data
|
||||||
|
assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer));
|
||||||
|
|
||||||
|
int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE);
|
||||||
|
if (!comms_healthy()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (recv == RECV_SIZE) {
|
||||||
|
LOGW("Panda receive buffer full");
|
||||||
|
}
|
||||||
|
receive_buffer_size += recv;
|
||||||
|
|
||||||
|
return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Panda::can_reset_communications() {
|
||||||
|
handle->control_write(0xc0, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec) {
|
||||||
|
int pos = 0;
|
||||||
|
|
||||||
|
while (pos <= size - sizeof(can_header)) {
|
||||||
|
can_header header;
|
||||||
|
memcpy(&header, &data[pos], sizeof(can_header));
|
||||||
|
|
||||||
|
const uint8_t data_len = dlc_to_len[header.data_len_code];
|
||||||
|
if (pos + sizeof(can_header) + data_len > size) {
|
||||||
|
// we don't have all the data for this message yet
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) {
|
||||||
|
// TODO: also reset CAN comms?
|
||||||
|
LOGE("Panda CAN checksum failed");
|
||||||
|
size = 0;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
can_frame &canData = out_vec.emplace_back();
|
||||||
|
canData.busTime = 0;
|
||||||
|
canData.address = header.addr;
|
||||||
|
canData.src = header.bus + bus_offset;
|
||||||
|
if (header.rejected) {
|
||||||
|
canData.src += CAN_REJECTED_BUS_OFFSET;
|
||||||
|
}
|
||||||
|
if (header.returned) {
|
||||||
|
canData.src += CAN_RETURNED_BUS_OFFSET;
|
||||||
|
}
|
||||||
|
|
||||||
|
canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len);
|
||||||
|
|
||||||
|
pos += sizeof(can_header) + data_len;
|
||||||
|
}
|
||||||
|
|
||||||
|
// move the overflowing data to the beginning of the buffer for the next round
|
||||||
|
memmove(data, &data[pos], size - pos);
|
||||||
|
size -= pos;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) {
|
||||||
|
uint8_t checksum = 0U;
|
||||||
|
for (uint32_t i = 0U; i < len; i++) {
|
||||||
|
checksum ^= data[i];
|
||||||
|
}
|
||||||
|
return checksum;
|
||||||
|
}
|
||||||
95
selfdrive/boardd/panda.h
Normal file
95
selfdrive/boardd/panda.h
Normal file
@@ -0,0 +1,95 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstdint>
|
||||||
|
#include <ctime>
|
||||||
|
#include <functional>
|
||||||
|
#include <list>
|
||||||
|
#include <memory>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cereal/gen/cpp/car.capnp.h"
|
||||||
|
#include "cereal/gen/cpp/log.capnp.h"
|
||||||
|
#include "panda/board/health.h"
|
||||||
|
#include "panda/board/can_definitions.h"
|
||||||
|
#include "selfdrive/boardd/panda_comms.h"
|
||||||
|
|
||||||
|
#define USB_TX_SOFT_LIMIT (0x100U)
|
||||||
|
#define USBPACKET_MAX_SIZE (0x40)
|
||||||
|
|
||||||
|
#define RECV_SIZE (0x4000U)
|
||||||
|
|
||||||
|
#define CAN_REJECTED_BUS_OFFSET 0xC0U
|
||||||
|
#define CAN_RETURNED_BUS_OFFSET 0x80U
|
||||||
|
|
||||||
|
struct __attribute__((packed)) can_header {
|
||||||
|
uint8_t reserved : 1;
|
||||||
|
uint8_t bus : 3;
|
||||||
|
uint8_t data_len_code : 4;
|
||||||
|
uint8_t rejected : 1;
|
||||||
|
uint8_t returned : 1;
|
||||||
|
uint8_t extended : 1;
|
||||||
|
uint32_t addr : 29;
|
||||||
|
uint8_t checksum : 8;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct can_frame {
|
||||||
|
long address;
|
||||||
|
std::string dat;
|
||||||
|
long busTime;
|
||||||
|
long src;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class Panda {
|
||||||
|
private:
|
||||||
|
std::unique_ptr<PandaCommsHandle> handle;
|
||||||
|
|
||||||
|
public:
|
||||||
|
Panda(std::string serial="", uint32_t bus_offset=0);
|
||||||
|
|
||||||
|
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
|
||||||
|
const uint32_t bus_offset;
|
||||||
|
|
||||||
|
bool connected();
|
||||||
|
bool comms_healthy();
|
||||||
|
std::string hw_serial();
|
||||||
|
|
||||||
|
// Static functions
|
||||||
|
static std::vector<std::string> list(bool usb_only=false);
|
||||||
|
|
||||||
|
// Panda functionality
|
||||||
|
cereal::PandaState::PandaType get_hw_type();
|
||||||
|
void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U);
|
||||||
|
void set_alternative_experience(uint16_t alternative_experience);
|
||||||
|
void set_fan_speed(uint16_t fan_speed);
|
||||||
|
uint16_t get_fan_speed();
|
||||||
|
void set_ir_pwr(uint16_t ir_pwr);
|
||||||
|
std::optional<health_t> get_state();
|
||||||
|
std::optional<can_health_t> get_can_state(uint16_t can_number);
|
||||||
|
void set_loopback(bool loopback);
|
||||||
|
std::optional<std::vector<uint8_t>> get_firmware_version();
|
||||||
|
bool up_to_date();
|
||||||
|
std::optional<std::string> get_serial();
|
||||||
|
void set_power_saving(bool power_saving);
|
||||||
|
void enable_deepsleep();
|
||||||
|
void send_heartbeat(bool engaged);
|
||||||
|
void set_can_speed_kbps(uint16_t bus, uint16_t speed);
|
||||||
|
void set_data_speed_kbps(uint16_t bus, uint16_t speed);
|
||||||
|
void set_canfd_non_iso(uint16_t bus, bool non_iso);
|
||||||
|
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
|
||||||
|
bool can_receive(std::vector<can_frame>& out_vec);
|
||||||
|
void can_reset_communications();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// for unit tests
|
||||||
|
uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
|
||||||
|
uint32_t receive_buffer_size = 0;
|
||||||
|
|
||||||
|
Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
|
||||||
|
void pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
|
||||||
|
std::function<void(uint8_t *, size_t)> write_func);
|
||||||
|
bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
|
||||||
|
uint8_t calculate_checksum(uint8_t *data, uint32_t len);
|
||||||
|
};
|
||||||
227
selfdrive/boardd/panda_comms.cc
Normal file
227
selfdrive/boardd/panda_comms.cc
Normal file
@@ -0,0 +1,227 @@
|
|||||||
|
#include "selfdrive/boardd/panda.h"
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
|
||||||
|
static libusb_context *init_usb_ctx() {
|
||||||
|
libusb_context *context = nullptr;
|
||||||
|
int err = libusb_init(&context);
|
||||||
|
if (err != 0) {
|
||||||
|
LOGE("libusb initialization error");
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if LIBUSB_API_VERSION >= 0x01000106
|
||||||
|
libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
|
||||||
|
#else
|
||||||
|
libusb_set_debug(context, 3);
|
||||||
|
#endif
|
||||||
|
return context;
|
||||||
|
}
|
||||||
|
|
||||||
|
PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) {
|
||||||
|
// init libusb
|
||||||
|
ssize_t num_devices;
|
||||||
|
libusb_device **dev_list = NULL;
|
||||||
|
int err = 0;
|
||||||
|
ctx = init_usb_ctx();
|
||||||
|
if (!ctx) { goto fail; }
|
||||||
|
|
||||||
|
// connect by serial
|
||||||
|
num_devices = libusb_get_device_list(ctx, &dev_list);
|
||||||
|
if (num_devices < 0) { goto fail; }
|
||||||
|
for (size_t i = 0; i < num_devices; ++i) {
|
||||||
|
libusb_device_descriptor desc;
|
||||||
|
libusb_get_device_descriptor(dev_list[i], &desc);
|
||||||
|
if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
|
||||||
|
int ret = libusb_open(dev_list[i], &dev_handle);
|
||||||
|
if (dev_handle == NULL || ret < 0) { goto fail; }
|
||||||
|
|
||||||
|
unsigned char desc_serial[26] = { 0 };
|
||||||
|
ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
|
||||||
|
if (ret < 0) { goto fail; }
|
||||||
|
|
||||||
|
hw_serial = std::string((char *)desc_serial, ret);
|
||||||
|
if (serial.empty() || serial == hw_serial) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
libusb_close(dev_handle);
|
||||||
|
dev_handle = NULL;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (dev_handle == NULL) goto fail;
|
||||||
|
libusb_free_device_list(dev_list, 1);
|
||||||
|
dev_list = nullptr;
|
||||||
|
|
||||||
|
if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
|
||||||
|
libusb_detach_kernel_driver(dev_handle, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
err = libusb_set_configuration(dev_handle, 1);
|
||||||
|
if (err != 0) { goto fail; }
|
||||||
|
|
||||||
|
err = libusb_claim_interface(dev_handle, 0);
|
||||||
|
if (err != 0) { goto fail; }
|
||||||
|
|
||||||
|
return;
|
||||||
|
|
||||||
|
fail:
|
||||||
|
if (dev_list != NULL) {
|
||||||
|
libusb_free_device_list(dev_list, 1);
|
||||||
|
}
|
||||||
|
cleanup();
|
||||||
|
throw std::runtime_error("Error connecting to panda");
|
||||||
|
}
|
||||||
|
|
||||||
|
PandaUsbHandle::~PandaUsbHandle() {
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
cleanup();
|
||||||
|
connected = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PandaUsbHandle::cleanup() {
|
||||||
|
if (dev_handle) {
|
||||||
|
libusb_release_interface(dev_handle, 0);
|
||||||
|
libusb_close(dev_handle);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ctx) {
|
||||||
|
libusb_exit(ctx);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::string> PandaUsbHandle::list() {
|
||||||
|
static std::unique_ptr<libusb_context, decltype(&libusb_exit)> context(init_usb_ctx(), libusb_exit);
|
||||||
|
// init libusb
|
||||||
|
ssize_t num_devices;
|
||||||
|
libusb_device **dev_list = NULL;
|
||||||
|
std::vector<std::string> serials;
|
||||||
|
if (!context) { return serials; }
|
||||||
|
|
||||||
|
num_devices = libusb_get_device_list(context.get(), &dev_list);
|
||||||
|
if (num_devices < 0) {
|
||||||
|
LOGE("libusb can't get device list");
|
||||||
|
goto finish;
|
||||||
|
}
|
||||||
|
for (size_t i = 0; i < num_devices; ++i) {
|
||||||
|
libusb_device *device = dev_list[i];
|
||||||
|
libusb_device_descriptor desc;
|
||||||
|
libusb_get_device_descriptor(device, &desc);
|
||||||
|
if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
|
||||||
|
libusb_device_handle *handle = NULL;
|
||||||
|
int ret = libusb_open(device, &handle);
|
||||||
|
if (ret < 0) { goto finish; }
|
||||||
|
|
||||||
|
unsigned char desc_serial[26] = { 0 };
|
||||||
|
ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
|
||||||
|
libusb_close(handle);
|
||||||
|
if (ret < 0) { goto finish; }
|
||||||
|
|
||||||
|
serials.push_back(std::string((char *)desc_serial, ret).c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
finish:
|
||||||
|
if (dev_list != NULL) {
|
||||||
|
libusb_free_device_list(dev_list, 1);
|
||||||
|
}
|
||||||
|
return serials;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PandaUsbHandle::handle_usb_issue(int err, const char func[]) {
|
||||||
|
LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
|
||||||
|
if (err == LIBUSB_ERROR_NO_DEVICE) {
|
||||||
|
LOGE("lost connection");
|
||||||
|
connected = false;
|
||||||
|
}
|
||||||
|
// TODO: check other errors, is simply retrying okay?
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) {
|
||||||
|
int err;
|
||||||
|
const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
|
||||||
|
|
||||||
|
if (!connected) {
|
||||||
|
return LIBUSB_ERROR_NO_DEVICE;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
do {
|
||||||
|
err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout);
|
||||||
|
if (err < 0) handle_usb_issue(err, __func__);
|
||||||
|
} while (err < 0 && connected);
|
||||||
|
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) {
|
||||||
|
int err;
|
||||||
|
const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
|
||||||
|
|
||||||
|
if (!connected) {
|
||||||
|
return LIBUSB_ERROR_NO_DEVICE;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
do {
|
||||||
|
err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);
|
||||||
|
if (err < 0) handle_usb_issue(err, __func__);
|
||||||
|
} while (err < 0 && connected);
|
||||||
|
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||||
|
int err;
|
||||||
|
int transferred = 0;
|
||||||
|
|
||||||
|
if (!connected) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
do {
|
||||||
|
// Try sending can messages. If the receive buffer on the panda is full it will NAK
|
||||||
|
// and libusb will try again. After 5ms, it will time out. We will drop the messages.
|
||||||
|
err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
|
||||||
|
|
||||||
|
if (err == LIBUSB_ERROR_TIMEOUT) {
|
||||||
|
LOGW("Transmit buffer full");
|
||||||
|
break;
|
||||||
|
} else if (err != 0 || length != transferred) {
|
||||||
|
handle_usb_issue(err, __func__);
|
||||||
|
}
|
||||||
|
} while (err != 0 && connected);
|
||||||
|
|
||||||
|
return transferred;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||||
|
int err;
|
||||||
|
int transferred = 0;
|
||||||
|
|
||||||
|
if (!connected) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
|
||||||
|
do {
|
||||||
|
err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
|
||||||
|
|
||||||
|
if (err == LIBUSB_ERROR_TIMEOUT) {
|
||||||
|
break; // timeout is okay to exit, recv still happened
|
||||||
|
} else if (err == LIBUSB_ERROR_OVERFLOW) {
|
||||||
|
comms_healthy = false;
|
||||||
|
LOGE_100("overflow got 0x%x", transferred);
|
||||||
|
} else if (err != 0) {
|
||||||
|
handle_usb_issue(err, __func__);
|
||||||
|
}
|
||||||
|
|
||||||
|
} while (err != 0 && connected);
|
||||||
|
|
||||||
|
return transferred;
|
||||||
|
}
|
||||||
82
selfdrive/boardd/panda_comms.h
Normal file
82
selfdrive/boardd/panda_comms.h
Normal file
@@ -0,0 +1,82 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#ifndef __APPLE__
|
||||||
|
#include <linux/spi/spidev.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <libusb-1.0/libusb.h>
|
||||||
|
|
||||||
|
|
||||||
|
#define TIMEOUT 0
|
||||||
|
#define SPI_BUF_SIZE 2048
|
||||||
|
|
||||||
|
|
||||||
|
// comms base class
|
||||||
|
class PandaCommsHandle {
|
||||||
|
public:
|
||||||
|
PandaCommsHandle(std::string serial) {}
|
||||||
|
virtual ~PandaCommsHandle() {}
|
||||||
|
virtual void cleanup() = 0;
|
||||||
|
|
||||||
|
std::string hw_serial;
|
||||||
|
std::atomic<bool> connected = true;
|
||||||
|
std::atomic<bool> comms_healthy = true;
|
||||||
|
static std::vector<std::string> list();
|
||||||
|
|
||||||
|
// HW communication
|
||||||
|
virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0;
|
||||||
|
virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0;
|
||||||
|
virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
|
||||||
|
virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
class PandaUsbHandle : public PandaCommsHandle {
|
||||||
|
public:
|
||||||
|
PandaUsbHandle(std::string serial);
|
||||||
|
~PandaUsbHandle();
|
||||||
|
int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
|
||||||
|
int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
|
||||||
|
int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||||
|
int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||||
|
void cleanup();
|
||||||
|
|
||||||
|
static std::vector<std::string> list();
|
||||||
|
|
||||||
|
private:
|
||||||
|
libusb_context *ctx = NULL;
|
||||||
|
libusb_device_handle *dev_handle = NULL;
|
||||||
|
std::recursive_mutex hw_lock;
|
||||||
|
void handle_usb_issue(int err, const char func[]);
|
||||||
|
};
|
||||||
|
|
||||||
|
#ifndef __APPLE__
|
||||||
|
class PandaSpiHandle : public PandaCommsHandle {
|
||||||
|
public:
|
||||||
|
PandaSpiHandle(std::string serial);
|
||||||
|
~PandaSpiHandle();
|
||||||
|
int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
|
||||||
|
int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
|
||||||
|
int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||||
|
int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||||
|
void cleanup();
|
||||||
|
|
||||||
|
static std::vector<std::string> list();
|
||||||
|
|
||||||
|
private:
|
||||||
|
int spi_fd = -1;
|
||||||
|
uint8_t tx_buf[SPI_BUF_SIZE];
|
||||||
|
uint8_t rx_buf[SPI_BUF_SIZE];
|
||||||
|
inline static std::recursive_mutex hw_lock;
|
||||||
|
|
||||||
|
int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length);
|
||||||
|
int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
|
||||||
|
int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
|
||||||
|
int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
|
||||||
|
};
|
||||||
|
#endif
|
||||||
@@ -4,13 +4,12 @@ import os
|
|||||||
import usb1
|
import usb1
|
||||||
import time
|
import time
|
||||||
import subprocess
|
import subprocess
|
||||||
from typing import List, NoReturn
|
from typing import NoReturn
|
||||||
from functools import cmp_to_key
|
from functools import cmp_to_key
|
||||||
|
|
||||||
from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
|
from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
|
||||||
from openpilot.common.basedir import BASEDIR
|
from openpilot.common.basedir import BASEDIR
|
||||||
from openpilot.common.params import Params
|
from openpilot.common.params import Params
|
||||||
from openpilot.selfdrive.boardd.set_time import set_time
|
|
||||||
from openpilot.system.hardware import HARDWARE
|
from openpilot.system.hardware import HARDWARE
|
||||||
from openpilot.common.swaglog import cloudlog
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
@@ -124,7 +123,7 @@ def main() -> NoReturn:
|
|||||||
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
|
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
|
||||||
|
|
||||||
# Flash pandas
|
# Flash pandas
|
||||||
pandas: List[Panda] = []
|
pandas: list[Panda] = []
|
||||||
for serial in panda_serials:
|
for serial in panda_serials:
|
||||||
pandas.append(flash_panda(serial))
|
pandas.append(flash_panda(serial))
|
||||||
|
|
||||||
@@ -154,10 +153,6 @@ def main() -> NoReturn:
|
|||||||
cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial())
|
cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial())
|
||||||
|
|
||||||
if first_run:
|
if first_run:
|
||||||
if panda.is_internal():
|
|
||||||
# update time from RTC
|
|
||||||
set_time(cloudlog)
|
|
||||||
|
|
||||||
# reset panda to ensure we're in a good state
|
# reset panda to ensure we're in a good state
|
||||||
cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
|
cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
|
||||||
if panda.is_internal():
|
if panda.is_internal():
|
||||||
|
|||||||
@@ -1,38 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
import os
|
|
||||||
import datetime
|
|
||||||
from panda import Panda
|
|
||||||
|
|
||||||
from openpilot.common.time import MIN_DATE
|
|
||||||
|
|
||||||
def set_time(logger):
|
|
||||||
sys_time = datetime.datetime.today()
|
|
||||||
if sys_time > MIN_DATE:
|
|
||||||
logger.info("System time valid")
|
|
||||||
return
|
|
||||||
|
|
||||||
try:
|
|
||||||
ps = Panda.list()
|
|
||||||
if len(ps) == 0:
|
|
||||||
logger.error("Failed to set time, no pandas found")
|
|
||||||
return
|
|
||||||
|
|
||||||
for s in ps:
|
|
||||||
with Panda(serial=s) as p:
|
|
||||||
if not p.is_internal():
|
|
||||||
continue
|
|
||||||
|
|
||||||
# Set system time from panda RTC time
|
|
||||||
panda_time = p.get_datetime()
|
|
||||||
if panda_time > MIN_DATE:
|
|
||||||
logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'")
|
|
||||||
os.system(f"TZ=UTC date -s '{panda_time}'")
|
|
||||||
break
|
|
||||||
except Exception:
|
|
||||||
logger.exception("Failed to fetch time from panda")
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
import logging
|
|
||||||
logging.basicConfig(level=logging.DEBUG)
|
|
||||||
|
|
||||||
set_time(logging)
|
|
||||||
377
selfdrive/boardd/spi.cc
Normal file
377
selfdrive/boardd/spi.cc
Normal file
@@ -0,0 +1,377 @@
|
|||||||
|
#ifndef __APPLE__
|
||||||
|
#include <sys/file.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <linux/spi/spidev.h>
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <cmath>
|
||||||
|
#include <cstring>
|
||||||
|
#include <iomanip>
|
||||||
|
#include <sstream>
|
||||||
|
|
||||||
|
#include "common/util.h"
|
||||||
|
#include "common/timing.h"
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
#include "panda/board/comms_definitions.h"
|
||||||
|
#include "selfdrive/boardd/panda_comms.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define SPI_SYNC 0x5AU
|
||||||
|
#define SPI_HACK 0x79U
|
||||||
|
#define SPI_DACK 0x85U
|
||||||
|
#define SPI_NACK 0x1FU
|
||||||
|
#define SPI_CHECKSUM_START 0xABU
|
||||||
|
|
||||||
|
|
||||||
|
enum SpiError {
|
||||||
|
NACK = -2,
|
||||||
|
ACK_TIMEOUT = -3,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct __attribute__((packed)) spi_header {
|
||||||
|
uint8_t sync;
|
||||||
|
uint8_t endpoint;
|
||||||
|
uint16_t tx_len;
|
||||||
|
uint16_t max_rx_len;
|
||||||
|
};
|
||||||
|
|
||||||
|
const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
|
||||||
|
const std::string SPI_DEVICE = "/dev/spidev0.0";
|
||||||
|
|
||||||
|
class LockEx {
|
||||||
|
public:
|
||||||
|
LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) {
|
||||||
|
m.lock();
|
||||||
|
flock(fd, LOCK_EX);
|
||||||
|
}
|
||||||
|
|
||||||
|
~LockEx() {
|
||||||
|
flock(fd, LOCK_UN);
|
||||||
|
m.unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
int fd;
|
||||||
|
std::recursive_mutex &m;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) {
|
||||||
|
int ret;
|
||||||
|
const int uid_len = 12;
|
||||||
|
uint8_t uid[uid_len] = {0};
|
||||||
|
|
||||||
|
uint32_t spi_mode = SPI_MODE_0;
|
||||||
|
uint8_t spi_bits_per_word = 8;
|
||||||
|
|
||||||
|
// 50MHz is the max of the 845. note that some older
|
||||||
|
// revs of the comma three may not support this speed
|
||||||
|
uint32_t spi_speed = 50000000;
|
||||||
|
|
||||||
|
if (!util::file_exists(SPI_DEVICE)) {
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
spi_fd = open(SPI_DEVICE.c_str(), O_RDWR);
|
||||||
|
if (spi_fd < 0) {
|
||||||
|
LOGE("failed opening SPI device %d", spi_fd);
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// SPI settings
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("failed setting SPI mode %d", ret);
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("failed setting SPI speed");
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("failed setting SPI bits per word");
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get hw UID/serial
|
||||||
|
ret = control_read(0xc3, 0, 0, uid, uid_len, 100);
|
||||||
|
if (ret == uid_len) {
|
||||||
|
std::stringstream stream;
|
||||||
|
for (int i = 0; i < uid_len; i++) {
|
||||||
|
stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]);
|
||||||
|
}
|
||||||
|
hw_serial = stream.str();
|
||||||
|
} else {
|
||||||
|
LOGD("failed to get serial %d", ret);
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!serial.empty() && (serial != hw_serial)) {
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
return;
|
||||||
|
|
||||||
|
fail:
|
||||||
|
cleanup();
|
||||||
|
throw std::runtime_error("Error connecting to panda");
|
||||||
|
}
|
||||||
|
|
||||||
|
PandaSpiHandle::~PandaSpiHandle() {
|
||||||
|
std::lock_guard lk(hw_lock);
|
||||||
|
cleanup();
|
||||||
|
}
|
||||||
|
|
||||||
|
void PandaSpiHandle::cleanup() {
|
||||||
|
if (spi_fd != -1) {
|
||||||
|
close(spi_fd);
|
||||||
|
spi_fd = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) {
|
||||||
|
ControlPacket_t packet = {
|
||||||
|
.request = request,
|
||||||
|
.param1 = param1,
|
||||||
|
.param2 = param2,
|
||||||
|
.length = 0
|
||||||
|
};
|
||||||
|
return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0, timeout);
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) {
|
||||||
|
ControlPacket_t packet = {
|
||||||
|
.request = request,
|
||||||
|
.param1 = param1,
|
||||||
|
.param2 = param2,
|
||||||
|
.length = length
|
||||||
|
};
|
||||||
|
return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length, timeout);
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||||
|
return bulk_transfer(endpoint, data, length, NULL, 0, timeout);
|
||||||
|
}
|
||||||
|
int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||||
|
return bulk_transfer(endpoint, NULL, 0, data, length, timeout);
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) {
|
||||||
|
const int xfer_size = SPI_BUF_SIZE - 0x40;
|
||||||
|
|
||||||
|
int ret = 0;
|
||||||
|
uint16_t length = (tx_data != NULL) ? tx_len : rx_len;
|
||||||
|
for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) {
|
||||||
|
int d;
|
||||||
|
if (tx_data != NULL) {
|
||||||
|
int len = std::min(xfer_size, tx_len - (xfer_size * i));
|
||||||
|
d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0, timeout);
|
||||||
|
} else {
|
||||||
|
uint16_t to_read = std::min(xfer_size, rx_len - ret);
|
||||||
|
d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (d < 0) {
|
||||||
|
LOGE("SPI: bulk transfer failed with %d", d);
|
||||||
|
comms_healthy = false;
|
||||||
|
return d;
|
||||||
|
}
|
||||||
|
|
||||||
|
ret += d;
|
||||||
|
if ((rx_data != NULL) && d < xfer_size) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::string> PandaSpiHandle::list() {
|
||||||
|
try {
|
||||||
|
PandaSpiHandle sh("");
|
||||||
|
return {sh.hw_serial};
|
||||||
|
} catch (std::exception &e) {
|
||||||
|
// no panda on SPI
|
||||||
|
}
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
|
||||||
|
void add_checksum(uint8_t *data, int data_len) {
|
||||||
|
data[data_len] = SPI_CHECKSUM_START;
|
||||||
|
for (int i=0; i < data_len; i++) {
|
||||||
|
data[data_len] ^= data[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool check_checksum(uint8_t *data, int data_len) {
|
||||||
|
uint8_t checksum = SPI_CHECKSUM_START;
|
||||||
|
for (uint16_t i = 0U; i < data_len; i++) {
|
||||||
|
checksum ^= data[i];
|
||||||
|
}
|
||||||
|
return checksum == 0U;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
|
||||||
|
int ret;
|
||||||
|
int nack_count = 0;
|
||||||
|
int timeout_count = 0;
|
||||||
|
bool timed_out = false;
|
||||||
|
double start_time = millis_since_boot();
|
||||||
|
|
||||||
|
do {
|
||||||
|
ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout);
|
||||||
|
|
||||||
|
if (ret < 0) {
|
||||||
|
timed_out = (timeout != 0) && (timeout_count > 5);
|
||||||
|
timeout_count += ret == SpiError::ACK_TIMEOUT;
|
||||||
|
|
||||||
|
// give other threads a chance to run
|
||||||
|
std::this_thread::yield();
|
||||||
|
|
||||||
|
if (ret == SpiError::NACK) {
|
||||||
|
// prevent busy waiting while the panda is NACK'ing
|
||||||
|
// due to full TX buffers
|
||||||
|
nack_count += 1;
|
||||||
|
if (nack_count > 3) {
|
||||||
|
usleep(std::clamp(nack_count*10, 200, 2000));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} while (ret < 0 && connected && !timed_out);
|
||||||
|
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time);
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) {
|
||||||
|
double start_millis = millis_since_boot();
|
||||||
|
if (timeout == 0) {
|
||||||
|
timeout = SPI_ACK_TIMEOUT;
|
||||||
|
}
|
||||||
|
timeout = std::clamp(timeout, 100U, SPI_ACK_TIMEOUT);
|
||||||
|
|
||||||
|
spi_ioc_transfer transfer = {
|
||||||
|
.tx_buf = (uint64_t)tx_buf,
|
||||||
|
.rx_buf = (uint64_t)rx_buf,
|
||||||
|
.len = length
|
||||||
|
};
|
||||||
|
tx_buf[0] = tx;
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("SPI: failed to send ACK request");
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rx_buf[0] == ack) {
|
||||||
|
break;
|
||||||
|
} else if (rx_buf[0] == SPI_NACK) {
|
||||||
|
LOGD("SPI: got NACK");
|
||||||
|
return SpiError::NACK;
|
||||||
|
}
|
||||||
|
|
||||||
|
// handle timeout
|
||||||
|
if (millis_since_boot() - start_millis > timeout) {
|
||||||
|
LOGD("SPI: timed out waiting for ACK");
|
||||||
|
return SpiError::ACK_TIMEOUT;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
|
||||||
|
int ret;
|
||||||
|
uint16_t rx_data_len;
|
||||||
|
LockEx lock(spi_fd, hw_lock);
|
||||||
|
|
||||||
|
// needs to be less, since we need to have space for the checksum
|
||||||
|
assert(tx_len < SPI_BUF_SIZE);
|
||||||
|
assert(max_rx_len < SPI_BUF_SIZE);
|
||||||
|
|
||||||
|
spi_header header = {
|
||||||
|
.sync = SPI_SYNC,
|
||||||
|
.endpoint = endpoint,
|
||||||
|
.tx_len = tx_len,
|
||||||
|
.max_rx_len = max_rx_len
|
||||||
|
};
|
||||||
|
|
||||||
|
spi_ioc_transfer transfer = {
|
||||||
|
.tx_buf = (uint64_t)tx_buf,
|
||||||
|
.rx_buf = (uint64_t)rx_buf
|
||||||
|
};
|
||||||
|
|
||||||
|
// Send header
|
||||||
|
memcpy(tx_buf, &header, sizeof(header));
|
||||||
|
add_checksum(tx_buf, sizeof(header));
|
||||||
|
transfer.len = sizeof(header) + 1;
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("SPI: failed to send header");
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait for (N)ACK
|
||||||
|
ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
|
||||||
|
if (ret < 0) {
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Send data
|
||||||
|
if (tx_data != NULL) {
|
||||||
|
memcpy(tx_buf, tx_data, tx_len);
|
||||||
|
}
|
||||||
|
add_checksum(tx_buf, tx_len);
|
||||||
|
transfer.len = tx_len + 1;
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("SPI: failed to send data");
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Wait for (N)ACK
|
||||||
|
ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
|
||||||
|
if (ret < 0) {
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Read data
|
||||||
|
rx_data_len = *(uint16_t *)(rx_buf+1);
|
||||||
|
if (rx_data_len >= SPI_BUF_SIZE) {
|
||||||
|
LOGE("SPI: RX data len larger than buf size %d", rx_data_len);
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
transfer.len = rx_data_len + 1;
|
||||||
|
transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
|
||||||
|
ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
|
||||||
|
if (ret < 0) {
|
||||||
|
LOGE("SPI: failed to read rx data");
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
if (!check_checksum(rx_buf, rx_data_len + 4)) {
|
||||||
|
LOGE("SPI: bad checksum");
|
||||||
|
goto transfer_fail;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rx_data != NULL) {
|
||||||
|
memcpy(rx_data, rx_buf + 3, rx_data_len);
|
||||||
|
}
|
||||||
|
|
||||||
|
return rx_data_len;
|
||||||
|
|
||||||
|
transfer_fail:
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
0
selfdrive/boardd/tests/__init__.py
Normal file
0
selfdrive/boardd/tests/__init__.py
Normal file
BIN
selfdrive/boardd/tests/bootstub.panda.bin
Normal file
BIN
selfdrive/boardd/tests/bootstub.panda.bin
Normal file
Binary file not shown.
BIN
selfdrive/boardd/tests/bootstub.panda_h7.bin
Normal file
BIN
selfdrive/boardd/tests/bootstub.panda_h7.bin
Normal file
Binary file not shown.
BIN
selfdrive/boardd/tests/bootstub.panda_h7_spiv0.bin
Normal file
BIN
selfdrive/boardd/tests/bootstub.panda_h7_spiv0.bin
Normal file
Binary file not shown.
135
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
Normal file
135
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
Normal file
@@ -0,0 +1,135 @@
|
|||||||
|
#define CATCH_CONFIG_MAIN
|
||||||
|
#define CATCH_CONFIG_ENABLE_BENCHMARKING
|
||||||
|
|
||||||
|
#include "catch2/catch.hpp"
|
||||||
|
#include "cereal/messaging/messaging.h"
|
||||||
|
#include "common/util.h"
|
||||||
|
#include "selfdrive/boardd/panda.h"
|
||||||
|
|
||||||
|
struct PandaTest : public Panda {
|
||||||
|
PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
|
||||||
|
void test_can_send();
|
||||||
|
void test_can_recv(uint32_t chunk_size = 0);
|
||||||
|
void test_chunked_can_recv();
|
||||||
|
|
||||||
|
std::map<int, std::string> test_data;
|
||||||
|
int can_list_size = 0;
|
||||||
|
int total_pakets_size = 0;
|
||||||
|
MessageBuilder msg;
|
||||||
|
capnp::List<cereal::CanData>::Reader can_data_list;
|
||||||
|
};
|
||||||
|
|
||||||
|
PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) {
|
||||||
|
this->hw_type = hw_type;
|
||||||
|
int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8);
|
||||||
|
// prepare test data
|
||||||
|
for (int i = 0; i < data_limit; ++i) {
|
||||||
|
std::random_device rd;
|
||||||
|
std::independent_bits_engine<std::default_random_engine, CHAR_BIT, unsigned char> rbe(rd());
|
||||||
|
|
||||||
|
int data_len = dlc_to_len[i];
|
||||||
|
std::string bytes(data_len, '\0');
|
||||||
|
std::generate(bytes.begin(), bytes.end(), std::ref(rbe));
|
||||||
|
test_data[data_len] = bytes;
|
||||||
|
}
|
||||||
|
|
||||||
|
// generate can messages for this panda
|
||||||
|
auto can_list = msg.initEvent().initSendcan(can_list_size);
|
||||||
|
for (uint8_t i = 0; i < can_list_size; ++i) {
|
||||||
|
auto can = can_list[i];
|
||||||
|
uint32_t id = util::random_int(0, std::size(dlc_to_len) - 1);
|
||||||
|
const std::string &dat = test_data[dlc_to_len[id]];
|
||||||
|
can.setAddress(i);
|
||||||
|
can.setSrc(util::random_int(0, 3) + bus_offset);
|
||||||
|
can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
|
||||||
|
total_pakets_size += sizeof(can_header) + dat.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
can_data_list = can_list.asReader();
|
||||||
|
INFO("test " << can_list_size << " packets, total size " << total_pakets_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PandaTest::test_can_send() {
|
||||||
|
std::vector<uint8_t> unpacked_data;
|
||||||
|
this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
|
||||||
|
unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]);
|
||||||
|
});
|
||||||
|
REQUIRE(unpacked_data.size() == total_pakets_size);
|
||||||
|
|
||||||
|
int cnt = 0;
|
||||||
|
INFO("test can message integrity");
|
||||||
|
for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) {
|
||||||
|
can_header header;
|
||||||
|
memcpy(&header, &unpacked_data[pos], sizeof(can_header));
|
||||||
|
const uint8_t data_len = dlc_to_len[header.data_len_code];
|
||||||
|
pckt_len = sizeof(can_header) + data_len;
|
||||||
|
|
||||||
|
REQUIRE(header.addr == cnt);
|
||||||
|
REQUIRE(test_data.find(data_len) != test_data.end());
|
||||||
|
const std::string &dat = test_data[data_len];
|
||||||
|
REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0);
|
||||||
|
++cnt;
|
||||||
|
}
|
||||||
|
REQUIRE(cnt == can_list_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PandaTest::test_can_recv(uint32_t rx_chunk_size) {
|
||||||
|
std::vector<can_frame> frames;
|
||||||
|
this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) {
|
||||||
|
if (rx_chunk_size == 0) {
|
||||||
|
REQUIRE(this->unpack_can_buffer(data, size, frames));
|
||||||
|
} else {
|
||||||
|
this->receive_buffer_size = 0;
|
||||||
|
uint32_t pos = 0;
|
||||||
|
|
||||||
|
while (pos < size) {
|
||||||
|
uint32_t chunk_size = std::min(rx_chunk_size, size - pos);
|
||||||
|
memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size);
|
||||||
|
this->receive_buffer_size += chunk_size;
|
||||||
|
pos += chunk_size;
|
||||||
|
|
||||||
|
REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
REQUIRE(frames.size() == can_list_size);
|
||||||
|
for (int i = 0; i < frames.size(); ++i) {
|
||||||
|
REQUIRE(frames[i].address == i);
|
||||||
|
REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end());
|
||||||
|
const std::string &dat = test_data[frames[i].dat.size()];
|
||||||
|
REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_CASE("send/recv CAN 2.0 packets") {
|
||||||
|
auto bus_offset = GENERATE(0, 4);
|
||||||
|
auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
|
||||||
|
PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS);
|
||||||
|
|
||||||
|
SECTION("can_send") {
|
||||||
|
test.test_can_send();
|
||||||
|
}
|
||||||
|
SECTION("can_receive") {
|
||||||
|
test.test_can_recv();
|
||||||
|
}
|
||||||
|
SECTION("chunked_can_receive") {
|
||||||
|
test.test_can_recv(0x40);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_CASE("send/recv CAN FD packets") {
|
||||||
|
auto bus_offset = GENERATE(0, 4);
|
||||||
|
auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
|
||||||
|
PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA);
|
||||||
|
|
||||||
|
SECTION("can_send") {
|
||||||
|
test.test_can_send();
|
||||||
|
}
|
||||||
|
SECTION("can_receive") {
|
||||||
|
test.test_can_recv();
|
||||||
|
}
|
||||||
|
SECTION("chunked_can_receive") {
|
||||||
|
test.test_can_recv(0x40);
|
||||||
|
}
|
||||||
|
}
|
||||||
119
selfdrive/boardd/tests/test_pandad.py
Normal file
119
selfdrive/boardd/tests/test_pandad.py
Normal file
@@ -0,0 +1,119 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import os
|
||||||
|
import pytest
|
||||||
|
import time
|
||||||
|
import unittest
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from cereal import log
|
||||||
|
from openpilot.common.gpio import gpio_set, gpio_init
|
||||||
|
from panda import Panda, PandaDFU, PandaProtocolMismatch
|
||||||
|
from openpilot.selfdrive.manager.process_config import managed_processes
|
||||||
|
from openpilot.system.hardware import HARDWARE
|
||||||
|
from openpilot.system.hardware.tici.pins import GPIO
|
||||||
|
|
||||||
|
HERE = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.tici
|
||||||
|
class TestPandad(unittest.TestCase):
|
||||||
|
|
||||||
|
def setUp(self):
|
||||||
|
# ensure panda is up
|
||||||
|
if len(Panda.list()) == 0:
|
||||||
|
self._run_test(60)
|
||||||
|
|
||||||
|
def tearDown(self):
|
||||||
|
managed_processes['pandad'].stop()
|
||||||
|
|
||||||
|
def _run_test(self, timeout=30):
|
||||||
|
managed_processes['pandad'].start()
|
||||||
|
|
||||||
|
sm = messaging.SubMaster(['peripheralState'])
|
||||||
|
for _ in range(timeout*10):
|
||||||
|
sm.update(100)
|
||||||
|
if sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
|
||||||
|
break
|
||||||
|
|
||||||
|
managed_processes['pandad'].stop()
|
||||||
|
|
||||||
|
if sm['peripheralState'].pandaType == log.PandaState.PandaType.unknown:
|
||||||
|
raise Exception("boardd failed to start")
|
||||||
|
|
||||||
|
def _go_to_dfu(self):
|
||||||
|
HARDWARE.recover_internal_panda()
|
||||||
|
assert Panda.wait_for_dfu(None, 10)
|
||||||
|
|
||||||
|
def _assert_no_panda(self):
|
||||||
|
assert not Panda.wait_for_dfu(None, 3)
|
||||||
|
assert not Panda.wait_for_panda(None, 3)
|
||||||
|
|
||||||
|
def _flash_bootstub_and_test(self, fn, expect_mismatch=False):
|
||||||
|
self._go_to_dfu()
|
||||||
|
pd = PandaDFU(None)
|
||||||
|
if fn is None:
|
||||||
|
fn = os.path.join(HERE, pd.get_mcu_type().config.bootstub_fn)
|
||||||
|
with open(fn, "rb") as f:
|
||||||
|
pd.program_bootstub(f.read())
|
||||||
|
pd.reset()
|
||||||
|
HARDWARE.reset_internal_panda()
|
||||||
|
|
||||||
|
assert Panda.wait_for_panda(None, 10)
|
||||||
|
if expect_mismatch:
|
||||||
|
with self.assertRaises(PandaProtocolMismatch):
|
||||||
|
Panda()
|
||||||
|
else:
|
||||||
|
with Panda() as p:
|
||||||
|
assert p.bootstub
|
||||||
|
|
||||||
|
self._run_test(45)
|
||||||
|
|
||||||
|
def test_in_dfu(self):
|
||||||
|
HARDWARE.recover_internal_panda()
|
||||||
|
self._run_test(60)
|
||||||
|
|
||||||
|
def test_in_bootstub(self):
|
||||||
|
with Panda() as p:
|
||||||
|
p.reset(enter_bootstub=True)
|
||||||
|
assert p.bootstub
|
||||||
|
self._run_test()
|
||||||
|
|
||||||
|
def test_internal_panda_reset(self):
|
||||||
|
gpio_init(GPIO.STM_RST_N, True)
|
||||||
|
gpio_set(GPIO.STM_RST_N, 1)
|
||||||
|
time.sleep(0.5)
|
||||||
|
assert all(not Panda(s).is_internal() for s in Panda.list())
|
||||||
|
self._run_test()
|
||||||
|
|
||||||
|
assert any(Panda(s).is_internal() for s in Panda.list())
|
||||||
|
|
||||||
|
def test_best_case_startup_time(self):
|
||||||
|
# run once so we're setup
|
||||||
|
self._run_test(60)
|
||||||
|
|
||||||
|
# should be fast this time
|
||||||
|
self._run_test(8)
|
||||||
|
|
||||||
|
def test_protocol_version_check(self):
|
||||||
|
if HARDWARE.get_device_type() == 'tici':
|
||||||
|
raise unittest.SkipTest("SPI test")
|
||||||
|
# flash old fw
|
||||||
|
fn = os.path.join(HERE, "bootstub.panda_h7_spiv0.bin")
|
||||||
|
self._flash_bootstub_and_test(fn, expect_mismatch=True)
|
||||||
|
|
||||||
|
def test_release_to_devel_bootstub(self):
|
||||||
|
self._flash_bootstub_and_test(None)
|
||||||
|
|
||||||
|
def test_recover_from_bad_bootstub(self):
|
||||||
|
self._go_to_dfu()
|
||||||
|
with PandaDFU(None) as pd:
|
||||||
|
pd.program_bootstub(b"\x00"*1024)
|
||||||
|
pd.reset()
|
||||||
|
HARDWARE.reset_internal_panda()
|
||||||
|
self._assert_no_panda()
|
||||||
|
|
||||||
|
self._run_test(60)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
Reference in New Issue
Block a user