diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 00000000000000..e372b2a30775ff --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,41 @@ +version: 2 +jobs: + safety: + machine: + docker_layer_caching: true + steps: + - checkout + - run: + name: Build image + command: "docker build -t panda_safety -f tests/safety/Dockerfile ." + - run: + name: Run safety test + command: | + docker run panda_safety /bin/bash -c "cd /panda/tests/safety; ./test.sh" + build: + machine: + docker_layer_caching: true + steps: + - checkout + - run: + name: Build image + command: "docker build -t panda_build -f tests/build/Dockerfile ." + - run: + name: Test python package installer + command: | + docker run panda_build /bin/bash -c "cd /panda; python setup.py install" + - run: + name: Build STM image + command: | + docker run panda_build /bin/bash -c "cd /panda/board; make bin" + - run: + name: Build ESP image + command: | + docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin" + +workflows: + version: 2 + main: + jobs: + - safety + - build diff --git a/.gitignore b/.gitignore index c589cb1c808100..7b7762ef490663 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,8 @@ .*.swp .*.swo *.o +*.so +*.d a.out *~ .#* diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 603848fd2434c3..00000000000000 --- a/.travis.yml +++ /dev/null @@ -1,20 +0,0 @@ -language: python - -cache: - directories: - - build/commaai/panda/boardesp/esp-open-sdk/crosstool-NG - -addons: - apt: - packages: - - gcc-arm-none-eabi - - libnewlib-arm-none-eabi - - gperf - - texinfo - - help2man - -script: - - python setup.py install - - pushd board && make bin && popd - - pushd boardesp && git clone --recursive https://github.com/pfalcon/esp-open-sdk.git && pushd esp-open-sdk && git checkout 03f5e898a059451ec5f3de30e7feff30455f7cec && LD_LIBRARY_PATH="" make STANDALONE=y && popd && popd - - pushd boardesp && make user1.bin && popd diff --git a/README.md b/README.md index 85de25c905f14f..c6c8f8ac892f04 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ It uses an [STM32F413](http://www.st.com/en/microcontrollers/stm32f413-423.html? It is 2nd gen hardware, reusing code and parts from the [NEO](https://github.com/commaai/neo) interface board. -[![Build Status](https://travis-ci.org/commaai/panda.svg?branch=master)](https://travis-ci.org/commaai/panda) +[![CircleCI](https://circleci.com/gh/commaai/panda.svg?style=svg)](https://circleci.com/gh/commaai/panda) Usage ------ @@ -35,7 +35,7 @@ And to send one on bus 0: ``` >>> panda.can_send(0x1aa, "message", 0) ``` -More examples coming soon +Find user made scripts on the [wiki](https://community.comma.ai/wiki/index.php/Panda_scripts) Software interface support ------ diff --git a/board/Makefile b/board/Makefile index af274d2b27e428..ed6dcaa0309fc8 100644 --- a/board/Makefile +++ b/board/Makefile @@ -2,7 +2,7 @@ PROJ_NAME = panda CFLAGS = -g -Wall CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 -CFLAGS += -mhard-float -DSTM32F4 -DSTM32F413xx +CFLAGS += -mhard-float -DSTM32F4 -DSTM32F413xx -mfpu=fpv4-sp-d16 -fsingle-precision-constant STARTUP_FILE = startup_stm32f413xx include build.mk diff --git a/board/build.mk b/board/build.mk index d662f6b1419ad6..aee724c19044c1 100644 --- a/board/build.mk +++ b/board/build.mk @@ -1,4 +1,5 @@ CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O2 + CFLAGS += -Tstm32_flash.ld CC = arm-none-eabi-gcc diff --git a/board/drivers/can.h b/board/drivers/can.h index b094d2b4cea57d..dccb2b5c9238de 100644 --- a/board/drivers/can.h +++ b/board/drivers/can.h @@ -86,6 +86,7 @@ int can_err_cnt = 0; uint8_t can_num_lookup[] = {0,1,2,-1}; int8_t can_forwarding[] = {-1,-1,-1,-1}; uint32_t can_speed[] = {5000, 5000, 5000, 333}; + bool can_autobaud_enabled[] = {true, true, true, false}; #define CAN_MAX 3 #else CAN_TypeDef *cans[] = {CAN1, CAN2}; @@ -93,10 +94,21 @@ int can_err_cnt = 0; uint8_t can_num_lookup[] = {1,0}; int8_t can_forwarding[] = {-1,-1}; uint32_t can_speed[] = {5000, 5000}; + bool can_autobaud_enabled[] = {true, true}; #define CAN_MAX 2 #endif +uint32_t can_autobaud_speeds[] = {5000, 2500, 1250, 1000, 10000}; +#define AUTOBAUD_SPEEDS_LEN (sizeof(can_autobaud_speeds) / sizeof(can_autobaud_speeds[0])) + #define CANIF_FROM_CAN_NUM(num) (cans[num]) +#ifdef PANDA +#define CAN_NUM_FROM_CANIF(CAN) (CAN==CAN1 ? 0 : (CAN==CAN2 ? 1 : 2)) +#define CAN_NAME_FROM_CANIF(CAN) (CAN==CAN1 ? "CAN1" : (CAN==CAN2 ? "CAN2" : "CAN3")) +#else +#define CAN_NUM_FROM_CANIF(CAN) (CAN==CAN1 ? 0 : 1) +#define CAN_NAME_FROM_CANIF(CAN) (CAN==CAN1 ? "CAN1" : "CAN2") +#endif #define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) #define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) @@ -115,43 +127,78 @@ int can_err_cnt = 0; // 5000 = 500 kbps #define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10 / (x)) -void process_can(uint8_t can_number); +void can_autobaud_speed_increment(uint8_t can_number) { + uint32_t autobaud_speed = can_autobaud_speeds[0]; + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + for (int i = 0; i < AUTOBAUD_SPEEDS_LEN; i++) { + if (can_speed[bus_number] == can_autobaud_speeds[i]) { + if (i+1 < AUTOBAUD_SPEEDS_LEN) { + autobaud_speed = can_autobaud_speeds[i+1]; + } + break; + } + } + can_speed[bus_number] = autobaud_speed; +#ifdef DEBUG + CAN_TypeDef* CAN = CANIF_FROM_CAN_NUM(can_number); + puts(CAN_NAME_FROM_CANIF(CAN)); + puts(" auto-baud test "); + putui(can_speed[bus_number]); + puts(" cbps\n"); +#endif +} -void can_init(uint8_t can_number) { - if (can_number == 0xff) return; +void process_can(uint8_t can_number); +void can_set_speed(uint8_t can_number) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); - set_can_enable(CAN, 1); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; - while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); + while (true) { + // initialization mode + CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; + while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); + + // set time quanta from defines + CAN->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | + (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | + (can_speed_to_prescaler(can_speed[bus_number]) - 1); + + // silent loopback mode for debugging + if (can_loopback) { + CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM; + } + if (can_silent & (1 << can_number)) { + CAN->BTR |= CAN_BTR_SILM; + } - // set time quanta from defines - CAN->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | - (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | - (can_speed_to_prescaler(can_speed[BUS_NUM_FROM_CAN_NUM(can_number)]) - 1); + // reset + CAN->MCR = CAN_MCR_TTCM | CAN_MCR_ABOM; - // silent loopback mode for debugging - if (can_loopback) { - CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM; - } + #define CAN_TIMEOUT 1000000 + int tmp = 0; + while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++; + if (tmp < CAN_TIMEOUT) { + return; + } - if (can_silent & (1 << can_number)) { - CAN->BTR |= CAN_BTR_SILM; + if (can_autobaud_enabled[bus_number]) { + can_autobaud_speed_increment(can_number); + } else { + puts("CAN init FAILED!!!!!\n"); + puth(can_number); puts(" "); + puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); + return; + } } +} - // reset - CAN->MCR = CAN_MCR_TTCM | CAN_MCR_ABOM; - - #define CAN_TIMEOUT 1000000 - int tmp = 0; - while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++; +void can_init(uint8_t can_number) { + if (can_number == 0xff) return; - if (tmp == CAN_TIMEOUT) { - puts("CAN init FAILED!!!!!\n"); - puth(can_number); puts(" "); - puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); - } + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + set_can_enable(CAN, 1); + can_set_speed(can_number); // accept all filter CAN->FMR |= CAN_FMR_FINIT; @@ -166,7 +213,7 @@ void can_init(uint8_t can_number) { CAN->FMR &= ~(CAN_FMR_FINIT); // enable certain CAN interrupts - CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0; + CAN->IER |= CAN_IER_TMEIE | CAN_IER_FMPIE0; switch (can_number) { case 0: @@ -244,6 +291,8 @@ void can_set_gmlan(int bus) { // CAN error void can_sce(CAN_TypeDef *CAN) { + enter_critical_section(); + can_err_cnt += 1; #ifdef DEBUG if (CAN==CAN1) puts("CAN1: "); @@ -264,9 +313,19 @@ void can_sce(CAN_TypeDef *CAN) { puts("\n"); #endif + uint8_t can_number = CAN_NUM_FROM_CANIF(CAN); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + if (can_autobaud_enabled[bus_number] && (CAN->ESR & CAN_ESR_LEC)) { + can_autobaud_speed_increment(can_number); + can_set_speed(can_number); + } + // clear current send CAN->TSR |= CAN_TSR_ABRQ0; + CAN->MSR &= ~(CAN_MSR_ERRI); CAN->MSR = CAN->MSR; + + exit_critical_section(); } // ***************************** CAN ***************************** @@ -334,6 +393,16 @@ void can_rx(uint8_t can_number) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); while (CAN->RF0R & CAN_RF0R_FMP0) { + if (can_autobaud_enabled[bus_number]) { + can_autobaud_enabled[bus_number] = false; + puts(CAN_NAME_FROM_CANIF(CAN)); + #ifdef DEBUG + puts(" auto-baud "); + putui(can_speed[bus_number]); + puts(" cbps\n"); + #endif + } + can_rx_cnt += 1; // can is live @@ -392,7 +461,7 @@ void CAN3_SCE_IRQHandler() { can_sce(CAN3); } #endif void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) { - if (safety_tx_hook(to_push)) { + if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) { if (bus_number < BUS_MAX) { // add CAN packet to send queue // bus number isn't passed through diff --git a/board/drivers/spi.h b/board/drivers/spi.h index fc6a5ab26dca0f..7a5945d37257cb 100644 --- a/board/drivers/spi.h +++ b/board/drivers/spi.h @@ -40,6 +40,7 @@ void spi_tx_dma(void *addr, int len) { // channel3, increment memory, memory -> periph, enable DMA2_Stream3->CR = DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_EN; + delay(0); DMA2_Stream3->CR |= DMA_SxCR_TCIE; SPI1->CR2 |= SPI_CR2_TXDMAEN; @@ -65,6 +66,7 @@ void spi_rx_dma(void *addr, int len) { // channel3, increment memory, periph -> memory, enable DMA2_Stream2->CR = DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_EN; + delay(0); DMA2_Stream2->CR |= DMA_SxCR_TCIE; SPI1->CR2 |= SPI_CR2_RXDMAEN; @@ -95,7 +97,7 @@ void DMA2_Stream2_IRQHandler(void) { void DMA2_Stream3_IRQHandler(void) { #ifdef DEBUG_SPI puts("SPI handshake\n"); - #endif + #endif // reset handshake back to pull up set_gpio_mode(GPIOB, 0, MODE_INPUT); diff --git a/board/drivers/uart.h b/board/drivers/uart.h index 9c01416920d230..5ca82888d4fe7a 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -266,6 +266,17 @@ int puts(const char *a) { return 0; } +void putui(uint32_t i) { + char str[11]; + uint8_t idx = 10; + str[idx--] = '\0'; + do { + str[idx--] = (i % 10) + 0x30; + i /= 10; + } while (i); + puts(str + idx + 1); +} + void puth(unsigned int i) { int pos; char c[] = "0123456789abcdef"; diff --git a/board/drivers/usb.h b/board/drivers/usb.h index c43a8ce4008c68..f2cffa1416e645 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -30,13 +30,35 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS; #define USB_REQ_SET_INTERFACE 0x0B #define USB_REQ_SYNCH_FRAME 0x0C -#define USB_DESC_TYPE_DEVICE 1 -#define USB_DESC_TYPE_CONFIGURATION 2 -#define USB_DESC_TYPE_STRING 3 -#define USB_DESC_TYPE_INTERFACE 4 -#define USB_DESC_TYPE_ENDPOINT 5 -#define USB_DESC_TYPE_DEVICE_QUALIFIER 6 -#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 7 +#define USB_DESC_TYPE_DEVICE 0x01 +#define USB_DESC_TYPE_CONFIGURATION 0x02 +#define USB_DESC_TYPE_STRING 0x03 +#define USB_DESC_TYPE_INTERFACE 0x04 +#define USB_DESC_TYPE_ENDPOINT 0x05 +#define USB_DESC_TYPE_DEVICE_QUALIFIER 0x06 +#define USB_DESC_TYPE_OTHER_SPEED_CONFIGURATION 0x07 +#define USB_DESC_TYPE_BINARY_OBJECT_STORE 0x0f + +// offsets for configuration strings +#define STRING_OFFSET_LANGID 0x00 +#define STRING_OFFSET_IMANUFACTURER 0x01 +#define STRING_OFFSET_IPRODUCT 0x02 +#define STRING_OFFSET_ISERIAL 0x03 +#define STRING_OFFSET_ICONFIGURATION 0x04 +#define STRING_OFFSET_IINTERFACE 0x05 + +// WebUSB requests +#define WEBUSB_REQ_GET_URL 0x02 + +// WebUSB types +#define WEBUSB_DESC_TYPE_URL 0x03 +#define WEBUSB_URL_SCHEME_HTTPS 0x01 +#define WEBUSB_URL_SCHEME_HTTP 0x00 + +// WinUSB requests +#define WINUSB_REQ_GET_COMPATID_DESCRIPTOR 0x04 +#define WINUSB_REQ_GET_EXT_PROPS_OS 0x05 +#define WINUSB_REQ_GET_DESCRIPTOR 0x07 #define STS_GOUT_NAK 1 #define STS_DATA_UPDT 2 @@ -50,15 +72,6 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS; uint8_t resp[MAX_RESP_LEN]; -// descriptor types -// same as setupdat.h -#define DSCR_DEVICE_TYPE 1 -#define DSCR_CONFIG_TYPE 2 -#define DSCR_STRING_TYPE 3 -#define DSCR_INTERFACE_TYPE 4 -#define DSCR_ENDPOINT_TYPE 5 -#define DSCR_DEVQUAL_TYPE 6 - // for the repeating interfaces #define DSCR_INTERFACE_LEN 9 #define DSCR_ENDPOINT_LEN 7 @@ -71,15 +84,26 @@ uint8_t resp[MAX_RESP_LEN]; #define ENDPOINT_TYPE_BULK 2 #define ENDPOINT_TYPE_INT 3 -// This is an arbitrary value used in bRequest +// These are arbitrary values used in bRequest #define MS_VENDOR_CODE 0x20 +#define WEBUSB_VENDOR_CODE 0x30 + +// BOS constants +#define BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH 0x05 +#define BINARY_OBJECT_STORE_DESCRIPTOR 0x0F +#define WINUSB_PLATFORM_DESCRIPTOR_LENGTH 0x9E -//Convert machine byte order to USB byte order +// Convert machine byte order to USB byte order #define TOUSBORDER(num)\ (num&0xFF), ((num>>8)&0xFF) +// take in string length and return the first 2 bytes of a string descriptor +#define STRING_DESCRIPTOR_HEADER(size)\ + (((size * 2 + 2)&0xFF) | 0x0300) + uint8_t device_desc[] = { - DSCR_DEVICE_LEN, DSCR_DEVICE_TYPE, 0x00, 0x02, //Length, Type, bcdUSB + DSCR_DEVICE_LEN, USB_DESC_TYPE_DEVICE, //Length, Type + 0x10, 0x02, // bcdUSB max version of USB supported (2.1) 0xFF, 0xFF, 0xFF, 0x40, // Class, Subclass, Protocol, Max Packet Size TOUSBORDER(USB_VID), // idVendor TOUSBORDER(USB_PID), // idProduct @@ -92,88 +116,107 @@ uint8_t device_desc[] = { 0x03, 0x01 // Serial Number, Num Configurations }; +uint8_t device_qualifier[] = { + 0x0a, USB_DESC_TYPE_DEVICE_QUALIFIER, //Length, Type + 0x10, 0x02, // bcdUSB max version of USB supported (2.1) + 0xFF, 0xFF, 0xFF, 0x40, // bDeviceClass, bDeviceSubClass, bDeviceProtocol, bMaxPacketSize0 + 0x01, 0x00 // bNumConfigurations, bReserved +}; + #define ENDPOINT_RCV 0x80 #define ENDPOINT_SND 0x00 uint8_t configuration_desc[] = { - DSCR_CONFIG_LEN, DSCR_CONFIG_TYPE, // Length, Type, + DSCR_CONFIG_LEN, USB_DESC_TYPE_CONFIGURATION, // Length, Type, TOUSBORDER(0x0045), // Total Len (uint16) - 0x01, 0x01, 0x00, // Num Interface, Config Value, Configuration + 0x01, 0x01, STRING_OFFSET_ICONFIGURATION, // Num Interface, Config Value, Configuration 0xc0, 0x32, // Attributes, Max Power // interface 0 ALT 0 - DSCR_INTERFACE_LEN, DSCR_INTERFACE_TYPE, // Length, Type + DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type 0x00, 0x00, 0x03, // Index, Alt Index idx, Endpoint count 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol 0x00, // Interface // endpoint 1, read CAN - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_RCV | 1, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x00, // Polling Interval (NA) // endpoint 2, send serial - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x00, // Polling Interval // endpoint 3, send CAN - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x00, // Polling Interval // interface 0 ALT 1 - DSCR_INTERFACE_LEN, DSCR_INTERFACE_TYPE, // Length, Type + DSCR_INTERFACE_LEN, USB_DESC_TYPE_INTERFACE, // Length, Type 0x00, 0x01, 0x03, // Index, Alt Index idx, Endpoint count 0XFF, 0xFF, 0xFF, // Class, Subclass, Protocol 0x00, // Interface // endpoint 1, read CAN - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_RCV | 1, ENDPOINT_TYPE_INT, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x05, // Polling Interval (5 frames) // endpoint 2, send serial - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_SND | 2, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x00, // Polling Interval // endpoint 3, send CAN - DSCR_ENDPOINT_LEN, DSCR_ENDPOINT_TYPE, // Length, Type + DSCR_ENDPOINT_LEN, USB_DESC_TYPE_ENDPOINT, // Length, Type ENDPOINT_SND | 3, ENDPOINT_TYPE_BULK, // Endpoint Num/Direction, Type TOUSBORDER(0x0040), // Max Packet (0x0040) 0x00, // Polling Interval }; -uint8_t string_0_desc[] = { - 0x04, DSCR_STRING_TYPE, 0x09, 0x04 +// STRING_DESCRIPTOR_HEADER is for uint16 string descriptors +// it takes in a string length, which is bytes/2 because unicode +uint16_t string_language_desc[] = { + STRING_DESCRIPTOR_HEADER(1), + 0x0409 // american english }; -uint16_t string_1_desc[] = { - 0x0312, +// these strings are all uint16's so that we don't need to spam ,0 after every character +uint16_t string_manufacturer_desc[] = { + STRING_DESCRIPTOR_HEADER(8), 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' }; #ifdef PANDA -uint16_t string_2_desc[] = { - 0x030c, +uint16_t string_product_desc[] = { + STRING_DESCRIPTOR_HEADER(5), 'p', 'a', 'n', 'd', 'a' }; #else -uint16_t string_2_desc[] = { - 0x030c, +uint16_t string_product_desc[] = { + STRING_DESCRIPTOR_HEADER(5), 'N', 'E', 'O', 'v', '1' }; #endif -uint16_t string_3_desc[] = { - 0x030a, +// default serial number when we're not a panda +uint16_t string_serial_desc[] = { + STRING_DESCRIPTOR_HEADER(4), 'n', 'o', 'n', 'e' }; +// a string containing the default configuration index +uint16_t string_configuration_desc[] = { + STRING_DESCRIPTOR_HEADER(2), + '0', '1' // "01" +}; + #ifdef PANDA // WCID (auto install WinUSB driver) // https://github.com/pbatard/libwdi/wiki/WCID-Devices // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/winusb-installation#automatic-installation-of--winusb-without-an-inf-file +// WinUSB 1.0 descriptors, this is mostly used by Windows XP uint8_t string_238_desc[] = { - 0x12, 0x03, // bLength, bDescriptorType + 0x12, USB_DESC_TYPE_STRING, // bLength, bDescriptorType 'M',0, 'S',0, 'F',0, 'T',0, '1',0, '0',0, '0',0, // qwSignature (MSFT100) MS_VENDOR_CODE, 0x00 // bMS_VendorCode, bPad }; @@ -202,6 +245,121 @@ uint8_t winusb_ext_prop_os_desc[] = { 0x4e, 0x00, 0x00, 0x00, // dwPropertyDataLength '{',0, 'c',0, 'c',0, 'e',0, '5',0, '2',0, '9',0, '1',0, 'c',0, '-',0, 'a',0, '6',0, '9',0, 'f',0, '-',0, '4',0 ,'9',0 ,'9',0 ,'5',0 ,'-',0, 'a',0, '4',0, 'c',0, '2',0, '-',0, '2',0, 'a',0, 'e',0, '5',0, '7',0, 'a',0, '5',0, '1',0, 'a',0, 'd',0, 'e',0, '9',0, '}',0, 0, 0, // bPropertyData ({CCE5291C-A69F-4995-A4C2-2AE57A51ADE9}) }; + +/* +Binary Object Store descriptor used to expose WebUSB (and more WinUSB) metadata +comments are from the wicg spec +References used: + https://wicg.github.io/webusb/#webusb-platform-capability-descriptor + https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c + https://os.mbed.com/users/larsgk/code/USBDevice_WebUSB/file/1d8a6665d607/WebUSBDevice/ + +*/ +uint8_t binary_object_store_desc[] = { + // BOS header + BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH, // bLength, this is only the length of the header + BINARY_OBJECT_STORE_DESCRIPTOR, // bDescriptorType + 0x40, 0x00, // wTotalLength (LSB, MSB) + 0x03, // bNumDeviceCaps (USB 2.0 + WebUSB + WinUSB) + + // ------------------------------------------------- + // USB 2.0 extension descriptor + 0x07, // bLength, Descriptor size + 0x10, // bDescriptorType, Device Capability Descriptor Type + 0x02, // bDevCapabilityType, USB 2.0 extension capability type + 0x00, 0x00, 0x00, 0x00, // bmAttributes, LIBUSB_BM_LPM_SUPPORT = 2 and its the only option + + // ------------------------------------------------- + // WebUSB descriptor + // header + 0x18, // bLength, Size of this descriptor. Must be set to 24. + 0x10, // bDescriptorType, DEVICE CAPABILITY descriptor + 0x05, // bDevCapabilityType, PLATFORM capability + 0x00, // bReserved, This field is reserved and shall be set to zero. + + // PlatformCapabilityUUID, Must be set to {3408b638-09a9-47a0-8bfd-a0768815b665}. + 0x38, 0xB6, 0x08, 0x34, + 0xA9, 0x09, 0xA0, 0x47, + 0x8B, 0xFD, 0xA0, 0x76, + 0x88, 0x15, 0xB6, 0x65, + // + + 0x00, 0x01, // bcdVersion, Protocol version supported. Must be set to 0x0100. + WEBUSB_VENDOR_CODE, // bVendorCode, bRequest value used for issuing WebUSB requests. + // there used to be a concept of "allowed origins", but it was removed from the spec + // it was intended to be a security feature, but then the entire security model relies on domain ownership + // https://github.com/WICG/webusb/issues/49 + // other implementations use various other indexed to leverate this no-longer-valid feature. we wont. + // the spec says we *must* reply to index 0x03 with the url, so we'll hint that that's the right index + 0x03, // iLandingPage, URL descriptor index of the device’s landing page. + + // ------------------------------------------------- + // WinUSB descriptor + // header + 0x1C, // Descriptor size (28 bytes) + 0x10, // Descriptor type (Device Capability) + 0x05, // Capability type (Platform) + 0x00, // Reserved + + // MS OS 2.0 Platform Capability ID (D8DD60DF-4589-4CC7-9CD2-659D9E648A9F) + // Indicates the device supports the Microsoft OS 2.0 descriptor + 0xDF, 0x60, 0xDD, 0xD8, + 0x89, 0x45, 0xC7, 0x4C, + 0x9C, 0xD2, 0x65, 0x9D, + 0x9E, 0x64, 0x8A, 0x9F, + + 0x00, 0x00, 0x03, 0x06, // Windows version, currently set to 8.1 (0x06030000) + + WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // MS OS 2.0 descriptor size (word) + MS_VENDOR_CODE, 0x00 // vendor code, no alternate enumeration +}; + +uint8_t webusb_url_descriptor[] = { + 0x14, /* bLength */ + WEBUSB_DESC_TYPE_URL, // bDescriptorType + WEBUSB_URL_SCHEME_HTTPS, // bScheme + 'u', 's', 'b', 'p', 'a', 'n', 'd', 'a', '.', 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' +}; + +// WinUSB 2.0 descriptor. This is what modern systems use +// https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c +// http://janaxelson.com/files/ms_os_20_descriptors.c +// https://books.google.com/books?id=pkefBgAAQBAJ&pg=PA353&lpg=PA353 +uint8_t winusb_20_desc[WINUSB_PLATFORM_DESCRIPTOR_LENGTH] = { + // Microsoft OS 2.0 descriptor set header (table 10) + 0x0A, 0x00, // Descriptor size (10 bytes) + 0x00, 0x00, // MS OS 2.0 descriptor set header + + 0x00, 0x00, 0x03, 0x06, // Windows version (8.1) (0x06030000) + WINUSB_PLATFORM_DESCRIPTOR_LENGTH, 0x00, // Total size of MS OS 2.0 descriptor set + + // Microsoft OS 2.0 compatible ID descriptor + 0x14, 0x00, // Descriptor size (20 bytes) + 0x03, 0x00, // MS OS 2.0 compatible ID descriptor + 'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, // compatible ID (WINUSB) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-compatible ID + + // Registry property descriptor + 0x80, 0x00, // Descriptor size (130 bytes) + 0x04, 0x00, // Registry Property descriptor + 0x01, 0x00, // Strings are null-terminated Unicode + 0x28, 0x00, // Size of Property Name (40 bytes) "DeviceInterfaceGUID" + + // bPropertyName (DeviceInterfaceGUID) + 'D', 0x00, 'e', 0x00, 'v', 0x00, 'i', 0x00, 'c', 0x00, 'e', 0x00, 'I', 0x00, 'n', 0x00, + 't', 0x00, 'e', 0x00, 'r', 0x00, 'f', 0x00, 'a', 0x00, 'c', 0x00, 'e', 0x00, 'G', 0x00, + 'U', 0x00, 'I', 0x00, 'D', 0x00, 0x00, 0x00, + + 0x4E, 0x00, // Size of Property Data (78 bytes) + + // Vendor-defined property data: {CCE5291C-A69F-4995-A4C2-2AE57A51ADE9} + '{', 0x00, 'c', 0x00, 'c', 0x00, 'e', 0x00, '5', 0x00, '2', 0x00, '9', 0x00, '1', 0x00, // 16 + 'c', 0x00, '-', 0x00, 'a', 0x00, '6', 0x00, '9', 0x00, 'f', 0x00, '-', 0x00, '4', 0x00, // 32 + '9', 0x00, '9', 0x00, '5', 0x00, '-', 0x00, 'a', 0x00, '4', 0x00, 'c', 0x00, '2', 0x00, // 48 + '-', 0x00, '2', 0x00, 'a', 0x00, 'e', 0x00, '5', 0x00, '7', 0x00, 'a', 0x00, '5', 0x00, // 64 + '1', 0x00, 'a', 0x00, 'd', 0x00, 'e', 0x00, '9', 0x00, '}', 0x00, 0x00, 0x00 // 78 bytes +}; + #endif // current packet @@ -376,18 +534,22 @@ void usb_setup() { USB_WritePacket(configuration_desc, min(sizeof(configuration_desc), setup.b.wLength.w), 0); USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; + case USB_DESC_TYPE_DEVICE_QUALIFIER: + USB_WritePacket(device_qualifier, min(sizeof(device_qualifier), setup.b.wLength.w), 0); + USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; case USB_DESC_TYPE_STRING: switch (setup.b.wValue.bw.msb) { - case 0: - USB_WritePacket((uint8_t*)string_0_desc, min(sizeof(string_0_desc), setup.b.wLength.w), 0); + case STRING_OFFSET_LANGID: + USB_WritePacket((uint8_t*)string_language_desc, min(sizeof(string_language_desc), setup.b.wLength.w), 0); break; - case 1: - USB_WritePacket((uint8_t*)string_1_desc, min(sizeof(string_1_desc), setup.b.wLength.w), 0); + case STRING_OFFSET_IMANUFACTURER: + USB_WritePacket((uint8_t*)string_manufacturer_desc, min(sizeof(string_manufacturer_desc), setup.b.wLength.w), 0); break; - case 2: - USB_WritePacket((uint8_t*)string_2_desc, min(sizeof(string_2_desc), setup.b.wLength.w), 0); + case STRING_OFFSET_IPRODUCT: + USB_WritePacket((uint8_t*)string_product_desc, min(sizeof(string_product_desc), setup.b.wLength.w), 0); break; - case 3: + case STRING_OFFSET_ISERIAL: #ifdef PANDA resp[0] = 0x02 + 12*4; resp[1] = 0x03; @@ -403,10 +565,13 @@ void usb_setup() { USB_WritePacket(resp, min(resp[0], setup.b.wLength.w), 0); #else - USB_WritePacket((const uint8_t *)string_3_desc, min(sizeof(string_3_desc), setup.b.wLength.w), 0); + USB_WritePacket((const uint8_t *)string_serial_desc, min(sizeof(string_serial_desc), setup.b.wLength.w), 0); #endif break; #ifdef PANDA + case STRING_OFFSET_ICONFIGURATION: + USB_WritePacket((uint8_t*)string_configuration_desc, min(sizeof(string_configuration_desc), setup.b.wLength.w), 0); + break; case 238: USB_WritePacket((uint8_t*)string_238_desc, min(sizeof(string_238_desc), setup.b.wLength.w), 0); break; @@ -418,6 +583,12 @@ void usb_setup() { } USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; + #ifdef PANDA + case USB_DESC_TYPE_BINARY_OBJECT_STORE: + USB_WritePacket(binary_object_store_desc, min(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); + USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + #endif default: // nothing here? USB_WritePacket(0, 0, 0); @@ -439,14 +610,31 @@ void usb_setup() { USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; #ifdef PANDA + case WEBUSB_VENDOR_CODE: + switch (setup.b.wIndex.w) { + case WEBUSB_REQ_GET_URL: + USB_WritePacket(webusb_url_descriptor, min(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0); + USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + default: + // probably asking for allowed origins, which was removed from the spec + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + break; + } + break; case MS_VENDOR_CODE: switch (setup.b.wIndex.w) { + // winusb 2.0 descriptor from BOS + case WINUSB_REQ_GET_DESCRIPTOR: + USB_WritePacket_EP0((uint8_t*)winusb_20_desc, min(sizeof(winusb_20_desc), setup.b.wLength.w)); + break; // Extended Compat ID OS Descriptor - case 4: + case WINUSB_REQ_GET_COMPATID_DESCRIPTOR: USB_WritePacket_EP0((uint8_t*)winusb_ext_compatid_os_desc, min(sizeof(winusb_ext_compatid_os_desc), setup.b.wLength.w)); break; // Extended Properties OS Descriptor - case 5: + case WINUSB_REQ_GET_EXT_PROPS_OS: USB_WritePacket_EP0((uint8_t*)winusb_ext_prop_os_desc, min(sizeof(winusb_ext_prop_os_desc), setup.b.wLength.w)); break; default: diff --git a/board/get_sdk_mac.sh b/board/get_sdk_mac.sh old mode 100644 new mode 100755 diff --git a/board/main.c b/board/main.c index e46a38856317e8..a15861a17e5c82 100644 --- a/board/main.c +++ b/board/main.c @@ -104,7 +104,14 @@ int get_health_pkt(void *dat) { #ifdef PANDA health->current = adc_get(ADCCHAN_CURRENT); - health->started = (GPIOA->IDR & (1 << 1)) == 0; + int safety_ignition = safety_ignition_hook(); + if (safety_ignition < 0) { + //Use the GPIO pin to determine ignition + health->started = (GPIOA->IDR & (1 << 1)) == 0; + } else { + //Current safety hooks want to determine ignition (ex: GM) + health->started = safety_ignition; + } #else health->current = 0; health->started = (GPIOC->IDR & (1 << 13)) != 0; @@ -281,9 +288,15 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { break; case SAFETY_ELM327: can_silent = ALL_CAN_BUT_MAIN_SILENT; + can_autobaud_enabled[0] = false; break; default: can_silent = ALL_CAN_LIVE; + can_autobaud_enabled[0] = false; + can_autobaud_enabled[1] = false; + #ifdef PANDA + can_autobaud_enabled[2] = false; + #endif break; } can_init_all(); @@ -303,6 +316,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { // **** 0xde: set can bitrate case 0xde: if (setup->b.wValue.w < BUS_MAX) { + can_autobaud_enabled[setup->b.wValue.w] = false; can_speed[setup->b.wValue.w] = setup->b.wIndex.w; can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); } @@ -476,6 +490,11 @@ void __initialize_hardware_early() { early(); } +void __attribute__ ((noinline)) enable_fpu() { + // enable the FPU + SCB->CPACR |= ((3UL << 10*2) | (3UL << 11*2)); +} + int main() { // shouldn't have interrupts here, but just in case __disable_irq(); @@ -501,6 +520,11 @@ int main() { puts(is_entering_bootmode ? " ESP wants bootmode\n" : " no bootmode\n"); gpio_init(); +#ifdef PANDA + // panda has an FPU, let's use it! + enable_fpu(); +#endif + // enable main uart if it's connected if (has_external_debug_serial) { // WEIRDNESS: without this gate around the UART, it would "crash", but only if the ESP is enabled @@ -552,6 +576,11 @@ int main() { __enable_irq(); + // if the error interrupt is enabled to quickly when the CAN bus is active + // something bad happens and you can't connect to the device over USB + delay(10000000); + CAN1->IER |= CAN_IER_ERRIE | CAN_IER_LECIE; + // LED should keep on blinking all the time uint64_t cnt = 0; diff --git a/board/safety.h b/board/safety.h index 3e0b0cf38a1386..c0bd303ec9b7ef 100644 --- a/board/safety.h +++ b/board/safety.h @@ -1,15 +1,18 @@ void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); int safety_tx_lin_hook(int lin_num, uint8_t *data, int len); +int safety_ignition_hook(); typedef void (*safety_hook_init)(int16_t param); typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send); typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len); +typedef int (*ign_hook)(); typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd); typedef struct { safety_hook_init init; + ign_hook ignition; rx_hook rx; tx_hook tx; tx_lin_hook tx_lin; @@ -23,6 +26,9 @@ int controls_allowed = 0; #include "safety/safety_defaults.h" #include "safety/safety_honda.h" #include "safety/safety_toyota.h" +#ifdef PANDA +#include "safety/safety_toyota_ipas.h" +#endif #include "safety/safety_gm.h" #include "safety/safety_elm327.h" @@ -40,6 +46,12 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len){ return current_hooks->tx_lin(lin_num, data, len); } +// -1 = Disabled (Use GPIO to determine ignition) +// 0 = Off (not started) +// 1 = On (started) +int safety_ignition_hook() { + return current_hooks->ignition(); +} int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return current_hooks->fwd(bus_num, to_fwd); } @@ -52,6 +64,7 @@ typedef struct { #define SAFETY_NOOUTPUT 0 #define SAFETY_HONDA 1 #define SAFETY_TOYOTA 2 +#define SAFETY_TOYOTA_IPAS 0x1335 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_GM 3 #define SAFETY_HONDA_BOSCH 4 @@ -64,6 +77,9 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_HONDA_BOSCH, &honda_bosch_hooks}, {SAFETY_TOYOTA, &toyota_hooks}, {SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks}, +#ifdef PANDA + {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks}, +#endif {SAFETY_GM, &gm_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, {SAFETY_ELM327, &elm327_hooks}, diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index 2411a41628e540..16ab3643f83af2 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -14,6 +14,9 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { return false; } +static int nooutput_ign_hook() { + return -1; +} static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } @@ -23,6 +26,7 @@ const safety_hooks nooutput_hooks = { .rx = default_rx_hook, .tx = nooutput_tx_hook, .tx_lin = nooutput_tx_lin_hook, + .ignition = nooutput_ign_hook, .fwd = nooutput_fwd_hook, }; @@ -40,6 +44,10 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) { return true; } +static int alloutput_ign_hook() { + return -1; +} + static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } @@ -49,6 +57,7 @@ const safety_hooks alloutput_hooks = { .rx = default_rx_hook, .tx = alloutput_tx_hook, .tx_lin = alloutput_tx_lin_hook, + .ignition = alloutput_ign_hook, .fwd = alloutput_fwd_hook, }; diff --git a/board/safety/safety_elm327.h b/board/safety/safety_elm327.h index 3a0efe6e6b5c8a..0b23fa5dac18e2 100644 --- a/board/safety/safety_elm327.h +++ b/board/safety/safety_elm327.h @@ -31,6 +31,10 @@ static void elm327_init(int16_t param) { controls_allowed = 1; } +static int elm327_ign_hook() { + return -1; +} + static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } @@ -40,5 +44,6 @@ const safety_hooks elm327_hooks = { .rx = elm327_rx_hook, .tx = elm327_tx_hook, .tx_lin = elm327_tx_lin_hook, + .ignition = elm327_ign_hook, .fwd = elm327_fwd_hook, }; diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index f5fbbfe8fe248b..03c36d6255dcbf 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -16,8 +16,10 @@ int gm_speed = 0; // silence everything if stock ECUs are still online int gm_ascm_detected = 0; -static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { +int gm_ignition_started = 0; +static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus_number = (to_push->RDTR >> 4) & 0xFF; uint32_t addr; if (to_push->RIR & 4) { // Extended @@ -29,6 +31,12 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { addr = to_push->RIR >> 21; } + if (addr == 0x135 && bus_number == 0) { + //Gear selector (used for determining ignition) + int gear = to_push->RDLR & 0x7; + gm_ignition_started = gear > 0; //Park = 0. If out of park, we're "on." + } + // sample speed, really only care if car is moving or not // rear left wheel speed if (addr == 842) { @@ -36,7 +44,6 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // check if stock ASCM ECU is still online - int bus_number = (to_push->RDTR >> 4) & 0xFF; if (bus_number == 0 && addr == 715) { gm_ascm_detected = 1; controls_allowed = 0; @@ -170,6 +177,11 @@ static int gm_tx_lin_hook(int lin_num, uint8_t *data, int len) { static void gm_init(int16_t param) { controls_allowed = 0; + gm_ignition_started = 0; +} + +static int gm_ign_hook() { + return gm_ignition_started; } static int gm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { @@ -181,6 +193,7 @@ const safety_hooks gm_hooks = { .rx = gm_rx_hook, .tx = gm_tx_hook, .tx_lin = gm_tx_lin_hook, + .ignition = gm_ign_hook, .fwd = gm_fwd_hook, }; diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index 5fe3ebf7c9ebf3..bc6ce6e9c08030 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -134,11 +134,16 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } +static int honda_ign_hook() { + return -1; +} + const safety_hooks honda_hooks = { .init = honda_init, .rx = honda_rx_hook, .tx = honda_tx_hook, .tx_lin = honda_tx_lin_hook, + .ignition = honda_ign_hook, .fwd = honda_fwd_hook, }; @@ -160,5 +165,6 @@ const safety_hooks honda_bosch_hooks = { .rx = honda_rx_hook, .tx = honda_tx_hook, .tx_lin = honda_tx_lin_hook, + .ignition = honda_ign_hook, .fwd = honda_bosch_fwd_hook, }; diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 78aaa36973c6fa..c78e090d09414d 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,6 +1,10 @@ // track the torque measured for limiting -int16_t torque_meas[3] = {0, 0, 0}; // last 3 motor torques produced by the eps -int16_t torque_meas_min = 0, torque_meas_max = 0; +struct sample_t { + int values[3]; + int min; + int max; +} sample_t_default = {{0, 0, 0}, 0, 0}; +struct sample_t torque_meas; // last 3 motor torques produced by the eps // global torque limit const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever @@ -28,6 +32,25 @@ int16_t dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS int16_t desired_torque_last = 0; // last desired steer torque int16_t rt_torque_last = 0; // last desired torque for real time check uint32_t ts_last = 0; +int cruise_engaged_last = 0; // cruise state + +uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) { + return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts; +} + +void update_sample(struct sample_t *sample, int sample_new) { + for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) { + sample->values[i] = sample->values[i-1]; + } + sample->values[0] = sample_new; + + // get the minimum and maximum measured torque over the last 3 frames + sample->min = sample->max = sample->values[0]; + for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) { + if (sample->values[i] < sample->min) sample->min = sample->values[i]; + if (sample->values[i] > sample->max) sample->max = sample->values[i]; + } +} static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // get eps motor torque (0.66 factor in dbc) @@ -37,28 +60,20 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // increase torque_meas by 1 to be conservative on rounding int torque_meas_new = ((int)(torque_meas_new_16) * dbc_eps_torque_factor / 100) + (torque_meas_new_16 > 0 ? 1 : -1); - // shift the array - for (int i = sizeof(torque_meas)/sizeof(torque_meas[0]) - 1; i > 0; i--) { - torque_meas[i] = torque_meas[i-1]; - } - torque_meas[0] = torque_meas_new; - - // get the minimum and maximum measured torque over the last 3 frames - torque_meas_min = torque_meas_max = torque_meas[0]; - for (int i = 1; i < sizeof(torque_meas)/sizeof(torque_meas[0]); i++) { - if (torque_meas[i] < torque_meas_min) torque_meas_min = torque_meas[i]; - if (torque_meas[i] > torque_meas_max) torque_meas_max = torque_meas[i]; - } + // update array of sample + update_sample(&torque_meas, torque_meas_new); } - // exit controls on ACC off + // enter controls on rising edge of ACC, exit controls on ACC off if ((to_push->RIR>>21) == 0x1D2) { // 4 bits: 55-52 - if (to_push->RDHR & 0xF00000) { + int cruise_engaged = to_push->RDHR & 0xF00000; + if (cruise_engaged && !cruise_engaged_last) { controls_allowed = 1; - } else { + } else if (!cruise_engaged) { controls_allowed = 0; } + cruise_engaged_last = cruise_engaged; } } @@ -67,6 +82,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // Check if msg is sent on BUS 0 if (((to_send->RDTR >> 4) & 0xF) == 0) { + // no IPAS in non IPAS mode + if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) return false; + // ACCEL: safety check on byte 1-2 if ((to_send->RIR>>21) == 0x343) { int16_t desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF); @@ -99,8 +117,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int16_t lowest_allowed_torque = min(desired_torque_last, 0) - MAX_RATE_UP; // if we've exceeded the applied torque, we must start moving toward 0 - highest_allowed_torque = min(highest_allowed_torque, max(desired_torque_last - MAX_RATE_DOWN, max(torque_meas_max, 0) + MAX_TORQUE_ERROR)); - lowest_allowed_torque = max(lowest_allowed_torque, min(desired_torque_last + MAX_RATE_DOWN, min(torque_meas_min, 0) - MAX_TORQUE_ERROR)); + highest_allowed_torque = min(highest_allowed_torque, max(desired_torque_last - MAX_RATE_DOWN, max(torque_meas.max, 0) + MAX_TORQUE_ERROR)); + lowest_allowed_torque = max(lowest_allowed_torque, min(desired_torque_last + MAX_RATE_DOWN, min(torque_meas.min, 0) - MAX_TORQUE_ERROR)); // check for violation if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) { @@ -121,7 +139,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts; + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); if (ts_elapsed > RT_INTERVAL) { rt_torque_last = desired_torque; ts_last = ts; @@ -161,6 +179,10 @@ static void toyota_init(int16_t param) { dbc_eps_torque_factor = param; } +static int toyota_ign_hook() { + return -1; +} + static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { return -1; } @@ -170,6 +192,7 @@ const safety_hooks toyota_hooks = { .rx = toyota_rx_hook, .tx = toyota_tx_hook, .tx_lin = toyota_tx_lin_hook, + .ignition = toyota_ign_hook, .fwd = toyota_fwd_hook, }; @@ -184,5 +207,6 @@ const safety_hooks toyota_nolimits_hooks = { .rx = toyota_rx_hook, .tx = toyota_tx_hook, .tx_lin = toyota_tx_lin_hook, + .ignition = toyota_ign_hook, .fwd = toyota_fwd_hook, }; diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h new file mode 100644 index 00000000000000..6b96c3cdd6783a --- /dev/null +++ b/board/safety/safety_toyota_ipas.h @@ -0,0 +1,193 @@ +// uses tons from safety_toyota +// TODO: refactor to repeat less code + +// IPAS override +const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value + +struct lookup_t { + float x[3]; + float y[3]; +}; + +// 2m/s are added to be less restrictive +const struct lookup_t LOOKUP_ANGLE_RATE_UP = { + {2., 7., 17.}, + {5., .8, .15}}; + +const struct lookup_t LOOKUP_ANGLE_RATE_DOWN = { + {2., 7., 17.}, + {5., 3.5, .4}}; + +const float RT_ANGLE_FUDGE = 1.5; // for RT checks allow 50% more angle change +const float CAN_TO_DEG = 2. / 3.; // convert angles from CAN unit to degrees + +int ipas_state = 1; // 1 disabled, 3 executing angle control, 5 override +int angle_control = 0; // 1 if direct angle control packets are seen +float speed = 0.; + +struct sample_t angle_meas; // last 3 steer angles +struct sample_t torque_driver; // last 3 driver steering torque + +// state of angle limits +int16_t desired_angle_last = 0; // last desired steer angle +int16_t rt_angle_last = 0; // last desired torque for real time check +uint32_t ts_angle_last = 0; + +int controls_allowed_last = 0; + +int to_signed(int d, int bits) { + if (d >= (1 << (bits - 1))) { + d -= (1 << bits); + } + return d; +} + +// interp function that holds extreme values +float interpolate(struct lookup_t xy, float x) { + int size = sizeof(xy.x) / sizeof(xy.x[0]); + // x is lower than the first point in the x array. Return the first point + if (x <= xy.x[0]) { + return xy.y[0]; + + } else { + // find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp + for (int i=0; i < size-1; i++) { + if (x < xy.x[i+1]) { + float x0 = xy.x[i]; + float y0 = xy.y[i]; + float dx = xy.x[i+1] - x0; + float dy = xy.y[i+1] - y0; + // dx should not be zero as xy.x is supposed ot be monotonic + if (dx <= 0.) dx = 0.0001; + return dy * (x - x0) / dx + y0; + } + } + // if no such point is found, then x > xy.x[size-1]. Return last point + return xy.y[size - 1]; + } +} + + +static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + // check standard toyota stuff as well + toyota_rx_hook(to_push); + + if ((to_push->RIR>>21) == 0x260) { + // get driver steering torque + int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF)); + + // update array of samples + update_sample(&torque_driver, torque_driver_new); + } + + // get steer angle + if ((to_push->RIR>>21) == 0x25) { + int angle_meas_new = ((to_push->RDLR & 0xf) << 8) + ((to_push->RDLR & 0xff00) >> 8); + uint32_t ts = TIM2->CNT; + + angle_meas_new = to_signed(angle_meas_new, 12); + + // update array of samples + update_sample(&angle_meas, angle_meas_new); + + // *** angle real time check + // add 1 to not false trigger the violation and multiply by 20 since the check is done every 250ms and steer angle is updated at 80Hz + int rt_delta_angle_up = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * 20. * CAN_TO_DEG + 1.))); + int rt_delta_angle_down = ((int)(RT_ANGLE_FUDGE * (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * 20. * CAN_TO_DEG + 1.))); + int highest_rt_angle = rt_angle_last + (rt_angle_last > 0? rt_delta_angle_up:rt_delta_angle_down); + int lowest_rt_angle = rt_angle_last - (rt_angle_last > 0? rt_delta_angle_down:rt_delta_angle_up); + + // every RT_INTERVAL or when controls are turned on, set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); + if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { + rt_angle_last = angle_meas_new; + ts_angle_last = ts; + } + + // check for violation + if (angle_control && + ((angle_meas_new < lowest_rt_angle) || + (angle_meas_new > highest_rt_angle))) { + controls_allowed = 0; + } + + controls_allowed_last = controls_allowed; + } + + // get speed + if ((to_push->RIR>>21) == 0xb4) { + speed = ((float) (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF))) * 0.01 / 3.6; + } + + // get ipas state + if ((to_push->RIR>>21) == 0x262) { + ipas_state = (to_push->RDLR & 0xf); + } + + // exit controls on high steering override + if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) || + (torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) || + (ipas_state==5))) { + controls_allowed = 0; + } +} + +static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + + // Check if msg is sent on BUS 0 + if (((to_send->RDTR >> 4) & 0xF) == 0) { + + // STEER ANGLE + if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) { + + angle_control = 1; // we are in angle control mode + int desired_angle = ((to_send->RDLR & 0xf) << 8) + ((to_send->RDLR & 0xff00) >> 8); + int ipas_state_cmd = ((to_send->RDLR & 0xff) >> 4); + int16_t violation = 0; + + desired_angle = to_signed(desired_angle, 12); + + if (controls_allowed) { + // add 1 to not false trigger the violation + int delta_angle_up = (int) (interpolate(LOOKUP_ANGLE_RATE_UP, speed) * CAN_TO_DEG + 1.); + int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.); + int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down); + int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up); + if ((desired_angle > highest_desired_angle) || + (desired_angle < lowest_desired_angle)){ + violation = 1; + controls_allowed = 0; + } + } + + // desired steer angle should be the same as steer angle measured when controls are off + if ((!controls_allowed) && + ((desired_angle < (angle_meas.min - 1)) || + (desired_angle > (angle_meas.max + 1)) || + (ipas_state_cmd != 1))) { + violation = 1; + } + + desired_angle_last = desired_angle; + + if (violation) { + return false; + } + + return true; + } + } + + // check standard toyota stuff as well + return toyota_tx_hook(to_send); +} + +const safety_hooks toyota_ipas_hooks = { + .init = toyota_init, + .rx = toyota_ipas_rx_hook, + .tx = toyota_ipas_tx_hook, + .tx_lin = toyota_tx_lin_hook, + .ignition = toyota_ign_hook, + .fwd = toyota_fwd_hook, +}; + diff --git a/boardesp/elm327.c b/boardesp/elm327.c index 993dc8460a62fb..6c18c4463b484d 100644 --- a/boardesp/elm327.c +++ b/boardesp/elm327.c @@ -1157,7 +1157,7 @@ static const elm_protocol_t elm_protocols[] = { {false, NA, 104, elm_process_obd_cmd_J1850, NULL, "SAE J1850 VPW", }, {false, LIN, 104, elm_process_obd_cmd_LIN5baud, NULL, "ISO 9141-2", }, {false, LIN, 104, elm_process_obd_cmd_LIN5baud, NULL, "ISO 14230-4 (KWP 5BAUD)", }, - {true, LIN, 104, elm_process_obd_cmd_LINFast, elm_init_LINFast, "ISO 14230-4 (KWP FAST)", }, + {true, LIN, 104, elm_process_obd_cmd_LINFast, NULL, "ISO 14230-4 (KWP FAST)", }, {true, CAN11, 5000, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 11/500)",}, {true, CAN29, 5000, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 29/500)",}, {true, CAN11, 2500, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 11/250)",}, diff --git a/boardesp/proxy.c b/boardesp/proxy.c index 35493869fde893..bb89743c198ec9 100644 --- a/boardesp/proxy.c +++ b/boardesp/proxy.c @@ -264,7 +264,7 @@ void ICACHE_FLASH_ATTR wifi_configure(int secure) { void ICACHE_FLASH_ATTR wifi_init() { // default ssid and password memset(ssid, 0, 32); - os_sprintf(ssid, "panda-%08x-BROKEN", system_get_chip_id()); + os_sprintf(ssid, "panda-%08x-BROKEN", system_get_chip_id()); // fetch secure ssid and password // update, try 20 times, for 1 second @@ -283,6 +283,9 @@ void ICACHE_FLASH_ATTR wifi_init() { } os_delay_us(50000); } + os_printf("Finished getting SID\n"); + os_printf(ssid); + os_printf("\n"); // set IP wifi_softap_dhcps_stop(); //stop DHCP before setting static IP @@ -311,7 +314,7 @@ void ICACHE_FLASH_ATTR user_init() { gpio_init(); // configure UART TXD to be GPIO1, set as output - PIN_FUNC_SELECT(PERIPHS_IO_MUX_U0TXD_U, FUNC_GPIO1); + PIN_FUNC_SELECT(PERIPHS_IO_MUX_U0TXD_U, FUNC_GPIO1); gpio_output_set(0, 0, (1 << pin), 0); // configure SPI @@ -331,7 +334,7 @@ void ICACHE_FLASH_ATTR user_init() { //SPICsPinSelect(SpiNum_HSPI, SpiPinCS_1); // configure UART TXD to be GPIO1, set as output - PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO5_U, FUNC_GPIO5); + PIN_FUNC_SELECT(PERIPHS_IO_MUX_GPIO5_U, FUNC_GPIO5); gpio_output_set(0, 0, (1 << 5), 0); gpio_output_set((1 << 5), 0, 0, 0); diff --git a/examples/can_bit_transition.py b/examples/can_bit_transition.py index 50d13f0a45a175..5c15e4bbc26eed 100755 --- a/examples/can_bit_transition.py +++ b/examples/can_bit_transition.py @@ -45,7 +45,7 @@ def load(self, filename, start, end): message_id = row[1][2:] # remove leading '0x' else: message_id = hex(int(row[1]))[2:] # old message IDs are in decimal - + message_id = '%s:%s' % (bus, message_id) if row[3].startswith('0x'): data = row[3][2:] # remove leading '0x' else: @@ -74,7 +74,7 @@ def PrintUnique(log_file, low_range, high_range): high.load(log_file, start, end) # print messages that go from low to high found = False - for message_id in high.messages: + for message_id in sorted(high.messages): if message_id in low.messages: high.messages[message_id].printBitDiff(low.messages[message_id]) found = True diff --git a/examples/can_unique.py b/examples/can_unique.py index 42488c64bb24fd..ad6de296ee4174 100755 --- a/examples/can_unique.py +++ b/examples/can_unique.py @@ -51,10 +51,12 @@ def load(self, filename): reader = csv.reader(input) next(reader, None) # skip the CSV header for row in reader: + bus = row[0] if row[1].startswith('0x'): message_id = row[1][2:] # remove leading '0x' else: message_id = hex(int(row[1]))[2:] # old message IDs are in decimal + message_id = '%s:%s' % (bus, message_id) if row[1].startswith('0x'): data = row[2][2:] # remove leading '0x' else: @@ -76,7 +78,7 @@ def PrintUnique(interesting_file, background_files): background.load(background_file) interesting = Info() interesting.load(interesting_file) - for message_id in interesting.messages: + for message_id in sorted(interesting.messages): if message_id not in background.messages: print 'New message_id: %s' % message_id else: diff --git a/python/__init__.py b/python/__init__.py index 6432f0262ffd8a..3de634d2fc6d73 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -155,6 +155,7 @@ def connect(self, claim=True, wait=False): if self._serial is None or this_serial == self._serial: self._serial = this_serial print("opening device", self._serial, hex(device.getProductID())) + time.sleep(1) self.bootstub = device.getProductID() == 0xddee self.legacy = (device.getbcdDevice() != 0x2300) self._handle = device.open() diff --git a/tests/automated/helpers.py b/tests/automated/helpers.py index a55f77c655c8c4..1ddced117980ee 100644 --- a/tests/automated/helpers.py +++ b/tests/automated/helpers.py @@ -35,12 +35,12 @@ def _connect_wifi(dongle_id, pw, insecure_okay=False): if sys.platform == "darwin": os.system("networksetup -setairportnetwork en0 %s %s" % (ssid, pw)) else: + wlan_interface = subprocess.check_output(["sh", "-c", "iw dev | awk '/Interface/ {print $2}'"]).strip() cnt = 0 MAX_TRIES = 10 while cnt < MAX_TRIES: print "WIFI: scanning %d" % cnt - if os.system("ifconfig | grep wlp3s0") == 0: - os.system("sudo iwlist wlp3s0 scanning > /dev/null") + os.system("sudo iwlist %s scanning > /dev/null" % wlan_interface) os.system("nmcli device wifi rescan") wifi_scan = filter(lambda x: ssid in x, subprocess.check_output(["nmcli","dev", "wifi", "list"]).split("\n")) if len(wifi_scan) != 0: diff --git a/tests/build/Dockerfile b/tests/build/Dockerfile new file mode 100644 index 00000000000000..276a25ed0b9495 --- /dev/null +++ b/tests/build/Dockerfile @@ -0,0 +1,17 @@ +FROM ubuntu:16.04 + +RUN apt-get update && apt-get install -y gcc-arm-none-eabi libnewlib-arm-none-eabi python python-pip gcc g++ git autoconf gperf bison flex automake texinfo wget help2man gawk libtool libtool-bin ncurses-dev unzip unrar-free libexpat-dev sed bzip2 + +RUN pip install pycrypto==2.6.1 + +# Build esp toolchain +RUN mkdir -p /panda/boardesp +WORKDIR /panda/boardesp +RUN git clone --recursive https://github.com/pfalcon/esp-open-sdk.git +WORKDIR /panda/boardesp/esp-open-sdk +RUN git checkout 03f5e898a059451ec5f3de30e7feff30455f7ce +RUN CT_ALLOW_BUILD_AS_ROOT_SURE=1 make STANDALONE=y + +COPY . /panda + +WORKDIR /panda diff --git a/tests/safety/Dockerfile b/tests/safety/Dockerfile new file mode 100644 index 00000000000000..9381fdc4085759 --- /dev/null +++ b/tests/safety/Dockerfile @@ -0,0 +1,6 @@ +FROM ubuntu:16.04 + +RUN apt-get update && apt-get install -y clang make python python-pip +COPY tests/safety/requirements.txt /panda/tests/safety/requirements.txt +RUN pip install -r /panda/tests/safety/requirements.txt +COPY . /panda diff --git a/tests/safety/Makefile b/tests/safety/Makefile new file mode 100644 index 00000000000000..334a29427d7d29 --- /dev/null +++ b/tests/safety/Makefile @@ -0,0 +1,18 @@ +CC = clang +CCFLAGS = -O3 -fPIC -DPANDA -I. + +.PHONY: all +all: libpandasafety.so + +libpandasafety.so: test.o + $(CC) -shared -o '$@' $^ -lm + +test.o: test.c + @echo "[ CC ] $@" + $(CC) $(CCFLAGS) -MMD -c -I../../board -o '$@' '$<' + +.PHONY: clean +clean: + rm -f libpandasafety.so test.o test.d + +-include test.d diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py new file mode 100644 index 00000000000000..2d908488a1ce81 --- /dev/null +++ b/tests/safety/libpandasafety_py.py @@ -0,0 +1,60 @@ +import os +import subprocess + +from cffi import FFI + +can_dir = os.path.dirname(os.path.abspath(__file__)) +libpandasafety_fn = os.path.join(can_dir, "libpandasafety.so") +subprocess.check_call(["make"], cwd=can_dir) + +ffi = FFI() +ffi.cdef(""" +typedef struct +{ + uint32_t TIR; /*!< CAN TX mailbox identifier register */ + uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */ + uint32_t TDLR; /*!< CAN mailbox data low register */ + uint32_t TDHR; /*!< CAN mailbox data high register */ +} CAN_TxMailBox_TypeDef; + +typedef struct +{ + uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */ + uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */ + uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */ + uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */ +} CAN_FIFOMailBox_TypeDef; + +typedef struct +{ + uint32_t CNT; +} TIM_TypeDef; + +void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +void toyota_init(int16_t param); +void set_controls_allowed(int c); +void reset_angle_control(void); +int get_controls_allowed(void); +void init_tests_toyota(void); +void set_timer(int t); +void set_torque_meas(int min, int max); +void set_rt_torque_last(int t); +void set_desired_torque_last(int t); +int get_torque_meas_min(void); +int get_torque_meas_max(void); + +void init_tests_honda(void); +int get_ego_speed(void); +void honda_init(int16_t param); +void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); +int get_brake_prev(void); +int get_gas_prev(void); + +void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push); +int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send); + +""") + +libpandasafety = ffi.dlopen(libpandasafety_fn) diff --git a/tests/safety/requirements.txt b/tests/safety/requirements.txt new file mode 100644 index 00000000000000..8bbfb1d7df38a2 --- /dev/null +++ b/tests/safety/requirements.txt @@ -0,0 +1,2 @@ +cffi==1.11.4 +numpy==1.14.1 diff --git a/tests/safety/test.c b/tests/safety/test.c new file mode 100644 index 00000000000000..eb19ab28c08b88 --- /dev/null +++ b/tests/safety/test.c @@ -0,0 +1,108 @@ +#include +#include + +typedef struct +{ + uint32_t TIR; /*!< CAN TX mailbox identifier register */ + uint32_t TDTR; /*!< CAN mailbox data length control and time stamp register */ + uint32_t TDLR; /*!< CAN mailbox data low register */ + uint32_t TDHR; /*!< CAN mailbox data high register */ +} CAN_TxMailBox_TypeDef; + +typedef struct +{ + uint32_t RIR; /*!< CAN receive FIFO mailbox identifier register */ + uint32_t RDTR; /*!< CAN receive FIFO mailbox data length control and time stamp register */ + uint32_t RDLR; /*!< CAN receive FIFO mailbox data low register */ + uint32_t RDHR; /*!< CAN receive FIFO mailbox data high register */ +} CAN_FIFOMailBox_TypeDef; + +typedef struct +{ + uint32_t CNT; +} TIM_TypeDef; + +struct sample_t torque_meas; + +TIM_TypeDef timer; +TIM_TypeDef *TIM2 = &timer; + +#define min(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a < _b ? _a : _b; }) + +#define max(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a > _b ? _a : _b; }) + + +#define PANDA +#define static +#include "safety.h" + +void set_controls_allowed(int c){ + controls_allowed = c; +} + +void reset_angle_control(void){ + angle_control = 0; +} + +int get_controls_allowed(void){ + return controls_allowed; +} + +void set_timer(int t){ + timer.CNT = t; +} + +void set_torque_meas(int min, int max){ + torque_meas.min = min; + torque_meas.max = max; +} + +int get_torque_meas_min(void){ + return torque_meas.min; +} + +int get_torque_meas_max(void){ + return torque_meas.max; +} + +void set_rt_torque_last(int t){ + rt_torque_last = t; +} + +void set_desired_torque_last(int t){ + desired_torque_last = t; +} + +int get_ego_speed(void){ + return ego_speed; +} + +int get_brake_prev(void){ + return brake_prev; +} + +int get_gas_prev(void){ + return gas_prev; +} + +void init_tests_toyota(void){ + torque_meas.min = 0; + torque_meas.max = 0; + desired_torque_last = 0; + rt_torque_last = 0; + ts_last = 0; + set_timer(0); +} + +void init_tests_honda(void){ + ego_speed = 0; + gas_interceptor_detected = 0; + brake_prev = 0; + gas_prev = 0; +} diff --git a/tests/safety/test.sh b/tests/safety/test.sh new file mode 100755 index 00000000000000..83d8f5b31fa420 --- /dev/null +++ b/tests/safety/test.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env sh +python -m unittest discover . diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py new file mode 100755 index 00000000000000..fb169af7e3bf8c --- /dev/null +++ b/tests/safety/test_honda.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python2 +import unittest +import numpy as np +import libpandasafety_py + + +class TestHondaSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.honda_init(0) + cls.safety.init_tests_honda() + + def _speed_msg(self, speed): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x158 << 21 + to_send[0].RDLR = speed + + return to_send + + def _button_msg(self, buttons): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x1A6 << 21 + to_send[0].RDLR = buttons << 5 + + return to_send + + def _brake_msg(self, brake): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x17C << 21 + to_send[0].RDHR = 0x200000 if brake else 0 + + return to_send + + def _gas_msg(self, gas): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x17C << 21 + to_send[0].RDLR = 1 if gas else 0 + + return to_send + + def _send_brake_msg(self, brake): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x1FA << 21 + to_send[0].RDLR = brake + + return to_send + + def _send_gas_msg(self, gas): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x200 << 21 + to_send[0].RDLR = gas + + return to_send + + def _send_steer_msg(self, steer): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0xE4 << 21 + to_send[0].RDLR = steer + + return to_send + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_resume_button(self): + RESUME_BTN = 4 + self.safety.honda_rx_hook(self._button_msg(RESUME_BTN)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_set_button(self): + SET_BTN = 3 + self.safety.honda_rx_hook(self._button_msg(SET_BTN)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_cancel_button(self): + CANCEL_BTN = 2 + self.safety.set_controls_allowed(1) + self.safety.honda_rx_hook(self._button_msg(CANCEL_BTN)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_sample_speed(self): + self.assertEqual(0, self.safety.get_ego_speed()) + self.safety.honda_rx_hook(self._speed_msg(100)) + self.assertEqual(100, self.safety.get_ego_speed()) + + def test_prev_brake(self): + self.assertFalse(self.safety.get_brake_prev()) + self.safety.honda_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_brake_prev()) + + def test_disengage_on_brake(self): + self.safety.set_controls_allowed(1) + self.safety.honda_rx_hook(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_allow_brake_at_zero_speed(self): + # Brake was already pressed + self.safety.honda_rx_hook(self._brake_msg(True)) + self.safety.set_controls_allowed(1) + + self.safety.honda_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_not_allow_brake_when_moving(self): + # Brake was already pressed + self.safety.honda_rx_hook(self._brake_msg(True)) + self.safety.honda_rx_hook(self._speed_msg(100)) + self.safety.set_controls_allowed(1) + + self.safety.honda_rx_hook(self._brake_msg(True)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_prev_gas(self): + self.assertFalse(self.safety.get_gas_prev()) + self.safety.honda_rx_hook(self._gas_msg(True)) + self.assertTrue(self.safety.get_gas_prev()) + + def test_disengage_on_gas(self): + self.safety.set_controls_allowed(1) + self.safety.honda_rx_hook(self._gas_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_allow_engage_with_gas_pressed(self): + self.safety.honda_rx_hook(self._gas_msg(1)) + self.safety.set_controls_allowed(1) + self.safety.honda_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_brake_safety_check(self): + self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) + + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x00F0))) + + def test_gas_safety_check(self): + self.assertTrue(self.safety.honda_tx_hook(self._send_brake_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_brake_msg(0x1000))) + + def test_steer_safety_check(self): + self.assertTrue(self.safety.honda_tx_hook(self._send_steer_msg(0x0000))) + self.assertFalse(self.safety.honda_tx_hook(self._send_steer_msg(0x1000))) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py new file mode 100644 index 00000000000000..1f7a9d0ac0abcc --- /dev/null +++ b/tests/safety/test_toyota.py @@ -0,0 +1,411 @@ +#!/usr/bin/env python2 +import unittest +import numpy as np +import libpandasafety_py + +MAX_RATE_UP = 10 +MAX_RATE_DOWN = 25 +MAX_TORQUE = 1500 + +MAX_ACCEL = 1500 +MIN_ACCEL = -3000 + +MAX_RT_DELTA = 375 +RT_INTERVAL = 250000 + +MAX_TORQUE_ERROR = 350 + +IPAS_OVERRIDE_THRESHOLD = 200 + +ANGLE_DELTA_BP = [0., 5., 15.] +ANGLE_DELTA_V = [5., .8, .15] # windup limit +ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + +def twos_comp(val, bits): + if val >= 0: + return val + else: + return (2**bits) + val + +def sign(a): + if a > 0: + return 1 + else: + return -1 + +class TestToyotaSafety(unittest.TestCase): + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.toyota_init(100) + cls.safety.init_tests_toyota() + + def _set_prev_torque(self, t): + self.safety.set_desired_torque_last(t) + self.safety.set_rt_torque_last(t) + self.safety.set_torque_meas(t, t) + + def _torque_meas_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x260 << 21 + + t = twos_comp(torque, 16) + to_send[0].RDHR = t | ((t & 0xFF) << 16) + return to_send + + def _torque_driver_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x260 << 21 + + t = twos_comp(torque, 16) + to_send[0].RDLR = t | ((t & 0xFF) << 16) + return to_send + + def _torque_driver_msg_array(self, torque): + for i in range(3): + self.safety.toyota_ipas_rx_hook(self._torque_driver_msg(torque)) + + def _angle_meas_msg(self, angle): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x25 << 21 + + t = twos_comp(angle, 12) + to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) + return to_send + + def _angle_meas_msg_array(self, angle): + for i in range(3): + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(angle)) + + def _torque_msg(self, torque): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x2E4 << 21 + + t = twos_comp(torque, 16) + to_send[0].RDLR = t | ((t & 0xFF) << 16) + return to_send + + def _ipas_state_msg(self, state): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x262 << 21 + + to_send[0].RDLR = state & 0xF + return to_send + + def _ipas_control_msg(self, angle, state): + # note: we command 2/3 of the angle due to CAN conversion + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x266 << 21 + + t = twos_comp(angle, 12) + to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8) + to_send[0].RDLR |= ((state & 0xf) << 4) + + return to_send + + def _speed_msg(self, speed): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0xb4 << 21 + speed = int(speed * 100 * 3.6) + + to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00) + return to_send + + def _accel_msg(self, accel): + to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_send[0].RIR = 0x343 << 21 + + a = twos_comp(accel, 16) + to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) + return to_send + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_manually_enable_controls_allowed(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_enable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x1D2 << 21 + to_push[0].RDHR = 0xF00000 + + self.safety.toyota_rx_hook(to_push) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') + to_push[0].RIR = 0x1D2 << 21 + to_push[0].RDHR = 0 + + self.safety.set_controls_allowed(1) + self.safety.toyota_rx_hook(to_push) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_accel_actuation_limits(self): + for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + + if controls_allowed: + send = MIN_ACCEL <= accel <= MAX_ACCEL + else: + send = accel == 0 + self.assertEqual(send, self.safety.toyota_tx_hook(self._accel_msg(accel))) + + def test_torque_absolute_limits(self): + for controls_allowed in [True, False]: + for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP): + self.safety.set_controls_allowed(controls_allowed) + self.safety.set_rt_torque_last(torque) + self.safety.set_torque_meas(torque, torque) + self.safety.set_desired_torque_last(torque - MAX_RATE_UP) + + if controls_allowed: + send = (-MAX_TORQUE <= torque <= MAX_TORQUE) + else: + send = torque == 0 + + self.assertEqual(send, self.safety.toyota_tx_hook(self._torque_msg(torque))) + + def test_non_realtime_limit_up(self): + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_controls_allowed(True) + + self.safety.set_rt_torque_last(1000) + self.safety.set_torque_meas(500, 500) + self.safety.set_desired_torque_last(1000) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN))) + + self.safety.set_rt_torque_last(1000) + self.safety.set_torque_meas(500, 500) + self.safety.set_desired_torque_last(1000) + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) + + def test_exceed_torque_sensor(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self._set_prev_torque(0) + for t in np.arange(0, MAX_TORQUE_ERROR + 10, 10): + t *= sign + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) + + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) + + def test_realtime_limit_up(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests_toyota() + self._set_prev_torque(0) + for t in np.arange(0, 380, 10): + t *= sign + self.safety.set_torque_meas(t, t) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) + self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * 380))) + + self._set_prev_torque(0) + for t in np.arange(0, 370, 10): + t *= sign + self.safety.set_torque_meas(t, t) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 370))) + self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(sign * 380))) + + def test_torque_measurements(self): + self.safety.toyota_rx_hook(self._torque_meas_msg(50)) + self.safety.toyota_rx_hook(self._torque_meas_msg(-50)) + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + + self.assertEqual(-51, self.safety.get_torque_meas_min()) + self.assertEqual(51, self.safety.get_torque_meas_max()) + + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + self.assertEqual(-1, self.safety.get_torque_meas_max()) + self.assertEqual(-51, self.safety.get_torque_meas_min()) + + self.safety.toyota_rx_hook(self._torque_meas_msg(0)) + self.assertEqual(-1, self.safety.get_torque_meas_max()) + self.assertEqual(-1, self.safety.get_torque_meas_min()) + + def test_ipas_override(self): + + ## angle control is not active + self.safety.set_controls_allowed(1) + + # 3 consecutive msgs where driver exceeds threshold but angle_control isn't active + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) + self.assertTrue(self.safety.get_controls_allowed()) + + # ipas state is override + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5)) + self.assertTrue(self.safety.get_controls_allowed()) + + ## now angle control is active + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 0)) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0)) + + # 3 consecutive msgs where driver does exceed threshold + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1) + self.assertFalse(self.safety.get_controls_allowed()) + + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1) + self.assertFalse(self.safety.get_controls_allowed()) + + # ipas state is override and torque isn't overriding any more + self.safety.set_controls_allowed(1) + self._torque_driver_msg_array(0) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(5)) + self.assertFalse(self.safety.get_controls_allowed()) + + # 3 consecutive msgs where driver does not exceed threshold and + # ipas state is not override + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._ipas_state_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD) + self.assertTrue(self.safety.get_controls_allowed()) + + self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD) + self.assertTrue(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_when_disabled(self): + + self.safety.set_controls_allowed(0) + + # test angle cmd too far from actual + angle_refs = [-10, 10] + deltas = range(-2, 3) + expected_results = [False, True, True, True, False] + + for a in angle_refs: + self._angle_meas_msg_array(a) + for i, d in enumerate(deltas): + self.assertEqual(expected_results[i], self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + d, 1))) + + # test ipas state cmd enabled + self._angle_meas_msg_array(0) + self.assertEqual(0, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_when_enabled(self): + + # ipas angle cmd should pass through when controls are enabled + + self.safety.set_controls_allowed(1) + self._angle_meas_msg_array(0) + self.safety.toyota_ipas_rx_hook(self._speed_msg(0.1)) + + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(4, 1))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 3))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-4, 3))) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-8, 3))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + + def test_angle_cmd_rate_when_disabled(self): + + # as long as the command is close to the measured, no rate limit is enforced when + # controls are disabled + self.safety.set_controls_allowed(0) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(0)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(0, 1))) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(100)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(100, 1))) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(-100)) + self.assertEqual(1, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(-100, 1))) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_cmd_rate_when_enabled(self): + + # when controls are allowed, angle cmd rate limit is enforced + # test 1: no limitations if we stay within limits + speeds = [0., 1., 5., 10., 15., 100.] + angles = [-300, -100, -10, 0, 10, 100, 300] + for a in angles: + for s in speeds: + + # first test against false positives + self._angle_meas_msg_array(a) + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)) + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._speed_msg(s)) + max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) + max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * max_delta_down, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + + # now inject too high rates + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * + (max_delta_up + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(1) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a + sign(a) * max_delta_up, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(True, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1))) + self.assertTrue(self.safety.get_controls_allowed()) + self.assertEqual(False, self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a - sign(a) * + (max_delta_down + 1), 1))) + self.assertFalse(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + def test_angle_measured_rate(self): + + speeds = [0., 1., 5., 10., 15., 100.] + angles = [-300, -100, -10, 0, 10, 100, 300] + angles = [10] + for a in angles: + for s in speeds: + self._angle_meas_msg_array(a) + self.safety.toyota_ipas_tx_hook(self._ipas_control_msg(a, 1)) + self.safety.set_controls_allowed(1) + self.safety.toyota_ipas_rx_hook(self._speed_msg(s)) + max_delta_up = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) * 2 / 3. + 1.) + max_delta_down = int(np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) * 2 / 3. + 1.) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.toyota_ipas_rx_hook(self._angle_meas_msg(a + 150)) + self.assertFalse(self.safety.get_controls_allowed()) + + # reset no angle control at the end of the test + self.safety.reset_angle_control() + + +if __name__ == "__main__": + unittest.main()