From 562394e2aeaab43841b754caaad160916c0cbe55 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 31 Jul 2020 23:14:31 +0200 Subject: [PATCH] Panda abstraction layer in boardd (#1919) * start on panda abstraction layer * handle bulk transfer in library * Safety model abstraction * Abstract hw type into library * Handle disconnect * RTC stuff * Fan control * Health * Panda fw version * Read serial * Power saving * Power mode * Cleanup pigeon thread init * Rename safety setter variable name * Remove comment * Unused global cleanup * malloc -> new * whitespace * Use std::thread * Use std::thread for safety setter * Cleanup igntion_last global * Heartbeat * logd * More global cleanup * Put back sleep * ir pwr * Always broadcast health * init struct with zeroes * Fix eon build * fix race condition * fix Adeebs comments * abstract can send and receive old-commit-hash: 80acb328252d8ed80c18a98457c6d788073ed26e --- release/files_common | 2 + selfdrive/boardd/SConscript | 3 +- selfdrive/boardd/boardd.cc | 837 ++++++------------ selfdrive/boardd/panda.cc | 314 +++++++ selfdrive/boardd/panda.h | 78 ++ .../boardd/tests/test_boardd_loopback.py | 3 +- 6 files changed, 657 insertions(+), 580 deletions(-) create mode 100644 selfdrive/boardd/panda.cc create mode 100644 selfdrive/boardd/panda.h diff --git a/release/files_common b/release/files_common index 01d743c482..8f926ac3c3 100644 --- a/release/files_common +++ b/release/files_common @@ -88,6 +88,8 @@ selfdrive/boardd/boardd.py selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/boardd_setup.py selfdrive/boardd/can_list_to_can_capnp.cc +selfdrive/boardd/panda.cc +selfdrive/boardd/panda.h selfdrive/car/__init__.py selfdrive/car/car_helpers.py diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index b29b886369..0ccbe6bd24 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,9 +1,8 @@ Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies') -env.Program('boardd.cc', LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']) +env.Program('boardd', ['boardd.cc', 'panda.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'], cython_dependencies + ['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/boardd.cc b/selfdrive/boardd/boardd.cc index 58f994e050..df865db27a 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -8,11 +7,15 @@ #include #include #include -#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include @@ -24,12 +27,8 @@ #include "common/timing.h" #include "messaging.hpp" -#include -#include +#include "panda.h" -// double the FIFO size -#define RECV_SIZE (0x1000) -#define TIMEOUT 0 #define MAX_IR_POWER 0.5f #define MIN_IR_POWER 0.0f @@ -38,85 +37,47 @@ #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 = NULL; -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; -float voltage_f = 12.5; // filtered voltage -uint32_t no_ignition_cnt = 0; -bool connected_once = false; -bool ignition_last = false; - #ifndef __x86_64__ 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; #endif -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; +namespace { -int usb_write(libusb_device_handle* dev_handle, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, pthread_mutex_t *lock=NULL, unsigned int timeout=TIMEOUT) { - const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; +Panda * panda = NULL; +std::atomic safety_setter_thread_running(false); +volatile sig_atomic_t do_exit = 0; +bool spoofing_started = false; +bool fake_send = false; +bool connected_once = false; - if (lock) pthread_mutex_lock(lock); - int err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); - if (lock) pthread_mutex_unlock(lock); - return err; -} +struct tm get_time(){ + time_t rawtime; + time(&rawtime); -int usb_read(libusb_device_handle* dev_handle, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, pthread_mutex_t *lock=NULL, unsigned int timeout=TIMEOUT) { - const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + struct tm sys_time; + gmtime_r(&rawtime, &sys_time); - if (lock) pthread_mutex_lock(lock); - int err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); - if (lock) pthread_mutex_unlock(lock); - return err; + return sys_time; } -void usb_close(libusb_device_handle* &dev_handle) { - if (!dev_handle) { - return; - } - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - dev_handle = NULL; +bool time_valid(struct tm sys_time){ + return 1900 + sys_time.tm_year >= 2019; } -void pigeon_init(); -void *pigeon_thread(void *crap); - -void *safety_setter_thread(void *s) { +void safety_setter_thread() { + LOGD("Starting safety setter thread"); // diagnostic only is the default, needed for VIN query - usb_write(dev_handle, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, &usb_lock); + panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327); // switch to SILENT when CarVin param is read while (1) { - if (do_exit) return NULL; + if (do_exit || !panda->connected){ + safety_setter_thread_running = false; + return; + }; + std::vector value_vin = read_db_bytes("CarVin"); if (value_vin.size() > 0) { // sanity check VIN format @@ -129,12 +90,15 @@ void *safety_setter_thread(void *s) { } // VIN query done, stop listening to OBDII - usb_write(dev_handle, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, &usb_lock); + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); std::vector params; LOGW("waiting for params to set safety model"); while (1) { - if (do_exit) return NULL; + if (do_exit || !panda->connected){ + safety_setter_thread_running = false; + return; + }; params = read_db_bytes("CarParams"); if (params.size() > 0) break; @@ -148,444 +112,103 @@ void *safety_setter_thread(void *s) { capnp::FlatArrayMessageReader cmsg(amsg); cereal::CarParams::Reader car_params = cmsg.getRoot(); + cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel(); - 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; + LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param); - usb_write(dev_handle, 0xdc, safety_model, safety_param); + panda->set_safety_model(safety_model, safety_param); - pthread_mutex_unlock(&usb_lock); - - return NULL; + safety_setter_thread_running = false; } -// 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; - - usb_close(dev_handle); - - 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; } + try { + assert(panda == NULL); + panda = new Panda(); + } catch (std::exception &e) { + return false; + } - if (loopback_can) { - usb_write(dev_handle, 0xe5, 1, 0); + if (getenv("BOARDD_LOOPBACK")) { + panda->set_loopback(true); } - // get panda fw - err = usb_read(dev_handle, 0xd3, 0, 0, fw_sig_buf, 64); - err2 = usb_read(dev_handle, 0xd4, 0, 0, fw_sig_buf + 64, 64); - if ((err == 64) && (err2 == 64)) { - printf("FW signature read\n"); - write_db_value("PandaFirmware", (const char *)fw_sig_buf, 128); + const char *fw_sig_buf = panda->get_firmware_version(); + if (fw_sig_buf){ + write_db_value("PandaFirmware", fw_sig_buf, 128); + // Convert to hex for offroad + char fw_sig_hex_buf[16] = {0}; 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); + fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4); + fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF); } - write_db_value("PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16); - } - else { goto fail; } + + write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16); + LOGW("fw signature: %.*s", 16, fw_sig_hex_buf); + + delete[] fw_sig_buf; + } else { return false; } // get panda serial - err = usb_read(dev_handle, 0xd0, 0, 0, serial_buf, 16); + const char *serial_buf = panda->get_serial(); + if (serial_buf) { + size_t serial_sz = strnlen(serial_buf, 16); - if (err > 0) { - serial = (const char *)serial_buf; - serial_sz = strnlen(serial, err); - write_db_value("PandaDongleId", serial, serial_sz); - printf("panda serial: %.*s\n", serial_sz, serial); - } - else { goto fail; } + write_db_value("PandaDongleId", serial_buf, serial_sz); + LOGW("panda serial: %.*s", serial_sz, serial_buf); + + delete[] serial_buf; + } else { return false; } // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton #ifndef __x86_64__ if (!connected_once) { - usb_write(dev_handle, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0); + panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); } #endif - connected_once = true; - - usb_read(dev_handle, 0xc1, 0, 0, hw_query, 1); - - 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_r(&rawtime, &sys_time); + if (panda->has_rtc){ + struct tm sys_time = get_time(); + struct tm rtc_time = panda->get_rtc(); - // Get time from RTC - timestamp_t rtc_time; - usb_read(dev_handle, 0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); - - //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){ + if (!time_valid(sys_time) && time_valid(rtc_time)) { 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}; + const struct timeval tv = {mktime(&rtc_time), 0}; settimeofday(&tv, 0); } } + connected_once = true; return true; -fail: - return false; } // must be called before threads or with mutex void usb_retry_connect() { - LOG("attempting to connect"); + LOGW("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 == LIBUSB_ERROR_NO_DEVICE) { - LOGE("lost connection"); - usb_retry_connect(); - } - // TODO: check other errors, is simply retrying okay? -} - void can_recv(PubMaster &pm) { - int err; - uint32_t data[RECV_SIZE/4]; - int recv; - 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 == LIBUSB_ERROR_OVERFLOW) { LOGE_100("overflow got 0x%x", recv); }; - - // timeout is okay to exit, recv still happened - if (err == LIBUSB_ERROR_TIMEOUT) { 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); - } - - pm.send("can", msg); -} -void can_health(PubMaster &pm) { - 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; - - // create message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto healthData = event.initHealth(); - - bool received = false; - - // recv from board - if (dev_handle != NULL) { - cnt = usb_read(dev_handle, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), &usb_lock); - received = (cnt == sizeof(health)); - } - - // No panda connected, send empty health packet - if (!received){ - healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); - pm.send("health", msg); - return; - } - - 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)) { - usb_write(dev_handle, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, &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) { - std::vector disable_power_down = read_db_bytes("DisablePowerDown"); - if (disable_power_down.size() != 1 || disable_power_down[0] != '1') { - printf("TURN OFF CHARGING!\n"); - usb_write(dev_handle, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, &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"); - usb_write(dev_handle, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, &usb_lock); - } - // set power save state enabled when car is off and viceversa when it's on - if (ignition && (health.power_save_enabled == 1)) { - usb_write(dev_handle, 0xe7, 0, 0, &usb_lock); - } - if (!ignition && (health.power_save_enabled == 0)) { - usb_write(dev_handle, 0xe7, 1, 0, &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))) { - usb_write(dev_handle, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, &usb_lock); + int recv = panda->can_receive(event); + if (recv){ + pm.send("can", msg); } -#endif - - // clear VIN, CarParams, and set new safety on car start - if (ignition && !ignition_last) { - int result = delete_db_value("CarVin"); - assert((result == 0) || (result == ERR_NO_VALUE)); - result = delete_db_value("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; - - usb_read(dev_handle, 0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm), &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_r(&rawtime, &sys_time); - - // Write time to RTC if it looks reasonable - if (1900 + sys_time.tm_year >= 2019){ - pthread_mutex_lock(&usb_lock); - usb_write(dev_handle, 0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); - usb_write(dev_handle, 0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); - usb_write(dev_handle, 0xa3, (uint16_t)sys_time.tm_mday, 0); - // usb_write(dev_handle, 0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); - usb_write(dev_handle, 0xa5, (uint16_t)sys_time.tm_hour, 0); - usb_write(dev_handle, 0xa6, (uint16_t)sys_time.tm_min, 0); - usb_write(dev_handle, 0xa7, (uint16_t)sys_time.tm_sec, 0); - pthread_mutex_unlock(&usb_lock); - } - } - - ignition_last = ignition; - - // 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)); - - // Convert faults bitset to capnp list - std::bitset fault_bits(health.faults); - auto faults = healthData.initFaults(fault_bits.count()); - - size_t i = 0; - for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_KLINE_INIT); f++){ - if (fault_bits.test(f)) { - faults.set(i, cereal::HealthData::FaultType(f)); - i++; - } - } - // send to health - pm.send("health", msg); - - // send heartbeat back to panda - usb_write(dev_handle, 0xf3, 1, 0, &usb_lock); } - -void can_send(cereal::Event::Reader &event) { - int err; - // recv from sendcan - if (nanos_since_boot() - event.getLogMonoTime() > 1e9) { - //Older than 1 second. Dont send. - return; - } - - auto can_data_list = event.getSendcan(); - int msg_count = can_data_list.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 = can_data_list[i]; - if (cmsg.getAddress() >= 0x800) { - // extended - send[i*4] = (cmsg.getAddress() << 3) | 5; - } else { - // normal - send[i*4] = (cmsg.getAddress() << 21) | 1; - } - auto can_data = cmsg.getDat(); - assert(can_data.size() <= 8); - send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4); - memcpy(&send[i*4+2], can_data.begin(), can_data.size()); - } - - // send to board - int sent; - pthread_mutex_lock(&usb_lock); - - if (!fake_send) { - do { - // Try sending can messages. If the receive buffer on the panda is full it will NAK - // and libusb will try again. After 5ms, it will time out. We will drop the messages. - err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, 5); - if (err == LIBUSB_ERROR_TIMEOUT) { - LOGW("Transmit buffer full"); - break; - } else if (err != 0 || msg_count*0x10 != sent) { - LOGW("Error"); - handle_usb_issue(err, __func__); - } - } while(err != 0); - } - - pthread_mutex_unlock(&usb_lock); - - // done - free(send); -} - -// **** threads **** - -void *can_send_thread(void *crap) { +void can_send_thread() { LOGD("start send thread"); Context * context = Context::create(); @@ -594,7 +217,7 @@ void *can_send_thread(void *crap) { subscriber->setTimeout(100); // run as fast as messages come in - while (!do_exit) { + while (!do_exit && panda->connected) { Message * msg = subscriber->receive(); if (!msg){ @@ -609,17 +232,22 @@ void *can_send_thread(void *crap) { capnp::FlatArrayMessageReader cmsg(amsg); cereal::Event::Reader event = cmsg.getRoot(); - can_send(event); + + //Dont send if older than 1 second + if (nanos_since_boot() - event.getLogMonoTime() < 1e9) { + if (!fake_send){ + panda->can_send(event.getSendcan()); + } + } + delete msg; } delete subscriber; delete context; - - return NULL; } -void *can_recv_thread(void *crap) { +void can_recv_thread() { LOGD("start recv thread"); // can = 8006 @@ -629,7 +257,7 @@ void *can_recv_thread(void *crap) { const uint64_t dt = 10000000ULL; uint64_t next_frame_time = nanos_since_boot() + dt; - while (!do_exit) { + while (!do_exit && panda->connected) { can_recv(pm); uint64_t cur_time = nanos_since_boot(); @@ -644,34 +272,157 @@ void *can_recv_thread(void *crap) { next_frame_time += dt; } - return NULL; } -void *can_health_thread(void *crap) { +void can_health_thread() { LOGD("start health thread"); - // health = 8011 PubMaster pm({"health"}); - // run at 2hz - while (!do_exit) { - can_health(pm); + uint32_t no_ignition_cnt = 0; + bool ignition_last = false; + float voltage_f = 12.5; // filtered voltage + + // Broadcast empty health message when panda is not yet connected + while (!panda){ + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto healthData = event.initHealth(); + + healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); + pm.send("health", msg); usleep(500*1000); } - return NULL; + // run at 2hz + while (!do_exit && panda->connected) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto healthData = event.initHealth(); + + health_t health = panda->get_health(); + + 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)) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } + + bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); + + if (ignition) { + no_ignition_cnt = 0; + } else { + no_ignition_cnt += 1; + } + +#ifdef QCOM + 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) { + std::vector disable_power_down = read_db_bytes("DisablePowerDown"); + if (disable_power_down.size() != 1 || disable_power_down[0] != '1') { + LOGW("TURN OFF CHARGING!\n"); + panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT); + LOGW("POWER DOWN DEVICE\n"); + system("service call power 17 i32 0 i32 1"); + } + } + if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) { + LOGW("TURN ON CHARGING!\n"); + panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); + } +#endif + +#ifndef __x86_64__ + bool power_save_desired = !ignition; + if (health.power_save_enabled != power_save_desired){ + panda->set_power_saving(power_save_desired); + } + + // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect + if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } +#endif + + // clear VIN, CarParams, and set new safety on car start + if (ignition && !ignition_last) { + int result = delete_db_value("CarVin"); + assert((result == 0) || (result == ERR_NO_VALUE)); + result = delete_db_value("CarParams"); + assert((result == 0) || (result == ERR_NO_VALUE)); + + if (!safety_setter_thread_running) { + safety_setter_thread_running = true; + std::thread(safety_setter_thread).detach(); + } else { + LOGW("Safety setter thread already running"); + } + } + + // Write to rtc once per minute when no ignition present + if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){ + // Write time to RTC if it looks reasonable + struct tm sys_time = get_time(); + if (time_valid(sys_time)){ + panda->set_rtc(sys_time); + } + } + + ignition_last = ignition; + uint16_t fan_speed_rpm = panda->get_fan_speed(); + + // 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(panda->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(panda->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)); + + // Convert faults bitset to capnp list + std::bitset fault_bits(health.faults); + auto faults = healthData.initFaults(fault_bits.count()); + + size_t i = 0; + for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION); + f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_KLINE_INIT); f++){ + if (fault_bits.test(f)) { + faults.set(i, cereal::HealthData::FaultType(f)); + i++; + } + } + pm.send("health", msg); + panda->send_heartbeat(); + usleep(500*1000); + } } -void *hardware_control_thread(void *crap) { +void hardware_control_thread() { LOGD("start hardware control thread"); SubMaster sm({"thermal", "frontFrame"}); - // 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; - + if (panda->hw_type != cereal::HealthData::HwType::UNO) return; uint64_t last_front_frame_t = 0; uint16_t prev_fan_speed = 999; @@ -679,13 +430,14 @@ void *hardware_control_thread(void *crap) { uint16_t prev_ir_pwr = 999; unsigned int cnt = 0; - while (!do_exit) { + while (!do_exit && panda->connected) { cnt++; - sm.update(1000); + sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? + if (sm.updated("thermal")){ uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); if (fan_speed != prev_fan_speed || cnt % 100 == 0){ - usb_write(dev_handle, 0xb1, fan_speed, 0, &usb_lock); + panda->set_fan_speed(fan_speed); prev_fan_speed = fan_speed; } } @@ -709,59 +461,32 @@ void *hardware_control_thread(void *crap) { } if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){ - usb_write(dev_handle, 0xb0, ir_pwr, 0, &usb_lock); + panda->set_ir_pwr(ir_pwr); prev_ir_pwr = ir_pwr; } } - - return NULL; } #define pigeon_send(x) _pigeon_send(x, sizeof(x)-1) - -void hexdump(unsigned char *d, int l) __attribute__((unused)); -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+1]; - int err; a[0] = 1; for (int i=0; iusb_bulk_write(2, a, ll+1); } } void pigeon_set_power(int power) { - pthread_mutex_lock(&usb_lock); - int err = usb_write(dev_handle, 0xd9, power, 0); - if (err < 0) { handle_usb_issue(err, __func__); } - pthread_mutex_unlock(&usb_lock); + panda->usb_write(0xd9, power, 0); } void pigeon_set_baud(int baud) { - int err; - pthread_mutex_lock(&usb_lock); - err = usb_write(dev_handle, 0xe2, 1, 0); - if (err < 0) { handle_usb_issue(err, __func__); } - err = usb_write(dev_handle, 0xe4, 1, baud/300); - if (err < 0) { handle_usb_issue(err, __func__); } - pthread_mutex_unlock(&usb_lock); + panda->usb_write(0xe2, 1, 0); + panda->usb_write(0xe4, 1, baud/300); } void pigeon_init() { @@ -824,24 +549,22 @@ static void pigeon_publish_raw(PubMaster &pm, unsigned char *dat, int alen) { } -void *pigeon_thread(void *crap) { +void pigeon_thread() { + if (!panda->is_pigeon){ return; }; + // ubloxRaw = 8042 PubMaster pm({"ubloxRaw"}); // run at ~100hz unsigned char dat[0x1000]; uint64_t cnt = 0; - while (!do_exit) { - if (pigeon_needs_init) { - pigeon_needs_init = false; - pigeon_init(); - } + + pigeon_init(); + + while (!do_exit && panda->connected) { int alen = 0; while (alen < 0xfc0) { - pthread_mutex_lock(&usb_lock); - int len = usb_read(dev_handle, 0xe0, 1, 0, dat+alen, 0x40); - if (len < 0) { handle_usb_issue(len, __func__); } - pthread_mutex_unlock(&usb_lock); + int len = panda->usb_read(0xe0, 1, 0, dat+alen, 0x40); if (len <= 0) break; //printf("got %d\n", len); @@ -860,7 +583,6 @@ void *pigeon_thread(void *crap) { usleep(10*1000); cnt++; } - return NULL; } } @@ -884,60 +606,21 @@ int main() { fake_send = true; } - if (getenv("BOARDD_LOOPBACK")){ - loopback_can = true; - } + while (!do_exit){ + std::vector threads; + threads.push_back(std::thread(can_health_thread)); - err = pthread_mutex_init(&usb_lock, NULL); - assert(err == 0); + // connect to the board + usb_retry_connect(); - // init libusb - err = libusb_init(&ctx); - assert(err == 0); + threads.push_back(std::thread(can_send_thread)); + threads.push_back(std::thread(can_recv_thread)); + threads.push_back(std::thread(hardware_control_thread)); + threads.push_back(std::thread(pigeon_thread)); -#if LIBUSB_API_VERSION >= 0x01000106 - libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); -#else - libusb_set_debug(ctx, 3); -#endif + for (auto &t : threads) t.join(); - pthread_t can_health_thread_handle; - err = pthread_create(&can_health_thread_handle, NULL, - can_health_thread, NULL); - assert(err == 0); - - // connect to the board - pthread_mutex_lock(&usb_lock); - usb_retry_connect(); - pthread_mutex_unlock(&usb_lock); - - // create threads - 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); - - // destruct libusb - usb_close(dev_handle); - libusb_exit(ctx); + delete panda; + panda = NULL; + } } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc new file mode 100644 index 0000000000..8bec943457 --- /dev/null +++ b/selfdrive/boardd/panda.cc @@ -0,0 +1,314 @@ +#include +#include +#include + +#include "common/swaglog.h" + +#include "panda.h" + +Panda::Panda(){ + int err; + + err = pthread_mutex_init(&usb_lock, NULL); + if (err != 0) { goto fail; } + + // init libusb + err = libusb_init(&ctx); + if (err != 0) { goto fail; } + +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else + libusb_set_debug(ctx, 3); +#endif + + 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; } + + hw_type = get_hw_type(); + is_pigeon = + (hw_type == cereal::HealthData::HwType::GREY_PANDA) || + (hw_type == cereal::HealthData::HwType::BLACK_PANDA) || + (hw_type == cereal::HealthData::HwType::UNO); + has_rtc = (hw_type == cereal::HealthData::HwType::UNO); + + return; + +fail: + cleanup(); + throw std::runtime_error("Error connecting to panda"); +} + +Panda::~Panda(){ + pthread_mutex_lock(&usb_lock); + cleanup(); + connected = false; + pthread_mutex_unlock(&usb_lock); +} + +void Panda::cleanup(){ + if (dev_handle){ + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + } + + if (ctx) { + libusb_exit(ctx); + } +} + +void Panda::handle_usb_issue(int err, const char func[]) { + LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); + if (err == LIBUSB_ERROR_NO_DEVICE) { + LOGE("lost connection"); + connected = false; + } + // TODO: check other errors, is simply retrying okay? +} + +int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + pthread_mutex_lock(&usb_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + + pthread_mutex_unlock(&usb_lock); + + return err; +} + +int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + pthread_mutex_lock(&usb_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); + if (err < 0) handle_usb_issue(err, __func__); + } while (err < 0 && connected); + pthread_mutex_unlock(&usb_lock); + + return err; +} + +int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + pthread_mutex_lock(&usb_lock); + do { + // Try sending can messages. If the receive buffer on the panda is full it will NAK + // and libusb will try again. After 5ms, it will time out. We will drop the messages. + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + LOGW("Transmit buffer full"); + break; + } else if (err != 0 || length != transferred) { + handle_usb_issue(err, __func__); + } + } while(err != 0 && connected); + + pthread_mutex_unlock(&usb_lock); + return transferred; +} + +int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + pthread_mutex_lock(&usb_lock); + + do { + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + break; // timeout is okay to exit, recv still happened + } else if (err == LIBUSB_ERROR_OVERFLOW) { + LOGE_100("overflow got 0x%x", transferred); + } else if (err != 0) { + handle_usb_issue(err, __func__); + } + + } while(err != 0 && connected); + + pthread_mutex_unlock(&usb_lock); + + return transferred; +} + +void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param){ + usb_write(0xdc, (uint16_t)safety_model, safety_param); +} + +cereal::HealthData::HwType Panda::get_hw_type() { + unsigned char hw_query[1] = {0}; + + usb_read(0xc1, 0, 0, hw_query, 1); + return (cereal::HealthData::HwType)(hw_query[0]); +} + +void Panda::set_rtc(struct tm sys_time){ + // tm struct has year defined as years since 1900 + usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); + usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); + usb_write(0xa3, (uint16_t)sys_time.tm_mday, 0); + // usb_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); + usb_write(0xa5, (uint16_t)sys_time.tm_hour, 0); + usb_write(0xa6, (uint16_t)sys_time.tm_min, 0); + usb_write(0xa7, (uint16_t)sys_time.tm_sec, 0); +} + +struct tm Panda::get_rtc(){ + struct __attribute__((packed)) timestamp_t { + uint16_t year; // Starts at 0 + uint8_t month; + uint8_t day; + uint8_t weekday; + uint8_t hour; + uint8_t minute; + uint8_t second; + } rtc_time = {0}; + + usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); + + struct tm new_time = { 0 }; + new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 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; + + return new_time; +} + +void Panda::set_fan_speed(uint16_t fan_speed){ + usb_write(0xb1, fan_speed, 0); +} + +uint16_t Panda::get_fan_speed(){ + uint16_t fan_speed_rpm = 0; + usb_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); + return fan_speed_rpm; +} + +void Panda::set_ir_pwr(uint16_t ir_pwr) { + usb_write(0xb0, ir_pwr, 0); +} + +health_t Panda::get_health(){ + health_t health {0}; + usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + return health; +} + +void Panda::set_loopback(bool loopback){ + usb_write(0xe5, loopback, 0); +} + +const char* Panda::get_firmware_version(){ + const char* fw_sig_buf = new char[128](); + + int read_1 = usb_read(0xd3, 0, 0, (unsigned char*)fw_sig_buf, 64); + int read_2 = usb_read(0xd4, 0, 0, (unsigned char*)fw_sig_buf + 64, 64); + + if ((read_1 == 64) && (read_2 == 64)) { + return fw_sig_buf; + } + + delete[] fw_sig_buf; + return NULL; +} + +const char* Panda::get_serial(){ + const char* serial_buf = new char[16](); + + int err = usb_read(0xd0, 0, 0, (unsigned char*)serial_buf, 16); + + if (err >= 0) { + return serial_buf; + } + + delete[] serial_buf; + return NULL; + +} + +void Panda::set_power_saving(bool power_saving){ + usb_write(0xe7, power_saving, 0); +} + +void Panda::set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode){ + usb_write(0xe6, (uint16_t)power_mode, 0); +} + +void Panda::send_heartbeat(){ + usb_write(0xf3, 1, 0); +} + +void Panda::can_send(capnp::List::Reader can_data_list){ + int msg_count = can_data_list.size(); + + uint32_t *send = new uint32_t[msg_count*0x10](); + + for (int i = 0; i < msg_count; i++) { + auto cmsg = can_data_list[i]; + if (cmsg.getAddress() >= 0x800) { // extended + send[i*4] = (cmsg.getAddress() << 3) | 5; + } else { // normal + send[i*4] = (cmsg.getAddress() << 21) | 1; + } + auto can_data = cmsg.getDat(); + assert(can_data.size() <= 8); + send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4); + memcpy(&send[i*4+2], can_data.begin(), can_data.size()); + } + + usb_bulk_write(3, (unsigned char*)send, msg_count*0x10, 5); + + delete[] send; +} + +int Panda::can_receive(cereal::Event::Builder &event){ + uint32_t data[RECV_SIZE/4]; + int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE); + + // return if length is 0 + if (recv <= 0) { + return 0; + } else if (recv == RECV_SIZE) { + LOGW("Receive buffer full"); + } + + 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); + } + + return recv; +} diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h new file mode 100644 index 0000000000..3586cda74f --- /dev/null +++ b/selfdrive/boardd/panda.h @@ -0,0 +1,78 @@ +#pragma once + +#include +#include +#include + +#include + +#include "cereal/gen/cpp/car.capnp.h" +#include "cereal/gen/cpp/log.capnp.h" + +// double the FIFO size +#define RECV_SIZE (0x1000) +#define TIMEOUT 0 + +// copied from panda/board/main.c +struct __attribute__((packed)) health_t { + 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; +}; + +class Panda { + private: + libusb_context *ctx = NULL; + libusb_device_handle *dev_handle = NULL; + pthread_mutex_t usb_lock; + void handle_usb_issue(int err, const char func[]); + void cleanup(); + + public: + Panda(); + ~Panda(); + + bool connected = true; + cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; + bool is_pigeon = false; + bool has_rtc = false; + + // HW communication + int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT); + int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT); + int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + + // Panda functionality + cereal::HealthData::HwType get_hw_type(); + void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0); + void set_rtc(struct tm sys_time); + struct tm get_rtc(); + void set_fan_speed(uint16_t fan_speed); + uint16_t get_fan_speed(); + void set_ir_pwr(uint16_t ir_pwr); + health_t get_health(); + void set_loopback(bool loopback); + const char* get_firmware_version(); + const char* get_serial(); + void set_power_saving(bool power_saving); + void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode); + void send_heartbeat(); + void can_send(capnp::List::Reader can_data_list); + int can_receive(cereal::Event::Builder &event); + +}; diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 8081984b30..d6c110bbbe 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -8,6 +8,7 @@ from functools import wraps import cereal.messaging as messaging from cereal import car from common.basedir import PARAMS +from common.android import ANDROID from common.params import Params from common.spinner import Spinner from panda import Panda @@ -35,7 +36,7 @@ os.environ['PARAMS_PATH'] = PARAMS @with_processes(['boardd']) def test_boardd_loopback(): # wait for boardd to init - spinner = Spinner() + spinner = Spinner(noop=(not ANDROID)) time.sleep(2) # boardd blocks on CarVin and CarParams