From ef93a715e113405708b11bfb922374ca74d8ef72 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Fri, 17 Jan 2020 10:50:44 -0800 Subject: [PATCH] selfdrive/boardd --- selfdrive/boardd/.gitignore | 2 + selfdrive/boardd/SConscript | 9 + selfdrive/boardd/__init__.py | 0 selfdrive/boardd/boardd.cc | 932 ++++++++++++++++++ selfdrive/boardd/boardd.py | 12 + selfdrive/boardd/boardd_api_impl.pyx | 26 + selfdrive/boardd/boardd_setup.py | 31 + selfdrive/boardd/can_list_to_can_capnp.cc | 37 + selfdrive/boardd/tests/__init__.py | 0 selfdrive/boardd/tests/boardd_old.py | 246 +++++ selfdrive/boardd/tests/fuzzer.py | 20 + selfdrive/boardd/tests/replay_many.py | 68 ++ selfdrive/boardd/tests/test_boardd_api.py | 77 ++ .../boardd/tests/test_boardd_loopback.py | 47 + 14 files changed, 1507 insertions(+) create mode 100644 selfdrive/boardd/.gitignore create mode 100644 selfdrive/boardd/SConscript create mode 100644 selfdrive/boardd/__init__.py create mode 100644 selfdrive/boardd/boardd.cc create mode 100644 selfdrive/boardd/boardd.py create mode 100644 selfdrive/boardd/boardd_api_impl.pyx create mode 100644 selfdrive/boardd/boardd_setup.py create mode 100644 selfdrive/boardd/can_list_to_can_capnp.cc create mode 100644 selfdrive/boardd/tests/__init__.py create mode 100755 selfdrive/boardd/tests/boardd_old.py create mode 100755 selfdrive/boardd/tests/fuzzer.py create mode 100755 selfdrive/boardd/tests/replay_many.py create mode 100644 selfdrive/boardd/tests/test_boardd_api.py create mode 100755 selfdrive/boardd/tests/test_boardd_loopback.py diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore new file mode 100644 index 000000000..1f653bde8 --- /dev/null +++ b/selfdrive/boardd/.gitignore @@ -0,0 +1,2 @@ +boardd +boardd_api_impl.cpp diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript new file mode 100644 index 000000000..14c1ff780 --- /dev/null +++ b/selfdrive/boardd/SConscript @@ -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") + diff --git a/selfdrive/boardd/__init__.py b/selfdrive/boardd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc new file mode 100644 index 000000000..37ee22e70 --- /dev/null +++ b/selfdrive/boardd/boardd.cc @@ -0,0 +1,932 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#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 + +// 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((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(); + + 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(); + 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(); + 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((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + 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((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(); + + 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(); + 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); +} diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py new file mode 100644 index 000000000..527f1f4f5 --- /dev/null +++ b/selfdrive/boardd/boardd.py @@ -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 diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx new file mode 100644 index 000000000..3f50fbaab --- /dev/null +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -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 diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py new file mode 100644 index 000000000..f987c7aa2 --- /dev/null +++ b/selfdrive/boardd/boardd_setup.py @@ -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"], + ) + ) +) diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc new file mode 100644 index 000000000..8ba9d3d56 --- /dev/null +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -0,0 +1,37 @@ +#include +#include +#include +#include "common/timing.h" +#include +#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_list, std::string &out, bool sendCan, bool valid) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + 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()); +} + +} diff --git a/selfdrive/boardd/tests/__init__.py b/selfdrive/boardd/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py new file mode 100755 index 000000000..48c7a9459 --- /dev/null +++ b/selfdrive/boardd/tests/boardd_old.py @@ -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() diff --git a/selfdrive/boardd/tests/fuzzer.py b/selfdrive/boardd/tests/fuzzer.py new file mode 100755 index 000000000..bb58f8b5e --- /dev/null +++ b/selfdrive/boardd/tests/fuzzer.py @@ -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) diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py new file mode 100755 index 000000000..024867769 --- /dev/null +++ b/selfdrive/boardd/tests/replay_many.py @@ -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 diff --git a/selfdrive/boardd/tests/test_boardd_api.py b/selfdrive/boardd/tests/test_boardd_api.py new file mode 100644 index 000000000..f41e1e357 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_api.py @@ -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() diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py new file mode 100755 index 000000000..cb88776a0 --- /dev/null +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -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()