Vehicle Researcher
6 years ago
commit
26da755a1e
19 changed files with 527 additions and 9 deletions
@ -1 +1 @@ |
||||
v1.1.8 |
||||
v1.2.0 |
||||
@ -0,0 +1 @@ |
||||
obj/* |
||||
@ -0,0 +1,58 @@ |
||||
# :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/*
|
||||
@ -0,0 +1,30 @@ |
||||
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 |
||||
@ -0,0 +1,291 @@ |
||||
//#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,52 @@ |
||||
// BUS 0 is on the LKAS module (ASCM) side
|
||||
// BUS 2 is on the actuator (EPS) side
|
||||
|
||||
static int gm_ascm_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { |
||||
|
||||
uint32_t addr = to_fwd->RIR>>21; |
||||
|
||||
if (bus_num == 0) { |
||||
|
||||
// do not propagate lkas messages from ascm to actuators
|
||||
// block 0x152 and 0x154, which are the lkas command from ASCM1 and ASCM2
|
||||
// block 0x315 and 0x2cb, which are the brake and accel commands from ASCM1
|
||||
//if ((addr == 0x152) || (addr == 0x154) || (addr == 0x315) || (addr == 0x2cb)) {
|
||||
if ((addr == 0x152) || (addr == 0x154)) { |
||||
int supercruise_on = (to_fwd->RDHR>>4) & 0x1; // bit 36
|
||||
if (!supercruise_on) return -1; |
||||
} |
||||
|
||||
// on the chassis bus, the OBDII port is on the module side, so we need to read
|
||||
// the lkas messages sent by openpilot (put on unused 0x151 ane 0x153 addrs) and send it to
|
||||
// the actuator as 0x152 and 0x154
|
||||
if (addr == 0x151) { |
||||
to_fwd->RIR = (0x152 << 21) | (to_fwd->RIR & 0x1fffff); |
||||
} |
||||
if (addr == 0x153) { |
||||
to_fwd->RIR = (0x154 << 21) | (to_fwd->RIR & 0x1fffff); |
||||
} |
||||
|
||||
// brake
|
||||
if (addr == 0x314) { |
||||
to_fwd->RIR = (0x315 << 21) | (to_fwd->RIR & 0x1fffff); |
||||
} |
||||
|
||||
return 2; |
||||
} |
||||
|
||||
if (bus_num == 2) { |
||||
return 0; |
||||
} |
||||
|
||||
return -1; |
||||
} |
||||
|
||||
const safety_hooks gm_ascm_hooks = { |
||||
.init = nooutput_init, |
||||
.rx = default_rx_hook, |
||||
.tx = alloutput_tx_hook, |
||||
.tx_lin = nooutput_tx_lin_hook, |
||||
.ignition = default_ign_hook, |
||||
.fwd = gm_ascm_fwd_hook, |
||||
}; |
||||
|
||||
@ -0,0 +1,49 @@ |
||||
void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {} |
||||
|
||||
// FIXME
|
||||
// *** all output safety mode ***
|
||||
|
||||
static void subaru_init(int16_t param) { |
||||
controls_allowed = 1; |
||||
} |
||||
|
||||
static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { |
||||
return true; |
||||
} |
||||
|
||||
static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { |
||||
|
||||
// shifts bits 29 > 11
|
||||
int32_t addr = to_fwd->RIR >> 21; |
||||
|
||||
// forward CAN 0 > 1
|
||||
if (bus_num == 0) { |
||||
return 1; // ES CAN
|
||||
} |
||||
// forward CAN 1 > 0, except ES_LKAS
|
||||
else if (bus_num == 1) { |
||||
|
||||
// outback 2015
|
||||
if (addr == 0x164) { |
||||
return -1; |
||||
} |
||||
// global platform
|
||||
if (addr == 0x122) { |
||||
return -1; |
||||
} |
||||
|
||||
return 0; // Main CAN
|
||||
} |
||||
|
||||
// fallback to do not forward
|
||||
return -1; |
||||
} |
||||
|
||||
const safety_hooks subaru_hooks = { |
||||
.init = subaru_init, |
||||
.rx = subaru_rx_hook, |
||||
.tx = subaru_tx_hook, |
||||
.tx_lin = nooutput_tx_lin_hook, |
||||
.ignition = default_ign_hook, |
||||
.fwd = subaru_fwd_hook, |
||||
}; |
||||
Loading…
Reference in new issue