George Hotz
5 years ago
parent
41d99c3b70
commit
ef93a715e1
14 changed files with 1507 additions and 0 deletions
@ -0,0 +1,2 @@ |
|||||||
|
boardd |
||||||
|
boardd_api_impl.cpp |
||||||
@ -0,0 +1,9 @@ |
|||||||
|
Import('env', 'common', 'messaging') |
||||||
|
|
||||||
|
env.Program('boardd.cc', LIBS=['usb-1.0', common, messaging, 'pthread', 'zmq', 'capnp', 'kj']) |
||||||
|
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) |
||||||
|
|
||||||
|
env.Command(['boardd_api_impl.so'], |
||||||
|
['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'], |
||||||
|
"cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace") |
||||||
|
|
||||||
@ -0,0 +1,932 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <time.h> |
||||||
|
#include <stdint.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <string.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <sched.h> |
||||||
|
#include <string.h> |
||||||
|
#include <sys/cdefs.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <sys/resource.h> |
||||||
|
|
||||||
|
#include <assert.h> |
||||||
|
#include <pthread.h> |
||||||
|
|
||||||
|
#include <libusb-1.0/libusb.h> |
||||||
|
|
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
#include "cereal/gen/cpp/car.capnp.h" |
||||||
|
|
||||||
|
#include "common/util.h" |
||||||
|
#include "common/messaging.h" |
||||||
|
#include "common/params.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
#include "common/timing.h" |
||||||
|
#include "messaging.hpp" |
||||||
|
|
||||||
|
#include <algorithm> |
||||||
|
|
||||||
|
// double the FIFO size
|
||||||
|
#define RECV_SIZE (0x1000) |
||||||
|
#define TIMEOUT 0 |
||||||
|
|
||||||
|
#define MAX_IR_POWER 0.5f |
||||||
|
#define MIN_IR_POWER 0.0f |
||||||
|
#define CUTOFF_GAIN 0.015625f // iso400
|
||||||
|
#define SATURATE_GAIN 0.0625f // iso1600
|
||||||
|
#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') |
||||||
|
#define VOLTAGE_K 0.091 // LPF gain for 5s tau (dt/tau / (dt/tau + 1))
|
||||||
|
|
||||||
|
namespace { |
||||||
|
|
||||||
|
volatile sig_atomic_t do_exit = 0; |
||||||
|
|
||||||
|
struct __attribute__((packed)) timestamp_t { |
||||||
|
uint16_t year; |
||||||
|
uint8_t month; |
||||||
|
uint8_t day; |
||||||
|
uint8_t weekday; |
||||||
|
uint8_t hour; |
||||||
|
uint8_t minute; |
||||||
|
uint8_t second; |
||||||
|
}; |
||||||
|
|
||||||
|
libusb_context *ctx = NULL; |
||||||
|
libusb_device_handle *dev_handle; |
||||||
|
pthread_mutex_t usb_lock; |
||||||
|
|
||||||
|
bool spoofing_started = false; |
||||||
|
bool fake_send = false; |
||||||
|
bool loopback_can = false; |
||||||
|
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; |
||||||
|
bool is_pigeon = false; |
||||||
|
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs
|
||||||
|
const float VBATT_START_CHARGING = 11.5; |
||||||
|
const float VBATT_PAUSE_CHARGING = 11.0; |
||||||
|
float voltage_f = 12.5; // filtered voltage
|
||||||
|
uint32_t no_ignition_cnt = 0; |
||||||
|
bool connected_once = false; |
||||||
|
bool ignition_last = false; |
||||||
|
|
||||||
|
bool safety_setter_thread_initialized = false; |
||||||
|
pthread_t safety_setter_thread_handle; |
||||||
|
|
||||||
|
bool pigeon_thread_initialized = false; |
||||||
|
pthread_t pigeon_thread_handle; |
||||||
|
|
||||||
|
bool pigeon_needs_init; |
||||||
|
|
||||||
|
void pigeon_init(); |
||||||
|
void *pigeon_thread(void *crap); |
||||||
|
|
||||||
|
void *safety_setter_thread(void *s) { |
||||||
|
// diagnostic only is the default, needed for VIN query
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
char *value_vin; |
||||||
|
size_t value_vin_sz = 0; |
||||||
|
|
||||||
|
// switch to SILENT when CarVin param is read
|
||||||
|
while (1) { |
||||||
|
if (do_exit) return NULL; |
||||||
|
const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz); |
||||||
|
if (value_vin_sz > 0) { |
||||||
|
// sanity check VIN format
|
||||||
|
assert(value_vin_sz == 17); |
||||||
|
break; |
||||||
|
} |
||||||
|
usleep(100*1000); |
||||||
|
} |
||||||
|
LOGW("got CarVin %s", value_vin); |
||||||
|
|
||||||
|
// VIN query done, stop listening to OBDII
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
char *value; |
||||||
|
size_t value_sz = 0; |
||||||
|
|
||||||
|
LOGW("waiting for params to set safety model"); |
||||||
|
while (1) { |
||||||
|
if (do_exit) return NULL; |
||||||
|
|
||||||
|
const int result = read_db_value(NULL, "CarParams", &value, &value_sz); |
||||||
|
if (value_sz > 0) break; |
||||||
|
usleep(100*1000); |
||||||
|
} |
||||||
|
LOGW("got %d bytes CarParams", value_sz); |
||||||
|
|
||||||
|
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1); |
||||||
|
memcpy(amsg.begin(), value, value_sz); |
||||||
|
free(value); |
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg); |
||||||
|
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>(); |
||||||
|
|
||||||
|
int safety_model = int(car_params.getSafetyModel()); |
||||||
|
auto safety_param = car_params.getSafetyParam(); |
||||||
|
LOGW("setting safety model: %d with param %d", safety_model, safety_param); |
||||||
|
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
|
||||||
|
// set in the mutex to avoid race
|
||||||
|
safety_setter_thread_initialized = false; |
||||||
|
|
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT); |
||||||
|
|
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
// must be called before threads or with mutex
|
||||||
|
bool usb_connect() { |
||||||
|
int err, err2; |
||||||
|
unsigned char hw_query[1] = {0}; |
||||||
|
unsigned char fw_sig_buf[128]; |
||||||
|
unsigned char fw_sig_hex_buf[16]; |
||||||
|
unsigned char serial_buf[16]; |
||||||
|
const char *serial; |
||||||
|
int serial_sz = 0; |
||||||
|
|
||||||
|
ignition_last = false; |
||||||
|
|
||||||
|
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc); |
||||||
|
if (dev_handle == NULL) { goto fail; } |
||||||
|
|
||||||
|
err = libusb_set_configuration(dev_handle, 1); |
||||||
|
if (err != 0) { goto fail; } |
||||||
|
|
||||||
|
err = libusb_claim_interface(dev_handle, 0); |
||||||
|
if (err != 0) { goto fail; } |
||||||
|
|
||||||
|
if (loopback_can) { |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT); |
||||||
|
} |
||||||
|
|
||||||
|
// get panda fw
|
||||||
|
err = libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, fw_sig_buf, 64, TIMEOUT); |
||||||
|
err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT); |
||||||
|
if ((err == 64) && (err2 == 64)) { |
||||||
|
printf("FW signature read\n"); |
||||||
|
write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128); |
||||||
|
|
||||||
|
for (size_t i = 0; i < 8; i++){ |
||||||
|
fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX(fw_sig_buf[i] >> 4); |
||||||
|
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX(fw_sig_buf[i] & 0xF); |
||||||
|
} |
||||||
|
write_db_value(NULL, "PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16); |
||||||
|
} |
||||||
|
else { goto fail; } |
||||||
|
|
||||||
|
// get panda serial
|
||||||
|
err = libusb_control_transfer(dev_handle, 0xc0, 0xd0, 0, 0, serial_buf, 16, TIMEOUT); |
||||||
|
|
||||||
|
if (err > 0) { |
||||||
|
serial = (const char *)serial_buf; |
||||||
|
serial_sz = strnlen(serial, err); |
||||||
|
write_db_value(NULL, "PandaDongleId", serial, serial_sz); |
||||||
|
printf("panda serial: %.*s\n", serial_sz, serial); |
||||||
|
} |
||||||
|
else { goto fail; } |
||||||
|
|
||||||
|
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
|
||||||
|
#ifndef __x86_64__ |
||||||
|
if (!connected_once) { |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT); |
||||||
|
} |
||||||
|
#endif |
||||||
|
connected_once = true; |
||||||
|
|
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT); |
||||||
|
|
||||||
|
hw_type = (cereal::HealthData::HwType)(hw_query[0]); |
||||||
|
is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || |
||||||
|
(hw_type == cereal::HealthData::HwType::BLACK_PANDA) || |
||||||
|
(hw_type == cereal::HealthData::HwType::UNO); |
||||||
|
if (is_pigeon) { |
||||||
|
LOGW("panda with gps detected"); |
||||||
|
pigeon_needs_init = true; |
||||||
|
if (!pigeon_thread_initialized) { |
||||||
|
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
pigeon_thread_initialized = true; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
if (hw_type == cereal::HealthData::HwType::UNO){ |
||||||
|
// Get time from system
|
||||||
|
time_t rawtime; |
||||||
|
time(&rawtime); |
||||||
|
|
||||||
|
struct tm * sys_time = gmtime(&rawtime); |
||||||
|
|
||||||
|
// Get time from RTC
|
||||||
|
timestamp_t rtc_time; |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time), TIMEOUT); |
||||||
|
|
||||||
|
//printf("System: %d-%d-%d\t%d:%d:%d\n", 1900 + sys_time->tm_year, 1 + sys_time->tm_mon, sys_time->tm_mday, sys_time->tm_hour, sys_time->tm_min, sys_time->tm_sec);
|
||||||
|
//printf("RTC: %d-%d-%d\t%d:%d:%d\n", rtc_time.year, rtc_time.month, rtc_time.day, rtc_time.hour, rtc_time.minute, rtc_time.second);
|
||||||
|
|
||||||
|
// Update system time from RTC if it looks off, and RTC time is good
|
||||||
|
if (1900 + sys_time->tm_year < 2019 && rtc_time.year >= 2019){ |
||||||
|
LOGE("System time wrong, setting from RTC"); |
||||||
|
|
||||||
|
struct tm new_time = { 0 }; |
||||||
|
new_time.tm_year = rtc_time.year - 1900; |
||||||
|
new_time.tm_mon = rtc_time.month - 1; |
||||||
|
new_time.tm_mday = rtc_time.day; |
||||||
|
new_time.tm_hour = rtc_time.hour; |
||||||
|
new_time.tm_min = rtc_time.minute; |
||||||
|
new_time.tm_sec = rtc_time.second; |
||||||
|
|
||||||
|
setenv("TZ","UTC",1); |
||||||
|
const struct timeval tv = {mktime(&new_time), 0}; |
||||||
|
settimeofday(&tv, 0); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
return true; |
||||||
|
fail: |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
void usb_retry_connect() { |
||||||
|
LOG("attempting to connect"); |
||||||
|
while (!usb_connect()) { usleep(100*1000); } |
||||||
|
LOGW("connected to board"); |
||||||
|
} |
||||||
|
|
||||||
|
void 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 == -4) { |
||||||
|
LOGE("lost connection"); |
||||||
|
usb_retry_connect(); |
||||||
|
} |
||||||
|
// TODO: check other errors, is simply retrying okay?
|
||||||
|
} |
||||||
|
|
||||||
|
void can_recv(PubSocket *publisher) { |
||||||
|
int err; |
||||||
|
uint32_t data[RECV_SIZE/4]; |
||||||
|
int recv; |
||||||
|
uint32_t f1, f2; |
||||||
|
|
||||||
|
uint64_t start_time = nanos_since_boot(); |
||||||
|
|
||||||
|
// do recv
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
|
||||||
|
do { |
||||||
|
err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT); |
||||||
|
if (err != 0) { handle_usb_issue(err, __func__); } |
||||||
|
if (err == -8) { LOGE_100("overflow got 0x%x", recv); }; |
||||||
|
|
||||||
|
// timeout is okay to exit, recv still happened
|
||||||
|
if (err == -7) { break; } |
||||||
|
} while(err != 0); |
||||||
|
|
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
// return if length is 0
|
||||||
|
if (recv <= 0) { |
||||||
|
return; |
||||||
|
} else if (recv == RECV_SIZE) { |
||||||
|
LOGW("Receive buffer full"); |
||||||
|
} |
||||||
|
|
||||||
|
// create message
|
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(start_time); |
||||||
|
size_t num_msg = recv / 0x10; |
||||||
|
auto canData = event.initCan(num_msg); |
||||||
|
|
||||||
|
// populate message
|
||||||
|
for (int i = 0; i < num_msg; i++) { |
||||||
|
if (data[i*4] & 4) { |
||||||
|
// extended
|
||||||
|
canData[i].setAddress(data[i*4] >> 3); |
||||||
|
//printf("got extended: %x\n", data[i*4] >> 3);
|
||||||
|
} else { |
||||||
|
// normal
|
||||||
|
canData[i].setAddress(data[i*4] >> 21); |
||||||
|
} |
||||||
|
canData[i].setBusTime(data[i*4+1] >> 16); |
||||||
|
int len = data[i*4+1]&0xF; |
||||||
|
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); |
||||||
|
canData[i].setSrc((data[i*4+1] >> 4) & 0xff); |
||||||
|
} |
||||||
|
|
||||||
|
// send to can
|
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
publisher->send((char*)bytes.begin(), bytes.size()); |
||||||
|
} |
||||||
|
|
||||||
|
void can_health(PubSocket *publisher) { |
||||||
|
int cnt; |
||||||
|
int err; |
||||||
|
|
||||||
|
// copied from panda/board/main.c
|
||||||
|
struct __attribute__((packed)) health { |
||||||
|
uint32_t uptime; |
||||||
|
uint32_t voltage; |
||||||
|
uint32_t current; |
||||||
|
uint32_t can_rx_errs; |
||||||
|
uint32_t can_send_errs; |
||||||
|
uint32_t can_fwd_errs; |
||||||
|
uint32_t gmlan_send_errs; |
||||||
|
uint32_t faults; |
||||||
|
uint8_t ignition_line; |
||||||
|
uint8_t ignition_can; |
||||||
|
uint8_t controls_allowed; |
||||||
|
uint8_t gas_interceptor_detected; |
||||||
|
uint8_t car_harness_status; |
||||||
|
uint8_t usb_power_mode; |
||||||
|
uint8_t safety_model; |
||||||
|
uint8_t fault_status; |
||||||
|
uint8_t power_save_enabled; |
||||||
|
} health; |
||||||
|
|
||||||
|
// recv from board
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
do { |
||||||
|
cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT); |
||||||
|
if (cnt != sizeof(health)) { |
||||||
|
handle_usb_issue(cnt, __func__); |
||||||
|
} |
||||||
|
} while(cnt != sizeof(health)); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
if (spoofing_started) { |
||||||
|
health.ignition_line = 1; |
||||||
|
} |
||||||
|
|
||||||
|
voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f; // LPF
|
||||||
|
|
||||||
|
// 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_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
|
||||||
|
bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); |
||||||
|
|
||||||
|
if (ignition) { |
||||||
|
no_ignition_cnt = 0; |
||||||
|
} else { |
||||||
|
no_ignition_cnt += 1; |
||||||
|
} |
||||||
|
|
||||||
|
#ifndef __x86_64__ |
||||||
|
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP); |
||||||
|
bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX; |
||||||
|
if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { |
||||||
|
printf("TURN OFF CHARGING!\n"); |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
printf("POWER DOWN DEVICE\n"); |
||||||
|
system("service call power 17 i32 0 i32 1"); |
||||||
|
} |
||||||
|
if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) { |
||||||
|
printf("TURN ON CHARGING!\n"); |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
// set power save state enabled when car is off and viceversa when it's on
|
||||||
|
if (ignition && (health.power_save_enabled == 1)) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe7, 0, 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
if (!ignition && (health.power_save_enabled == 0)) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0xc0, 0xe7, 1, 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
// set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
|
||||||
|
if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
// clear VIN, CarParams, and set new safety on car start
|
||||||
|
if (ignition && !ignition_last) { |
||||||
|
int result = delete_db_value(NULL, "CarVin"); |
||||||
|
assert((result == 0) || (result == ERR_NO_VALUE)); |
||||||
|
result = delete_db_value(NULL, "CarParams"); |
||||||
|
assert((result == 0) || (result == ERR_NO_VALUE)); |
||||||
|
|
||||||
|
if (!safety_setter_thread_initialized) { |
||||||
|
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
safety_setter_thread_initialized = true; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// Get fan RPM
|
||||||
|
uint16_t fan_speed_rpm = 0; |
||||||
|
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
int sz = libusb_control_transfer(dev_handle, 0xc0, 0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm), TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
// Write to rtc once per minute when no ignition present
|
||||||
|
if ((hw_type == cereal::HealthData::HwType::UNO) && !ignition && (no_ignition_cnt % 120 == 1)){ |
||||||
|
// Get time from system
|
||||||
|
time_t rawtime; |
||||||
|
time(&rawtime); |
||||||
|
|
||||||
|
struct tm * sys_time = gmtime(&rawtime); |
||||||
|
|
||||||
|
// Write time to RTC if it looks reasonable
|
||||||
|
if (1900 + sys_time->tm_year >= 2019){ |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa1, (uint16_t)(1900 + sys_time->tm_year), 0, NULL, 0, TIMEOUT); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa2, (uint16_t)(1 + sys_time->tm_mon), 0, NULL, 0, TIMEOUT); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa3, (uint16_t)sys_time->tm_mday, 0, NULL, 0, TIMEOUT); |
||||||
|
// libusb_control_transfer(dev_handle, 0x40, 0xa4, (uint16_t)(1 + sys_time->tm_wday), 0, NULL, 0, TIMEOUT);
|
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa5, (uint16_t)sys_time->tm_hour, 0, NULL, 0, TIMEOUT); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa6, (uint16_t)sys_time->tm_min, 0, NULL, 0, TIMEOUT); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xa7, (uint16_t)sys_time->tm_sec, 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
ignition_last = ignition; |
||||||
|
|
||||||
|
// create message
|
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto healthData = event.initHealth(); |
||||||
|
|
||||||
|
// set fields
|
||||||
|
healthData.setUptime(health.uptime); |
||||||
|
healthData.setVoltage(health.voltage); |
||||||
|
healthData.setCurrent(health.current); |
||||||
|
healthData.setIgnitionLine(health.ignition_line); |
||||||
|
healthData.setIgnitionCan(health.ignition_can); |
||||||
|
healthData.setControlsAllowed(health.controls_allowed); |
||||||
|
healthData.setGasInterceptorDetected(health.gas_interceptor_detected); |
||||||
|
healthData.setHasGps(is_pigeon); |
||||||
|
healthData.setCanRxErrs(health.can_rx_errs); |
||||||
|
healthData.setCanSendErrs(health.can_send_errs); |
||||||
|
healthData.setCanFwdErrs(health.can_fwd_errs); |
||||||
|
healthData.setGmlanSendErrs(health.gmlan_send_errs); |
||||||
|
healthData.setHwType(hw_type); |
||||||
|
healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode)); |
||||||
|
healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model)); |
||||||
|
healthData.setFanSpeedRpm(fan_speed_rpm); |
||||||
|
healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status)); |
||||||
|
healthData.setPowerSaveEnabled((bool)(health.power_save_enabled)); |
||||||
|
|
||||||
|
// send to health
|
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
publisher->send((char*)bytes.begin(), bytes.size()); |
||||||
|
|
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
|
||||||
|
// send heartbeat back to panda
|
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT); |
||||||
|
|
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void can_send(SubSocket *subscriber) { |
||||||
|
int err; |
||||||
|
|
||||||
|
// recv from sendcan
|
||||||
|
Message * msg = subscriber->receive(); |
||||||
|
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); |
||||||
|
memcpy(amsg.begin(), msg->getData(), msg->getSize()); |
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg); |
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||||
|
if (nanos_since_boot() - event.getLogMonoTime() > 1e9) { |
||||||
|
//Older than 1 second. Dont send.
|
||||||
|
delete msg; |
||||||
|
return; |
||||||
|
} |
||||||
|
int msg_count = event.getSendcan().size(); |
||||||
|
|
||||||
|
uint32_t *send = (uint32_t*)malloc(msg_count*0x10); |
||||||
|
memset(send, 0, msg_count*0x10); |
||||||
|
|
||||||
|
for (int i = 0; i < msg_count; i++) { |
||||||
|
auto cmsg = event.getSendcan()[i]; |
||||||
|
if (cmsg.getAddress() >= 0x800) { |
||||||
|
// extended
|
||||||
|
send[i*4] = (cmsg.getAddress() << 3) | 5; |
||||||
|
} else { |
||||||
|
// normal
|
||||||
|
send[i*4] = (cmsg.getAddress() << 21) | 1; |
||||||
|
} |
||||||
|
assert(cmsg.getDat().size() <= 8); |
||||||
|
send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4); |
||||||
|
memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size()); |
||||||
|
} |
||||||
|
|
||||||
|
// release msg
|
||||||
|
delete msg; |
||||||
|
|
||||||
|
// send to board
|
||||||
|
int sent; |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
|
||||||
|
if (!fake_send) { |
||||||
|
do { |
||||||
|
err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, TIMEOUT); |
||||||
|
if (err != 0 || msg_count*0x10 != sent) { handle_usb_issue(err, __func__); } |
||||||
|
} while(err != 0); |
||||||
|
} |
||||||
|
|
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
// done
|
||||||
|
free(send); |
||||||
|
} |
||||||
|
|
||||||
|
// **** threads ****
|
||||||
|
|
||||||
|
void *can_send_thread(void *crap) { |
||||||
|
LOGD("start send thread"); |
||||||
|
|
||||||
|
// sendcan = 8017
|
||||||
|
Context * context = Context::create(); |
||||||
|
SubSocket * subscriber = SubSocket::create(context, "sendcan"); |
||||||
|
assert(subscriber != NULL); |
||||||
|
|
||||||
|
|
||||||
|
// drain sendcan to delete any stale messages from previous runs
|
||||||
|
while (true){ |
||||||
|
Message * msg = subscriber->receive(true); |
||||||
|
if (msg == NULL){ |
||||||
|
break; |
||||||
|
} |
||||||
|
delete msg; |
||||||
|
} |
||||||
|
|
||||||
|
// run as fast as messages come in
|
||||||
|
while (!do_exit) { |
||||||
|
can_send(subscriber); |
||||||
|
} |
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
void *can_recv_thread(void *crap) { |
||||||
|
LOGD("start recv thread"); |
||||||
|
|
||||||
|
// can = 8006
|
||||||
|
Context * c = Context::create(); |
||||||
|
PubSocket * publisher = PubSocket::create(c, "can"); |
||||||
|
assert(publisher != NULL); |
||||||
|
|
||||||
|
// run at 100hz
|
||||||
|
const uint64_t dt = 10000000ULL; |
||||||
|
uint64_t next_frame_time = nanos_since_boot() + dt; |
||||||
|
|
||||||
|
while (!do_exit) { |
||||||
|
can_recv(publisher); |
||||||
|
|
||||||
|
uint64_t cur_time = nanos_since_boot(); |
||||||
|
int64_t remaining = next_frame_time - cur_time; |
||||||
|
if (remaining > 0){ |
||||||
|
useconds_t sleep = remaining / 1000; |
||||||
|
usleep(sleep); |
||||||
|
} else { |
||||||
|
LOGW("missed cycle"); |
||||||
|
next_frame_time = cur_time; |
||||||
|
} |
||||||
|
|
||||||
|
next_frame_time += dt; |
||||||
|
} |
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
void *can_health_thread(void *crap) { |
||||||
|
LOGD("start health thread"); |
||||||
|
// health = 8011
|
||||||
|
Context * c = Context::create(); |
||||||
|
PubSocket * publisher = PubSocket::create(c, "health"); |
||||||
|
assert(publisher != NULL); |
||||||
|
|
||||||
|
// run at 2hz
|
||||||
|
while (!do_exit) { |
||||||
|
can_health(publisher); |
||||||
|
usleep(500*1000); |
||||||
|
} |
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
void *hardware_control_thread(void *crap) { |
||||||
|
LOGD("start hardware control thread"); |
||||||
|
Context * c = Context::create(); |
||||||
|
SubSocket * thermal_sock = SubSocket::create(c, "thermal"); |
||||||
|
SubSocket * front_frame_sock = SubSocket::create(c, "frontFrame"); |
||||||
|
assert(thermal_sock != NULL); |
||||||
|
assert(front_frame_sock != NULL); |
||||||
|
|
||||||
|
Poller * poller = Poller::create({thermal_sock, front_frame_sock}); |
||||||
|
|
||||||
|
// Wait for hardware type to be set.
|
||||||
|
while (hw_type == cereal::HealthData::HwType::UNKNOWN){ |
||||||
|
usleep(100*1000); |
||||||
|
} |
||||||
|
// Only control fan speed on UNO
|
||||||
|
if (hw_type != cereal::HealthData::HwType::UNO) return NULL; |
||||||
|
|
||||||
|
|
||||||
|
uint16_t prev_fan_speed = 999; |
||||||
|
uint16_t prev_ir_pwr = 999; |
||||||
|
unsigned int cnt = 0; |
||||||
|
|
||||||
|
while (!do_exit) { |
||||||
|
cnt++; |
||||||
|
for (auto sock : poller->poll(1000)){ |
||||||
|
Message * msg = sock->receive(); |
||||||
|
if (msg == NULL) continue; |
||||||
|
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); |
||||||
|
memcpy(amsg.begin(), msg->getData(), msg->getSize()); |
||||||
|
|
||||||
|
delete msg; |
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg); |
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||||
|
|
||||||
|
auto type = event.which(); |
||||||
|
if(type == cereal::Event::THERMAL){ |
||||||
|
uint16_t fan_speed = event.getThermal().getFanSpeed(); |
||||||
|
if (fan_speed != prev_fan_speed || cnt % 100 == 0){ |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xb1, fan_speed, 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
prev_fan_speed = fan_speed; |
||||||
|
} |
||||||
|
} else if (type == cereal::Event::FRONT_FRAME){ |
||||||
|
float cur_front_gain = event.getFrontFrame().getGainFrac(); |
||||||
|
uint16_t ir_pwr; |
||||||
|
if (cur_front_gain <= CUTOFF_GAIN) { |
||||||
|
ir_pwr = 100.0 * MIN_IR_POWER; |
||||||
|
} else if (cur_front_gain > SATURATE_GAIN) { |
||||||
|
ir_pwr = 100.0 * MAX_IR_POWER; |
||||||
|
} else { |
||||||
|
ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_front_gain - CUTOFF_GAIN) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_GAIN - CUTOFF_GAIN))); |
||||||
|
} |
||||||
|
|
||||||
|
if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){ |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT); |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
|
||||||
|
prev_ir_pwr = ir_pwr; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
delete poller; |
||||||
|
delete thermal_sock; |
||||||
|
delete c; |
||||||
|
|
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1) |
||||||
|
|
||||||
|
void hexdump(unsigned char *d, int l) { |
||||||
|
for (int i = 0; i < l; i++) { |
||||||
|
if (i!=0 && i%0x10 == 0) printf("\n"); |
||||||
|
printf("%2.2X ", d[i]); |
||||||
|
} |
||||||
|
printf("\n"); |
||||||
|
} |
||||||
|
|
||||||
|
void _pigeon_send(const char *dat, int len) { |
||||||
|
int sent; |
||||||
|
unsigned char a[0x20]; |
||||||
|
int err; |
||||||
|
a[0] = 1; |
||||||
|
for (int i=0; i<len; i+=0x20) { |
||||||
|
int ll = std::min(0x20, len-i); |
||||||
|
memcpy(&a[1], &dat[i], ll); |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
err = libusb_bulk_transfer(dev_handle, 2, a, ll+1, &sent, TIMEOUT); |
||||||
|
if (err < 0) { handle_usb_issue(err, __func__); } |
||||||
|
/*assert(err == 0);
|
||||||
|
assert(sent == ll+1);*/ |
||||||
|
//hexdump(a, ll+1);
|
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void pigeon_set_power(int power) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
int err = libusb_control_transfer(dev_handle, 0xc0, 0xd9, power, 0, NULL, 0, TIMEOUT); |
||||||
|
if (err < 0) { handle_usb_issue(err, __func__); } |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
|
||||||
|
void pigeon_set_baud(int baud) { |
||||||
|
int err; |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
err = libusb_control_transfer(dev_handle, 0xc0, 0xe2, 1, 0, NULL, 0, TIMEOUT); |
||||||
|
if (err < 0) { handle_usb_issue(err, __func__); } |
||||||
|
err = libusb_control_transfer(dev_handle, 0xc0, 0xe4, 1, baud/300, NULL, 0, TIMEOUT); |
||||||
|
if (err < 0) { handle_usb_issue(err, __func__); } |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
} |
||||||
|
|
||||||
|
void pigeon_init() { |
||||||
|
usleep(1000*1000); |
||||||
|
LOGW("panda GPS start"); |
||||||
|
|
||||||
|
// power off pigeon
|
||||||
|
pigeon_set_power(0); |
||||||
|
usleep(100*1000); |
||||||
|
|
||||||
|
// 9600 baud at init
|
||||||
|
pigeon_set_baud(9600); |
||||||
|
|
||||||
|
// power on pigeon
|
||||||
|
pigeon_set_power(1); |
||||||
|
usleep(500*1000); |
||||||
|
|
||||||
|
// baud rate upping
|
||||||
|
pigeon_send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"); |
||||||
|
usleep(100*1000); |
||||||
|
|
||||||
|
// set baud rate to 460800
|
||||||
|
pigeon_set_baud(460800); |
||||||
|
usleep(100*1000); |
||||||
|
|
||||||
|
// init from ubloxd
|
||||||
|
pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"); |
||||||
|
pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x00\x00\x06\x18"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"); |
||||||
|
pigeon_send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"); |
||||||
|
pigeon_send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"); |
||||||
|
pigeon_send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"); |
||||||
|
pigeon_send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"); |
||||||
|
pigeon_send("\xB5\x62\x06\x24\x00\x00\x2A\x84"); |
||||||
|
pigeon_send("\xB5\x62\x06\x23\x00\x00\x29\x81"); |
||||||
|
pigeon_send("\xB5\x62\x06\x1E\x00\x00\x24\x72"); |
||||||
|
pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"); |
||||||
|
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"); |
||||||
|
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"); |
||||||
|
|
||||||
|
LOGW("panda GPS on"); |
||||||
|
} |
||||||
|
|
||||||
|
static void pigeon_publish_raw(PubSocket *publisher, unsigned char *dat, int alen) { |
||||||
|
// create message
|
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
auto ublox_raw = event.initUbloxRaw(alen); |
||||||
|
memcpy(ublox_raw.begin(), dat, alen); |
||||||
|
|
||||||
|
// send to ubloxRaw
|
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
publisher->send((char*)bytes.begin(), bytes.size()); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void *pigeon_thread(void *crap) { |
||||||
|
// ubloxRaw = 8042
|
||||||
|
Context * context = Context::create(); |
||||||
|
PubSocket * publisher = PubSocket::create(context, "ubloxRaw"); |
||||||
|
assert(publisher != NULL); |
||||||
|
|
||||||
|
// run at ~100hz
|
||||||
|
unsigned char dat[0x1000]; |
||||||
|
uint64_t cnt = 0; |
||||||
|
while (!do_exit) { |
||||||
|
if (pigeon_needs_init) { |
||||||
|
pigeon_needs_init = false; |
||||||
|
pigeon_init(); |
||||||
|
} |
||||||
|
int alen = 0; |
||||||
|
while (alen < 0xfc0) { |
||||||
|
pthread_mutex_lock(&usb_lock); |
||||||
|
int len = libusb_control_transfer(dev_handle, 0xc0, 0xe0, 1, 0, dat+alen, 0x40, TIMEOUT); |
||||||
|
if (len < 0) { handle_usb_issue(len, __func__); } |
||||||
|
pthread_mutex_unlock(&usb_lock); |
||||||
|
if (len <= 0) break; |
||||||
|
|
||||||
|
//printf("got %d\n", len);
|
||||||
|
alen += len; |
||||||
|
} |
||||||
|
if (alen > 0) { |
||||||
|
if (dat[0] == (char)0x00){ |
||||||
|
LOGW("received invalid ublox message, resetting panda GPS"); |
||||||
|
pigeon_init(); |
||||||
|
} else { |
||||||
|
pigeon_publish_raw(publisher, dat, alen); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// 10ms
|
||||||
|
usleep(10*1000); |
||||||
|
cnt++; |
||||||
|
} |
||||||
|
|
||||||
|
return NULL; |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
int main() { |
||||||
|
int err; |
||||||
|
LOGW("starting boardd"); |
||||||
|
|
||||||
|
// set process priority
|
||||||
|
err = set_realtime_priority(4); |
||||||
|
LOG("setpriority returns %d", err); |
||||||
|
|
||||||
|
// check the environment
|
||||||
|
if (getenv("STARTED")) { |
||||||
|
spoofing_started = true; |
||||||
|
} |
||||||
|
|
||||||
|
if (getenv("FAKESEND")) { |
||||||
|
fake_send = true; |
||||||
|
} |
||||||
|
|
||||||
|
if (getenv("BOARDD_LOOPBACK")){ |
||||||
|
loopback_can = true; |
||||||
|
} |
||||||
|
|
||||||
|
// init libusb
|
||||||
|
err = libusb_init(&ctx); |
||||||
|
assert(err == 0); |
||||||
|
libusb_set_debug(ctx, 3); |
||||||
|
|
||||||
|
// connect to the board
|
||||||
|
usb_retry_connect(); |
||||||
|
|
||||||
|
|
||||||
|
// create threads
|
||||||
|
pthread_t can_health_thread_handle; |
||||||
|
err = pthread_create(&can_health_thread_handle, NULL, |
||||||
|
can_health_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
pthread_t can_send_thread_handle; |
||||||
|
err = pthread_create(&can_send_thread_handle, NULL, |
||||||
|
can_send_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
pthread_t can_recv_thread_handle; |
||||||
|
err = pthread_create(&can_recv_thread_handle, NULL, |
||||||
|
can_recv_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
pthread_t hardware_control_thread_handle; |
||||||
|
err = pthread_create(&hardware_control_thread_handle, NULL, |
||||||
|
hardware_control_thread, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
// join threads
|
||||||
|
|
||||||
|
err = pthread_join(can_recv_thread_handle, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
err = pthread_join(can_send_thread_handle, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
err = pthread_join(can_health_thread_handle, NULL); |
||||||
|
assert(err == 0); |
||||||
|
|
||||||
|
//while (!do_exit) usleep(1000);
|
||||||
|
|
||||||
|
// destruct libusb
|
||||||
|
|
||||||
|
libusb_close(dev_handle); |
||||||
|
libusb_exit(ctx); |
||||||
|
} |
||||||
@ -0,0 +1,12 @@ |
|||||||
|
# pylint: skip-file |
||||||
|
|
||||||
|
# Cython, now uses scons to build |
||||||
|
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp |
||||||
|
assert can_list_to_can_capnp |
||||||
|
|
||||||
|
def can_capnp_to_can_list(can, src_filter=None): |
||||||
|
ret = [] |
||||||
|
for msg in can: |
||||||
|
if src_filter is None or msg.src in src_filter: |
||||||
|
ret.append((msg.address, msg.busTime, msg.dat, msg.src)) |
||||||
|
return ret |
||||||
@ -0,0 +1,26 @@ |
|||||||
|
# distutils: language = c++ |
||||||
|
# cython: language_level=3 |
||||||
|
from libcpp.vector cimport vector |
||||||
|
from libcpp.string cimport string |
||||||
|
from libcpp cimport bool |
||||||
|
|
||||||
|
cdef struct can_frame: |
||||||
|
long address |
||||||
|
string dat |
||||||
|
long busTime |
||||||
|
long src |
||||||
|
|
||||||
|
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid) |
||||||
|
|
||||||
|
def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): |
||||||
|
cdef vector[can_frame] can_list |
||||||
|
cdef can_frame f |
||||||
|
for can_msg in can_msgs: |
||||||
|
f.address = can_msg[0] |
||||||
|
f.busTime = can_msg[1] |
||||||
|
f.dat = can_msg[2] |
||||||
|
f.src = can_msg[3] |
||||||
|
can_list.push_back(f) |
||||||
|
cdef string out |
||||||
|
can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid) |
||||||
|
return out |
||||||
@ -0,0 +1,31 @@ |
|||||||
|
import subprocess |
||||||
|
from distutils.core import Extension, setup |
||||||
|
|
||||||
|
from Cython.Build import cythonize |
||||||
|
|
||||||
|
from common.cython_hacks import BuildExtWithoutPlatformSuffix |
||||||
|
from common.basedir import BASEDIR |
||||||
|
import os |
||||||
|
|
||||||
|
PHONELIBS = os.path.join(BASEDIR, 'phonelibs') |
||||||
|
|
||||||
|
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() |
||||||
|
ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64' |
||||||
|
|
||||||
|
setup(name='Boardd API Implementation', |
||||||
|
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, |
||||||
|
ext_modules=cythonize( |
||||||
|
Extension( |
||||||
|
"boardd_api_impl", |
||||||
|
libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libkj.a'] if ARCH == "x86_64" else [':libcan_list_to_can_capnp.a', 'capnp', 'kj'], |
||||||
|
library_dirs=[ |
||||||
|
'./', |
||||||
|
PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', |
||||||
|
PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/' |
||||||
|
], |
||||||
|
sources=['boardd_api_impl.pyx'], |
||||||
|
language="c++", |
||||||
|
extra_compile_args=["-std=c++11"], |
||||||
|
) |
||||||
|
) |
||||||
|
) |
||||||
@ -0,0 +1,37 @@ |
|||||||
|
#include <vector> |
||||||
|
#include <tuple> |
||||||
|
#include <string> |
||||||
|
#include "common/timing.h" |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
#include "cereal/gen/cpp/car.capnp.h" |
||||||
|
|
||||||
|
typedef struct { |
||||||
|
long address; |
||||||
|
std::string dat; |
||||||
|
long busTime; |
||||||
|
long src; |
||||||
|
} can_frame; |
||||||
|
|
||||||
|
extern "C" { |
||||||
|
|
||||||
|
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) { |
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
event.setValid(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++) { |
||||||
|
canData[j].setAddress(it->address); |
||||||
|
canData[j].setBusTime(it->busTime); |
||||||
|
canData[j].setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size())); |
||||||
|
canData[j].setSrc(it->src); |
||||||
|
} |
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
out.append((const char *)bytes.begin(), bytes.size()); |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,246 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
|
||||||
|
# This file is not used by OpenPilot. Only boardd.cc is used. |
||||||
|
# The python version is slower, but has more options for development. |
||||||
|
|
||||||
|
# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and |
||||||
|
# delete this python version of boardd |
||||||
|
|
||||||
|
import os |
||||||
|
import struct |
||||||
|
import time |
||||||
|
|
||||||
|
import cereal.messaging as messaging |
||||||
|
from common.realtime import Ratekeeper |
||||||
|
from selfdrive.swaglog import cloudlog |
||||||
|
from selfdrive.boardd.boardd import can_capnp_to_can_list |
||||||
|
from cereal import car |
||||||
|
|
||||||
|
SafetyModel = car.CarParams.SafetyModel |
||||||
|
|
||||||
|
# USB is optional |
||||||
|
try: |
||||||
|
import usb1 |
||||||
|
from usb1 import USBErrorIO, USBErrorOverflow #pylint: disable=no-name-in-module |
||||||
|
except Exception: |
||||||
|
pass |
||||||
|
|
||||||
|
# *** serialization functions *** |
||||||
|
def can_list_to_can_capnp(can_msgs, msgtype='can'): |
||||||
|
dat = messaging.new_message() |
||||||
|
dat.init(msgtype, len(can_msgs)) |
||||||
|
for i, can_msg in enumerate(can_msgs): |
||||||
|
if msgtype == 'sendcan': |
||||||
|
cc = dat.sendcan[i] |
||||||
|
else: |
||||||
|
cc = dat.can[i] |
||||||
|
cc.address = can_msg[0] |
||||||
|
cc.busTime = can_msg[1] |
||||||
|
cc.dat = bytes(can_msg[2]) |
||||||
|
cc.src = can_msg[3] |
||||||
|
return dat |
||||||
|
|
||||||
|
|
||||||
|
# *** can driver *** |
||||||
|
def can_health(): |
||||||
|
while 1: |
||||||
|
try: |
||||||
|
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x16) |
||||||
|
break |
||||||
|
except (USBErrorIO, USBErrorOverflow): |
||||||
|
cloudlog.exception("CAN: BAD HEALTH, RETRYING") |
||||||
|
v, i = struct.unpack("II", dat[0:8]) |
||||||
|
ign_line, ign_can = struct.unpack("BB", dat[20:22]) |
||||||
|
return {"voltage": v, "current": i, "ignition_line": bool(ign_line), "ignition_can": bool(ign_can)} |
||||||
|
|
||||||
|
def __parse_can_buffer(dat): |
||||||
|
ret = [] |
||||||
|
for j in range(0, len(dat), 0x10): |
||||||
|
ddat = dat[j:j+0x10] |
||||||
|
f1, f2 = struct.unpack("II", ddat[0:8]) |
||||||
|
ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&0xFF)) |
||||||
|
return ret |
||||||
|
|
||||||
|
def can_send_many(arr): |
||||||
|
snds = [] |
||||||
|
for addr, _, dat, alt in arr: |
||||||
|
if addr < 0x800: # only support 11 bit addr |
||||||
|
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat |
||||||
|
snd = snd.ljust(0x10, b'\x00') |
||||||
|
snds.append(snd) |
||||||
|
while 1: |
||||||
|
try: |
||||||
|
handle.bulkWrite(3, b''.join(snds)) |
||||||
|
break |
||||||
|
except (USBErrorIO, USBErrorOverflow): |
||||||
|
cloudlog.exception("CAN: BAD SEND MANY, RETRYING") |
||||||
|
|
||||||
|
def can_recv(): |
||||||
|
dat = b"" |
||||||
|
while 1: |
||||||
|
try: |
||||||
|
dat = handle.bulkRead(1, 0x10*256) |
||||||
|
break |
||||||
|
except (USBErrorIO, USBErrorOverflow): |
||||||
|
cloudlog.exception("CAN: BAD RECV, RETRYING") |
||||||
|
return __parse_can_buffer(dat) |
||||||
|
|
||||||
|
def can_init(): |
||||||
|
global handle, context |
||||||
|
handle = None |
||||||
|
cloudlog.info("attempting can init") |
||||||
|
|
||||||
|
context = usb1.USBContext() |
||||||
|
#context.setDebug(9) |
||||||
|
|
||||||
|
for device in context.getDeviceList(skip_on_error=True): |
||||||
|
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc: |
||||||
|
handle = device.open() |
||||||
|
handle.claimInterface(0) |
||||||
|
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') |
||||||
|
|
||||||
|
if handle is None: |
||||||
|
cloudlog.warning("CAN NOT FOUND") |
||||||
|
exit(-1) |
||||||
|
|
||||||
|
cloudlog.info("got handle") |
||||||
|
cloudlog.info("can init done") |
||||||
|
|
||||||
|
def boardd_mock_loop(): |
||||||
|
can_init() |
||||||
|
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') |
||||||
|
|
||||||
|
logcan = messaging.sub_sock('can') |
||||||
|
sendcan = messaging.pub_sock('sendcan') |
||||||
|
|
||||||
|
while 1: |
||||||
|
tsc = messaging.drain_sock(logcan, wait_for_one=True) |
||||||
|
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc) |
||||||
|
snd = [] |
||||||
|
for s in snds: |
||||||
|
snd += s |
||||||
|
snd = list(filter(lambda x: x[-1] <= 2, snd)) |
||||||
|
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd))) |
||||||
|
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd))) |
||||||
|
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd))) |
||||||
|
can_send_many(snd) |
||||||
|
|
||||||
|
# recv @ 100hz |
||||||
|
can_msgs = can_recv() |
||||||
|
got_0 = len(list(filter(lambda x: x[-1] == 0+0x80, can_msgs))) |
||||||
|
got_1 = len(list(filter(lambda x: x[-1] == 1+0x80, can_msgs))) |
||||||
|
got_2 = len(list(filter(lambda x: x[-1] == 2+0x80, can_msgs))) |
||||||
|
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" % |
||||||
|
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2)) |
||||||
|
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan') |
||||||
|
sendcan.send(m.to_bytes()) |
||||||
|
|
||||||
|
def boardd_test_loop(): |
||||||
|
can_init() |
||||||
|
cnt = 0 |
||||||
|
while 1: |
||||||
|
can_send_many([[0xbb,0,"\xaa\xaa\xaa\xaa",0], [0xaa,0,"\xaa\xaa\xaa\xaa"+struct.pack("!I", cnt),1]]) |
||||||
|
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",0]]) |
||||||
|
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]]) |
||||||
|
# recv @ 100hz |
||||||
|
can_msgs = can_recv() |
||||||
|
print("got %d" % (len(can_msgs))) |
||||||
|
time.sleep(0.01) |
||||||
|
cnt += 1 |
||||||
|
|
||||||
|
# *** main loop *** |
||||||
|
def boardd_loop(rate=100): |
||||||
|
rk = Ratekeeper(rate) |
||||||
|
|
||||||
|
can_init() |
||||||
|
|
||||||
|
# *** publishes can and health |
||||||
|
logcan = messaging.pub_sock('can') |
||||||
|
health_sock = messaging.pub_sock('health') |
||||||
|
|
||||||
|
# *** subscribes to can send |
||||||
|
sendcan = messaging.sub_sock('sendcan') |
||||||
|
|
||||||
|
# drain sendcan to delete any stale messages from previous runs |
||||||
|
messaging.drain_sock(sendcan) |
||||||
|
|
||||||
|
while 1: |
||||||
|
# health packet @ 2hz |
||||||
|
if (rk.frame % (rate // 2)) == 0: |
||||||
|
health = can_health() |
||||||
|
msg = messaging.new_message() |
||||||
|
msg.init('health') |
||||||
|
|
||||||
|
# store the health to be logged |
||||||
|
msg.health.voltage = health['voltage'] |
||||||
|
msg.health.current = health['current'] |
||||||
|
msg.health.ignitionLine = health['ignition_line'] |
||||||
|
msg.health.ignitionCan = health['ignition_can'] |
||||||
|
msg.health.controlsAllowed = True |
||||||
|
|
||||||
|
health_sock.send(msg.to_bytes()) |
||||||
|
|
||||||
|
# recv @ 100hz |
||||||
|
can_msgs = can_recv() |
||||||
|
|
||||||
|
# publish to logger |
||||||
|
# TODO: refactor for speed |
||||||
|
if len(can_msgs) > 0: |
||||||
|
dat = can_list_to_can_capnp(can_msgs).to_bytes() |
||||||
|
logcan.send(dat) |
||||||
|
|
||||||
|
# send can if we have a packet |
||||||
|
tsc = messaging.recv_sock(sendcan) |
||||||
|
if tsc is not None: |
||||||
|
can_send_many(can_capnp_to_can_list(tsc.sendcan)) |
||||||
|
|
||||||
|
rk.keep_time() |
||||||
|
|
||||||
|
# *** main loop *** |
||||||
|
def boardd_proxy_loop(rate=100, address="192.168.2.251"): |
||||||
|
rk = Ratekeeper(rate) |
||||||
|
|
||||||
|
can_init() |
||||||
|
|
||||||
|
# *** subscribes can |
||||||
|
logcan = messaging.sub_sock('can', addr=address) |
||||||
|
# *** publishes to can send |
||||||
|
sendcan = messaging.pub_sock('sendcan') |
||||||
|
|
||||||
|
# drain sendcan to delete any stale messages from previous runs |
||||||
|
messaging.drain_sock(sendcan) |
||||||
|
|
||||||
|
while 1: |
||||||
|
# recv @ 100hz |
||||||
|
can_msgs = can_recv() |
||||||
|
#for m in can_msgs: |
||||||
|
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) |
||||||
|
|
||||||
|
# publish to logger |
||||||
|
# TODO: refactor for speed |
||||||
|
if len(can_msgs) > 0: |
||||||
|
dat = can_list_to_can_capnp(can_msgs, "sendcan") |
||||||
|
sendcan.send(dat) |
||||||
|
|
||||||
|
# send can if we have a packet |
||||||
|
tsc = messaging.recv_sock(logcan) |
||||||
|
if tsc is not None: |
||||||
|
cl = can_capnp_to_can_list(tsc.can) |
||||||
|
#for m in cl: |
||||||
|
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex"))) |
||||||
|
can_send_many(cl) |
||||||
|
|
||||||
|
rk.keep_time() |
||||||
|
|
||||||
|
def main(gctx=None): |
||||||
|
if os.getenv("MOCK") is not None: |
||||||
|
boardd_mock_loop() |
||||||
|
elif os.getenv("PROXY") is not None: |
||||||
|
boardd_proxy_loop() |
||||||
|
elif os.getenv("BOARDTEST") is not None: |
||||||
|
boardd_test_loop() |
||||||
|
else: |
||||||
|
boardd_loop() |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
@ -0,0 +1,20 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
from __future__ import print_function |
||||||
|
import time |
||||||
|
import random |
||||||
|
|
||||||
|
from boardd_old import can_init, can_recv, can_send_many, can_health |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
can_init() |
||||||
|
while 1: |
||||||
|
c = random.randint(0, 3) |
||||||
|
if c == 0: |
||||||
|
print(can_recv()) |
||||||
|
elif c == 1: |
||||||
|
print(can_health()) |
||||||
|
elif c == 2: |
||||||
|
many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10) |
||||||
|
can_send_many(many) |
||||||
|
elif c == 3: |
||||||
|
time.sleep(random.randint(0, 100) / 1000.0) |
||||||
@ -0,0 +1,68 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
import os |
||||||
|
import sys |
||||||
|
import time |
||||||
|
import signal |
||||||
|
import traceback |
||||||
|
from panda import Panda |
||||||
|
from multiprocessing import Pool |
||||||
|
|
||||||
|
jungle = "JUNGLE" in os.environ |
||||||
|
if jungle: |
||||||
|
from panda_jungle import PandaJungle # pylint: disable=import-error |
||||||
|
|
||||||
|
import cereal.messaging as messaging |
||||||
|
from selfdrive.boardd.boardd import can_capnp_to_can_list |
||||||
|
|
||||||
|
def initializer(): |
||||||
|
"""Ignore CTRL+C in the worker process. |
||||||
|
source: https://stackoverflow.com/a/44869451 """ |
||||||
|
signal.signal(signal.SIGINT, signal.SIG_IGN) |
||||||
|
|
||||||
|
def send_thread(sender_serial): |
||||||
|
global jungle |
||||||
|
try: |
||||||
|
if jungle: |
||||||
|
sender = PandaJungle(sender_serial) |
||||||
|
else: |
||||||
|
sender = Panda(sender_serial) |
||||||
|
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) |
||||||
|
sender.set_can_loopback(False) |
||||||
|
|
||||||
|
can_sock = messaging.sub_sock('can') |
||||||
|
|
||||||
|
while True: |
||||||
|
# Send messages one bus 0 and 1 |
||||||
|
tsc = messaging.recv_one(can_sock) |
||||||
|
snd = can_capnp_to_can_list(tsc.can) |
||||||
|
snd = list(filter(lambda x: x[-1] <= 2, snd)) |
||||||
|
sender.can_send_many(snd) |
||||||
|
|
||||||
|
# Drain panda message buffer |
||||||
|
sender.can_recv() |
||||||
|
except Exception: |
||||||
|
traceback.print_exc() |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
if jungle: |
||||||
|
serials = PandaJungle.list() |
||||||
|
else: |
||||||
|
serials = Panda.list() |
||||||
|
num_senders = len(serials) |
||||||
|
|
||||||
|
if num_senders == 0: |
||||||
|
print("No senders found. Exiting") |
||||||
|
sys.exit(1) |
||||||
|
else: |
||||||
|
print("%d senders found. Starting broadcast" % num_senders) |
||||||
|
|
||||||
|
pool = Pool(num_senders, initializer=initializer) |
||||||
|
pool.map_async(send_thread, serials) |
||||||
|
|
||||||
|
while True: |
||||||
|
try: |
||||||
|
time.sleep(10) |
||||||
|
except KeyboardInterrupt: |
||||||
|
pool.terminate() |
||||||
|
pool.join() |
||||||
|
raise |
||||||
@ -0,0 +1,77 @@ |
|||||||
|
import random |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
import selfdrive.boardd.tests.boardd_old as boardd_old |
||||||
|
import selfdrive.boardd.boardd as boardd |
||||||
|
|
||||||
|
from common.realtime import sec_since_boot |
||||||
|
from cereal import log |
||||||
|
import unittest |
||||||
|
|
||||||
|
|
||||||
|
def generate_random_can_data_list(): |
||||||
|
can_list = [] |
||||||
|
cnt = random.randint(1, 64) |
||||||
|
for j in range(cnt): |
||||||
|
can_data = np.random.bytes(random.randint(1, 8)) |
||||||
|
can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)]) |
||||||
|
return can_list, cnt |
||||||
|
|
||||||
|
|
||||||
|
class TestBoarddApiMethods(unittest.TestCase): |
||||||
|
def test_correctness(self): |
||||||
|
for i in range(1000): |
||||||
|
can_list, _ = generate_random_can_data_list() |
||||||
|
|
||||||
|
# Sendcan |
||||||
|
# Old API |
||||||
|
m_old = boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes() |
||||||
|
# new API |
||||||
|
m = boardd.can_list_to_can_capnp(can_list, 'sendcan') |
||||||
|
|
||||||
|
ev_old = log.Event.from_bytes(m_old) |
||||||
|
ev = log.Event.from_bytes(m) |
||||||
|
|
||||||
|
self.assertEqual(ev_old.which(), ev.which()) |
||||||
|
self.assertEqual(len(ev.sendcan), len(ev_old.sendcan)) |
||||||
|
for i in range(len(ev.sendcan)): |
||||||
|
attrs = ['address', 'busTime', 'dat', 'src'] |
||||||
|
for attr in attrs: |
||||||
|
self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old')) |
||||||
|
|
||||||
|
# Can |
||||||
|
m_old = boardd_old.can_list_to_can_capnp(can_list, 'can').to_bytes() |
||||||
|
# new API |
||||||
|
m = boardd.can_list_to_can_capnp(can_list, 'can') |
||||||
|
|
||||||
|
ev_old = log.Event.from_bytes(m_old) |
||||||
|
ev = log.Event.from_bytes(m) |
||||||
|
self.assertEqual(ev_old.which(), ev.which()) |
||||||
|
self.assertEqual(len(ev.can), len(ev_old.can)) |
||||||
|
for i in range(len(ev.can)): |
||||||
|
attrs = ['address', 'busTime', 'dat', 'src'] |
||||||
|
for attr in attrs: |
||||||
|
self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old')) |
||||||
|
|
||||||
|
def test_performance(self): |
||||||
|
can_list, cnt = generate_random_can_data_list() |
||||||
|
recursions = 1000 |
||||||
|
|
||||||
|
n1 = sec_since_boot() |
||||||
|
for i in range(recursions): |
||||||
|
boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes() |
||||||
|
n2 = sec_since_boot() |
||||||
|
elapsed_old = n2 - n1 |
||||||
|
|
||||||
|
# print('Old API, elapsed time: {} secs'.format(elapsed_old)) |
||||||
|
n1 = sec_since_boot() |
||||||
|
for i in range(recursions): |
||||||
|
boardd.can_list_to_can_capnp(can_list) |
||||||
|
n2 = sec_since_boot() |
||||||
|
elapsed_new = n2 - n1 |
||||||
|
# print('New API, elapsed time: {} secs'.format(elapsed_new)) |
||||||
|
self.assertTrue(elapsed_new < elapsed_old / 2) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__': |
||||||
|
unittest.main() |
||||||
@ -0,0 +1,47 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
"""Run boardd with the BOARDD_LOOPBACK envvar before running this test.""" |
||||||
|
|
||||||
|
import os |
||||||
|
import random |
||||||
|
import time |
||||||
|
|
||||||
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||||
|
from cereal.messaging import drain_sock, pub_sock, sub_sock |
||||||
|
|
||||||
|
def get_test_string(): |
||||||
|
return b"test"+os.urandom(10) |
||||||
|
|
||||||
|
BUS = 0 |
||||||
|
|
||||||
|
def main(): |
||||||
|
rcv = sub_sock('can') # port 8006 |
||||||
|
snd = pub_sock('sendcan') # port 8017 |
||||||
|
time.sleep(0.3) # wait to bind before send/recv |
||||||
|
|
||||||
|
for i in range(10): |
||||||
|
print("Loop %d" % i) |
||||||
|
at = random.randint(1024, 2000) |
||||||
|
st = get_test_string()[0:8] |
||||||
|
snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes()) |
||||||
|
time.sleep(0.1) |
||||||
|
res = drain_sock(rcv, True) |
||||||
|
assert len(res) == 1 |
||||||
|
|
||||||
|
res = res[0].can |
||||||
|
assert len(res) == 2 |
||||||
|
|
||||||
|
msg0, msg1 = res |
||||||
|
|
||||||
|
assert msg0.dat == st |
||||||
|
assert msg1.dat == st |
||||||
|
|
||||||
|
assert msg0.address == at |
||||||
|
assert msg1.address == at |
||||||
|
|
||||||
|
assert msg0.src == 0x80 | BUS |
||||||
|
assert msg1.src == BUS |
||||||
|
|
||||||
|
print("Success") |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
Loading…
Reference in new issue