mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-21 09:12:25 +08:00
Squashed 'panda/' changes from 39c1e39e..06958e42
06958e42 Fix pedal bootstub build f5817e6b Fix Pedal bootstub version c00fe867 CircleCI needs to check bootstub doesn't break too 9b5b696e Subaru: remove GM leftover 20c76ad5 Power Saving (#169) c6eeaad6 Subaru: added last engage/disengage regression test 37d46e0c Subaru: added subaru safety tests 5686dae2 Subaru updated driver factor a6193a82 Dcp remove (#168) e437b9b4 Subaru: fixed bug and added safety tests 176f1325 Subaru: added proper safety model 0b10bb70 Subaru safety: move camera to bus 2 bce279a6 Pedal: only one firmware (#164) 4f73cb48 Toyota pedal: checking for no pedal being commanded when openpilot is off 0b2327e5 Merge pull request #160 from commaai/capture_make_failure 7b504d2f panda safety test that replays drives of saved CAN messages (#151) d7d08892 Capture make failure so it can be logged to sentry git-subtree-dir: panda git-subtree-split: 06958e424cad7efa3fb35d262480c29817733059
This commit is contained in:
Vehicle Researcher
@@ -2,6 +2,14 @@ CFLAGS += -I inc -I ../ -nostdlib -fno-builtin -std=gnu11 -Os
|
||||
|
||||
CFLAGS += -Tstm32_flash.ld
|
||||
|
||||
# Compile fast charge (DCP) only not on EON
|
||||
ifeq (,$(wildcard /EON))
|
||||
BUILDER = DEV
|
||||
else
|
||||
CFLAGS += "-DEON"
|
||||
BUILDER = EON
|
||||
endif
|
||||
|
||||
CC = arm-none-eabi-gcc
|
||||
OBJCOPY = arm-none-eabi-objcopy
|
||||
OBJDUMP = arm-none-eabi-objdump
|
||||
|
||||
@@ -454,6 +454,7 @@ void early() {
|
||||
#ifdef PANDA
|
||||
// enable the ESP, disable ESP boot mode
|
||||
// unless we are on a giant panda, then there's no ESP
|
||||
// dont disable on grey panda
|
||||
if (is_giant_panda) {
|
||||
set_esp_mode(ESP_DISABLED);
|
||||
} else {
|
||||
|
||||
+7
-4
@@ -539,6 +539,9 @@ int main() {
|
||||
} else {
|
||||
// enable ESP uart
|
||||
uart_init(USART1, 115200);
|
||||
#ifdef EON
|
||||
set_esp_mode(ESP_DISABLED);
|
||||
#endif
|
||||
}
|
||||
// enable LIN
|
||||
uart_init(UART5, 10400);
|
||||
@@ -590,8 +593,6 @@ int main() {
|
||||
uint64_t marker = 0;
|
||||
#define CURRENT_THRESHOLD 0xF00
|
||||
#define CLICKS 8
|
||||
// Enough clicks to ensure that enumeration happened. Should be longer than bootup time of the device connected to EON
|
||||
#define CLICKS_BOOTUP 30
|
||||
#endif
|
||||
|
||||
for (cnt=0;;cnt++) {
|
||||
@@ -618,8 +619,9 @@ int main() {
|
||||
}
|
||||
break;
|
||||
case USB_POWER_CDP:
|
||||
// been CLICKS_BOOTUP clicks since we switched to CDP
|
||||
if ((cnt-marker) >= CLICKS_BOOTUP ) {
|
||||
#ifndef EON
|
||||
// been CLICKS clicks since we switched to CDP
|
||||
if ((cnt-marker) >= CLICKS) {
|
||||
// measure current draw, if positive and no enumeration, switch to DCP
|
||||
if (!is_enumerated && current < CURRENT_THRESHOLD) {
|
||||
puts("USBP: no enumeration with current draw, switching to DCP mode\n");
|
||||
@@ -631,6 +633,7 @@ int main() {
|
||||
if (current >= CURRENT_THRESHOLD) {
|
||||
marker = cnt;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case USB_POWER_DCP:
|
||||
// been at least CLICKS clicks since we switched to DCP
|
||||
|
||||
@@ -30,12 +30,20 @@ recover: obj/bootstub.bin obj/$(PROJ_NAME).bin
|
||||
$(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
|
||||
|
||||
include ../../common/version.mk
|
||||
|
||||
obj/cert.h: ../../crypto/getcertheader.py
|
||||
../../crypto/getcertheader.py ../../certs/debug.pub ../../certs/release.pub > $@
|
||||
|
||||
obj/main.o: main.c ../*.h
|
||||
mkdir -p obj
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/bootstub.o: ../bootstub.c ../*.h
|
||||
obj/bootstub.o: ../bootstub.c ../*.h obj/gitversion.h obj/cert.h
|
||||
mkdir -p obj
|
||||
mkdir -p ../obj
|
||||
cp obj/gitversion.h ../obj/gitversion.h
|
||||
cp obj/cert.h ../obj/cert.h
|
||||
$(CC) $(CFLAGS) -o $@ -c $<
|
||||
|
||||
obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s
|
||||
@@ -70,21 +70,24 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
|
||||
|
||||
#endif
|
||||
|
||||
// ***************************** honda can checksum *****************************
|
||||
// ***************************** pedal can checksum *****************************
|
||||
|
||||
int can_cksum(uint8_t *dat, int len, int addr, int idx) {
|
||||
int i;
|
||||
int s = 0;
|
||||
for (i = 0; i < len; i++) {
|
||||
s += (dat[i] >> 4);
|
||||
s += dat[i] & 0xF;
|
||||
uint8_t pedal_checksum(uint8_t *dat, int len) {
|
||||
uint8_t crc = 0xFF;
|
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
int i, j;
|
||||
for (i = len - 1; i >= 0; i--) {
|
||||
crc ^= dat[i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
}
|
||||
else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
s += (addr>>0)&0xF;
|
||||
s += (addr>>4)&0xF;
|
||||
s += (addr>>8)&0xF;
|
||||
s += idx;
|
||||
s = 8-s;
|
||||
return s&0xF;
|
||||
return crc;
|
||||
}
|
||||
|
||||
// ***************************** can port *****************************
|
||||
@@ -92,6 +95,8 @@ int can_cksum(uint8_t *dat, int len, int addr, int idx) {
|
||||
// addresses to be used on CAN
|
||||
#define CAN_GAS_INPUT 0x200
|
||||
#define CAN_GAS_OUTPUT 0x201
|
||||
#define CAN_GAS_SIZE 6
|
||||
#define COUNTER_CYCLE 0xF
|
||||
|
||||
void CAN1_TX_IRQHandler() {
|
||||
// clear interrupt
|
||||
@@ -134,14 +139,19 @@ void CAN1_RX0_IRQHandler() {
|
||||
}
|
||||
|
||||
// normal packet
|
||||
uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR;
|
||||
uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR;
|
||||
uint8_t dat[8];
|
||||
uint8_t *rdlr = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR;
|
||||
uint8_t *rdhr = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR;
|
||||
for (int i=0; i<4; i++) {
|
||||
dat[i] = rdlr[i];
|
||||
dat[i+4] = rdhr[i];
|
||||
}
|
||||
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) {
|
||||
uint8_t enable = (dat[4] >> 7) & 1;
|
||||
uint8_t index = dat[4] & COUNTER_CYCLE;
|
||||
if (pedal_checksum(dat, CAN_GAS_SIZE - 1) == dat[5]) {
|
||||
if (((current_index + 1) & COUNTER_CYCLE) == index) {
|
||||
#ifdef DEBUG
|
||||
puts("setting gas ");
|
||||
puth(value);
|
||||
@@ -196,18 +206,18 @@ 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[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);
|
||||
dat[0] = (pdl0>>8) & 0xFF;
|
||||
dat[1] = (pdl0>>0) & 0xFF;
|
||||
dat[2] = (pdl1>>8) & 0xFF;
|
||||
dat[3] = (pdl1>>0) & 0xFF;
|
||||
dat[4] = (state & 0xF) << 4 | pkt_idx;
|
||||
dat[5] = pedal_checksum(dat, CAN_GAS_SIZE - 1);
|
||||
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;
|
||||
pkt_idx &= COUNTER_CYCLE;
|
||||
} else {
|
||||
// old can packet hasn't sent!
|
||||
state = FAULT_SEND;
|
||||
@@ -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;
|
||||
}
|
||||
@@ -1,28 +1,110 @@
|
||||
void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {}
|
||||
const int SUBARU_MAX_STEER = 2047; // 1s
|
||||
// real time torque limit to prevent controls spamming
|
||||
// the real time limit is 1500/sec
|
||||
const int SUBARU_MAX_RT_DELTA = 940; // max delta torque allowed for real time checks
|
||||
const int32_t SUBARU_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
const int SUBARU_MAX_RATE_UP = 50;
|
||||
const int SUBARU_MAX_RATE_DOWN = 70;
|
||||
const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
|
||||
const int SUBARU_DRIVER_TORQUE_FACTOR = 10;
|
||||
|
||||
// FIXME
|
||||
// *** all output safety mode ***
|
||||
int subaru_cruise_engaged_last = 0;
|
||||
int subaru_rt_torque_last = 0;
|
||||
int subaru_desired_torque_last = 0;
|
||||
uint32_t subaru_ts_last = 0;
|
||||
struct sample_t subaru_torque_driver; // last few driver torques measured
|
||||
|
||||
static void subaru_init(int16_t param) {
|
||||
controls_allowed = 1;
|
||||
|
||||
static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int bus_number = (to_push->RDTR >> 4) & 0xFF;
|
||||
uint32_t addr = to_push->RIR >> 21;
|
||||
|
||||
if ((addr == 0x119) && (bus_number == 0)){
|
||||
int torque_driver_new = ((to_push->RDLR >> 16) & 0x7FF);
|
||||
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||
// update array of samples
|
||||
update_sample(&subaru_torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if ((addr == 0x240) && (bus_number == 0)) {
|
||||
int cruise_engaged = (to_push->RDHR >> 9) & 1;
|
||||
if (cruise_engaged && !subaru_cruise_engaged_last) {
|
||||
controls_allowed = 1;
|
||||
} else if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
subaru_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
}
|
||||
|
||||
static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
uint32_t addr = to_send->RIR >> 21;
|
||||
|
||||
// steer cmd checks
|
||||
if (addr == 0x122) {
|
||||
int desired_torque = ((to_send->RDLR >> 16) & 0x1FFF);
|
||||
int violation = 0;
|
||||
uint32_t ts = TIM2->CNT;
|
||||
desired_torque = to_signed(desired_torque, 13);
|
||||
|
||||
if (controls_allowed) {
|
||||
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
int desired_torque_last = subaru_desired_torque_last;
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &subaru_torque_driver,
|
||||
SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN,
|
||||
SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR);
|
||||
|
||||
// used next time
|
||||
subaru_desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, subaru_rt_torque_last, SUBARU_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, subaru_ts_last);
|
||||
if (ts_elapsed > SUBARU_RT_INTERVAL) {
|
||||
subaru_rt_torque_last = desired_torque;
|
||||
subaru_ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
// no torque if controls is not allowed
|
||||
if (!controls_allowed && (desired_torque != 0)) {
|
||||
violation = 1;
|
||||
}
|
||||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
subaru_desired_torque_last = 0;
|
||||
subaru_rt_torque_last = 0;
|
||||
subaru_ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
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
|
||||
return 2; // ES CAN
|
||||
}
|
||||
// forward CAN 1 > 0, except ES_LKAS
|
||||
else if (bus_num == 1) {
|
||||
|
||||
else if (bus_num == 2) {
|
||||
|
||||
// outback 2015
|
||||
if (addr == 0x164) {
|
||||
return -1;
|
||||
@@ -40,10 +122,10 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
||||
}
|
||||
|
||||
const safety_hooks subaru_hooks = {
|
||||
.init = subaru_init,
|
||||
.init = nooutput_init,
|
||||
.rx = subaru_rx_hook,
|
||||
.tx = subaru_tx_hook,
|
||||
.tx_lin = nooutput_tx_lin_hook,
|
||||
.ignition = default_ign_hook,
|
||||
.fwd = subaru_fwd_hook,
|
||||
};
|
||||
};
|
||||
|
||||
@@ -79,6 +79,15 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
||||
// no IPAS in non IPAS mode
|
||||
if (((to_send->RIR>>21) == 0x266) || ((to_send->RIR>>21) == 0x167)) return false;
|
||||
|
||||
// GAS PEDAL: safety check
|
||||
if ((to_send->RIR>>21) == 0x200) {
|
||||
if (controls_allowed && toyota_actuation_limits) {
|
||||
// all messages are fine here
|
||||
} else {
|
||||
if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// ACCEL: safety check on byte 1-2
|
||||
if ((to_send->RIR>>21) == 0x343) {
|
||||
int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF);
|
||||
|
||||
Reference in New Issue
Block a user