From e6e6ad2e1fd1c0bed381a4cbabf8d8afd9f64716 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Sat, 14 Apr 2018 06:06:42 +0000 Subject: [PATCH] Squashed 'panda/' changes from 3125232..2253dd3 2253dd3 fix volt ign detect 3b299d7 add ignition and refactor af9af6d Merge pull request #110 from Jamezz/volt 13e850e more correct f295063 add new define to tests fec9758 gate that with debug 5516ebf one more ifdef cac7b31 only panda has float 938d474 fpu enable ffbf0c7 cleaner de30f27 Revert "need f to not be double" 4142acf need f to not be double 3eb15c8 refactor to share code a4c8b64 change to O2 to fix make recover 711fd11 Enable compiler optimizations, fix things it breaks 2e6f774 block IPAS in main toyota safety mode e7a2b3a add ipas tests 894572c fix tests 367c9ad add safety toyota ipas 95919b9 Bounty: panda high quality CAN autobaud (#96) 6557cd2 Toyota Safety: allow controls only on rising edge of cruise_engaged 02c1ddf Revert "added steer override check when IPAS is in control (#106)" 9f925ba Fix the merge mess 23d3833 Merge from comma upstream a0cc51a Undo safety mode override ea1c1dc make wlan interface name generic 6dbd8c9 Implement WebUSB and upgrade WinUSB to 2.0 (#107) 4fc83a5 Add safety hook for ignition and have GM use gear selector to determine ignition 52b2ac0 switch from travis to circleci 48e2374 build panda esp image 065572a circleci build stm image 7a1f319 add panda python package test and fix safety test 021dde7 move saftey test helper files into safety folder ce0545f add ci files 6a3307c no LIN over ELM 7d21acb added steer override check when IPAS is in control (#106) 1c88caf Safety code testing (#104) f4efd1f Merge pull request #101 from adhintz/master c02618b Merge pull request #102 from quillford/master 1ba5f8a added link to wiki for user scripts de2b19e add support for multiple buses to can_unique and can_bittransition output data in sorted order. git-subtree-dir: panda git-subtree-split: 2253dd3c48e21abb82fe161d6f58237490111206 --- .circleci/config.yml | 41 +++ .gitignore | 2 + .travis.yml | 20 -- README.md | 4 +- board/Makefile | 2 +- board/build.mk | 1 + board/drivers/can.h | 127 ++++++--- board/drivers/spi.h | 4 +- board/drivers/uart.h | 11 + board/drivers/usb.h | 288 +++++++++++++++++---- board/get_sdk_mac.sh | 0 board/main.c | 31 ++- board/safety.h | 16 ++ board/safety/safety_defaults.h | 9 + board/safety/safety_elm327.h | 5 + board/safety/safety_gm.h | 17 +- board/safety/safety_honda.h | 6 + board/safety/safety_toyota.h | 64 +++-- board/safety/safety_toyota_ipas.h | 193 ++++++++++++++ boardesp/elm327.c | 2 +- boardesp/proxy.c | 9 +- examples/can_bit_transition.py | 4 +- examples/can_unique.py | 4 +- python/__init__.py | 1 + tests/automated/helpers.py | 4 +- tests/build/Dockerfile | 17 ++ tests/safety/Dockerfile | 6 + tests/safety/Makefile | 18 ++ tests/safety/libpandasafety_py.py | 60 +++++ tests/safety/requirements.txt | 2 + tests/safety/test.c | 108 ++++++++ tests/safety/test.sh | 2 + tests/safety/test_honda.py | 148 +++++++++++ tests/safety/test_toyota.py | 411 ++++++++++++++++++++++++++++++ 34 files changed, 1502 insertions(+), 135 deletions(-) create mode 100644 .circleci/config.yml delete mode 100644 .travis.yml mode change 100644 => 100755 board/get_sdk_mac.sh create mode 100644 board/safety/safety_toyota_ipas.h create mode 100644 tests/build/Dockerfile create mode 100644 tests/safety/Dockerfile create mode 100644 tests/safety/Makefile create mode 100644 tests/safety/libpandasafety_py.py create mode 100644 tests/safety/requirements.txt create mode 100644 tests/safety/test.c create mode 100755 tests/safety/test.sh create mode 100755 tests/safety/test_honda.py create mode 100644 tests/safety/test_toyota.py 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()