Merge panda subtree
commit
94a27e351f
|
@ -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,6 +2,8 @@
|
|||
.*.swp
|
||||
.*.swo
|
||||
*.o
|
||||
*.so
|
||||
*.d
|
||||
a.out
|
||||
*~
|
||||
.#*
|
||||
|
|
|
@ -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
|
|
@ -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,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,4 +1,5 @@
|
|||
CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O2
|
||||
|
||||
CFLAGS += -Tstm32_flash.ld
|
||||
|
||||
CC = arm-none-eabi-gcc
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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";
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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},
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
|
|
@ -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,
|
||||
};
|
||||
|
|
@ -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)",},
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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)
|
|
@ -0,0 +1,2 @@
|
|||
cffi==1.11.4
|
||||
numpy==1.14.1
|
|
@ -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;
|
||||
}
|
|
@ -0,0 +1,2 @@
|
|||
#!/usr/bin/env sh
|
||||
python -m unittest discover .
|
|
@ -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()
|
|
@ -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()
|
Loading…
Reference in New Issue