sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

kinda works

Browse Source
pypanda
Adeeb Shihadeh 3 months ago
parent cea3572b74
commit 22167461c5
23 changed files with 488 additions and 1990 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      selfdrive/pandad/.gitignore
  2. 11
      selfdrive/pandad/SConscript
  3. 22
      selfdrive/pandad/main.cc
  4. 312
      selfdrive/pandad/panda.cc
  5. 99
      selfdrive/pandad/panda.h
  6. 227
      selfdrive/pandad/panda_comms.cc
  7. 93
      selfdrive/pandad/panda_comms.h
  8. 81
      selfdrive/pandad/panda_safety.cc
  9. 525
      selfdrive/pandad/pandad.cc
  10. 27
      selfdrive/pandad/pandad.h
  11. 56
      selfdrive/pandad/pandad.py
  12. 101
      selfdrive/pandad/peripheral.py
  13. 127
      selfdrive/pandad/runner.py
  14. 76
      selfdrive/pandad/safety.py
  15. 410
      selfdrive/pandad/spi.cc
  16. 126
      selfdrive/pandad/state_manager.py
  17. 135
      selfdrive/pandad/tests/test_pandad_usbprotocol.cc
  18. 6
      selfdrive/test/test_onroad.py
  19. 1
      system/hardware/base.h
  20. 9
      system/hardware/base.py
  21. 13
      system/hardware/tici/hardware.h
  22. 18
      system/hardware/tici/hardware.py
  23. 1
      system/manager/process_config.py

2
selfdrive/pandad/.gitignore vendored
Unescape View File

@ -1,3 +1 @@
pandad
pandad_api_impl.cpp
tests/test_pandad_usbprotocol

11
selfdrive/pandad/SConscript
Unescape View File

@ -1,13 +1,4 @@
Import('env', 'envCython', 'common', 'messaging')
libs = ['usb-1.0', common, messaging, 'pthread']
panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc'])
env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs)
Import('env', 'envCython')
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
pandad_python = envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
Export('pandad_python')
if GetOption('extras'):
env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs)

22
selfdrive/pandad/main.cc
Unescape View File

@ -1,22 +0,0 @@
#include <cassert>
#include "selfdrive/pandad/pandad.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/hardware/hw.h"
int main(int argc, char *argv[]) {
LOGW("starting pandad");
if (!Hardware::PC()) {
int err;
err = util::set_realtime_priority(54);
assert(err == 0);
err = util::set_core_affinity({3});
assert(err == 0);
}
std::vector<std::string> serials(argv + 1, argv + argc);
pandad_main_thread(serials);
return 0;
}

312
selfdrive/pandad/panda.cc
Unescape View File

@ -1,312 +0,0 @@
#include "selfdrive/pandad/panda.h"
#include <unistd.h>
#include <cassert>
#include <stdexcept>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "common/swaglog.h"
#include "common/util.h"
const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr;
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();
}
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 (const 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);
}
std::string Panda::serial_read(int port_number) {
std::string ret;
char buffer[USBPACKET_MAX_SIZE] = {};
while (true) {
int bytes_read = handle->control_read(0xe0, port_number, 0, (unsigned char *)buffer, USBPACKET_MAX_SIZE);
if (bytes_read <= 0) {
break;
}
ret.append(buffer, bytes_read);
}
return ret;
}
void Panda::set_uart_baud(int uart, int rate) {
handle->control_write(0xe4, uart, int(rate / 300));
}
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_can_fd_auto(uint16_t bus, bool enabled) {
handle->control_write(0xe8, bus, enabled);
}
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 (const 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_OFFSET)) {
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(const 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 (PANDAD_MAXOUT) {
static uint8_t junk[RECV_SIZE];
handle->bulk_read(0xab, junk, RECV_SIZE - recv);
}
bool ret = true;
if (recv > 0) {
receive_buffer_size += recv;
ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
}
return ret;
}
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) {
LOGE("Panda CAN checksum failed");
size = 0;
can_reset_communications();
return false;
}
can_frame &canData = out_vec.emplace_back();
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;
}

99
selfdrive/pandad/panda.h
Unescape View File

@ -1,99 +0,0 @@
#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.h"
#include "selfdrive/pandad/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
#define PANDA_BUS_OFFSET 4
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 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);
std::string serial_read(int port_number = 0);
void set_uart_baud(int uart, int rate);
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_can_fd_auto(uint16_t bus, bool enabled);
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(const 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/pandad/panda_comms.cc
Unescape View File

@ -1,227 +0,0 @@
#include "selfdrive/pandad/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 == 0x3801 && 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 == 0x3801 && 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));
}
}
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;
}

93
selfdrive/pandad/panda_comms.h
Unescape View File

@ -1,93 +0,0 @@
#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__
struct __attribute__((packed)) spi_header {
uint8_t sync;
uint8_t endpoint;
uint16_t tx_len;
uint16_t max_rx_len;
};
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);
int lltransfer(spi_ioc_transfer &t);
spi_header header;
uint32_t xfer_count = 0;
};
#endif

81
selfdrive/pandad/panda_safety.cc
Unescape View File

@ -1,81 +0,0 @@
#include "selfdrive/pandad/pandad.h"
#include "cereal/messaging/messaging.h"
#include "common/swaglog.h"
void PandaSafety::configureSafetyMode(bool is_onroad) {
if (is_onroad && !safety_configured_) {
updateMultiplexingMode();
auto car_params = fetchCarParams();
if (!car_params.empty()) {
LOGW("got %lu bytes CarParams", car_params.size());
setSafetyMode(car_params);
safety_configured_ = true;
}
} else if (!is_onroad) {
initialized_ = false;
safety_configured_ = false;
log_once_ = false;
}
}
void PandaSafety::updateMultiplexingMode() {
// Initialize to ELM327 without OBD multiplexing for initial fingerprinting
if (!initialized_) {
prev_obd_multiplexing_ = false;
for (int i = 0; i < pandas_.size(); ++i) {
pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
}
initialized_ = true;
}
// Switch between multiplexing modes based on the OBD multiplexing request
bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled");
if (obd_multiplexing_requested != prev_obd_multiplexing_) {
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);
}
prev_obd_multiplexing_ = obd_multiplexing_requested;
params_.putBool("ObdMultiplexingChanged", true);
}
}
std::string PandaSafety::fetchCarParams() {
if (!params_.getBool("FirmwareQueryDone")) {
return {};
}
if (!log_once_) {
LOGW("Finished FW query, Waiting for params to set safety model");
log_once_ = true;
}
if (!params_.getBool("ControlsReady")) {
return {};
}
return params_.get("CarParams");
}
void PandaSafety::setSafetyMode(const std::string &params_string) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size()));
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
auto safety_configs = car_params.getSafetyConfigs();
uint16_t alternative_experience = car_params.getAlternativeExperience();
for (int i = 0; i < pandas_.size(); ++i) {
// Default to SILENT safety model if not specified
cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT;
uint16_t safety_param = 0U;
if (i < safety_configs.size()) {
safety_model = safety_configs[i].getSafetyModel();
safety_param = safety_configs[i].getSafetyParam();
}
LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
pandas_[i]->set_alternative_experience(alternative_experience);
pandas_[i]->set_safety_model(safety_model, safety_param);
}
}

525
selfdrive/pandad/pandad.cc
Unescape View File

@ -1,525 +0,0 @@
#include "selfdrive/pandad/pandad.h"
#include <algorithm>
#include <array>
#include <bitset>
#include <cassert>
#include <cerrno>
#include <memory>
#include <thread>
#include <utility>
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/messaging/messaging.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, pandad will reconnect to all pandas
// - If a panda is added, we will only reconnect when we are offroad
// CAN buses:
// - Each panda will have its 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_PANDA_VAL 50
#define CUTOFF_IL 400
#define SATURATE_IL 1000
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;
}
Panda *connect(std::string serial="", uint32_t index=0) {
std::unique_ptr<Panda> panda;
try {
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_OFFSET));
} catch (std::exception &e) {
return nullptr;
}
// common panda config
if (getenv("BOARDD_LOOPBACK")) {
panda->set_loopback(true);
}
//panda->enable_deepsleep();
for (int i = 0; i < PANDA_BUS_CNT; i++) {
panda->set_can_fd_auto(i, true);
}
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("pandad_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) {
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(std::vector<Panda *> &pandas, PubMaster *pm) {
static std::vector<can_frame> raw_can_data;
{
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 (size_t i = 0; i < raw_can_data.size(); ++i) {
canData[i].setAddress(raw_can_data[i].address);
canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
canData[i].setSrc(raw_can_data[i].src);
}
pm->send("can", msg);
}
}
void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) {
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.setPandaType(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.setSpiErrorCount(health.spi_error_count_pkt);
ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
}
void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) {
cs.setBusOff((bool)can_health.bus_off);
cs.setBusOffCnt(can_health.bus_off_cnt);
cs.setErrorWarning((bool)can_health.error_warning);
cs.setErrorPassive((bool)can_health.error_passive);
cs.setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
cs.setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
cs.setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
cs.setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
cs.setReceiveErrorCnt(can_health.receive_error_cnt);
cs.setTransmitErrorCnt(can_health.transmit_error_cnt);
cs.setTotalErrorCnt(can_health.total_error_cnt);
cs.setTotalTxLostCnt(can_health.total_tx_lost_cnt);
cs.setTotalRxLostCnt(can_health.total_rx_lost_cnt);
cs.setTotalTxCnt(can_health.total_tx_cnt);
cs.setTotalRxCnt(can_health.total_rx_cnt);
cs.setTotalFwdCnt(can_health.total_fwd_cnt);
cs.setCanSpeed(can_health.can_speed);
cs.setCanDataSpeed(can_health.can_data_speed);
cs.setCanfdEnabled(can_health.canfd_enabled);
cs.setBrsEnabled(can_health.brs_enabled);
cs.setCanfdNonIso(can_health.canfd_non_iso);
cs.setIrq0CallRate(can_health.irq0_call_rate);
cs.setIrq1CallRate(can_health.irq1_call_rate);
cs.setIrq2CallRate(can_health.irq2_call_rate);
cs.setCanCoreResetCnt(can_health.can_core_reset_cnt);
}
std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool is_onroad, 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 or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect
bool should_close_relay = !ignition_local || !is_onroad;
if (should_close_relay && (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];
fill_panda_state(ps, panda->hw_type, health);
auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
fill_panda_can_state(cs[j], pandaCanStates[i][j]);
}
// 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(Panda *panda, PubMaster *pm) {
// 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);
}
// fall back to panda's voltage and current measurement
if (ps.getVoltage() == 0 && ps.getCurrent() == 0) {
auto health_opt = panda->get_state();
if (health_opt) {
health_t health = *health_opt;
ps.setVoltage(health.voltage_pkt);
ps.setCurrent(health.current_pkt);
}
}
uint16_t fan_speed_rpm = panda->get_fan_speed();
ps.setFanSpeedRpm(fan_speed_rpm);
pm->send("peripheralState", msg);
}
void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) {
std::vector<std::string> connected_serials;
for (Panda *p : pandas) {
connected_serials.push_back(p->hw_serial());
}
{
auto ignition_opt = send_panda_states(pm, pandas, is_onroad, spoofing_started);
if (!ignition_opt) {
LOGE("Failed to get ignition_opt");
return;
}
// check if we should have pandad reconnect
if (!ignition_opt.value()) {
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;
}
}
}
}
for (const auto &panda : pandas) {
panda->send_heartbeat(engaged);
}
}
}
void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) {
static SubMaster sm({"deviceState", "driverCameraState"});
static uint64_t last_driver_camera_t = 0;
static uint16_t prev_fan_speed = 999;
static int ir_pwr = 0;
static int prev_ir_pwr = 999;
static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05);
{
sm.update(0);
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 = 0;
} else if (cur_integ_lines > SATURATE_IL) {
ir_pwr = 100;
} else {
ir_pwr = 100 * (cur_integ_lines - CUTOFF_IL) / (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) {
int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL);
panda->set_ir_pwr(ir_panda);
Hardware::set_ir_power(ir_pwr);
prev_ir_pwr = ir_pwr;
}
}
}
void pandad_run(std::vector<Panda *> &pandas) {
const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr;
const bool spoofing_started = getenv("STARTED") != nullptr;
const bool fake_send = getenv("FAKESEND") != nullptr;
// Start the CAN send thread
std::thread send_thread(can_send_thread, pandas, fake_send);
Params params;
RateKeeper rk("pandad", 100);
SubMaster sm({"selfdriveState"});
PubMaster pm({"can", "pandaStates", "peripheralState"});
PandaSafety panda_safety(pandas);
Panda *peripheral_panda = pandas[0];
bool engaged = false;
bool is_onroad = false;
// Main loop: receive CAN data and process states
while (!do_exit && check_all_connected(pandas)) {
can_recv(pandas, &pm);
// Process peripheral state at 20 Hz
if (rk.frame() % 5 == 0) {
process_peripheral_state(peripheral_panda, &pm, no_fan_control);
}
// Process panda state at 10 Hz
if (rk.frame() % 10 == 0) {
sm.update(0);
engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
is_onroad = params.getBool("IsOnroad");
process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started);
panda_safety.configureSafetyMode(is_onroad);
}
// Send out peripheralState at 2Hz
if (rk.frame() % 50 == 0) {
send_peripheral_state(peripheral_panda, &pm);
}
// Forward logs from pandas to cloudlog if available
for (auto *panda : pandas) {
std::string log = panda->serial_read();
if (!log.empty()) {
if (log.find("Register 0x") != std::string::npos) {
// Log register divergent faults as errors
LOGE("%s", log.c_str());
} else {
LOGD("%s", log.c_str());
}
}
}
rk.keepTime();
}
// Close relay on exit to prevent a fault
if (is_onroad && !engaged) {
for (auto &p : pandas) {
if (p->connected()) {
p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
}
}
send_thread.join();
}
void pandad_main_thread(std::vector<std::string> serials) {
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");
pandad_run(pandas);
}
for (Panda *panda : pandas) {
delete panda;
}
}

27
selfdrive/pandad/pandad.h
Unescape View File

@ -1,27 +0,0 @@
#pragma once
#include <string>
#include <vector>
#include "common/params.h"
#include "selfdrive/pandad/panda.h"
void pandad_main_thread(std::vector<std::string> serials);
class PandaSafety {
public:
PandaSafety(const std::vector<Panda *> &pandas) : pandas_(pandas) {}
void configureSafetyMode(bool is_onroad);
private:
void updateMultiplexingMode();
std::string fetchCarParams();
void setSafetyMode(const std::string &params_string);
bool initialized_ = false;
bool log_once_ = false;
bool safety_configured_ = false;
bool prev_obd_multiplexing_ = false;
std::vector<Panda *> pandas_;
Params params_;
};

56
selfdrive/pandad/pandad.py
Unescape View File

@ -3,14 +3,13 @@
import os
import usb1
import time
import signal
import subprocess
import threading
from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
from openpilot.common.basedir import BASEDIR
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process
from openpilot.system.hardware import HARDWARE
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.pandad.runner import PandaRunner
def get_expected_signature(panda: Panda) -> bytes:
@ -62,24 +61,17 @@ def flash_panda(panda_serial: str) -> Panda:
def main() -> None:
# signal pandad to close the relay and exit
def signal_handler(signum, frame):
cloudlog.info(f"Caught signal {signum}, exiting")
nonlocal do_exit
do_exit = True
if process is not None:
process.send_signal(signal.SIGINT)
process = None
do_exit = False
signal.signal(signal.SIGINT, signal_handler)
config_realtime_process(3, 54)
# signal pandad to close the relay and exit
evt = threading.Event()
count = 0
first_run = True
params = Params()
no_internal_panda_count = 0
pandas: list[Panda] = []
while not do_exit:
while True:
try:
count += 1
cloudlog.event("pandad.flash_and_connect", count=count)
@ -116,9 +108,7 @@ def main() -> None:
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
# Flash pandas
pandas: list[Panda] = []
for serial in panda_serials:
pandas.append(flash_panda(serial))
pandas = [flash_panda(serial) for serial in panda_serials]
# Ensure internal panda is present if expected
internal_pandas = [panda for panda in pandas if panda.is_internal()]
@ -151,11 +141,22 @@ def main() -> None:
if first_run:
# reset panda to ensure we're in a good state
cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
panda.reset(reconnect=True)
# TODO: this hangs now, but nothing should've changed here
#panda.reset(reconnect=True)
evt.clear()
first_run = False
runner = PandaRunner(panda_serials, pandas)
runner.run(evt)
for p in pandas:
p.close()
# TODO: wrap all panda exceptions in a base panda exception
except KeyboardInterrupt:
cloudlog.info("Caught Ctrl+C, exiting")
evt.set()
break
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")
@ -166,14 +167,13 @@ def main() -> None:
except Exception:
cloudlog.exception("pandad.uncaught_exception")
continue
first_run = False
# run pandad with all connected serials as arguments
os.environ['MANAGER_DAEMON'] = 'pandad'
process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "selfdrive/pandad"))
process.wait()
finally:
for p in pandas:
try:
p.close()
except Exception:
cloudlog.exception("Error closing panda connection")
pandas = []
if __name__ == "__main__":
main()

101
selfdrive/pandad/peripheral.py
Unescape View File

@ -0,0 +1,101 @@
import os
import threading
import time
import cereal.messaging as messaging
from openpilot.common.swaglog import cloudlog
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.system.hardware import HARDWARE
NO_FAN_CONTROL = os.getenv("NO_FAN_CONTROL") == "1"
MAX_IR_PANDA_VAL = 50
CUTOFF_IL = 400
SATURATE_IL = 1000
class HardwareReader:
def __init__(self):
self.voltage = 0
self.current = 0
self.running = True
self.thread = threading.Thread(target=self._read_loop, daemon=True)
self.thread.start()
def _read_loop(self):
while self.running:
start = time.monotonic()
try:
self.voltage = HARDWARE.get_voltage()
self.current = HARDWARE.get_current()
elapsed = (time.monotonic() - start) * 1000
if elapsed > 50:
cloudlog.warning(f"hwmon read took {elapsed:.2f}ms")
except Exception as e:
cloudlog.error(f"Hardware read error: {e}")
time.sleep(0.5) # 500ms update rate
def get_values(self):
return self.voltage, self.current
def stop(self):
self.running = False
self.thread.join()
class PeripheralManager:
def __init__(self, panda, hw_type):
self.panda = panda
self.last_camera_t = 0
self.prev_fan = 999
self.prev_ir_pwr = 999
self.ir_pwr = 0
self.filter = FirstOrderFilter(0, 30.0, 0.05)
self.hw_type = hw_type
self.hw_reader = HardwareReader()
def process(self, sm):
if sm.updated["deviceState"] and not NO_FAN_CONTROL:
fan = sm["deviceState"].fanSpeedPercentDesired
if fan != self.prev_fan or sm.frame % 100 == 0:
self.panda.set_fan_power(fan)
self.prev_fan = fan
if sm.updated["driverCameraState"]:
state = sm["driverCameraState"]
lines = self.filter.update(state.integLines)
self.last_camera_t = sm.logMonoTime['driverCameraState']
if lines <= CUTOFF_IL:
self.ir_pwr = 0
elif lines > SATURATE_IL:
self.ir_pwr = 100
else:
self.ir_pwr = 100 * (lines - CUTOFF_IL) / (SATURATE_IL - CUTOFF_IL)
if time.monotonic_ns() - self.last_camera_t > 1e9:
self.ir_pwr = 0
if self.ir_pwr != self.prev_ir_pwr or sm.frame % 100 == 0:
ir_panda = int(self.ir_pwr * MAX_IR_PANDA_VAL / 100)
self.panda.set_ir_power(ir_panda)
HARDWARE.set_ir_power(self.ir_pwr)
self.prev_ir_pwr = self.ir_pwr
def send_state(self, pm):
msg = messaging.new_message('peripheralState')
msg.valid = True
ps = msg.peripheralState
ps.pandaType = self.hw_type
ps.voltage, ps.current = self.hw_reader.get_values()
if not (ps.voltage or ps.current):
h = self.panda.health() or {}
ps.voltage = h.get("voltage", 0)
ps.current = h.get("current", 0)
ps.fanSpeedRpm = self.panda.get_fan_rpm()
pm.send("peripheralState", msg)
def cleanup(self):
self.hw_reader.stop()

127
selfdrive/pandad/runner.py
Unescape View File

@ -0,0 +1,127 @@
import os
import time
from cereal import car
from cereal.messaging import SubMaster, PubMaster
import cereal.messaging as messaging
from panda import Panda
from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.pandad.state_manager import PandaStateManager
from openpilot.selfdrive.pandad.peripheral import PeripheralManager
from openpilot.selfdrive.pandad.safety import PandaSafetyManager
FAKE_SEND = os.getenv("FAKESEND") == "1"
class PandaRunner:
def __init__(self, serials, pandas):
self.pandas = pandas
self.usb_pandas = {p.get_usb_serial() for p in pandas if not p.spi}
self.hw_types = [int.from_bytes(p.get_type(), 'big') for p in pandas]
for panda in self.pandas:
if os.getenv("BOARDD_LOOPBACK"):
panda.set_can_loopback(True)
for i in range(3):
panda.set_canfd_auto(i, True)
self.sm = SubMaster(["selfdriveState", "deviceState", "driverCameraState"])
self.pm = PubMaster(["can", "pandaStates", "peripheralState"])
self.sendcan_sock = messaging.sub_sock('sendcan', timeout=10)
self.sendcan_buffer = None
self.state_mgr = PandaStateManager(pandas, self.hw_types)
self.periph_mgr = PeripheralManager(pandas[0], self.hw_types[0])
self.safety_mgr = PandaSafetyManager(pandas)
def _can_send(self):
# TODO: this needs to have a strict timeout of <10ms and handle NACKs well (buffer the data)
while (msg := messaging.recv_one_or_none(self.sendcan_sock)):
# drop msg if too old
if (time.monotonic_ns() - msg.logMonoTime) / 1e9 > 1.0:
cloudlog.warning("skipping CAN send, too old")
continue
# Group CAN messages by panda based on bus offset
panda_msgs = [[] for _ in self.pandas]
for c in msg.sendcan:
panda_idx = c.src // 4 # Each panda handles 4 buses
if panda_idx < len(self.pandas):
# Adjust bus number for the panda (remove offset)
adjusted_bus = c.src % 4
panda_msgs[panda_idx].append((c.address, c.dat, adjusted_bus))
# Send messages to each panda
for panda_idx, can_msgs in enumerate(panda_msgs):
if can_msgs:
self.pandas[panda_idx].can_send_many(can_msgs)
def _can_recv(self):
cans = []
for panda_idx, p in enumerate(self.pandas):
bus_offset = panda_idx * 4 # Each panda gets 4 buses
for address, dat, src in p.can_recv():
if src >= 192: # Rejected message
base_bus = src - 192
adjusted_src = base_bus + bus_offset + 192
elif src >= 128: # Returned message
base_bus = src - 128
adjusted_src = base_bus + bus_offset + 128
else: # Normal message
adjusted_src = src + bus_offset
cans.append((address, dat, adjusted_src))
msg = messaging.new_message('can', len(cans) if cans else 0)
msg.valid = True
if cans:
for i, can_info in enumerate(cans):
can = msg.can[i]
can.address, can.dat, can.src = can_info
self.pm.send("can", msg)
def run(self, evt):
rk = Ratekeeper(100, print_delay_threshold=None)
engaged = False
try:
while not evt.is_set():
# receive CAN messages
self._can_recv()
# send CAN messages
self._can_send()
# Process peripheral state at 20 Hz
if rk.frame % 5 == 0:
self.sm.update(0)
engaged = self.sm.all_checks() and self.sm["selfdriveState"].enabled
self.periph_mgr.process(self.sm)
# Process panda state at 10 Hz
if rk.frame % 10 == 0:
ignition = self.state_mgr.process(engaged, self.pm)
if not ignition and rk.frame % 100 == 0:
if set(Panda.list(usb_only=True)) != self.usb_pandas:
cloudlog.warning("Reconnecting to new panda")
evt.set()
break
self.safety_mgr.configure_safety_mode()
# Send out peripheralState at 2Hz
if rk.frame % 50 == 0:
self.periph_mgr.send_state(self.pm)
rk.keep_time()
except Exception as e:
cloudlog.error(f"Exception in main loop: {e}")
finally:
evt.set()
self.periph_mgr.cleanup()
# Close relay on exit to prevent a fault
is_onroad = Params().get_bool("IsOnroad")
if is_onroad and not engaged:
for p in self.pandas:
p.set_safety_mode(car.CarParams.SafetyModel.noOutput)

76
selfdrive/pandad/safety.py
Unescape View File

@ -0,0 +1,76 @@
from panda import Panda
from cereal import car
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
class PandaSafetyManager:
def __init__(self, pandas: list[Panda]):
self.pandas = pandas
self.params = Params()
self.safety_configured = False
self.initialized = False
self.prev_obd_multiplexing = False
self.log_once = False
def configure_safety_mode(self):
is_onroad = self.params.get_bool("IsOnroad")
if is_onroad and not self.safety_configured:
self.update_multiplexing_mode()
car_params = self.fetch_car_params()
if car_params:
cloudlog.warning(f"got {len(car_params)} bytes CarParams")
self.set_safety_mode(car_params)
self.safety_configured = True
elif not is_onroad:
self.initialized = False
self.safety_configured = False
self.log_once = False
def update_multiplexing_mode(self):
# Initialize to ELM327 without OBD multiplexing for initial fingerprinting
if not self.initialized:
self.prev_obd_multiplexing = False
for panda in self.pandas:
panda.set_safety_mode(car.CarParams.SafetyModel.elm327, 1)
self.initialized = True
# Switch between multiplexing modes based on OBD multiplexing request
obd_multiplexing_requested = self.params.get_bool("ObdMultiplexingEnabled")
if obd_multiplexing_requested != self.prev_obd_multiplexing:
for i, panda in enumerate(self.pandas):
safety_param = 1 if i > 0 or not obd_multiplexing_requested else 0
panda.set_safety_mode(car.CarParams.SafetyModel.elm327, safety_param)
self.prev_obd_multiplexing = obd_multiplexing_requested
self.params.put_bool("ObdMultiplexingChanged", True)
def fetch_car_params(self) -> bytes:
if not self.params.get_bool("FirmwareQueryDone"):
return b""
if not self.log_once:
cloudlog.warning("Finished FW query, waiting for params to set safety model")
self.log_once = True
if not self.params.get_bool("ControlsReady"):
return b""
return self.params.get("CarParams") or b""
def set_safety_mode(self, params_bytes: bytes):
# Parse CarParams from bytes
with car.CarParams.from_bytes(params_bytes) as car_params:
safety_configs = car_params.safetyConfigs
alternative_experience = car_params.alternativeExperience
for i, panda in enumerate(self.pandas):
# Default to SILENT if no config for this panda
safety_model = car.CarParams.SafetyModel.silent
safety_param = 0
if i < len(safety_configs):
safety_model = car.CarParams.SafetyModel.schema.enumerants[safety_configs[i].safetyModel]
safety_param = safety_configs[i].safetyParam
cloudlog.warning(f"Panda {i}: setting safety model: {safety_model}, param: {safety_param}, alternative experience: {alternative_experience}")
panda._handle.controlWrite(Panda.REQUEST_OUT, 0xdf, alternative_experience, 0, b'')
panda.set_safety_mode(safety_model, safety_param)

410
selfdrive/pandad/spi.cc
Unescape View File

@ -1,410 +0,0 @@
#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/pandad/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,
};
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;
};
#define SPILOG(fn, fmt, ...) do { \
fn(fmt, ## __VA_ARGS__); \
fn(" %d / 0x%x / %d / %d / tx: %s", \
xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \
util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \
} while (0)
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;
try {
if (!util::file_exists(SPI_DEVICE)) {
throw std::runtime_error("Error connecting to panda: SPI device not found");
}
spi_fd = open(SPI_DEVICE.c_str(), O_RDWR);
if (spi_fd < 0) {
LOGE("failed opening SPI device %d", spi_fd);
throw std::runtime_error("Error connecting to panda: failed to open SPI device");
}
// SPI settings
util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode, "failed setting SPI mode");
util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed, "failed setting SPI speed");
util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word");
// 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);
throw std::runtime_error("Error connecting to panda: failed to get serial");
}
if (!serial.empty() && (serial != hw_serial)) {
throw std::runtime_error("Error connecting to panda: serial mismatch");
}
} catch (...) {
cleanup();
throw;
}
return;
}
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) {
SPILOG(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) {
SPILOG(LOGD, "NACK sleep %d", nack_count);
usleep(std::clamp(nack_count*10, 200, 2000));
}
}
}
} while (ret < 0 && connected && !timed_out);
if (ret < 0) {
SPILOG(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, 20U, SPI_ACK_TIMEOUT);
spi_ioc_transfer transfer = {
.tx_buf = (uint64_t)tx_buf,
.rx_buf = (uint64_t)rx_buf,
.len = length,
};
memset(tx_buf, tx, length);
while (true) {
int ret = lltransfer(transfer);
if (ret < 0) {
SPILOG(LOGE, "SPI: failed to send ACK request");
return ret;
}
if (rx_buf[0] == ack) {
break;
} else if (rx_buf[0] == SPI_NACK) {
SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack);
return SpiError::NACK;
}
// handle timeout
if (millis_since_boot() - start_millis > timeout) {
SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack);
return SpiError::ACK_TIMEOUT;
}
}
return 0;
}
int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1"));
if (err_prob > 0) {
if ((static_cast<double>(rand()) / RAND_MAX) < err_prob) {
printf("transfer len error\n");
t.len = rand() % SPI_BUF_SIZE;
}
if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) {
printf("corrupting TX\n");
for (int i = 0; i < t.len; i++) {
if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256);
}
}
}
}
int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t);
if (err_prob > 0) {
if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) {
printf("corrupting RX\n");
for (int i = 0; i < t.len; i++) {
if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256);
}
}
}
}
return ret;
}
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);
xfer_count++;
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 = lltransfer(transfer);
if (ret < 0) {
SPILOG(LOGE, "SPI: failed to send header");
goto fail;
}
// Wait for (N)ACK
ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
if (ret < 0) {
goto 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 = lltransfer(transfer);
if (ret < 0) {
SPILOG(LOGE, "SPI: failed to send data");
goto fail;
}
// Wait for (N)ACK
ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
if (ret < 0) {
goto fail;
}
// Read data
rx_data_len = *(uint16_t *)(rx_buf+1);
if (rx_data_len >= SPI_BUF_SIZE) {
SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len);
goto fail;
}
transfer.len = rx_data_len + 1;
transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
ret = lltransfer(transfer);
if (ret < 0) {
SPILOG(LOGE, "SPI: failed to read rx data");
goto fail;
}
if (!check_checksum(rx_buf, rx_data_len + 4)) {
SPILOG(LOGE, "SPI: bad checksum");
goto fail;
}
if (rx_data != NULL) {
memcpy(rx_data, rx_buf + 3, rx_data_len);
}
return rx_data_len;
fail:
// ensure slave is in a consistent state
// and ready for the next transfer
int nack_cnt = 0;
while (nack_cnt < 3) {
if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) {
nack_cnt += 1;
} else {
nack_cnt = 0;
}
}
if (ret >= 0) ret = -1;
return ret;
}
#endif

126
selfdrive/pandad/state_manager.py
Unescape View File

@ -0,0 +1,126 @@
import os
from cereal import car, log
import cereal.messaging as messaging
from panda.python import PANDA_BUS_CNT
SPOOFING_STARTED = os.getenv("STARTED") == "1"
LEC_ERROR_CODE = {
"No error": 0, "Stuff error": 1, "Form error": 2, "AckError": 3,
"Bit1Error": 4, "Bit0Error": 5, "CRCError": 6, "NoChange": 7,
}
class PandaStateManager:
def __init__(self, pandas, hw_types):
self.pandas = pandas
self.hw_types = hw_types
self.is_comma_three_red = (
len(pandas) == 2 and
self.hw_types[0] == log.PandaState.PandaType.dos and
self.hw_types[1] == log.PandaState.PandaType.redPanda
)
def process(self, engaged, pm) -> bool:
msg = messaging.new_message('pandaStates', len(self.pandas))
msg.valid = True
panda_states = msg.pandaStates
ignition = False
for i, panda in enumerate(self.pandas):
health = panda.health() or {}
if SPOOFING_STARTED:
health['ignition_line'] = 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 self.is_comma_three_red and i == 0:
health['ignition_line'] = 0
ignition |= bool(health['ignition_line']) or bool(health['ignition_can'])
ps = panda_states[i]
self._fill_state(ps, self.hw_types[i], health)
# Fill can state
for j in range(PANDA_BUS_CNT):
can_health = panda.can_health(j)
can_state = ps.init(f'canState{j}')
self._fill_can_state(can_state, can_health)
# Set faults
fault_bits = int(health.get('faults', 0))
faults_list = [f for f in range(log.PandaState.FaultType.relayMalfunction,
log.PandaState.FaultType.heartbeatLoopWatchdog + 1)
if fault_bits & (1 << f)]
faults = ps.init('faults', len(faults_list))
for idx, fault in enumerate(faults_list):
faults[idx] = fault
for panda, ps in zip(self.pandas, panda_states, strict=False):
if ps.safetyModel == car.CarParams.SafetyModel.silent or (
not ignition and ps.safetyModel != car.CarParams.SafetyModel.noOutput):
panda.set_safety_mode(car.CarParams.SafetyModel.noOutput)
if ps.powerSaveEnabled != (not ignition):
panda.set_power_save(not ignition)
panda.send_heartbeat(engaged)
pm.send("pandaStates", msg)
return ignition
def _fill_state(self, ps, hw_type, health):
ps.voltage = health['voltage']
ps.current = health['current']
ps.uptime = health['uptime']
ps.safetyTxBlocked = health['safety_tx_blocked']
ps.safetyRxInvalid = health['safety_rx_invalid']
ps.ignitionLine = bool(health['ignition_line'])
ps.ignitionCan = bool(health['ignition_can'])
ps.controlsAllowed = bool(health['controls_allowed'])
ps.txBufferOverflow = health['tx_buffer_overflow']
ps.rxBufferOverflow = health['rx_buffer_overflow']
ps.pandaType = hw_type
ps.safetyModel = health['safety_mode']
ps.safetyParam = health['safety_param']
ps.faultStatus = health['fault_status']
ps.powerSaveEnabled = bool(health['power_save_enabled'])
ps.heartbeatLost = bool(health['heartbeat_lost'])
ps.alternativeExperience = health['alternative_experience']
ps.harnessStatus = health['car_harness_status']
ps.interruptLoad = health['interrupt_load']
ps.fanPower = health['fan_power']
ps.fanStallCount = health['fan_stall_count']
ps.safetyRxChecksInvalid = bool(health['safety_rx_checks_invalid'])
ps.spiErrorCount = health['spi_error_count']
ps.sbu1Voltage = health['sbu1_voltage_mV'] / 1000.0
ps.sbu2Voltage = health['sbu2_voltage_mV'] / 1000.0
def _fill_can_state(self, cs, can_health):
cs.busOff = bool(can_health['bus_off'])
cs.busOffCnt = can_health['bus_off_cnt']
cs.errorWarning = bool(can_health['error_warning'])
cs.errorPassive = bool(can_health['error_passive'])
cs.lastError = LEC_ERROR_CODE[can_health['last_error']]
cs.lastStoredError = LEC_ERROR_CODE[can_health['last_stored_error']]
cs.lastDataError = LEC_ERROR_CODE[can_health['last_data_error']]
cs.lastDataStoredError = LEC_ERROR_CODE[can_health['last_data_stored_error']]
cs.receiveErrorCnt = can_health['receive_error_cnt']
cs.transmitErrorCnt = can_health['transmit_error_cnt']
cs.totalErrorCnt = can_health['total_error_cnt']
cs.totalTxLostCnt = can_health['total_tx_lost_cnt']
cs.totalRxLostCnt = can_health['total_rx_lost_cnt']
cs.totalTxCnt = can_health['total_tx_cnt']
cs.totalRxCnt = can_health['total_rx_cnt']
cs.totalFwdCnt = can_health['total_fwd_cnt']
cs.canSpeed = can_health['can_speed']
cs.canDataSpeed = can_health['can_data_speed']
cs.canfdEnabled = bool(can_health['canfd_enabled'])
cs.brsEnabled = bool(can_health['brs_enabled'])
cs.canfdNonIso = bool(can_health['canfd_non_iso'])
cs.irq0CallRate = can_health['irq0_call_rate']
cs.irq1CallRate = can_health['irq1_call_rate']
cs.irq2CallRate = can_health['irq2_call_rate']
cs.canCoreResetCnt = can_health['can_core_reset_count']

135
selfdrive/pandad/tests/test_pandad_usbprotocol.cc
Unescape View File

@ -1,135 +0,0 @@
#define CATCH_CONFIG_MAIN
#define CATCH_CONFIG_ENABLE_BENCHMARKING
#include "catch2/catch.hpp"
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "selfdrive/pandad/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, 2) + 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);
}
}

6
selfdrive/test/test_onroad.py
Unescape View File

@ -33,7 +33,7 @@ CPU usage budget
TEST_DURATION = 25
LOG_OFFSET = 8
MAX_TOTAL_CPU = 280. # total for all 8 cores
MAX_TOTAL_CPU = 310. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 16.0,
@ -63,7 +63,7 @@ PROCS = {
"./logcatd": 1.0,
"system.micd": 5.0,
"system.timed": 0,
"selfdrive.pandad.pandad": 0,
"selfdrive.pandad.pandad": 22.0,
"system.statsd": 1.0,
"system.loggerd.uploader": 15.0,
"system.loggerd.deleter": 1.0,
@ -71,12 +71,10 @@ PROCS = {
PROCS.update({
"tici": {
"./pandad": 5.0,
"./ubloxd": 1.0,
"system.ubloxd.pigeond": 6.0,
},
"tizi": {
"./pandad": 19.0,
"system.qcomgpsd.qcomgpsd": 1.0,
}
}.get(HARDWARE.get_device_type(), {}))

1
system/hardware/base.h
Unescape View File

@ -25,7 +25,6 @@ public:
static void reboot() {}
static void poweroff() {}
static void set_brightness(int percent) {}
static void set_ir_power(int percentage) {}
static void set_display_power(bool on) {}
static bool get_ssh_enabled() { return false; }

9
system/hardware/base.py
Unescape View File

@ -225,3 +225,12 @@ class HardwareBase(ABC):
def get_modem_data_usage(self):
return -1, -1
def get_voltage(self) -> float:
return 0.
def get_current(self) -> float:
return 0.
def set_ir_power(self, percent: int):
pass

13
system/hardware/tici/hardware.h
Unescape View File

@ -66,19 +66,6 @@ public:
std::ofstream("/sys/class/backlight/panel0-backlight/bl_power") << (on ? "0" : "4") << "\n";
}
static void set_ir_power(int percent) {
auto device = get_device_type();
if (device == cereal::InitData::DeviceType::TICI ||
device == cereal::InitData::DeviceType::TIZI) {
return;
}
int value = util::map_val(std::clamp(percent, 0, 100), 0, 100, 0, 300);
std::ofstream("/sys/class/leds/led:switch_2/brightness") << 0 << "\n";
std::ofstream("/sys/class/leds/led:torch_2/brightness") << value << "\n";
std::ofstream("/sys/class/leds/led:switch_2/brightness") << value << "\n";
}
static std::map<std::string, std::string> get_init_logs() {
std::map<std::string, std::string> ret = {
{"/BUILD", util::read_file("/BUILD")},

18
system/hardware/tici/hardware.py
Unescape View File

@ -116,6 +116,24 @@ class Tici(HardwareBase):
def get_serial(self):
return self.get_cmdline()['androidboot.serialno']
def get_voltage(self):
with open("/sys/class/hwmon/hwmon1/in1_input") as f:
return int(f.read())
def get_current(self):
with open("/sys/class/hwmon/hwmon1/curr1_input") as f:
return int(f.read())
def set_ir_power(self, percent: int):
if self.get_device_type() in ("tici", "tizi"):
return
value = int((percent / 100) * 300)
with open("/sys/class/leds/led:torch_2/brightness", "w") as f:
f.write(f"{value}\n")
with open("/sys/class/leds/led:switch_2/brightness", "w") as f:
f.write(f"{value}\n")
def get_network_type(self):
try:
primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)

1
system/manager/process_config.py
Unescape View File

@ -84,7 +84,6 @@ procs = [
PythonProcess("raylib_ui", "selfdrive.ui.ui", always_run, enabled=False, watchdog_max_dt=(5 if not PC else None)),
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)),

Write Preview
Loading…
Cancel
Save