Vehicle Researcher
7 years ago
commit
113d4c1b70
25 changed files with 476 additions and 456 deletions
@ -1 +0,0 @@ |
|||||||
obj/* |
|
||||||
@ -1,58 +0,0 @@ |
|||||||
# :set noet
|
|
||||||
PROJ_NAME = comma
|
|
||||||
|
|
||||||
CFLAGS = -O2 -Wall -std=gnu11 -DPEDAL
|
|
||||||
CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
|
|
||||||
CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
|
|
||||||
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"
|
|
||||||
|
|
||||||
# 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/bootstub.o: ../bootstub.c ../*.h |
|
||||||
mkdir -p obj
|
|
||||||
$(CC) $(CFLAGS) -o $@ -c $<
|
|
||||||
|
|
||||||
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/*
|
|
||||||
@ -1,30 +0,0 @@ |
|||||||
MOVE ALL FILES TO board/pedal TO FLASH |
|
||||||
|
|
||||||
|
|
||||||
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. 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 |
|
||||||
@ -1,291 +0,0 @@ |
|||||||
//#define DEBUG
|
|
||||||
//#define CAN_LOOPBACK_MODE
|
|
||||||
//#define USE_INTERNAL_OSC
|
|
||||||
|
|
||||||
#include "../config.h" |
|
||||||
|
|
||||||
#include "drivers/drivers.h" |
|
||||||
#include "drivers/llgpio.h" |
|
||||||
#include "gpio.h" |
|
||||||
|
|
||||||
#define CUSTOM_CAN_INTERRUPTS |
|
||||||
|
|
||||||
#include "libc.h" |
|
||||||
#include "safety.h" |
|
||||||
#include "drivers/adc.h" |
|
||||||
#include "drivers/uart.h" |
|
||||||
#include "drivers/dac.h" |
|
||||||
#include "drivers/can.h" |
|
||||||
#include "drivers/timer.h" |
|
||||||
|
|
||||||
#define CAN CAN1 |
|
||||||
|
|
||||||
//#define PEDAL_USB
|
|
||||||
|
|
||||||
#ifdef PEDAL_USB |
|
||||||
#include "drivers/usb.h" |
|
||||||
#endif |
|
||||||
|
|
||||||
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef |
|
||||||
uint32_t enter_bootloader_mode; |
|
||||||
|
|
||||||
void __initialize_hardware_early() { |
|
||||||
early(); |
|
||||||
} |
|
||||||
|
|
||||||
// ********************* serial debugging *********************
|
|
||||||
|
|
||||||
void debug_ring_callback(uart_ring *ring) { |
|
||||||
char rcv; |
|
||||||
while (getc(ring, &rcv)) { |
|
||||||
putc(ring, rcv); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
#ifdef PEDAL_USB |
|
||||||
|
|
||||||
int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; } |
|
||||||
void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {} |
|
||||||
void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {} |
|
||||||
void usb_cb_enumeration_complete() {} |
|
||||||
|
|
||||||
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { |
|
||||||
int resp_len = 0; |
|
||||||
uart_ring *ur = NULL; |
|
||||||
switch (setup->b.bRequest) { |
|
||||||
// **** 0xe0: uart read
|
|
||||||
case 0xe0: |
|
||||||
ur = get_ring_by_number(setup->b.wValue.w); |
|
||||||
if (!ur) break; |
|
||||||
if (ur == &esp_ring) uart_dma_drain(); |
|
||||||
// read
|
|
||||||
while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) && |
|
||||||
getc(ur, (char*)&resp[resp_len])) { |
|
||||||
++resp_len; |
|
||||||
} |
|
||||||
break; |
|
||||||
} |
|
||||||
return resp_len; |
|
||||||
} |
|
||||||
|
|
||||||
#endif |
|
||||||
|
|
||||||
// ***************************** toyota can checksum ****************************
|
|
||||||
|
|
||||||
int can_cksum(uint8_t *dat, uint8_t len, uint16_t addr) |
|
||||||
{ |
|
||||||
uint8_t checksum = 0; |
|
||||||
checksum =((addr & 0xFF00) >> 8) + (addr & 0x00FF) + len + 1; |
|
||||||
//uint16_t temp_msg = msg;
|
|
||||||
|
|
||||||
for (int ii = 0; ii < len; ii++) |
|
||||||
{ |
|
||||||
checksum += (dat[ii]); |
|
||||||
//temp_msg = temp_msg >> 8;
|
|
||||||
} |
|
||||||
//return ((msg & ~0xFF) & (checksum & 0xFF));
|
|
||||||
return checksum; |
|
||||||
} |
|
||||||
|
|
||||||
// ***************************** can port *****************************
|
|
||||||
|
|
||||||
// addresses to be used on CAN
|
|
||||||
#define CAN_GAS_INPUT 0x200 |
|
||||||
#define CAN_GAS_OUTPUT 0x201 |
|
||||||
|
|
||||||
void CAN1_TX_IRQHandler() { |
|
||||||
// clear interrupt
|
|
||||||
CAN->TSR |= CAN_TSR_RQCP0; |
|
||||||
} |
|
||||||
|
|
||||||
// two independent values
|
|
||||||
uint16_t gas_set_0 = 0; |
|
||||||
uint16_t gas_set_1 = 0; |
|
||||||
|
|
||||||
#define MAX_TIMEOUT 10 |
|
||||||
uint32_t timeout = 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 |
|
||||||
puts("CAN RX\n"); |
|
||||||
#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; |
|
||||||
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 = 0; |
|
||||||
if (can_cksum(dat, 5, CAN_GAS_INPUT) == dat2[1]) { |
|
||||||
if (index == 0) { |
|
||||||
#ifdef DEBUG |
|
||||||
puts("setting gas "); |
|
||||||
puth(value); |
|
||||||
puts("\n"); |
|
||||||
#endif |
|
||||||
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; |
|
||||||
} |
|
||||||
|
|
||||||
} else { |
|
||||||
// wrong checksum = fault
|
|
||||||
state = FAULT_BAD_CHECKSUM; |
|
||||||
} |
|
||||||
} |
|
||||||
// next
|
|
||||||
CAN->RF0R |= CAN_RF0R_RFOM0; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
void CAN1_SCE_IRQHandler() { |
|
||||||
state = FAULT_SCE; |
|
||||||
can_sce(CAN); |
|
||||||
} |
|
||||||
|
|
||||||
int pdl0 = 0, pdl1 = 0; |
|
||||||
|
|
||||||
|
|
||||||
int led_value = 0; |
|
||||||
|
|
||||||
void TIM3_IRQHandler() { |
|
||||||
#ifdef DEBUG |
|
||||||
puth(TIM3->CNT); |
|
||||||
puts(" "); |
|
||||||
puth(pdl0); |
|
||||||
puts(" "); |
|
||||||
puth(pdl1); |
|
||||||
puts("\n"); |
|
||||||
#endif |
|
||||||
|
|
||||||
// check timer for sending the user pedal and clearing the CAN
|
|
||||||
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { |
|
||||||
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); |
|
||||||
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; |
|
||||||
} else { |
|
||||||
// old can packet hasn't sent!
|
|
||||||
state = FAULT_SEND; |
|
||||||
#ifdef DEBUG |
|
||||||
puts("CAN MISS\n"); |
|
||||||
#endif |
|
||||||
} |
|
||||||
|
|
||||||
// blink the LED
|
|
||||||
set_led(LED_GREEN, led_value); |
|
||||||
led_value = !led_value; |
|
||||||
|
|
||||||
TIM3->SR = 0; |
|
||||||
|
|
||||||
// up timeout for gas set
|
|
||||||
if (timeout == MAX_TIMEOUT) { |
|
||||||
state = FAULT_TIMEOUT; |
|
||||||
} else { |
|
||||||
timeout += 1; |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
// ***************************** main code *****************************
|
|
||||||
|
|
||||||
void pedal() { |
|
||||||
// read/write
|
|
||||||
pdl0 = adc_get(ADCCHAN_ACCEL0); |
|
||||||
pdl1 = adc_get(ADCCHAN_ACCEL1); |
|
||||||
|
|
||||||
// write the pedal to the DAC
|
|
||||||
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() { |
|
||||||
__disable_irq(); |
|
||||||
|
|
||||||
// init devices
|
|
||||||
clock_init(); |
|
||||||
periph_init(); |
|
||||||
gpio_init(); |
|
||||||
|
|
||||||
#ifdef PEDAL_USB |
|
||||||
// enable USB
|
|
||||||
usb_init(); |
|
||||||
#endif |
|
||||||
|
|
||||||
// pedal stuff
|
|
||||||
dac_init(); |
|
||||||
adc_init(); |
|
||||||
|
|
||||||
// init can
|
|
||||||
can_silent = ALL_CAN_LIVE; |
|
||||||
can_init(0); |
|
||||||
|
|
||||||
// 48mhz / 65536 ~= 732
|
|
||||||
timer_init(TIM3, 15); |
|
||||||
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(); |
|
||||||
|
|
||||||
// main pedal loop
|
|
||||||
while (1) { |
|
||||||
pedal(); |
|
||||||
} |
|
||||||
|
|
||||||
return 0; |
|
||||||
} |
|
||||||
@ -0,0 +1,170 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
import unittest |
||||||
|
import numpy as np |
||||||
|
import libpandasafety_py |
||||||
|
|
||||||
|
MAX_RATE_UP = 50 |
||||||
|
MAX_RATE_DOWN = 70 |
||||||
|
MAX_STEER = 2047 |
||||||
|
|
||||||
|
MAX_RT_DELTA = 940 |
||||||
|
RT_INTERVAL = 250000 |
||||||
|
|
||||||
|
DRIVER_TORQUE_ALLOWANCE = 60; |
||||||
|
DRIVER_TORQUE_FACTOR = 10; |
||||||
|
|
||||||
|
def twos_comp(val, bits): |
||||||
|
if val >= 0: |
||||||
|
return val |
||||||
|
else: |
||||||
|
return (2**bits) + val |
||||||
|
|
||||||
|
def sign(a): |
||||||
|
if a > 0: |
||||||
|
return 1 |
||||||
|
else: |
||||||
|
return -1 |
||||||
|
|
||||||
|
class TestSubaruSafety(unittest.TestCase): |
||||||
|
@classmethod |
||||||
|
def setUp(cls): |
||||||
|
cls.safety = libpandasafety_py.libpandasafety |
||||||
|
cls.safety.nooutput_init(0) |
||||||
|
cls.safety.init_tests_subaru() |
||||||
|
|
||||||
|
def _set_prev_torque(self, t): |
||||||
|
self.safety.set_subaru_desired_torque_last(t) |
||||||
|
self.safety.set_subaru_rt_torque_last(t) |
||||||
|
|
||||||
|
def _torque_driver_msg(self, torque): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x119 << 21 |
||||||
|
|
||||||
|
t = twos_comp(torque, 11) |
||||||
|
to_send[0].RDLR = ((t & 0x7FF) << 16) |
||||||
|
return to_send |
||||||
|
|
||||||
|
def _torque_msg(self, torque): |
||||||
|
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') |
||||||
|
to_send[0].RIR = 0x122 << 21 |
||||||
|
|
||||||
|
t = twos_comp(torque, 13) |
||||||
|
to_send[0].RDLR = (t << 16) |
||||||
|
return to_send |
||||||
|
|
||||||
|
def test_default_controls_not_allowed(self): |
||||||
|
self.assertFalse(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 = 0x240 << 21 |
||||||
|
to_push[0].RDHR = 1 << 9 |
||||||
|
|
||||||
|
self.safety.subaru_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 = 0x240 << 21 |
||||||
|
to_push[0].RDHR = 0 |
||||||
|
|
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.safety.subaru_rx_hook(to_push) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_steer_safety_check(self): |
||||||
|
for enabled in [0, 1]: |
||||||
|
for t in range(-3000, 3000): |
||||||
|
self.safety.set_controls_allowed(enabled) |
||||||
|
self._set_prev_torque(t) |
||||||
|
if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(t))) |
||||||
|
else: |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t))) |
||||||
|
|
||||||
|
def test_manually_enable_controls_allowed(self): |
||||||
|
self.safety.set_controls_allowed(1) |
||||||
|
self.assertTrue(self.safety.get_controls_allowed()) |
||||||
|
self.safety.set_controls_allowed(0) |
||||||
|
self.assertFalse(self.safety.get_controls_allowed()) |
||||||
|
|
||||||
|
def test_non_realtime_limit_up(self): |
||||||
|
self.safety.set_subaru_torque_driver(0, 0) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(MAX_RATE_UP))) |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(-MAX_RATE_UP))) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) |
||||||
|
|
||||||
|
def test_non_realtime_limit_down(self): |
||||||
|
self.safety.set_subaru_torque_driver(0, 0) |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
def test_against_torque_driver(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): |
||||||
|
t *= -sign |
||||||
|
self.safety.set_subaru_torque_driver(t, t) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(MAX_STEER * sign))) |
||||||
|
|
||||||
|
self.safety.set_subaru_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(-MAX_STEER))) |
||||||
|
|
||||||
|
# spot check some individual cases |
||||||
|
for sign in [-1, 1]: |
||||||
|
driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign |
||||||
|
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign |
||||||
|
delta = 1 * sign |
||||||
|
self._set_prev_torque(torque_desired) |
||||||
|
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(torque_desired))) |
||||||
|
self._set_prev_torque(torque_desired + delta) |
||||||
|
self.safety.set_subaru_torque_driver(-driver_torque, -driver_torque) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(torque_desired + delta))) |
||||||
|
|
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(0))) |
||||||
|
self._set_prev_torque(MAX_STEER * sign) |
||||||
|
self.safety.set_subaru_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) |
||||||
|
|
||||||
|
|
||||||
|
def test_realtime_limits(self): |
||||||
|
self.safety.set_controls_allowed(True) |
||||||
|
|
||||||
|
for sign in [-1, 1]: |
||||||
|
self.safety.init_tests_subaru() |
||||||
|
self._set_prev_torque(0) |
||||||
|
self.safety.set_subaru_torque_driver(0, 0) |
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1): |
||||||
|
t *= sign |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t))) |
||||||
|
self.assertFalse(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) |
||||||
|
|
||||||
|
self._set_prev_torque(0) |
||||||
|
for t in np.arange(0, MAX_RT_DELTA, 1): |
||||||
|
t *= sign |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(t))) |
||||||
|
|
||||||
|
# Increase timer to update rt_torque_last |
||||||
|
self.safety.set_timer(RT_INTERVAL + 1) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA - 1)))) |
||||||
|
self.assertTrue(self.safety.subaru_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
@ -0,0 +1,21 @@ |
|||||||
|
#!/usr/bin/env python2 |
||||||
|
|
||||||
|
# This trims CAN message CSV files to just the messages relevant for Panda testing. |
||||||
|
# Usage: |
||||||
|
# cat input.csv | ./trim_csv.py > output.csv |
||||||
|
import fileinput |
||||||
|
|
||||||
|
addr_to_keep = [544, 0x1f4, 0x292] # For Chrysler, update to the addresses that matter for you. |
||||||
|
|
||||||
|
for line in fileinput.input(): |
||||||
|
line = line.strip() |
||||||
|
cols = line.split(',') |
||||||
|
if len(cols) != 4: |
||||||
|
continue # malformed, such as at the end or every 60s. |
||||||
|
(_, addr, bus, _) = cols |
||||||
|
if (addr == 'addr'): |
||||||
|
continue |
||||||
|
if (int(bus) == 128): # Keep all messages sent by OpenPilot. |
||||||
|
print line |
||||||
|
elif (int(addr) in addr_to_keep): |
||||||
|
print line |
||||||
Loading…
Reference in new issue