Merge from comma upstream
Jamezz
commit
23d3833d77
|
|
@ -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
|
||||
*~
|
||||
.#*
|
||||
|
|
|
|||
20
.travis.yml
20
.travis.yml
|
|
@ -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.
|
||||
|
||||
[](https://travis-ci.org/commaai/panda)
|
||||
[](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
|
||||
------
|
||||
|
|
|
|||
|
|
@ -24,6 +24,12 @@
|
|||
#include "drivers/usb.h"
|
||||
//#include "drivers/uart.h"
|
||||
|
||||
#ifdef PEDAL
|
||||
#define CUSTOM_CAN_INTERRUPTS
|
||||
#include "safety.h"
|
||||
#include "drivers/can.h"
|
||||
#endif
|
||||
|
||||
int puts(const char *a) { return 0; }
|
||||
void puth(unsigned int i) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O2
|
||||
CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -O0
|
||||
CFLAGS += -Tstm32_flash.ld
|
||||
|
||||
CC = arm-none-eabi-gcc
|
||||
|
|
|
|||
|
|
@ -348,13 +348,14 @@ void can_rx(uint8_t can_number) {
|
|||
|
||||
// forwarding (panda only)
|
||||
#ifdef PANDA
|
||||
if (can_forwarding[bus_number] != -1) {
|
||||
int bus_fwd_num = can_forwarding[bus_number] != -1 ? can_forwarding[bus_number] : safety_fwd_hook(bus_number, &to_push);
|
||||
if (bus_fwd_num != -1) {
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RIR = to_push.RIR | 1; // TXRQ
|
||||
to_send.RDTR = to_push.RDTR;
|
||||
to_send.RDLR = to_push.RDLR;
|
||||
to_send.RDHR = to_push.RDHR;
|
||||
can_send(&to_send, can_forwarding[bus_number]);
|
||||
can_send(&to_send, bus_fwd_num);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -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,10 @@ void usb_setup() {
|
|||
}
|
||||
USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
|
||||
break;
|
||||
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;
|
||||
default:
|
||||
// nothing here?
|
||||
USB_WritePacket(0, 0, 0);
|
||||
|
|
@ -439,14 +608,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:
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
#!/bin/bash
|
||||
sudo apt-get install gcc-arm-none-eabi python-pip
|
||||
sudo pip2 install libusb1
|
||||
sudo pip2 install libusb1 pycrypto requests
|
||||
|
|
|
|||
|
|
@ -2,4 +2,4 @@
|
|||
# Need formula for gcc
|
||||
brew tap ArmMbed/homebrew-formulae
|
||||
brew install python dfu-util arm-none-eabi-gcc
|
||||
pip2 install libusb1
|
||||
pip2 install libusb1 pycrypto requests
|
||||
|
|
|
|||
|
|
@ -1,31 +1,58 @@
|
|||
# :set noet
|
||||
PROJ_NAME = comma
|
||||
|
||||
CFLAGS = -O2 -Wall -std=gnu11
|
||||
CFLAGS = -O2 -Wall -std=gnu11 -DPEDAL
|
||||
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
|
||||
CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
|
||||
CFLAGS += -I ../inc -I ../ -nostdlib
|
||||
CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib
|
||||
CFLAGS += -T../stm32_flash.ld
|
||||
|
||||
STARTUP_FILE = startup_stm32f205xx
|
||||
|
||||
CC = arm-none-eabi-gcc
|
||||
OBJCOPY = arm-none-eabi-objcopy
|
||||
OBJDUMP = arm-none-eabi-objdump
|
||||
DFU_UTIL = "dfu-util"
|
||||
|
||||
all: obj/$(PROJ_NAME).bin
|
||||
#$(OBJDUMP) -d obj/$(PROJ_NAME).elf
|
||||
dfu-util -d 0483:df11 -a 0 -s 0x08000000:leave -D $<
|
||||
# pedal only uses the debug cert
|
||||
CERT = ../../certs/debug
|
||||
CFLAGS += "-DALLOW_DEBUG"
|
||||
|
||||
canflash: obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py $<
|
||||
|
||||
usbflash: obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py; sleep 0.5
|
||||
PYTHONPATH=../../ python -c "from python import Panda; p = [x for x in [Panda(x) for x in Panda.list()] if x.bootstub]; assert(len(p)==1); p[0].flash('obj/$(PROJ_NAME).bin', reconnect=False)"
|
||||
|
||||
recover: obj/bootstub.bin obj/$(PROJ_NAME).bin
|
||||
../../tests/pedal/enter_canloader.py --recover; sleep 0.5
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin
|
||||
$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin
|
||||
|
||||
obj/main.o: main.c ../*.h
|
||||
mkdir -p obj
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/startup_stm32f205xx.o: ../startup_stm32f205xx.s
|
||||
obj/bootstub.o: ../bootstub.c ../*.h
|
||||
mkdir -p obj
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(PROJ_NAME).bin: obj/startup_stm32f205xx.o obj/main.o
|
||||
$(CC) $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf $@
|
||||
obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/%.o: ../../crypto/%.c
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.o
|
||||
# hack
|
||||
$(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
|
||||
SETLEN=1 ../../crypto/sign.py obj/code.bin $@ $(CERT)
|
||||
|
||||
obj/bootstub.bin: obj/$(STARTUP_FILE).o obj/bootstub.o obj/sha.o obj/rsa.o
|
||||
$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^
|
||||
$(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@
|
||||
|
||||
clean:
|
||||
rm -f obj/*
|
||||
|
|
|
|||
|
|
@ -2,5 +2,27 @@ This is the firmware for the comma pedal. It borrows a lot from panda.
|
|||
|
||||
The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "virtually" press the pedal.
|
||||
|
||||
This is the open source software. Open source hardware coming soon.
|
||||
This is the open source software. Note that it is not ready to use yet.
|
||||
|
||||
== Test Plan ==
|
||||
|
||||
* Startup
|
||||
** Confirm STATE_FAULT_STARTUP
|
||||
* Timeout
|
||||
** Send value
|
||||
** Confirm value is output
|
||||
** Stop sending messages
|
||||
** Confirm value is passthru after 100ms
|
||||
** Confirm STATE_FAULT_TIMEOUT
|
||||
* Random values
|
||||
** Send random 6 byte messages
|
||||
** Confirm random values cause passthru
|
||||
** Confirm STATE_FAULT_BAD_CHECKSUM
|
||||
* Same message lockout
|
||||
** Send same message repeated
|
||||
** Confirm timeout behavior
|
||||
* Don't set enable
|
||||
** Confirm no output
|
||||
* Set enable and values
|
||||
** Confirm output
|
||||
|
||||
|
|
|
|||
|
|
@ -2,8 +2,6 @@
|
|||
//#define CAN_LOOPBACK_MODE
|
||||
//#define USE_INTERNAL_OSC
|
||||
|
||||
#define PEDAL
|
||||
|
||||
#include "../config.h"
|
||||
|
||||
#include "drivers/drivers.h"
|
||||
|
|
@ -32,14 +30,7 @@
|
|||
uint32_t enter_bootloader_mode;
|
||||
|
||||
void __initialize_hardware_early() {
|
||||
if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
|
||||
enter_bootloader_mode = 0;
|
||||
void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004));
|
||||
bootloader();
|
||||
|
||||
// LOOP
|
||||
while(1);
|
||||
}
|
||||
early();
|
||||
}
|
||||
|
||||
// ********************* serial debugging *********************
|
||||
|
|
@ -107,10 +98,23 @@ void CAN1_TX_IRQHandler() {
|
|||
CAN->TSR |= CAN_TSR_RQCP0;
|
||||
}
|
||||
|
||||
uint16_t gas_set = 0;
|
||||
// two independent values
|
||||
uint16_t gas_set_0 = 0;
|
||||
uint16_t gas_set_1 = 0;
|
||||
|
||||
#define MAX_TIMEOUT 10
|
||||
uint32_t timeout = 0;
|
||||
uint32_t current_index = 0;
|
||||
|
||||
#define NO_FAULT 0
|
||||
#define FAULT_BAD_CHECKSUM 1
|
||||
#define FAULT_SEND 2
|
||||
#define FAULT_SCE 3
|
||||
#define FAULT_STARTUP 4
|
||||
#define FAULT_TIMEOUT 5
|
||||
#define FAULT_INVALID 6
|
||||
uint8_t state = FAULT_STARTUP;
|
||||
|
||||
void CAN1_RX0_IRQHandler() {
|
||||
while (CAN->RF0R & CAN_RF0R_FMP0) {
|
||||
#ifdef DEBUG
|
||||
|
|
@ -118,22 +122,50 @@ void CAN1_RX0_IRQHandler() {
|
|||
#endif
|
||||
uint32_t address = CAN->sFIFOMailBox[0].RIR>>21;
|
||||
if (address == CAN_GAS_INPUT) {
|
||||
// softloader entry
|
||||
if (CAN->sFIFOMailBox[0].RDLR == 0xdeadface) {
|
||||
if (CAN->sFIFOMailBox[0].RDHR == 0x0ab00b1e) {
|
||||
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
} else if (CAN->sFIFOMailBox[0].RDHR == 0x02b00b1e) {
|
||||
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
}
|
||||
|
||||
// normal packet
|
||||
uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR;
|
||||
uint16_t value = (dat[0] << 8) | dat[1];
|
||||
uint8_t index = (dat[2] >> 4) & 3;
|
||||
if (can_cksum(dat, 2, CAN_GAS_INPUT, index) == (dat[2] & 0xF)) {
|
||||
uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR;
|
||||
uint16_t value_0 = (dat[0] << 8) | dat[1];
|
||||
uint16_t value_1 = (dat[2] << 8) | dat[3];
|
||||
uint8_t enable = (dat2[0] >> 7) & 1;
|
||||
uint8_t index = (dat2[1] >> 4) & 3;
|
||||
if (can_cksum(dat, 5, CAN_GAS_INPUT, index) == (dat2[1] & 0xF)) {
|
||||
if (((current_index+1)&3) == index) {
|
||||
// TODO: set and start timeout
|
||||
#ifdef DEBUG
|
||||
puts("setting gas ");
|
||||
puth(value);
|
||||
puts("\n");
|
||||
#endif
|
||||
gas_set = value;
|
||||
if (enable) {
|
||||
gas_set_0 = value_0;
|
||||
gas_set_1 = value_1;
|
||||
} else {
|
||||
// clear the fault state if values are 0
|
||||
if (value_0 == 0 && value_1 == 0) {
|
||||
state = NO_FAULT;
|
||||
} else {
|
||||
state = FAULT_INVALID;
|
||||
}
|
||||
gas_set_0 = gas_set_1 = 0;
|
||||
}
|
||||
// clear the timeout
|
||||
timeout = 0;
|
||||
}
|
||||
// TODO: better lockout? prevents same spam
|
||||
current_index = index;
|
||||
} else {
|
||||
// wrong checksum = fault
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
}
|
||||
}
|
||||
// next
|
||||
|
|
@ -142,6 +174,7 @@ void CAN1_RX0_IRQHandler() {
|
|||
}
|
||||
|
||||
void CAN1_SCE_IRQHandler() {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN);
|
||||
}
|
||||
|
||||
|
|
@ -162,25 +195,27 @@ void TIM3_IRQHandler() {
|
|||
|
||||
// check timer for sending the user pedal and clearing the CAN
|
||||
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
uint8_t *dat = (uint8_t *)&CAN->sTxMailBox[0].TDLR;
|
||||
CAN->sTxMailBox[0].TDLR = (((pdl0>>8)&0xFF)<<0) |
|
||||
(((pdl0>>0)&0xFF)<<8) |
|
||||
(((pdl1>>8)&0xFF)<<16) |
|
||||
(((pdl1>>0)&0xFF)<<24);
|
||||
CAN->sTxMailBox[0].TDHR = can_cksum(dat, 4, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx << 4);
|
||||
CAN->sTxMailBox[0].TDTR = 5; // len of packet is 4
|
||||
uint8_t dat[8];
|
||||
dat[0] = (pdl0>>8)&0xFF;
|
||||
dat[1] = (pdl0>>0)&0xFF;
|
||||
dat[2] = (pdl1>>8)&0xFF;
|
||||
dat[3] = (pdl1>>0)&0xFF;
|
||||
dat[4] = state;
|
||||
dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx<<4);
|
||||
CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24);
|
||||
CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8);
|
||||
CAN->sTxMailBox[0].TDTR = 6; // len of packet is 5
|
||||
CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1;
|
||||
++pkt_idx;
|
||||
pkt_idx &= 3;
|
||||
} else {
|
||||
// old can packet hasn't sent!
|
||||
// TODO: do something?
|
||||
state = FAULT_SEND;
|
||||
#ifdef DEBUG
|
||||
puts("CAN MISS\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// blink the LED
|
||||
set_led(LED_GREEN, led_value);
|
||||
led_value = !led_value;
|
||||
|
|
@ -188,7 +223,11 @@ void TIM3_IRQHandler() {
|
|||
TIM3->SR = 0;
|
||||
|
||||
// up timeout for gas set
|
||||
timeout++;
|
||||
if (timeout == MAX_TIMEOUT) {
|
||||
state = FAULT_TIMEOUT;
|
||||
} else {
|
||||
timeout += 1;
|
||||
}
|
||||
}
|
||||
|
||||
// ***************************** main code *****************************
|
||||
|
|
@ -199,13 +238,16 @@ void pedal() {
|
|||
pdl1 = adc_get(ADCCHAN_ACCEL1);
|
||||
|
||||
// write the pedal to the DAC
|
||||
if (timeout < 10) {
|
||||
dac_set(0, max(gas_set, pdl0));
|
||||
dac_set(1, max(gas_set*2, pdl1));
|
||||
if (state == NO_FAULT) {
|
||||
dac_set(0, max(gas_set_0, pdl0));
|
||||
dac_set(1, max(gas_set_1, pdl1));
|
||||
} else {
|
||||
dac_set(0, pdl0);
|
||||
dac_set(1, pdl1);
|
||||
}
|
||||
|
||||
// feed the watchdog
|
||||
IWDG->KR = 0xAAAA;
|
||||
}
|
||||
|
||||
int main() {
|
||||
|
|
@ -227,18 +269,19 @@ int main() {
|
|||
|
||||
// init can
|
||||
can_silent = ALL_CAN_LIVE;
|
||||
can_init_all();
|
||||
can_init(0);
|
||||
|
||||
// 48mhz / 65536 ~= 732
|
||||
timer_init(TIM3, 15);
|
||||
|
||||
// needed?
|
||||
NVIC_EnableIRQ(CAN1_TX_IRQn);
|
||||
NVIC_EnableIRQ(CAN1_RX0_IRQn);
|
||||
NVIC_EnableIRQ(CAN1_SCE_IRQn);
|
||||
|
||||
NVIC_EnableIRQ(TIM3_IRQn);
|
||||
|
||||
// setup watchdog
|
||||
IWDG->KR = 0x5555;
|
||||
IWDG->PR = 0; // divider /4
|
||||
// 0 = 0.125 ms, let's have a 50ms watchdog
|
||||
IWDG->RLR = 400 - 1;
|
||||
IWDG->KR = 0xCCCC;
|
||||
|
||||
puts("**** INTERRUPTS ON ****\n");
|
||||
__enable_irq();
|
||||
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ 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;
|
||||
|
|
@ -15,6 +16,7 @@ typedef struct {
|
|||
rx_hook rx;
|
||||
tx_hook tx;
|
||||
tx_lin_hook tx_lin;
|
||||
fwd_hook fwd;
|
||||
} safety_hooks;
|
||||
|
||||
// This can be set by the safety hooks.
|
||||
|
|
@ -47,6 +49,9 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len){
|
|||
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);
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
uint16_t id;
|
||||
|
|
@ -58,12 +63,14 @@ typedef struct {
|
|||
#define SAFETY_TOYOTA 2
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_GM 3
|
||||
#define SAFETY_HONDA_BOSCH 4
|
||||
#define SAFETY_ALLOUTPUT 0x1337
|
||||
#define SAFETY_ELM327 0xE327
|
||||
|
||||
const safety_hook_config safety_hook_registry[] = {
|
||||
{SAFETY_NOOUTPUT, &nooutput_hooks},
|
||||
{SAFETY_HONDA, &honda_hooks},
|
||||
{SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
|
||||
{SAFETY_TOYOTA, &toyota_hooks},
|
||||
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
|
||||
{SAFETY_GM, &gm_hooks},
|
||||
|
|
|
|||
|
|
@ -17,6 +17,9 @@ static int nooutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|||
static int nooutput_ign_hook() {
|
||||
return -1;
|
||||
}
|
||||
static int nooutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks nooutput_hooks = {
|
||||
.init = nooutput_init,
|
||||
|
|
@ -24,6 +27,7 @@ const safety_hooks nooutput_hooks = {
|
|||
.tx = nooutput_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.ignition = nooutput_ign_hook
|
||||
.fwd = nooutput_fwd_hook,
|
||||
};
|
||||
|
||||
// *** all output safety mode ***
|
||||
|
|
@ -41,6 +45,10 @@ static int alloutput_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|||
}
|
||||
|
||||
static int alloutput_ign_hook() {
|
||||
return -1
|
||||
}
|
||||
|
||||
static int alloutput_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -50,5 +58,6 @@ const safety_hooks alloutput_hooks = {
|
|||
.tx = alloutput_tx_hook,
|
||||
.tx_lin = alloutput_tx_lin_hook,
|
||||
.ignition = alloutput_ign_hook
|
||||
.fwd = alloutput_fwd_hook,
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -35,10 +35,15 @@ static int elm327_ign_hook() {
|
|||
return -1;
|
||||
}
|
||||
|
||||
static int elm327_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks elm327_hooks = {
|
||||
.init = elm327_init,
|
||||
.rx = elm327_rx_hook,
|
||||
.tx = elm327_tx_hook,
|
||||
.tx_lin = elm327_tx_lin_hook,
|
||||
.ignition = elm327_ign_hook
|
||||
.fwd = elm327_fwd_hook,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -184,11 +184,16 @@ static int gm_ign_hook() {
|
|||
return gm_ignition_started;
|
||||
}
|
||||
|
||||
static int gm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks gm_hooks = {
|
||||
.init = gm_init,
|
||||
.rx = gm_rx_hook,
|
||||
.tx = gm_tx_hook,
|
||||
.tx_lin = gm_tx_lin_hook,
|
||||
.ignition = gm_ign_hook
|
||||
.fwd = gm_fwd_hook,
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -8,11 +8,14 @@
|
|||
// brake > 0mph
|
||||
|
||||
// these are set in the Honda safety hooks...this is the wrong place
|
||||
const int gas_interceptor_threshold = 328;
|
||||
int gas_interceptor_detected = 0;
|
||||
int brake_prev = 0;
|
||||
int gas_prev = 0;
|
||||
int gas_interceptor_prev = 0;
|
||||
int ego_speed = 0;
|
||||
// TODO: auto-detect bosch hardware based on CAN messages?
|
||||
bool bosch_hardware = false;
|
||||
|
||||
static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
|
||||
|
|
@ -33,22 +36,28 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
}
|
||||
}
|
||||
|
||||
// user brake signal is different for nidec vs bosch hardware
|
||||
// nidec hardware: 0x17C bit 53
|
||||
// bosch hardware: 0x1BE bit 4
|
||||
#define IS_USER_BRAKE_MSG(to_push) (!bosch_hardware ? to_push->RIR>>21 == 0x17C : to_push->RIR>>21 == 0x1BE)
|
||||
#define USER_BRAKE_VALUE(to_push) (!bosch_hardware ? to_push->RDHR & 0x200000 : to_push->RDLR & 0x10)
|
||||
// exit controls on rising edge of brake press or on brake press when
|
||||
// speed > 0
|
||||
if ((to_push->RIR>>21) == 0x17C) {
|
||||
// bit 53
|
||||
int brake = to_push->RDHR & 0x200000;
|
||||
if (IS_USER_BRAKE_MSG(to_push)) {
|
||||
int brake = USER_BRAKE_VALUE(to_push);
|
||||
if (brake && (!(brake_prev) || ego_speed)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_prev = brake;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press if interceptor
|
||||
if ((to_push->RIR>>21) == 0x201) {
|
||||
// exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
|
||||
// length check because bosch hardware also uses this id (0x201 w/ len = 8)
|
||||
if ((to_push->RIR>>21) == 0x201 && (to_push->RDTR & 0xf) == 6) {
|
||||
gas_interceptor_detected = 1;
|
||||
int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8);
|
||||
if ((gas_interceptor > 328) && (gas_interceptor_prev <= 328)) {
|
||||
if ((gas_interceptor > gas_interceptor_threshold) &&
|
||||
(gas_interceptor_prev <= gas_interceptor_threshold)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
gas_interceptor_prev = gas_interceptor;
|
||||
|
|
@ -76,7 +85,8 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// disallow actuator commands if gas or brake (with vehicle moving) are pressed
|
||||
// and the the latching controls_allowed flag is True
|
||||
int pedal_pressed = gas_prev || gas_interceptor_prev || (brake_prev && ego_speed);
|
||||
int pedal_pressed = gas_prev || (gas_interceptor_prev > gas_interceptor_threshold) ||
|
||||
(brake_prev && ego_speed);
|
||||
int current_controls_allowed = controls_allowed && !(pedal_pressed);
|
||||
|
||||
// BRAKE: safety check
|
||||
|
|
@ -117,6 +127,11 @@ static int honda_tx_lin_hook(int lin_num, uint8_t *data, int len) {
|
|||
|
||||
static void honda_init(int16_t param) {
|
||||
controls_allowed = 0;
|
||||
bosch_hardware = false;
|
||||
}
|
||||
|
||||
static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int honda_ign_hook() {
|
||||
|
|
@ -129,5 +144,26 @@ const safety_hooks honda_hooks = {
|
|||
.tx = honda_tx_hook,
|
||||
.tx_lin = honda_tx_lin_hook,
|
||||
.ignition = honda_ign_hook
|
||||
.fwd = honda_fwd_hook,
|
||||
};
|
||||
|
||||
static void honda_bosch_init(int16_t param) {
|
||||
controls_allowed = 0;
|
||||
bosch_hardware = true;
|
||||
}
|
||||
|
||||
static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
if (bus_num == 1 || bus_num == 2) {
|
||||
int addr = to_fwd->RIR>>21;
|
||||
return addr != 0xE4 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks honda_bosch_hooks = {
|
||||
.init = honda_bosch_init,
|
||||
.rx = honda_rx_hook,
|
||||
.tx = honda_tx_hook,
|
||||
.tx_lin = honda_tx_lin_hook,
|
||||
.fwd = honda_bosch_fwd_hook,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,15 @@
|
|||
int cruise_engaged_last = 0; // cruise state
|
||||
int ipas_state = 0;
|
||||
|
||||
// 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;
|
||||
int16_t torque_driver[3] = {0, 0, 0}; // last 3 driver steering torque
|
||||
int16_t torque_driver_min = 0, torque_driver_max = 0;
|
||||
|
||||
// IPAS override
|
||||
const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
|
||||
int angle_control = 0; // 1 if direct angle control packets are seen
|
||||
|
||||
// global torque limit
|
||||
const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever
|
||||
|
|
@ -30,12 +39,14 @@ int16_t rt_torque_last = 0; // last desired torque for real time chec
|
|||
uint32_t ts_last = 0;
|
||||
|
||||
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
// get eps motor torque (0.66 factor in dbc)
|
||||
|
||||
// EPS torque sensor
|
||||
if ((to_push->RIR>>21) == 0x260) {
|
||||
int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
|
||||
// get eps motor torque (see dbc_eps_torque_factor in dbc)
|
||||
int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
|
||||
|
||||
// increase torque_meas by 1 to be conservative on rounding
|
||||
torque_meas_new = (torque_meas_new * dbc_eps_torque_factor / 100) + (torque_meas_new > 0 ? 1 : -1);
|
||||
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--) {
|
||||
|
|
@ -49,16 +60,46 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
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];
|
||||
}
|
||||
|
||||
// get driver steering torque
|
||||
int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF));
|
||||
|
||||
// shift the array
|
||||
for (int i = sizeof(torque_driver)/sizeof(torque_driver[0]) - 1; i > 0; i--) {
|
||||
torque_driver[i] = torque_driver[i-1];
|
||||
}
|
||||
torque_driver[0] = torque_driver_new;
|
||||
|
||||
// get the minimum and maximum driver torque over the last 3 frames
|
||||
torque_driver_min = torque_driver_max = torque_driver[0];
|
||||
for (int i = 1; i < sizeof(torque_driver)/sizeof(torque_driver[0]); i++) {
|
||||
if (torque_driver[i] < torque_driver_min) torque_driver_min = torque_driver[i];
|
||||
if (torque_driver[i] > torque_driver_max) torque_driver_max = torque_driver[i];
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -79,7 +120,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
}
|
||||
}
|
||||
|
||||
// STEER: safety check on bytes 2-3
|
||||
// STEER ANGLE
|
||||
if ((to_send->RIR>>21) == 0x266) {
|
||||
angle_control = 1;
|
||||
}
|
||||
|
||||
// STEER TORQUE: safety check on bytes 2-3
|
||||
if ((to_send->RIR>>21) == 0x2E4) {
|
||||
int16_t desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
|
||||
int16_t violation = 0;
|
||||
|
|
@ -165,12 +211,17 @@ static int toyota_ign_hook() {
|
|||
return -1;
|
||||
}
|
||||
|
||||
static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.tx_lin = toyota_tx_lin_hook,
|
||||
.ignition = toyota_ign_hook
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
|
||||
static void toyota_nolimits_init(int16_t param) {
|
||||
|
|
@ -185,4 +236,5 @@ const safety_hooks toyota_nolimits_hooks = {
|
|||
.tx = toyota_tx_hook,
|
||||
.tx_lin = toyota_tx_lin_hook,
|
||||
.ignition = toyota_ign_hook
|
||||
.fwd = toyota_fwd_hook,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -130,6 +130,120 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
|
|||
return resp_len;
|
||||
}
|
||||
|
||||
#ifdef PEDAL
|
||||
|
||||
#define CAN CAN1
|
||||
|
||||
#define CAN_BL_INPUT 0x1
|
||||
#define CAN_BL_OUTPUT 0x2
|
||||
|
||||
void CAN1_TX_IRQHandler() {
|
||||
// clear interrupt
|
||||
CAN->TSR |= CAN_TSR_RQCP0;
|
||||
}
|
||||
|
||||
#define ISOTP_BUF_SIZE 0x110
|
||||
|
||||
uint8_t isotp_buf[ISOTP_BUF_SIZE];
|
||||
uint8_t *isotp_buf_ptr = NULL;
|
||||
int isotp_buf_remain = 0;
|
||||
|
||||
uint8_t isotp_buf_out[ISOTP_BUF_SIZE];
|
||||
uint8_t *isotp_buf_out_ptr = NULL;
|
||||
int isotp_buf_out_remain = 0;
|
||||
int isotp_buf_out_idx = 0;
|
||||
|
||||
void bl_can_send(uint8_t *odat) {
|
||||
// wait for send
|
||||
while (!(CAN->TSR & CAN_TSR_TME0));
|
||||
|
||||
// send continue
|
||||
CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0];
|
||||
CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1];
|
||||
CAN->sTxMailBox[0].TDTR = 8;
|
||||
CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
|
||||
}
|
||||
|
||||
void CAN1_RX0_IRQHandler() {
|
||||
while (CAN->RF0R & CAN_RF0R_FMP0) {
|
||||
if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
|
||||
uint8_t dat[8];
|
||||
((uint32_t*)dat)[0] = CAN->sFIFOMailBox[0].RDLR;
|
||||
((uint32_t*)dat)[1] = CAN->sFIFOMailBox[0].RDHR;
|
||||
uint8_t odat[8];
|
||||
uint8_t type = dat[0] & 0xF0;
|
||||
if (type == 0x30) {
|
||||
// continue
|
||||
while (isotp_buf_out_remain > 0) {
|
||||
// wait for send
|
||||
while (!(CAN->TSR & CAN_TSR_TME0));
|
||||
|
||||
odat[0] = 0x20 | isotp_buf_out_idx;
|
||||
memcpy(odat+1, isotp_buf_out_ptr, 7);
|
||||
isotp_buf_out_remain -= 7;
|
||||
isotp_buf_out_ptr += 7;
|
||||
isotp_buf_out_idx++;
|
||||
|
||||
bl_can_send(odat);
|
||||
}
|
||||
} else if (type == 0x20) {
|
||||
if (isotp_buf_remain > 0) {
|
||||
memcpy(isotp_buf_ptr, dat+1, 7);
|
||||
isotp_buf_ptr += 7;
|
||||
isotp_buf_remain -= 7;
|
||||
}
|
||||
if (isotp_buf_remain <= 0) {
|
||||
int len = isotp_buf_ptr - isotp_buf + isotp_buf_remain;
|
||||
|
||||
// call the function
|
||||
memset(isotp_buf_out, 0, ISOTP_BUF_SIZE);
|
||||
isotp_buf_out_remain = spi_cb_rx(isotp_buf, len, isotp_buf_out);
|
||||
isotp_buf_out_ptr = isotp_buf_out;
|
||||
isotp_buf_out_idx = 0;
|
||||
|
||||
// send initial
|
||||
if (isotp_buf_out_remain <= 7) {
|
||||
odat[0] = isotp_buf_out_remain;
|
||||
memcpy(odat+1, isotp_buf_out_ptr, isotp_buf_out_remain);
|
||||
} else {
|
||||
odat[0] = 0x10 | (isotp_buf_out_remain>>8);
|
||||
odat[1] = isotp_buf_out_remain & 0xFF;
|
||||
memcpy(odat+2, isotp_buf_out_ptr, 6);
|
||||
isotp_buf_out_remain -= 6;
|
||||
isotp_buf_out_ptr += 6;
|
||||
isotp_buf_out_idx++;
|
||||
}
|
||||
|
||||
bl_can_send(odat);
|
||||
}
|
||||
} else if (type == 0x10) {
|
||||
int len = ((dat[0]&0xF)<<8) | dat[1];
|
||||
|
||||
// setup buffer
|
||||
isotp_buf_ptr = isotp_buf;
|
||||
memcpy(isotp_buf_ptr, dat+2, 6);
|
||||
|
||||
if (len < (ISOTP_BUF_SIZE-0x10)) {
|
||||
isotp_buf_ptr += 6;
|
||||
isotp_buf_remain = len-6;
|
||||
}
|
||||
|
||||
memset(odat, 0, 8);
|
||||
odat[0] = 0x30;
|
||||
bl_can_send(odat);
|
||||
}
|
||||
}
|
||||
// next
|
||||
CAN->RF0R |= CAN_RF0R_RFOM0;
|
||||
}
|
||||
}
|
||||
|
||||
void CAN1_SCE_IRQHandler() {
|
||||
can_sce(CAN);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
void soft_flasher_start() {
|
||||
puts("\n\n\n************************ FLASHER START ************************\n");
|
||||
|
||||
|
|
@ -140,6 +254,20 @@ void soft_flasher_start() {
|
|||
RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
|
||||
RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
|
||||
|
||||
// pedal has the canloader
|
||||
#ifdef PEDAL
|
||||
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
|
||||
|
||||
// B8,B9: CAN 1
|
||||
set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
|
||||
set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
|
||||
set_can_enable(CAN1, 1);
|
||||
|
||||
// init can
|
||||
can_silent = ALL_CAN_LIVE;
|
||||
can_init(0);
|
||||
#endif
|
||||
|
||||
// A4,A5,A6,A7: setup SPI
|
||||
set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
|
||||
set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
|
||||
|
|
|
|||
|
|
@ -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)",},
|
||||
|
|
|
|||
|
|
@ -137,5 +137,14 @@ More details here:
|
|||
relaxed to allow serialization of messages based on their address
|
||||
(making multiple queues, effectively one queue per address).
|
||||
|
||||
# Troubleshooting:
|
||||
troubleshooting:
|
||||
1. Install DrewTech J2534-1 Bus Analysis Tool
|
||||
http://www.drewtech.com/downloads/tools/Drew%20Technologies%20Tool%20for%20J2534-1%20API%20v1.07.msi
|
||||
2. Open DrewTech tool and make sure it shows "panda" as a device listed (this means registry settings are correct)
|
||||
3. When DrewTech tool attempts to load the driver it will show an error if it fails
|
||||
4. To figure out why the driver fails to load install Process Monitor and filter by the appropriate process name
|
||||
https://docs.microsoft.com/en-us/sysinternals/downloads/procmon
|
||||
|
||||
# Other:
|
||||
Panda head ASCII art by dcau
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
# How to use can_bit_transition.py to reverse engineer a single bit field
|
||||
|
||||
Let's say our goal is to find a brake pedal signal (caused by your foot pressing the brake pedal vs adaptive cruise control braking).
|
||||
|
||||
The following process will allow you to quickly find bits that are always 0 during a period of time (when you know you were not pressing the brake with your foot) and always 1 in a different period of time (when you know you were pressing the brake with your foot).
|
||||
|
||||
Open up a drive in cabana where you can find a place you used the brake pedal and another place where you did not use the brake pedal (and you can identify when you were on the brake pedal and when you were not). You may want to go out for a drive and put something in front of the camera after you put your foot on the brake and take it away before you take your foot off the brake so you can easily identify exactly when you had your foot on the brake based on the video in cabana. This is critical because this script needs the brake signal to always be high the entire time for one of the time frames. A 10 second time frame worked well for me.
|
||||
|
||||
I found a drive where I knew I was not pressing the brake between timestamp 50.0 thru 65.0 and I was pressing the brake between timestamp 69.0 thru 79.0. Determine what the timestamps are in cabana by plotting any message and putting your mouse over the plot at the location you want to discover the timestamp. The tool tip on mouse hover has the format: timestamp: value
|
||||
|
||||
Now download the log from cabana (Save Log button) and run the script passing in the timestamps
|
||||
(replace csv file name with cabana log you downloaded and time ranges with your own)
|
||||
```
|
||||
./can_bit_transition.py ./honda_crv_ex_2017_can-1520354796875.csv 50.0-65.0 69.0-79.0
|
||||
```
|
||||
|
||||
The script will output bits that were always low in the first time range and always high in the second time range (and vice versa)
|
||||
```
|
||||
id 17c 0 -> 1 at byte 4 bitmask 1
|
||||
id 17c 0 -> 1 at byte 6 bitmask 32
|
||||
id 221 1 -> 0 at byte 0 bitmask 4
|
||||
id 1be 0 -> 1 at byte 0 bitmask 16
|
||||
```
|
||||
|
||||
Now I go back to cabana and graph the above bits by searching for the message by id, double clicking on the appropriate bit, and then selecting "show plot". I already knew that message id 0x17c is both user brake and adaptive cruise control braking combined, so I plotted one of those signals along side 0x221 and 0x1be. By replaying a drive I could see that 0x221 was not a brake signal (when high at random times that did not correspond to braking). Next I looked at 0x1be and I found it was the brake pedal signal I was looking for (went high whenever I pressed the brake pedal and did not go high when adaptive cruise control was braking).
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import binascii
|
||||
import csv
|
||||
import sys
|
||||
|
||||
class Message():
|
||||
"""Details about a specific message ID."""
|
||||
def __init__(self, message_id):
|
||||
self.message_id = message_id
|
||||
self.ones = [0] * 8 # bit set if 1 is always seen
|
||||
self.zeros = [0] * 8 # bit set if 0 is always seen
|
||||
|
||||
def printBitDiff(self, other):
|
||||
"""Prints bits that transition from always zero to always 1 and vice versa."""
|
||||
for i in xrange(len(self.ones)):
|
||||
zero_to_one = other.zeros[i] & self.ones[i]
|
||||
if zero_to_one:
|
||||
print 'id %s 0 -> 1 at byte %d bitmask %d' % (self.message_id, i, zero_to_one)
|
||||
one_to_zero = other.ones[i] & self.zeros[i]
|
||||
if one_to_zero:
|
||||
print 'id %s 1 -> 0 at byte %d bitmask %d' % (self.message_id, i, one_to_zero)
|
||||
|
||||
|
||||
class Info():
|
||||
"""A collection of Messages."""
|
||||
|
||||
def __init__(self):
|
||||
self.messages = {} # keyed by MessageID
|
||||
|
||||
def load(self, filename, start, end):
|
||||
"""Given a CSV file, adds information about message IDs and their values."""
|
||||
with open(filename, 'rb') as input:
|
||||
reader = csv.reader(input)
|
||||
next(reader, None) # skip the CSV header
|
||||
for row in reader:
|
||||
if not len(row): continue
|
||||
time = float(row[0])
|
||||
bus = int(row[2])
|
||||
if time < start or bus > 127:
|
||||
continue
|
||||
elif time > end:
|
||||
break
|
||||
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[3].startswith('0x'):
|
||||
data = row[3][2:] # remove leading '0x'
|
||||
else:
|
||||
data = row[3]
|
||||
new_message = False
|
||||
if message_id not in self.messages:
|
||||
self.messages[message_id] = Message(message_id)
|
||||
new_message = True
|
||||
message = self.messages[message_id]
|
||||
bytes = bytearray.fromhex(data)
|
||||
for i in xrange(len(bytes)):
|
||||
ones = int(bytes[i])
|
||||
message.ones[i] = ones if new_message else message.ones[i] & ones
|
||||
# Inverts the data and masks it to a byte to get the zeros as ones.
|
||||
zeros = (~int(bytes[i])) & 0xff
|
||||
message.zeros[i] = zeros if new_message else message.zeros[i] & zeros
|
||||
|
||||
def PrintUnique(log_file, low_range, high_range):
|
||||
# find messages with bits that are always low
|
||||
start, end = map(float, low_range.split('-'))
|
||||
low = Info()
|
||||
low.load(log_file, start, end)
|
||||
# find messages with bits that are always high
|
||||
start, end = map(float, high_range.split('-'))
|
||||
high = Info()
|
||||
high.load(log_file, start, end)
|
||||
# print messages that go from low to high
|
||||
found = False
|
||||
for message_id in sorted(high.messages):
|
||||
if message_id in low.messages:
|
||||
high.messages[message_id].printBitDiff(low.messages[message_id])
|
||||
found = True
|
||||
if not found: print 'No messages that transition from always low to always high found!'
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) < 4:
|
||||
print 'Usage:\n%s log.csv <low-start>-<low-end> <high-start>-<high-end>' % sys.argv[0]
|
||||
sys.exit(0)
|
||||
PrintUnique(sys.argv[1], sys.argv[2], sys.argv[3])
|
||||
|
|
@ -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,85 +0,0 @@
|
|||
DEBUG = False
|
||||
|
||||
def msg(x):
|
||||
if DEBUG:
|
||||
print "S:",x.encode("hex")
|
||||
if len(x) <= 7:
|
||||
ret = chr(len(x)) + x
|
||||
else:
|
||||
assert False
|
||||
return ret.ljust(8, "\x00")
|
||||
|
||||
kmsgs = []
|
||||
def recv(panda, cnt, addr, nbus):
|
||||
global kmsgs
|
||||
ret = []
|
||||
|
||||
while len(ret) < cnt:
|
||||
kmsgs += panda.can_recv()
|
||||
nmsgs = []
|
||||
for ids, ts, dat, bus in kmsgs:
|
||||
if ids == addr and bus == nbus and len(ret) < cnt:
|
||||
ret.append(dat)
|
||||
else:
|
||||
# leave around
|
||||
nmsgs.append((ids, ts, dat, bus))
|
||||
kmsgs = nmsgs[-256:]
|
||||
return map(str, ret)
|
||||
|
||||
def isotp_send(panda, x, addr, bus=0, recvaddr=None):
|
||||
if recvaddr is None:
|
||||
recvaddr = addr+8
|
||||
|
||||
if len(x) <= 7:
|
||||
panda.can_send(addr, msg(x), bus)
|
||||
else:
|
||||
ss = chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:6]
|
||||
x = x[6:]
|
||||
idx = 1
|
||||
sends = []
|
||||
while len(x) > 0:
|
||||
sends.append(((chr(0x20 + (idx&0xF)) + x[0:7]).ljust(8, "\x00")))
|
||||
x = x[7:]
|
||||
idx += 1
|
||||
|
||||
# actually send
|
||||
panda.can_send(addr, ss, bus)
|
||||
rr = recv(panda, 1, recvaddr, bus)[0]
|
||||
panda.can_send_many([(addr, None, s, 0) for s in sends])
|
||||
|
||||
def isotp_recv(panda, addr, bus=0, sendaddr=None):
|
||||
msg = recv(panda, 1, addr, bus)[0]
|
||||
|
||||
if sendaddr is None:
|
||||
sendaddr = addr-8
|
||||
|
||||
|
||||
if ord(msg[0])&0xf0 == 0x10:
|
||||
# first
|
||||
tlen = ((ord(msg[0]) & 0xf) << 8) | ord(msg[1])
|
||||
dat = msg[2:]
|
||||
|
||||
# 0 block size?
|
||||
CONTINUE = "\x30" + "\x00"*7
|
||||
|
||||
panda.can_send(sendaddr, CONTINUE, bus)
|
||||
|
||||
idx = 1
|
||||
for mm in recv(panda, (tlen-len(dat) + 7)/8, addr, bus):
|
||||
assert ord(mm[0]) == (0x20 | idx)
|
||||
dat += mm[1:]
|
||||
idx += 1
|
||||
elif ord(msg[0])&0xf0 == 0x00:
|
||||
# single
|
||||
tlen = ord(msg[0]) & 0xf
|
||||
dat = msg[1:]
|
||||
else:
|
||||
assert False
|
||||
|
||||
dat = dat[0:tlen]
|
||||
|
||||
if DEBUG:
|
||||
print "R:",dat.encode("hex")
|
||||
|
||||
return dat
|
||||
|
||||
|
|
@ -13,8 +13,9 @@ from esptool import ESPROM, CesantaFlasher
|
|||
from flash_release import flash_release
|
||||
from update import ensure_st_up_to_date
|
||||
from serial import PandaSerial
|
||||
from isotp import isotp_send, isotp_recv
|
||||
|
||||
__version__ = '0.0.6'
|
||||
__version__ = '0.0.7'
|
||||
|
||||
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
|
||||
|
||||
|
|
@ -104,6 +105,7 @@ class Panda(object):
|
|||
SAFETY_NOOUTPUT = 0
|
||||
SAFETY_HONDA = 1
|
||||
SAFETY_TOYOTA = 2
|
||||
SAFETY_HONDA_BOSCH = 4
|
||||
SAFETY_TOYOTA_NOLIMITS = 0x1336
|
||||
SAFETY_ALLOUTPUT = 0x1337
|
||||
SAFETY_ELM327 = 0xE327
|
||||
|
|
@ -153,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()
|
||||
|
|
@ -181,27 +184,58 @@ class Panda(object):
|
|||
except Exception:
|
||||
pass
|
||||
if not enter_bootloader:
|
||||
self.close()
|
||||
time.sleep(1.0)
|
||||
success = False
|
||||
# wait up to 15 seconds
|
||||
for i in range(0, 15):
|
||||
try:
|
||||
self.connect()
|
||||
success = True
|
||||
break
|
||||
except Exception:
|
||||
print("reconnecting is taking %d seconds..." % (i+1))
|
||||
try:
|
||||
dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial))
|
||||
dfu.recover()
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(1.0)
|
||||
if not success:
|
||||
raise Exception("reset failed")
|
||||
self.reconnect()
|
||||
|
||||
def flash(self, fn=None, code=None):
|
||||
def reconnect(self):
|
||||
self.close()
|
||||
time.sleep(1.0)
|
||||
success = False
|
||||
# wait up to 15 seconds
|
||||
for i in range(0, 15):
|
||||
try:
|
||||
self.connect()
|
||||
success = True
|
||||
break
|
||||
except Exception:
|
||||
print("reconnecting is taking %d seconds..." % (i+1))
|
||||
try:
|
||||
dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(self._serial))
|
||||
dfu.recover()
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(1.0)
|
||||
if not success:
|
||||
raise Exception("reconnect failed")
|
||||
|
||||
@staticmethod
|
||||
def flash_static(handle, code):
|
||||
# confirm flasher is present
|
||||
fr = handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc)
|
||||
assert fr[4:8] == "\xde\xad\xd0\x0d"
|
||||
|
||||
# unlock flash
|
||||
print("flash: unlocking")
|
||||
handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
|
||||
|
||||
# erase sectors 1 and 2
|
||||
print("flash: erasing")
|
||||
handle.controlWrite(Panda.REQUEST_IN, 0xb2, 1, 0, b'')
|
||||
handle.controlWrite(Panda.REQUEST_IN, 0xb2, 2, 0, b'')
|
||||
|
||||
# flash over EP2
|
||||
STEP = 0x10
|
||||
print("flash: flashing")
|
||||
for i in range(0, len(code), STEP):
|
||||
handle.bulkWrite(2, code[i:i+STEP])
|
||||
|
||||
# reset
|
||||
print("flash: resetting")
|
||||
try:
|
||||
handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'')
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def flash(self, fn=None, code=None, reconnect=True):
|
||||
if not self.bootstub:
|
||||
self.reset(enter_bootstub=True)
|
||||
assert(self.bootstub)
|
||||
|
|
@ -224,28 +258,12 @@ class Panda(object):
|
|||
# get version
|
||||
print("flash: version is "+self.get_version())
|
||||
|
||||
# confirm flasher is present
|
||||
fr = self._handle.controlRead(Panda.REQUEST_IN, 0xb0, 0, 0, 0xc)
|
||||
assert fr[4:8] == "\xde\xad\xd0\x0d"
|
||||
# do flash
|
||||
Panda.flash_static(self._handle, code)
|
||||
|
||||
# unlock flash
|
||||
print("flash: unlocking")
|
||||
self._handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'')
|
||||
|
||||
# erase sectors 1 and 2
|
||||
print("flash: erasing")
|
||||
self._handle.controlWrite(Panda.REQUEST_IN, 0xb2, 1, 0, b'')
|
||||
self._handle.controlWrite(Panda.REQUEST_IN, 0xb2, 2, 0, b'')
|
||||
|
||||
# flash over EP2
|
||||
STEP = 0x10
|
||||
print("flash: flashing")
|
||||
for i in range(0, len(code), STEP):
|
||||
self._handle.bulkWrite(2, code[i:i+STEP])
|
||||
|
||||
# reset
|
||||
print("flash: resetting")
|
||||
self.reset()
|
||||
# reconnect
|
||||
if reconnect:
|
||||
self.reconnect()
|
||||
|
||||
def recover(self):
|
||||
self.reset(enter_bootloader=True)
|
||||
|
|
@ -424,6 +442,14 @@ class Panda(object):
|
|||
"""
|
||||
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf1, bus, 0, b'')
|
||||
|
||||
# ******************* isotp *******************
|
||||
|
||||
def isotp_send(self, addr, dat, bus, recvaddr=None, subaddr=None):
|
||||
return isotp_send(self, dat, addr, bus, recvaddr, subaddr)
|
||||
|
||||
def isotp_recv(self, addr, bus=0, sendaddr=None, subaddr=None):
|
||||
return isotp_recv(self, addr, bus, sendaddr, subaddr)
|
||||
|
||||
# ******************* serial *******************
|
||||
|
||||
def serial_read(self, port_number):
|
||||
|
|
|
|||
|
|
@ -0,0 +1,135 @@
|
|||
DEBUG = False
|
||||
|
||||
def msg(x):
|
||||
if DEBUG:
|
||||
print "S:",x.encode("hex")
|
||||
if len(x) <= 7:
|
||||
ret = chr(len(x)) + x
|
||||
else:
|
||||
assert False
|
||||
return ret.ljust(8, "\x00")
|
||||
|
||||
kmsgs = []
|
||||
def recv(panda, cnt, addr, nbus):
|
||||
global kmsgs
|
||||
ret = []
|
||||
|
||||
while len(ret) < cnt:
|
||||
kmsgs += panda.can_recv()
|
||||
nmsgs = []
|
||||
for ids, ts, dat, bus in kmsgs:
|
||||
if ids == addr and bus == nbus and len(ret) < cnt:
|
||||
ret.append(dat)
|
||||
else:
|
||||
# leave around
|
||||
nmsgs.append((ids, ts, dat, bus))
|
||||
kmsgs = nmsgs[-256:]
|
||||
return map(str, ret)
|
||||
|
||||
def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
|
||||
msg = recv(panda, 1, addr, bus)[0]
|
||||
|
||||
# TODO: handle other subaddr also communicating
|
||||
assert ord(msg[0]) == subaddr
|
||||
|
||||
if ord(msg[1])&0xf0 == 0x10:
|
||||
# first
|
||||
tlen = ((ord(msg[1]) & 0xf) << 8) | ord(msg[2])
|
||||
dat = msg[3:]
|
||||
|
||||
# 0 block size?
|
||||
CONTINUE = chr(subaddr) + "\x30" + "\x00"*6
|
||||
panda.can_send(sendaddr, CONTINUE, bus)
|
||||
|
||||
idx = 1
|
||||
for mm in recv(panda, (tlen-len(dat) + 5)/6, addr, bus):
|
||||
assert ord(mm[0]) == subaddr
|
||||
assert ord(mm[1]) == (0x20 | idx)
|
||||
dat += mm[2:]
|
||||
idx += 1
|
||||
elif ord(msg[1])&0xf0 == 0x00:
|
||||
# single
|
||||
tlen = ord(msg[1]) & 0xf
|
||||
dat = msg[2:]
|
||||
else:
|
||||
print msg.encode("hex")
|
||||
assert False
|
||||
|
||||
return dat[0:tlen]
|
||||
|
||||
# **** import below this line ****
|
||||
|
||||
def isotp_send(panda, x, addr, bus=0, recvaddr=None, subaddr=None):
|
||||
if recvaddr is None:
|
||||
recvaddr = addr+8
|
||||
|
||||
if len(x) <= 7 and subaddr is None:
|
||||
panda.can_send(addr, msg(x), bus)
|
||||
elif len(x) <= 6 and subaddr is not None:
|
||||
panda.can_send(addr, chr(subaddr)+msg(x)[0:7], bus)
|
||||
else:
|
||||
if subaddr:
|
||||
ss = chr(subaddr) + chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:5]
|
||||
x = x[5:]
|
||||
else:
|
||||
ss = chr(0x10 + (len(x)>>8)) + chr(len(x)&0xFF) + x[0:6]
|
||||
x = x[6:]
|
||||
idx = 1
|
||||
sends = []
|
||||
while len(x) > 0:
|
||||
if subaddr:
|
||||
sends.append(((chr(subaddr) + chr(0x20 + (idx&0xF)) + x[0:6]).ljust(8, "\x00")))
|
||||
x = x[6:]
|
||||
else:
|
||||
sends.append(((chr(0x20 + (idx&0xF)) + x[0:7]).ljust(8, "\x00")))
|
||||
x = x[7:]
|
||||
idx += 1
|
||||
|
||||
# actually send
|
||||
panda.can_send(addr, ss, bus)
|
||||
rr = recv(panda, 1, recvaddr, bus)[0]
|
||||
if rr.find("\x30\x01") != -1:
|
||||
for s in sends[:-1]:
|
||||
panda.can_send(addr, s, 0)
|
||||
rr = recv(panda, 1, recvaddr, bus)[0]
|
||||
panda.can_send(addr, sends[-1], 0)
|
||||
else:
|
||||
panda.can_send_many([(addr, None, s, 0) for s in sends])
|
||||
|
||||
def isotp_recv(panda, addr, bus=0, sendaddr=None, subaddr=None):
|
||||
if sendaddr is None:
|
||||
sendaddr = addr-8
|
||||
|
||||
if subaddr is not None:
|
||||
dat = isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr)
|
||||
else:
|
||||
msg = recv(panda, 1, addr, bus)[0]
|
||||
|
||||
if ord(msg[0])&0xf0 == 0x10:
|
||||
# first
|
||||
tlen = ((ord(msg[0]) & 0xf) << 8) | ord(msg[1])
|
||||
dat = msg[2:]
|
||||
|
||||
# 0 block size?
|
||||
CONTINUE = "\x30" + "\x00"*7
|
||||
|
||||
panda.can_send(sendaddr, CONTINUE, bus)
|
||||
|
||||
idx = 1
|
||||
for mm in recv(panda, (tlen-len(dat) + 6)/7, addr, bus):
|
||||
assert ord(mm[0]) == (0x20 | idx)
|
||||
dat += mm[1:]
|
||||
idx += 1
|
||||
elif ord(msg[0])&0xf0 == 0x00:
|
||||
# single
|
||||
tlen = ord(msg[0]) & 0xf
|
||||
dat = msg[1:]
|
||||
else:
|
||||
assert False
|
||||
dat = dat[0:tlen]
|
||||
|
||||
if DEBUG:
|
||||
print "R:",dat.encode("hex")
|
||||
|
||||
return dat
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -4,6 +4,7 @@ import os
|
|||
import sys
|
||||
import time
|
||||
from collections import defaultdict
|
||||
import binascii
|
||||
|
||||
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
|
||||
from panda import Panda
|
||||
|
|
@ -14,6 +15,7 @@ def sec_since_boot():
|
|||
|
||||
def can_printer():
|
||||
p = Panda()
|
||||
p.set_safety_mode(0x1337)
|
||||
|
||||
start = sec_since_boot()
|
||||
lp = sec_since_boot()
|
||||
|
|
@ -28,7 +30,7 @@ def can_printer():
|
|||
if sec_since_boot() - lp > 0.1:
|
||||
dd = chr(27) + "[2J"
|
||||
dd += "%5.2f\n" % (sec_since_boot() - start)
|
||||
for k,v in sorted(zip(msgs.keys(), map(lambda x: str(x[-1]).encode("hex"), msgs.values()))):
|
||||
for k,v in sorted(zip(msgs.keys(), map(lambda x: binascii.hexlify(x[-1]), msgs.values()))):
|
||||
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
|
||||
print(dd)
|
||||
lp = sec_since_boot()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
#!/usr/bin/env python
|
||||
import sys
|
||||
import time
|
||||
import struct
|
||||
import argparse
|
||||
import signal
|
||||
from panda import Panda
|
||||
|
||||
class CanHandle(object):
|
||||
def __init__(self, p):
|
||||
self.p = p
|
||||
|
||||
def transact(self, dat):
|
||||
#print "W:",dat.encode("hex")
|
||||
self.p.isotp_send(1, dat, 0, recvaddr=2)
|
||||
|
||||
def _handle_timeout(signum, frame):
|
||||
# will happen on reset
|
||||
raise Exception("timeout")
|
||||
|
||||
signal.signal(signal.SIGALRM, _handle_timeout)
|
||||
signal.alarm(1)
|
||||
try:
|
||||
ret = self.p.isotp_recv(2, 0, sendaddr=1)
|
||||
finally:
|
||||
signal.alarm(0)
|
||||
|
||||
#print "R:",ret.encode("hex")
|
||||
return ret
|
||||
|
||||
def controlWrite(self, request_type, request, value, index, data, timeout=0):
|
||||
# ignore data in reply, panda doesn't use it
|
||||
return self.controlRead(request_type, request, value, index, 0, timeout)
|
||||
|
||||
def controlRead(self, request_type, request, value, index, length, timeout=0):
|
||||
dat = struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, length)
|
||||
return self.transact(dat)
|
||||
|
||||
def bulkWrite(self, endpoint, data, timeout=0):
|
||||
if len(data) > 0x10:
|
||||
raise ValueError("Data must not be longer than 0x10")
|
||||
dat = struct.pack("HH", endpoint, len(data))+data
|
||||
return self.transact(dat)
|
||||
|
||||
def bulkRead(self, endpoint, length, timeout=0):
|
||||
dat = struct.pack("HH", endpoint, 0)
|
||||
return self.transact(dat)
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description='Flash pedal over can')
|
||||
parser.add_argument('--recover', action='store_true')
|
||||
parser.add_argument("fn", type=str, nargs='?', help="flash file")
|
||||
args = parser.parse_args()
|
||||
|
||||
p = Panda()
|
||||
p.set_safety_mode(0x1337)
|
||||
|
||||
while 1:
|
||||
if len(p.can_recv()) == 0:
|
||||
break
|
||||
|
||||
if args.recover:
|
||||
p.can_send(0x200, "\xce\xfa\xad\xde\x1e\x0b\xb0\x02", 0)
|
||||
exit(0)
|
||||
else:
|
||||
p.can_send(0x200, "\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", 0)
|
||||
|
||||
if args.fn:
|
||||
time.sleep(0.1)
|
||||
print "flashing", args.fn
|
||||
code = open(args.fn).read()
|
||||
Panda.flash_static(CanHandle(p), code)
|
||||
|
||||
print "can flash done"
|
||||
|
||||
|
||||
|
|
@ -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 -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,56 @@
|
|||
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);
|
||||
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);
|
||||
|
||||
""")
|
||||
|
||||
libpandasafety = ffi.dlopen(libpandasafety_fn)
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
cffi==1.11.4
|
||||
numpy==1.14.1
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
#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;
|
||||
|
||||
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 static
|
||||
#include "safety.h"
|
||||
|
||||
void set_controls_allowed(int c){
|
||||
controls_allowed = c;
|
||||
}
|
||||
|
||||
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,260 @@
|
|||
#!/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
|
||||
|
||||
def twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
|
||||
|
||||
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_rx_hook(self._torque_driver_msg(torque))
|
||||
|
||||
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):
|
||||
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
|
||||
to_send[0].RIR = 0x266 << 21
|
||||
|
||||
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_rx_hook(self._ipas_state_msg(5))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
## now angle control is active
|
||||
self.safety.toyota_tx_hook(self._ipas_control_msg())
|
||||
self.safety.toyota_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_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_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())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Loading…
Reference in New Issue