spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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
pull/236/head
Vehicle Researcher 2018-04-14 06:06:42 +00:00
parent a8d110ad74
commit e6e6ad2e1f
34 changed files with 1505 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
.circleci/config.yml 100644
View File

@ -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

2
.gitignore vendored
View File

@ -2,6 +2,8 @@
.*.swp
.*.swo
*.o
*.so
*.d
a.out
*~
.#*

20
.travis.yml
View File

@ -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

4
README.md
View File

@ -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
------

2
board/Makefile
View File

@ -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

1
board/build.mk
View File

@ -1,4 +1,5 @@
CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O2
CFLAGS += -Tstm32_flash.ld
CC = arm-none-eabi-gcc

133
board/drivers/can.h
View File

@ -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 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 process_can(uint8_t can_number);
void can_set_speed(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 (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;
}
// 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++;
if (tmp < CAN_TIMEOUT) {
return;
}
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;
}
}
}
void can_init(uint8_t can_number) {
if (can_number == 0xff) return;
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
set_can_enable(CAN, 1);
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_NUM_FROM_CAN_NUM(can_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;
}
// 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++;
if (tmp == CAN_TIMEOUT) {
puts("CAN init FAILED!!!!!\n");
puth(can_number); puts(" ");
puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n");
}
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

4
board/drivers/spi.h
View File

@ -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);

11
board/drivers/uart.h
View File

@ -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";

288
board/drivers/usb.h
View File

@ -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
//Convert machine byte order to USB byte order
// 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
#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,
// </PlatformCapabilityUUID>
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:

0
board/get_sdk_mac.sh 100644 → 100755
View File

31
board/main.c
View File

@ -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;

16
board/safety.h
View File

@ -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},

9
board/safety/safety_defaults.h
View File

@ -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,
};

5
board/safety/safety_elm327.h
View File

@ -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,
};

17
board/safety/safety_gm.h
View File

@ -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,
};

6
board/safety/safety_honda.h
View File

@ -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,
};

64
board/safety/safety_toyota.h
View File

@ -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,
};

193
board/safety/safety_toyota_ipas.h 100644
View File

@ -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,
};

2
boardesp/elm327.c
View File

@ -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)",},

9
boardesp/proxy.c
View File

@ -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);

4
examples/can_bit_transition.py
View File

@ -45,7 +45,7 @@ class Info():
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

4
examples/can_unique.py
View File

@ -51,10 +51,12 @@ class Info():
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:

1
python/__init__.py
View File

@ -155,6 +155,7 @@ class Panda(object):
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()

4
tests/automated/helpers.py
View File

@ -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:

17
tests/build/Dockerfile 100644
View File

@ -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

6
tests/safety/Dockerfile 100644
View File

@ -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

18
tests/safety/Makefile 100644
View File

@ -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

60
tests/safety/libpandasafety_py.py 100644
View File

@ -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)

2
tests/safety/requirements.txt 100644
View File

@ -0,0 +1,2 @@
cffi==1.11.4
numpy==1.14.1

108
tests/safety/test.c 100644
View File

@ -0,0 +1,108 @@
#include <stdint.h>
#include <stdbool.h>
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;
}

2
tests/safety/test.sh 100755
View File

@ -0,0 +1,2 @@
#!/usr/bin/env sh
python -m unittest discover .

148
tests/safety/test_honda.py 100755
View File

@ -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()

411
tests/safety/test_toyota.py 100644
View File

@ -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()