add smartdsu and ibst boards
parent
54b12251dc
commit
7aa6e7ae1f
|
@ -21,6 +21,38 @@ if os.getenv("PEDAL"):
|
|||
"-DPEDAL",
|
||||
]
|
||||
|
||||
if os.getenv("IBST"):
|
||||
PROJECT = "panda"
|
||||
STARTUP_FILE = "startup_stm32f413xx.s"
|
||||
MAIN = "ibst/main.c"
|
||||
PROJECT_FLAGS = [
|
||||
"-mcpu=cortex-m4",
|
||||
"-mhard-float",
|
||||
"-DSTM32F4",
|
||||
"-DSTM32F413xx",
|
||||
"-mfpu=fpv4-sp-d16",
|
||||
"-fsingle-precision-constant",
|
||||
"-Os",
|
||||
"-g",
|
||||
"-DGATEWAY",
|
||||
]
|
||||
|
||||
if os.getenv("SDSU"):
|
||||
PROJECT = "panda"
|
||||
STARTUP_FILE = "startup_stm32f413xx.s"
|
||||
MAIN = "smart_dsu/main.c"
|
||||
PROJECT_FLAGS = [
|
||||
"-mcpu=cortex-m4",
|
||||
"-mhard-float",
|
||||
"-DSTM32F4",
|
||||
"-DSTM32F413xx",
|
||||
"-mfpu=fpv4-sp-d16",
|
||||
"-fsingle-precision-constant",
|
||||
"-Os",
|
||||
"-g",
|
||||
"-DGATEWAY",
|
||||
]
|
||||
|
||||
else:
|
||||
PROJECT = "panda"
|
||||
STARTUP_FILE = "startup_stm32f413xx.s"
|
||||
|
|
|
@ -18,6 +18,9 @@
|
|||
#else
|
||||
#include "boards/pedal.h"
|
||||
#endif
|
||||
#ifdef GATEWAY
|
||||
#include "boards/gateway.h"
|
||||
#endif
|
||||
|
||||
void detect_board_type(void) {
|
||||
#ifdef PANDA
|
||||
|
@ -44,11 +47,16 @@ void detect_board_type(void) {
|
|||
#ifdef PEDAL
|
||||
hw_type = HW_TYPE_PEDAL;
|
||||
current_board = &board_pedal;
|
||||
#else
|
||||
#ifdef GATEWAY
|
||||
hw_type = HW_TYPE_GATEWAY;
|
||||
current_board = &board_gateway;
|
||||
#else
|
||||
hw_type = HW_TYPE_UNKNOWN;
|
||||
puts("Hardware type is UNKNOWN!\n");
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -44,6 +44,7 @@ struct board {
|
|||
#define HW_TYPE_PEDAL 4U
|
||||
#define HW_TYPE_UNO 5U
|
||||
#define HW_TYPE_DOS 6U
|
||||
#define HW_TYPE_GATEWAY 8U
|
||||
|
||||
// LED colors
|
||||
#define LED_RED 0U
|
||||
|
|
|
@ -0,0 +1,125 @@
|
|||
// /////////// //
|
||||
// gateway //
|
||||
// /////////// //
|
||||
|
||||
void gateway_enable_can_transceiver(uint8_t transceiver, bool enabled) {
|
||||
switch (transceiver){
|
||||
case 1U:
|
||||
set_gpio_output(GPIOC, 1, !enabled);
|
||||
break;
|
||||
case 2U:
|
||||
set_gpio_output(GPIOC, 13, !enabled);
|
||||
break;
|
||||
case 3U:
|
||||
set_gpio_output(GPIOA, 0, !enabled);
|
||||
break;
|
||||
default:
|
||||
puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void gateway_enable_can_transceivers(bool enabled) {
|
||||
uint8_t t1 = enabled ? 1U : 2U; // leave transceiver 1 enabled to detect CAN ignition
|
||||
for(uint8_t i=t1; i<=3U; i++) {
|
||||
gateway_enable_can_transceiver(i, enabled);
|
||||
}
|
||||
}
|
||||
|
||||
void gateway_set_led(uint8_t color, bool enabled) {
|
||||
UNUSED(color);
|
||||
UNUSED(enabled);
|
||||
}
|
||||
|
||||
void gateway_set_usb_power_mode(uint8_t mode){
|
||||
UNUSED(mode);
|
||||
}
|
||||
|
||||
void gateway_set_gps_mode(uint8_t mode) {
|
||||
UNUSED(mode);
|
||||
}
|
||||
|
||||
void gateway_set_can_mode(uint8_t mode){
|
||||
switch (mode) {
|
||||
case CAN_MODE_NORMAL:
|
||||
set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
|
||||
set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
|
||||
// B5,B6: normal CAN2 mode
|
||||
set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
|
||||
set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
|
||||
|
||||
// A8,A15: normal CAN3 mode
|
||||
set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
|
||||
set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
|
||||
break;
|
||||
default:
|
||||
puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t gateway_read_current(void){
|
||||
return 0U;
|
||||
}
|
||||
|
||||
void gateway_usb_power_mode_tick(uint32_t uptime){
|
||||
UNUSED(uptime);
|
||||
}
|
||||
|
||||
void gateway_set_ir_power(uint8_t percentage){
|
||||
UNUSED(percentage);
|
||||
}
|
||||
|
||||
void gateway_set_fan_power(uint8_t percentage){
|
||||
UNUSED(percentage);
|
||||
}
|
||||
|
||||
bool gateway_check_ignition(void){
|
||||
// ignition is on PA1
|
||||
return 0U;
|
||||
}
|
||||
|
||||
void gateway_set_phone_power(bool enabled){
|
||||
UNUSED(enabled);
|
||||
}
|
||||
|
||||
void gateway_set_clock_source_mode(uint8_t mode){
|
||||
UNUSED(mode);
|
||||
}
|
||||
|
||||
void gateway_set_siren(bool enabled){
|
||||
UNUSED(enabled);
|
||||
}
|
||||
|
||||
void gateway_init(void) {
|
||||
common_init_gpio();
|
||||
|
||||
// Enable CAN transceivers
|
||||
gateway_enable_can_transceivers(true);
|
||||
// Set normal CAN mode
|
||||
gateway_set_can_mode(CAN_MODE_NORMAL);
|
||||
}
|
||||
|
||||
const harness_configuration gateway_harness_config = {
|
||||
.has_harness = false
|
||||
};
|
||||
|
||||
const board board_gateway = {
|
||||
.board_type = "White",
|
||||
.harness_config = &gateway_harness_config,
|
||||
.init = gateway_init,
|
||||
.enable_can_transceiver = gateway_enable_can_transceiver,
|
||||
.enable_can_transceivers = gateway_enable_can_transceivers,
|
||||
.set_led = gateway_set_led,
|
||||
.set_usb_power_mode = gateway_set_usb_power_mode,
|
||||
.set_gps_mode = gateway_set_gps_mode,
|
||||
.set_can_mode = gateway_set_can_mode,
|
||||
.usb_power_mode_tick = gateway_usb_power_mode_tick,
|
||||
.check_ignition = gateway_check_ignition,
|
||||
.read_current = gateway_read_current,
|
||||
.set_fan_power = gateway_set_fan_power,
|
||||
.set_ir_power = gateway_set_ir_power,
|
||||
.set_phone_power = gateway_set_phone_power,
|
||||
.set_clock_source_mode = gateway_set_clock_source_mode,
|
||||
.set_siren = gateway_set_siren
|
||||
};
|
|
@ -7,7 +7,6 @@
|
|||
#include "obj/gitversion.h"
|
||||
|
||||
#ifdef STM32F4
|
||||
#define PANDA
|
||||
#include "stm32f4xx.h"
|
||||
#include "stm32f4xx_hal_gpio_ex.h"
|
||||
#else
|
||||
|
|
|
@ -8,8 +8,10 @@
|
|||
//#define DEBUG_FAULTS
|
||||
|
||||
#ifdef STM32F4
|
||||
#define PANDA
|
||||
#include "stm32f4xx.h"
|
||||
#ifndef GATEWAY
|
||||
#define PANDA
|
||||
#endif
|
||||
#else
|
||||
#include "stm32f2xx.h"
|
||||
#endif
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
obj/*
|
|
@ -0,0 +1,370 @@
|
|||
// cut down version of the can.h driver. allows pedal-like devices to work like Pandas
|
||||
|
||||
// IRQs: CAN1_TX, CAN1_RX0, CAN1_SCE
|
||||
// CAN2_TX, CAN2_RX0, CAN2_SCE
|
||||
// CAN3_TX, CAN3_RX0, CAN3_SCE
|
||||
|
||||
typedef struct {
|
||||
volatile uint32_t w_ptr;
|
||||
volatile uint32_t r_ptr;
|
||||
uint32_t fifo_size;
|
||||
CAN_FIFOMailBox_TypeDef *elems;
|
||||
} can_ring;
|
||||
|
||||
#define CAN_BUS_RET_FLAG 0x80U
|
||||
#define CAN_BUS_NUM_MASK 0x7FU
|
||||
|
||||
#define BUS_MAX 4U
|
||||
|
||||
uint32_t can_rx_errs = 0;
|
||||
uint32_t can_send_errs = 0;
|
||||
uint32_t can_fwd_errs = 0;
|
||||
extern int can_live, pending_can_live;
|
||||
|
||||
// must reinit after changing these
|
||||
extern int can_loopback, can_silent;
|
||||
extern uint32_t can_speed[4];
|
||||
|
||||
void can_set_forwarding(int from, int to);
|
||||
|
||||
bool can_init(uint8_t can_number);
|
||||
void can_init_all(void);
|
||||
bool can_tx_check_min_slots_free(uint32_t min);
|
||||
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook);
|
||||
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
|
||||
|
||||
// Ignition detected from CAN meessages
|
||||
bool ignition_can = false;
|
||||
bool ignition_cadillac = false;
|
||||
uint32_t ignition_can_cnt = 0U;
|
||||
|
||||
// end API
|
||||
|
||||
#define ALL_CAN_SILENT 0xFF
|
||||
#define ALL_CAN_LIVE 0
|
||||
|
||||
int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT;
|
||||
|
||||
// ********************* instantiate queues *********************
|
||||
|
||||
#define can_buffer(x, size) \
|
||||
CAN_FIFOMailBox_TypeDef elems_##x[size]; \
|
||||
can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = size, .elems = (CAN_FIFOMailBox_TypeDef *)&elems_##x };
|
||||
|
||||
can_buffer(rx_q, 0x1000)
|
||||
can_buffer(tx1_q, 0x100)
|
||||
can_buffer(tx2_q, 0x100)
|
||||
can_buffer(tx3_q, 0x100)
|
||||
can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q};
|
||||
|
||||
// global CAN stats
|
||||
int can_rx_cnt = 0;
|
||||
int can_tx_cnt = 0;
|
||||
int can_txd_cnt = 0;
|
||||
int can_err_cnt = 0;
|
||||
int can_overflow_cnt = 0;
|
||||
|
||||
// ********************* interrupt safe queue *********************
|
||||
|
||||
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
|
||||
bool ret = 0;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if (q->w_ptr != q->r_ptr) {
|
||||
*elem = q->elems[q->r_ptr];
|
||||
if ((q->r_ptr + 1U) == q->fifo_size) {
|
||||
q->r_ptr = 0;
|
||||
} else {
|
||||
q->r_ptr += 1U;
|
||||
}
|
||||
ret = 1;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
|
||||
bool ret = false;
|
||||
uint32_t next_w_ptr;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if ((q->w_ptr + 1U) == q->fifo_size) {
|
||||
next_w_ptr = 0;
|
||||
} else {
|
||||
next_w_ptr = q->w_ptr + 1U;
|
||||
}
|
||||
if (next_w_ptr != q->r_ptr) {
|
||||
q->elems[q->w_ptr] = *elem;
|
||||
q->w_ptr = next_w_ptr;
|
||||
ret = true;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
if (!ret) {
|
||||
can_overflow_cnt++;
|
||||
#ifdef DEBUG
|
||||
puts("can_push failed!\n");
|
||||
#endif
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint32_t can_slots_empty(can_ring *q) {
|
||||
uint32_t ret = 0;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if (q->w_ptr >= q->r_ptr) {
|
||||
ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr;
|
||||
} else {
|
||||
ret = q->r_ptr - q->w_ptr - 1U;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void can_clear(can_ring *q) {
|
||||
ENTER_CRITICAL();
|
||||
q->w_ptr = 0;
|
||||
q->r_ptr = 0;
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
|
||||
// assign CAN numbering
|
||||
// bus num: Can bus number on ODB connector. Sent to/from USB
|
||||
// Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1)
|
||||
// cans: Look up MCU can interface from bus number
|
||||
// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc);
|
||||
// bus_lookup: Translates from 'can number' to 'bus number'.
|
||||
// can_num_lookup: Translates from 'bus number' to 'can number'.
|
||||
// can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward.
|
||||
|
||||
// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3
|
||||
CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3};
|
||||
uint8_t bus_lookup[] = {0,1,2};
|
||||
uint8_t can_num_lookup[] = {0,1,2,-1};
|
||||
int8_t can_forwarding[] = {-1,-1,-1,-1};
|
||||
uint32_t can_speed[] = {5000, 5000, 5000, 333};
|
||||
#define CAN_MAX 3U
|
||||
|
||||
#define CANIF_FROM_CAN_NUM(num) (cans[num])
|
||||
#define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2))
|
||||
#define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num])
|
||||
#define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num])
|
||||
|
||||
void process_can(uint8_t can_number);
|
||||
|
||||
bool can_set_speed(uint8_t can_number) {
|
||||
bool ret = true;
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
|
||||
ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number));
|
||||
return ret;
|
||||
}
|
||||
|
||||
void can_init_all(void) {
|
||||
bool ret = true;
|
||||
for (uint8_t i=0U; i < CAN_MAX; i++) {
|
||||
can_clear(can_queues[i]);
|
||||
ret &= can_init(i);
|
||||
}
|
||||
UNUSED(ret);
|
||||
}
|
||||
|
||||
void can_flip_buses(uint8_t bus1, uint8_t bus2){
|
||||
bus_lookup[bus1] = bus2;
|
||||
bus_lookup[bus2] = bus1;
|
||||
can_num_lookup[bus1] = bus2;
|
||||
can_num_lookup[bus2] = bus1;
|
||||
}
|
||||
|
||||
|
||||
// CAN error
|
||||
void can_sce(CAN_TypeDef *CAN) {
|
||||
ENTER_CRITICAL();
|
||||
|
||||
#ifdef DEBUG
|
||||
if (CAN==CAN1) puts("CAN1: ");
|
||||
if (CAN==CAN2) puts("CAN2: ");
|
||||
#ifdef CAN3
|
||||
if (CAN==CAN3) puts("CAN3: ");
|
||||
#endif
|
||||
puts("MSR:");
|
||||
puth(CAN->MSR);
|
||||
puts(" TSR:");
|
||||
puth(CAN->TSR);
|
||||
puts(" RF0R:");
|
||||
puth(CAN->RF0R);
|
||||
puts(" RF1R:");
|
||||
puth(CAN->RF1R);
|
||||
puts(" ESR:");
|
||||
puth(CAN->ESR);
|
||||
puts("\n");
|
||||
#endif
|
||||
|
||||
can_err_cnt += 1;
|
||||
llcan_clear_send(CAN);
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
|
||||
// ***************************** CAN *****************************
|
||||
|
||||
void process_can(uint8_t can_number) {
|
||||
if (can_number != 0xffU) {
|
||||
|
||||
ENTER_CRITICAL();
|
||||
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
|
||||
// check for empty mailbox
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
// add successfully transmitted message to my fifo
|
||||
if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
|
||||
can_txd_cnt += 1;
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
|
||||
CAN_FIFOMailBox_TypeDef to_push;
|
||||
to_push.RIR = CAN->sTxMailBox[0].TIR;
|
||||
to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
|
||||
to_push.RDLR = CAN->sTxMailBox[0].TDLR;
|
||||
to_push.RDHR = CAN->sTxMailBox[0].TDHR;
|
||||
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
|
||||
}
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
|
||||
#ifdef DEBUG
|
||||
puts("CAN TX ERROR!\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
|
||||
#ifdef DEBUG
|
||||
puts("CAN TX ARBITRATION LOST!\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
// clear interrupt
|
||||
// careful, this can also be cleared by requesting a transmission
|
||||
CAN->TSR |= CAN_TSR_RQCP0;
|
||||
}
|
||||
|
||||
if (can_pop(can_queues[bus_number], &to_send)) {
|
||||
can_tx_cnt += 1;
|
||||
// only send if we have received a packet
|
||||
CAN->sTxMailBox[0].TDLR = to_send.RDLR;
|
||||
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
|
||||
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
|
||||
CAN->sTxMailBox[0].TIR = to_send.RIR;
|
||||
|
||||
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
|
||||
usb_outep3_resume_if_paused();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
}
|
||||
|
||||
void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
|
||||
ignition_can_cnt = 0U; // reset counter
|
||||
|
||||
if (bus == 0) {
|
||||
// TODO: verify on all supported GM models that we can reliably detect ignition using only this signal,
|
||||
// since the 0x1F1 signal can briefly go low immediately after ignition
|
||||
if ((addr == 0x160) && (len == 5)) {
|
||||
// this message isn't all zeros when ignition is on
|
||||
ignition_cadillac = GET_BYTES_04(to_push) != 0;
|
||||
}
|
||||
// GM exception
|
||||
if ((addr == 0x1F1) && (len == 8)) {
|
||||
// Bit 5 is ignition "on"
|
||||
bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20) != 0);
|
||||
ignition_can = ignition_gm || ignition_cadillac;
|
||||
}
|
||||
// Tesla exception
|
||||
if ((addr == 0x348) && (len == 8)) {
|
||||
// GTW_status
|
||||
ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// CAN receive handlers
|
||||
// blink blue when we are receiving CAN messages
|
||||
void can_rx(uint8_t can_number) {
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
can_rx_cnt += 1;
|
||||
|
||||
// can is live
|
||||
pending_can_live = 1;
|
||||
|
||||
// add to my fifo
|
||||
CAN_FIFOMailBox_TypeDef to_push;
|
||||
to_push.RIR = CAN->sFIFOMailBox[0].RIR;
|
||||
to_push.RDTR = CAN->sFIFOMailBox[0].RDTR;
|
||||
to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
|
||||
to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
|
||||
|
||||
// modify RDTR for our API
|
||||
to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
|
||||
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
|
||||
|
||||
// next
|
||||
CAN->RF0R |= CAN_RF0R_RFOM0;
|
||||
}
|
||||
}
|
||||
|
||||
bool can_tx_check_min_slots_free(uint32_t min) {
|
||||
return
|
||||
(can_slots_empty(&can_tx1_q) >= min) &&
|
||||
(can_slots_empty(&can_tx2_q) >= min) &&
|
||||
(can_slots_empty(&can_tx3_q) >= min);
|
||||
}
|
||||
|
||||
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
|
||||
if (!skip_tx_hook) {
|
||||
#ifdef DEBUG
|
||||
puts("TX CAN");
|
||||
puth2(bus_number);
|
||||
puts(" ");
|
||||
puth(to_push->RIR >> 21);
|
||||
puts("\n");
|
||||
#endif
|
||||
if (bus_number < BUS_MAX) {
|
||||
// add CAN packet to send queue
|
||||
// bus number isn't passed through
|
||||
to_push->RDTR &= 0xF;
|
||||
can_fwd_errs += can_push(can_queues[bus_number], to_push) ? 0U : 1U;
|
||||
process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void can_set_forwarding(int from, int to) {
|
||||
can_forwarding[from] = to;
|
||||
}
|
||||
|
||||
bool can_init(uint8_t can_number) {
|
||||
bool ret = false;
|
||||
|
||||
if (can_number != 0xffU) {
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
ret &= can_set_speed(can_number);
|
||||
ret &= llcan_init(CAN);
|
||||
// in case there are queued up messages
|
||||
process_can(can_number);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
cd ..
|
||||
IBST=1 scons -u
|
||||
cd ibst
|
||||
|
||||
../../tests/gateway/enter_canloader.py ../obj/panda.bin.signed
|
|
@ -0,0 +1,814 @@
|
|||
// ********************* Includes *********************
|
||||
#include "../config.h"
|
||||
#include "libc.h"
|
||||
|
||||
#include "main_declarations.h"
|
||||
#include "critical.h"
|
||||
#include "faults.h"
|
||||
|
||||
#include "drivers/registers.h"
|
||||
#include "drivers/interrupts.h"
|
||||
#include "drivers/llcan.h"
|
||||
#include "drivers/llgpio.h"
|
||||
#include "drivers/adc.h"
|
||||
|
||||
#include "board.h"
|
||||
|
||||
#include "drivers/clock.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#include "gpio.h"
|
||||
#include "crc.h"
|
||||
|
||||
// uncomment for usb debugging via debug_console.py
|
||||
#define IBST_USB
|
||||
#define DEBUG
|
||||
|
||||
#ifdef IBST_USB
|
||||
#include "drivers/uart.h"
|
||||
#include "drivers/usb.h"
|
||||
#else
|
||||
// no serial either
|
||||
void puts(const char *a) {
|
||||
UNUSED(a);
|
||||
}
|
||||
void puth(unsigned int i) {
|
||||
UNUSED(i);
|
||||
}
|
||||
void puth2(unsigned int i) {
|
||||
UNUSED(i);
|
||||
}
|
||||
#endif
|
||||
|
||||
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
|
||||
uint32_t enter_bootloader_mode;
|
||||
|
||||
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
|
||||
void __initialize_hardware_early(void) {
|
||||
early();
|
||||
}
|
||||
|
||||
#ifdef IBST_USB
|
||||
|
||||
#include "ibst/can.h"
|
||||
|
||||
// ********************* usb debugging *********************
|
||||
void debug_ring_callback(uart_ring *ring) {
|
||||
char rcv;
|
||||
while (getc(ring, &rcv) != 0) {
|
||||
(void)putc(ring, rcv);
|
||||
}
|
||||
}
|
||||
|
||||
int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
|
||||
int ilen = 0;
|
||||
while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) {
|
||||
ilen++;
|
||||
}
|
||||
return ilen*0x10;
|
||||
}
|
||||
// send on serial, first byte to select the ring
|
||||
void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
uint8_t *usbdata8 = (uint8_t *)usbdata;
|
||||
uart_ring *ur = get_ring_by_number(usbdata8[0]);
|
||||
if ((len != 0) && (ur != NULL)) {
|
||||
if ((usbdata8[0] < 2U)) {
|
||||
for (int i = 1; i < len; i++) {
|
||||
while (!putc(ur, usbdata8[i])) {
|
||||
// wait
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// send on CAN
|
||||
void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(usbdata);
|
||||
UNUSED(len);
|
||||
UNUSED(hardwired);
|
||||
}
|
||||
void usb_cb_ep3_out_complete() {
|
||||
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
|
||||
usb_outep3_resume_if_paused();
|
||||
}
|
||||
}
|
||||
|
||||
void usb_cb_enumeration_complete() {
|
||||
puts("USB enumeration complete\n");
|
||||
is_enumerated = 1;
|
||||
}
|
||||
|
||||
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
unsigned int resp_len = 0;
|
||||
uart_ring *ur = NULL;
|
||||
switch (setup->b.bRequest) {
|
||||
// **** 0xd1: enter bootloader mode
|
||||
case 0xd1:
|
||||
// this allows reflashing of the bootstub
|
||||
// so it's blocked over wifi
|
||||
switch (setup->b.wValue.w) {
|
||||
case 0:
|
||||
// only allow bootloader entry on debug builds
|
||||
#ifdef ALLOW_DEBUG
|
||||
if (hardwired) {
|
||||
puts("-> entering bootloader\n");
|
||||
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 1:
|
||||
puts("-> entering softloader\n");
|
||||
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
default:
|
||||
puts("Bootloader mode invalid\n");
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// **** 0xd8: reset ST
|
||||
case 0xd8:
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
// **** 0xe0: uart read
|
||||
case 0xe0:
|
||||
ur = get_ring_by_number(setup->b.wValue.w);
|
||||
if (!ur) {
|
||||
break;
|
||||
}
|
||||
// read
|
||||
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
||||
getc(ur, (char*)&resp[resp_len])) {
|
||||
++resp_len;
|
||||
}
|
||||
break;
|
||||
// **** 0xf1: Clear CAN ring buffer.
|
||||
case 0xf1:
|
||||
if (setup->b.wValue.w == 0xFFFFU) {
|
||||
puts("Clearing CAN Rx queue\n");
|
||||
can_clear(&can_rx_q);
|
||||
} else if (setup->b.wValue.w < BUS_MAX) {
|
||||
puts("Clearing CAN Tx queue\n");
|
||||
can_clear(can_queues[setup->b.wValue.w]);
|
||||
} else {
|
||||
puts("Clearing CAN CAN ring buffer failed: wrong bus number\n");
|
||||
}
|
||||
break;
|
||||
// **** 0xf2: Clear UART ring buffer.
|
||||
case 0xf2:
|
||||
{
|
||||
uart_ring * rb = get_ring_by_number(setup->b.wValue.w);
|
||||
if (rb != NULL) {
|
||||
puts("Clearing UART queue.\n");
|
||||
clear_uart_buff(rb);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
puts("NO HANDLER ");
|
||||
puth(setup->b.bRequest);
|
||||
puts("\n");
|
||||
break;
|
||||
}
|
||||
return resp_len;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// ***************************** can port *****************************
|
||||
#define CAN_UPDATE 0x341 //bootloader
|
||||
#define COUNTER_CYCLE 0xFU
|
||||
|
||||
void CAN1_TX_IRQ_Handler(void) {
|
||||
process_can(0);
|
||||
}
|
||||
|
||||
void CAN2_TX_IRQ_Handler(void) {
|
||||
// CAN2->TSR |= CAN_TSR_RQCP0;
|
||||
process_can(1);
|
||||
}
|
||||
|
||||
void CAN3_TX_IRQ_Handler(void) {
|
||||
process_can(2);
|
||||
}
|
||||
|
||||
bool sent;
|
||||
|
||||
// OUTPUTS
|
||||
// 0x38B
|
||||
#define P_EST_MAX 0
|
||||
#define P_EST_MAX_QF 1
|
||||
#define VEHICLE_QF 1
|
||||
#define IGNITION_ON 0
|
||||
uint8_t current_speed = 0;
|
||||
|
||||
// 0x38C
|
||||
#define P_LIMIT_EXTERNAL 120
|
||||
#define Q_TARGET_DEFAULT 0x7e00 // this is the zero point
|
||||
uint16_t q_target_ext = Q_TARGET_DEFAULT;
|
||||
bool q_target_ext_qf = 0;
|
||||
|
||||
// 0x38D
|
||||
#define P_TARGET_DRIVER 0
|
||||
#define P_TARGET_DRIVER_QF 0
|
||||
#define ABS_ACTIVE 0
|
||||
#define P_MC 0
|
||||
#define P_MC_QF 1
|
||||
|
||||
// INPUTS
|
||||
uint16_t rel_input = 0;
|
||||
uint16_t pos_input = 0;
|
||||
bool pid_enable = 0;
|
||||
bool rel_enable = 0;
|
||||
|
||||
// 0x38E
|
||||
uint16_t output_rod_target = 0;
|
||||
bool driver_brake_applied = 0;
|
||||
bool brake_applied = 0;
|
||||
bool brake_ok = 0;
|
||||
|
||||
// 0x38F
|
||||
uint8_t ibst_status;
|
||||
uint8_t ext_req_status;
|
||||
|
||||
// COUNTERS
|
||||
uint8_t can1_count_out = 0;
|
||||
uint8_t can1_count_in;
|
||||
uint8_t can2_count_out_1 = 0;
|
||||
uint8_t can2_count_out_2 = 0;
|
||||
uint8_t can2_count_in_1;
|
||||
uint8_t can2_count_in_2;
|
||||
uint8_t can2_count_in_3;
|
||||
|
||||
#define MAX_TIMEOUT 50U
|
||||
uint32_t timeout = 0;
|
||||
|
||||
#define NO_FAULT 0U
|
||||
#define FAULT_BAD_CHECKSUM 1U
|
||||
#define FAULT_SEND 2U
|
||||
#define FAULT_SCE 3U
|
||||
#define FAULT_STARTUP 4U
|
||||
#define FAULT_TIMEOUT 5U
|
||||
#define FAULT_INVALID 6U
|
||||
#define FAULT_COUNTER 7U
|
||||
|
||||
uint8_t state = FAULT_STARTUP;
|
||||
|
||||
#define NO_EXTFAULT1 0U
|
||||
#define EXTFAULT1_CHECKSUM1 1U
|
||||
#define EXTFAULT1_CHECKSUM2 2U
|
||||
#define EXTFAULT1_CHECKSUM3 3U
|
||||
#define EXTFAULT1_SCE 4U
|
||||
#define EXTFAULT1_COUNTER1 5U
|
||||
#define EXTFAULT1_COUNTER2 6U
|
||||
#define EXTFAULT1_COUNTER3 7U
|
||||
#define EXTFAULT1_TIMEOUT 8U
|
||||
#define EXTFAULT1_SEND1 9U
|
||||
#define EXTFAULT1_SEND2 10U
|
||||
#define EXTFAULT1_SEND3 11U
|
||||
|
||||
uint8_t can2state = NO_EXTFAULT1;
|
||||
|
||||
#define NO_EXTFAULT2 0U
|
||||
#define EXTFAULT2_CHECKSUM1 1U
|
||||
#define EXTFAULT2_CHECKSUM2 2U
|
||||
#define EXTFAULT2_SCE 3U
|
||||
#define EXTFAULT2_COUNTER1 4U
|
||||
#define EXTFAULT2_COUNTER2 5U
|
||||
#define EXTFAULT2_TIMEOUT 6U
|
||||
|
||||
const uint8_t crc_poly = 0x1D; // standard crc8 SAE J1850
|
||||
uint8_t crc8_lut_1d[256];
|
||||
|
||||
void CAN1_RX0_IRQ_Handler(void) {
|
||||
while ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
uint16_t address = CAN1->sFIFOMailBox[0].RIR >> 21;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN1 RX: ");
|
||||
puth(address);
|
||||
puts("\n");
|
||||
#endif
|
||||
switch (address) {
|
||||
case CAN_UPDATE:
|
||||
if (GET_BYTES_04(&CAN1->sFIFOMailBox[0]) == 0xdeadface) {
|
||||
if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x0ab00b1e) {
|
||||
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
} else if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x02b00b1e) {
|
||||
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
} else {
|
||||
puts("Failed entering Softloader or Bootloader\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 0x20E: ;
|
||||
//uint64_t data; //sendESP_private2
|
||||
//uint8_t *dat = (uint8_t *)&data;
|
||||
uint8_t dat[6];
|
||||
for (int i=0; i<6; i++) {
|
||||
dat[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i);
|
||||
}
|
||||
uint8_t index = dat[1] & COUNTER_CYCLE;
|
||||
if(dat[0] == lut_checksum(dat, 6, crc8_lut_1d)) {
|
||||
if (((can1_count_in + 1U) & COUNTER_CYCLE) == index) {
|
||||
//if counter and checksum valid accept commands
|
||||
pid_enable = ((dat[1] >> 5U) & 1U);
|
||||
rel_enable = ((dat[1] >> 4U) & 1U);
|
||||
pos_input = ((dat[5] & 0xFU) << 8U) | dat[4];
|
||||
rel_input = ((dat[3] << 8U) | dat[2]);
|
||||
can1_count_in++;
|
||||
}
|
||||
else {
|
||||
state = FAULT_COUNTER;
|
||||
}
|
||||
state = NO_FAULT;
|
||||
timeout = 0;
|
||||
}
|
||||
else {
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
puts("checksum fail 0x20E \n");
|
||||
puts("DATA: ");
|
||||
for(int ii = 0; ii < 5; ii++){
|
||||
puth2(dat[ii]);
|
||||
}
|
||||
puts("\n");
|
||||
puts("expected: ");
|
||||
puth2(lut_checksum(dat, 5, crc8_lut_1d));
|
||||
puts(" got: ");
|
||||
puth2(dat[0]);
|
||||
puts("\n");
|
||||
}
|
||||
break;
|
||||
case 0x366: ;
|
||||
uint8_t dat2[4];
|
||||
for (int i=0; i<4; i++) {
|
||||
dat2[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i);
|
||||
}
|
||||
if(dat2[0] == lut_checksum(dat2, 4, crc8_lut_1d)) {
|
||||
current_speed = dat2[3];
|
||||
}
|
||||
else {
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
puts("checksum fail 0x366 \n");
|
||||
puts("DATA: ");
|
||||
for(int ii = 0; ii < 4; ii++){
|
||||
puth2(dat2[ii]);
|
||||
}
|
||||
puts("\n");
|
||||
puts("expected: ");
|
||||
puth2(lut_checksum(dat2, 4, crc8_lut_1d));
|
||||
puts(" got: ");
|
||||
puth2(dat2[0]);
|
||||
puts("\n");
|
||||
}
|
||||
default: ;
|
||||
}
|
||||
can_rx(0);
|
||||
// next
|
||||
// CAN1->RF0R |= CAN_RF0R_RFOM0;
|
||||
}
|
||||
}
|
||||
|
||||
void CAN1_SCE_IRQ_Handler(void) {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN1);
|
||||
llcan_clear_send(CAN1);
|
||||
}
|
||||
|
||||
void CAN2_RX0_IRQ_Handler(void) {
|
||||
while ((CAN2->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
uint16_t address = CAN2->sFIFOMailBox[0].RIR >> 21;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN2 RX: ");
|
||||
puth(address);
|
||||
puts("\n");
|
||||
#endif
|
||||
switch (address) {
|
||||
/* case 0x391:
|
||||
uint8_t dat[5];
|
||||
for (int i=0; i<5; i++) {
|
||||
dat[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i);
|
||||
}
|
||||
uint64_t *data = (uint8_t *)&dat;
|
||||
uint8_t index = dat[6] & COUNTER_CYCLE;
|
||||
if(dat[0] = lut_checksum(dat, 8, crc8_lut_1d)) {
|
||||
if (((can2_count_in1 + 1U) & COUNTER_CYCLE) == index) {
|
||||
//if counter and checksum valid accept commands
|
||||
ebr_mode = (dat[1] >> 4) & 0x7;
|
||||
ebr_system_mode = (data >> 15) & 0x7;
|
||||
can2_count_in1++;
|
||||
}
|
||||
else {
|
||||
state = EXTFAULT1_COUNTER1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
state = EXTFAULT1_CHECKSUM1;
|
||||
}
|
||||
break;*/
|
||||
case 0x38E: ;
|
||||
uint8_t dat[8]; //IBST_private1
|
||||
for (int i=0; i<8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN2->sFIFOMailBox[0], i);
|
||||
}
|
||||
uint8_t index = dat[1] & COUNTER_CYCLE;
|
||||
if(dat[0] == lut_checksum(dat, 8, crc8_lut_1d)) {
|
||||
if (((can2_count_in_1 + 1U) & COUNTER_CYCLE) == index) {
|
||||
//if counter and checksum valid accept commands
|
||||
output_rod_target = ((dat[4] & 0xFU) << 8U) | dat[3];
|
||||
can2_count_in_1++;
|
||||
}
|
||||
else {
|
||||
can2state = EXTFAULT1_COUNTER2;
|
||||
}
|
||||
}
|
||||
else {
|
||||
can2state = EXTFAULT1_CHECKSUM2;
|
||||
}
|
||||
break;
|
||||
case 0x38F: ;
|
||||
uint64_t data2; //IBST_private2
|
||||
uint8_t *dat2 = (uint8_t *)&data2;
|
||||
for (int i=0; i<8; i++) {
|
||||
dat2[i] = GET_BYTE(&CAN2->sFIFOMailBox[0], i);
|
||||
}
|
||||
uint8_t index2 = dat2[1] & COUNTER_CYCLE;
|
||||
if(dat2[0] == lut_checksum(dat2, 8, crc8_lut_1d)) {
|
||||
if (((can2_count_in_3 + 1U) & COUNTER_CYCLE) == index2) {
|
||||
//if counter and checksum valid accept commands
|
||||
ibst_status = (data2 >> 19) & 0x7;
|
||||
driver_brake_applied = ((dat2[2] & 0x1) | (!((dat2[2] >> 1) & 0x1))); //Sends brake applied if ibooster says brake applied or if there's a fault with the brake sensor, assumes worst case scenario
|
||||
brake_applied = (driver_brake_applied | (output_rod_target > 0x23FU));
|
||||
can2_count_in_3++;
|
||||
}
|
||||
else {
|
||||
can2state = EXTFAULT1_COUNTER3;
|
||||
}
|
||||
}
|
||||
else {
|
||||
can2state = EXTFAULT1_CHECKSUM3;
|
||||
}
|
||||
break;
|
||||
default: ;
|
||||
}
|
||||
// next
|
||||
can_rx(1);
|
||||
}
|
||||
}
|
||||
|
||||
void CAN2_SCE_IRQ_Handler(void) {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN2);
|
||||
llcan_clear_send(CAN2);
|
||||
}
|
||||
|
||||
void CAN3_RX0_IRQ_Handler(void) {
|
||||
while ((CAN3->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
uint16_t address = CAN3->sFIFOMailBox[0].RIR >> 21;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN1 RX: ");
|
||||
puth(address);
|
||||
puts("\n");
|
||||
#else
|
||||
UNUSED(address);
|
||||
#endif
|
||||
|
||||
// next
|
||||
can_rx(2);
|
||||
}
|
||||
}
|
||||
|
||||
void CAN3_SCE_IRQ_Handler(void) {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN3);
|
||||
llcan_clear_send(CAN3);
|
||||
}
|
||||
|
||||
// q_target_ext values
|
||||
// 7e00 max rod return
|
||||
// 8200 max brake.. TODO: check this?
|
||||
|
||||
// position values
|
||||
// BC0 max rod position (3008)
|
||||
// EC0 min rod position (-320)
|
||||
|
||||
#define P 2 // brake_rel is 2x brake_pos scale
|
||||
#define I 0
|
||||
#define D 0
|
||||
|
||||
#define OUTMAX 0x9200 //+-40ml/s
|
||||
#define OUTMIN 0x6A00
|
||||
|
||||
uint16_t last_input;
|
||||
int32_t output_sum;
|
||||
int16_t error;
|
||||
|
||||
int to_signed(int d, int bits) {
|
||||
int d_signed = d;
|
||||
if (d >= (1 << MAX((bits - 1), 0))) {
|
||||
d_signed = d - (1 << MAX(bits, 0));
|
||||
}
|
||||
return d_signed;
|
||||
}
|
||||
|
||||
void TIM3_IRQ_Handler(void) {
|
||||
|
||||
if(pid_enable & !rel_enable) { //run PID loop
|
||||
q_target_ext_qf = 1;
|
||||
|
||||
int q_target_out = Q_TARGET_DEFAULT; // this value is the zero point.
|
||||
|
||||
error = to_signed(pos_input, 16) - to_signed(output_rod_target, 16); // position error
|
||||
q_target_out += (error * P);
|
||||
|
||||
if (q_target_out > OUTMAX){
|
||||
q_target_out = OUTMAX;
|
||||
}
|
||||
|
||||
if (q_target_out < OUTMIN){
|
||||
q_target_out = OUTMIN;
|
||||
}
|
||||
|
||||
q_target_ext = q_target_out;
|
||||
|
||||
}
|
||||
|
||||
if (rel_enable && !pid_enable) { //relative mode
|
||||
q_target_ext_qf = 1;
|
||||
q_target_ext = rel_input;
|
||||
}
|
||||
|
||||
if ((!rel_enable && !pid_enable) || (rel_enable && pid_enable)){ //both are 0 or both are 1
|
||||
q_target_ext_qf = 0;
|
||||
q_target_ext = Q_TARGET_DEFAULT;
|
||||
rel_enable = 0;
|
||||
pid_enable = 0;
|
||||
state = FAULT_INVALID;
|
||||
}
|
||||
|
||||
if (brake_applied) { // handle relay
|
||||
set_gpio_output(GPIOB, 13, 1);
|
||||
} else {
|
||||
set_gpio_output(GPIOB, 13, 0);
|
||||
}
|
||||
|
||||
if (driver_brake_applied){ // reset values
|
||||
q_target_ext_qf = 0;
|
||||
q_target_ext = Q_TARGET_DEFAULT;
|
||||
}
|
||||
|
||||
// cmain loop for sending 100hz messages
|
||||
if ((CAN2->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
uint8_t dat[8]; //sendESP_private3
|
||||
uint16_t pTargetDriver = P_TARGET_DRIVER * 4;
|
||||
dat[2] = pTargetDriver & 0xFFU;
|
||||
dat[3] = (pTargetDriver & 0x3U) >> 8;
|
||||
dat[4] = 0x0;
|
||||
dat[5] = 0x0;
|
||||
dat[6] = (uint8_t) P_MC_QF << 5;
|
||||
dat[7] = 0x0;
|
||||
dat[1] = can2_count_out_1;
|
||||
dat[0] = lut_checksum(dat, 8, crc8_lut_1d);
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_send.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
|
||||
to_send.RDTR = 8;
|
||||
to_send.RIR = (0x38D << 21) | 1U;
|
||||
can_send(&to_send, 1, false);
|
||||
|
||||
}
|
||||
else {
|
||||
// old can packet hasn't sent!
|
||||
state = EXTFAULT1_SEND1;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN2 MISS1\n");
|
||||
#endif
|
||||
}
|
||||
if ((CAN2->TSR & CAN_TSR_TME1) == CAN_TSR_TME1) {
|
||||
uint8_t dat[8]; //sendESP_private2
|
||||
uint16_t p_limit_external = P_LIMIT_EXTERNAL * 2;
|
||||
|
||||
dat[1] = can2_count_out_1 & COUNTER_CYCLE;
|
||||
dat[2] = p_limit_external & 0xFF;
|
||||
dat[3] = ((p_limit_external >> 8U) & 0x1U) | (q_target_ext & 0xFU) << 4U;
|
||||
dat[4] = (q_target_ext >> 4U) & 0xFF;
|
||||
dat[5] = ((q_target_ext >> 12U) & 0xFU) | (q_target_ext_qf << 4U); // what is ESP_diagnosticESP?
|
||||
dat[6] = 0x00;
|
||||
dat[7] = 0x00;
|
||||
dat[0] = lut_checksum(dat, 8, crc8_lut_1d);
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_send.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
|
||||
to_send.RDTR = 8;
|
||||
to_send.RIR = (0x38C << 21) | 1U;
|
||||
can_send(&to_send, 1, false);;
|
||||
|
||||
can2_count_out_1++;
|
||||
can2_count_out_1 &= COUNTER_CYCLE;
|
||||
|
||||
}
|
||||
else {
|
||||
// old can packet hasn't sent!
|
||||
state = EXTFAULT1_SEND2;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN2 MISS2\n");
|
||||
#endif
|
||||
}
|
||||
if (!sent){
|
||||
if ((CAN2->TSR & CAN_TSR_TME2) == CAN_TSR_TME2) {
|
||||
uint8_t dat[8]; //sendESP_private1 every 20ms
|
||||
uint16_t ESP_vehicleSpeed = (((current_speed*16) / 9) & 0x3FFF);
|
||||
|
||||
dat[1] = can2_count_out_2 & COUNTER_CYCLE;
|
||||
dat[2] = P_EST_MAX;
|
||||
dat[3] = P_EST_MAX_QF | (ESP_vehicleSpeed & 0x3FU);
|
||||
dat[4] = (ESP_vehicleSpeed >> 6U) & 0xFF;
|
||||
dat[5] = VEHICLE_QF | (IGNITION_ON << 3U);
|
||||
dat[6] = 0x00;
|
||||
dat[7] = 0x00;
|
||||
dat[0] = lut_checksum(dat, 8, crc8_lut_1d);
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_send.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
|
||||
to_send.RDTR = 8;
|
||||
to_send.RIR = (0x38B << 21) | 1U;
|
||||
can_send(&to_send, 1, false);
|
||||
|
||||
can2_count_out_2++;
|
||||
can2_count_out_2 &= COUNTER_CYCLE;
|
||||
|
||||
}
|
||||
else {
|
||||
// old can packet hasn't sent!
|
||||
state = EXTFAULT1_SEND3;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN2 MISS3\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
sent = !sent;
|
||||
|
||||
|
||||
//send to EON
|
||||
if ((CAN1->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
uint8_t dat[5];
|
||||
brake_ok = (ibst_status == 0x7);
|
||||
|
||||
dat[4] = (can2state & 0xFU) << 4;
|
||||
dat[3] = (output_rod_target >> 8U) & 0x3FU;
|
||||
dat[2] = (brake_ok) | (driver_brake_applied << 1U) | (brake_applied << 2U) | (output_rod_target & 0x3FU);
|
||||
dat[1] = ((state & 0xFU) << 4) | can1_count_out;
|
||||
dat[0] = lut_checksum(dat, 5, crc8_lut_1d);
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_send.RDHR = dat[4];
|
||||
to_send.RDTR = 5;
|
||||
to_send.RIR = (0x20F << 21) | 1U;
|
||||
can_send(&to_send, 0, false);
|
||||
|
||||
can1_count_out++;
|
||||
can1_count_out &= COUNTER_CYCLE;
|
||||
|
||||
}
|
||||
else {
|
||||
// old can packet hasn't sent!
|
||||
state = FAULT_SEND;
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN1 MISS1\n");
|
||||
#endif
|
||||
}
|
||||
// blink the LED
|
||||
|
||||
TIM3->SR = 0;
|
||||
|
||||
// up timeout for gas set
|
||||
if (timeout == MAX_TIMEOUT) {
|
||||
state = FAULT_TIMEOUT;
|
||||
q_target_ext_qf = 0;
|
||||
q_target_ext = Q_TARGET_DEFAULT;
|
||||
pid_enable = 0;
|
||||
rel_enable = 0;
|
||||
} else {
|
||||
timeout += 1U;
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
puts("MODE: ");
|
||||
puth((pid_enable << 1U) | rel_enable);
|
||||
puts(" BRAKE_REQ: ");
|
||||
if (((pid_enable << 1U) | rel_enable) == 2){
|
||||
puth(pos_input);
|
||||
puts(" BRAKE_POS_ERR: ");
|
||||
puth(error);
|
||||
} else {
|
||||
puth(q_target_ext_qf);
|
||||
}
|
||||
puts(" BRAKE_POS: ");
|
||||
puth(output_rod_target);
|
||||
puts(" Q_TARGET_EXT: ");
|
||||
puth(q_target_ext);
|
||||
puts("\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
// ***************************** main code *****************************
|
||||
|
||||
|
||||
void ibst(void) {
|
||||
// read/write
|
||||
watchdog_feed();
|
||||
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
// Init interrupt table
|
||||
init_interrupts(true);
|
||||
|
||||
REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN2_TX_IRQn, CAN2_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
REGISTER_INTERRUPT(CAN2_RX0_IRQn, CAN2_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
REGISTER_INTERRUPT(CAN2_SCE_IRQn, CAN2_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
REGISTER_INTERRUPT(CAN3_TX_IRQn, CAN3_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
REGISTER_INTERRUPT(CAN3_RX0_IRQn, CAN3_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
REGISTER_INTERRUPT(CAN3_SCE_IRQn, CAN3_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
|
||||
// Should run at around 732Hz (see init below)
|
||||
REGISTER_INTERRUPT(TIM3_IRQn, TIM3_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TIM3)
|
||||
|
||||
disable_interrupts();
|
||||
|
||||
// init devices
|
||||
clock_init();
|
||||
peripherals_init();
|
||||
detect_configuration();
|
||||
detect_board_type();
|
||||
|
||||
// init board
|
||||
current_board->init();
|
||||
// enable USB
|
||||
#ifdef IBST_USB
|
||||
USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
|
||||
USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN;
|
||||
usb_init();
|
||||
#endif
|
||||
|
||||
// init can
|
||||
bool llcan_speed_set = llcan_set_speed(CAN1, 5000, false, false);
|
||||
if (!llcan_speed_set) {
|
||||
puts("Failed to set llcan1 speed");
|
||||
}
|
||||
llcan_speed_set = llcan_set_speed(CAN2, 5000, false, false);
|
||||
if (!llcan_speed_set) {
|
||||
puts("Failed to set llcan2 speed");
|
||||
}
|
||||
llcan_speed_set = llcan_set_speed(CAN3, 5000, false, false);
|
||||
if (!llcan_speed_set) {
|
||||
puts("Failed to set llcan3 speed");
|
||||
}
|
||||
|
||||
bool ret = llcan_init(CAN1);
|
||||
UNUSED(ret);
|
||||
ret = llcan_init(CAN2);
|
||||
UNUSED(ret);
|
||||
ret = llcan_init(CAN3);
|
||||
UNUSED(ret);
|
||||
|
||||
gen_crc_lookup_table(crc_poly, crc8_lut_1d);
|
||||
|
||||
// 48mhz / 65536 ~= 732
|
||||
timer_init(TIM3, 7);
|
||||
NVIC_EnableIRQ(TIM3_IRQn);
|
||||
|
||||
// power on ibooster. needs to power on AFTER sending CAN to prevent ibst state from being 4
|
||||
set_gpio_mode(GPIOB, 12, MODE_OUTPUT);
|
||||
set_gpio_output_type(GPIOB, 12, OUTPUT_TYPE_PUSH_PULL);
|
||||
set_gpio_output(GPIOB, 12, 1);
|
||||
|
||||
//Brake switch relay
|
||||
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
|
||||
set_gpio_output_type(GPIOB, 13, OUTPUT_TYPE_PUSH_PULL);
|
||||
|
||||
watchdog_init();
|
||||
|
||||
puts("**** INTERRUPTS ON ****\n");
|
||||
enable_interrupts();
|
||||
|
||||
// main pedal loop
|
||||
while (1) {
|
||||
ibst();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
|
@ -0,0 +1,17 @@
|
|||
// ******************** Prototypes ********************
|
||||
void puts(const char *a);
|
||||
void puth(unsigned int i);
|
||||
void puth2(unsigned int i);
|
||||
typedef struct board board;
|
||||
typedef struct harness_configuration harness_configuration;
|
||||
void can_flip_buses(uint8_t bus1, uint8_t bus2);
|
||||
void can_set_obd(uint8_t harness_orientation, bool obd);
|
||||
|
||||
// ********************* Globals **********************
|
||||
uint8_t hw_type = 0;
|
||||
const board *current_board;
|
||||
bool is_enumerated = 0;
|
||||
uint32_t heartbeat_counter = 0;
|
||||
uint32_t uptime_cnt = 0;
|
||||
bool siren_enabled = false;
|
||||
bool green_led_enabled = false;
|
|
@ -0,0 +1,11 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
DFU_UTIL="dfu-util"
|
||||
|
||||
cd ..
|
||||
IBST=1 scons -u
|
||||
cd ibst
|
||||
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.panda.bin
|
|
@ -0,0 +1 @@
|
|||
obj/*
|
|
@ -0,0 +1,370 @@
|
|||
// cut down version of the can.h driver. allows pedal-like devices to work like Pandas
|
||||
|
||||
// IRQs: CAN1_TX, CAN1_RX0, CAN1_SCE
|
||||
// CAN2_TX, CAN2_RX0, CAN2_SCE
|
||||
// CAN3_TX, CAN3_RX0, CAN3_SCE
|
||||
|
||||
typedef struct {
|
||||
volatile uint32_t w_ptr;
|
||||
volatile uint32_t r_ptr;
|
||||
uint32_t fifo_size;
|
||||
CAN_FIFOMailBox_TypeDef *elems;
|
||||
} can_ring;
|
||||
|
||||
#define CAN_BUS_RET_FLAG 0x80U
|
||||
#define CAN_BUS_NUM_MASK 0x7FU
|
||||
|
||||
#define BUS_MAX 4U
|
||||
|
||||
uint32_t can_rx_errs = 0;
|
||||
uint32_t can_send_errs = 0;
|
||||
uint32_t can_fwd_errs = 0;
|
||||
extern int can_live, pending_can_live;
|
||||
|
||||
// must reinit after changing these
|
||||
extern int can_loopback, can_silent;
|
||||
extern uint32_t can_speed[4];
|
||||
|
||||
void can_set_forwarding(int from, int to);
|
||||
|
||||
bool can_init(uint8_t can_number);
|
||||
void can_init_all(void);
|
||||
bool can_tx_check_min_slots_free(uint32_t min);
|
||||
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook);
|
||||
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
|
||||
|
||||
// Ignition detected from CAN meessages
|
||||
bool ignition_can = false;
|
||||
bool ignition_cadillac = false;
|
||||
uint32_t ignition_can_cnt = 0U;
|
||||
|
||||
// end API
|
||||
|
||||
#define ALL_CAN_SILENT 0xFF
|
||||
#define ALL_CAN_LIVE 0
|
||||
|
||||
int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT;
|
||||
|
||||
// ********************* instantiate queues *********************
|
||||
|
||||
#define can_buffer(x, size) \
|
||||
CAN_FIFOMailBox_TypeDef elems_##x[size]; \
|
||||
can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = size, .elems = (CAN_FIFOMailBox_TypeDef *)&elems_##x };
|
||||
|
||||
can_buffer(rx_q, 0x1000)
|
||||
can_buffer(tx1_q, 0x100)
|
||||
can_buffer(tx2_q, 0x100)
|
||||
can_buffer(tx3_q, 0x100)
|
||||
can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q};
|
||||
|
||||
// global CAN stats
|
||||
int can_rx_cnt = 0;
|
||||
int can_tx_cnt = 0;
|
||||
int can_txd_cnt = 0;
|
||||
int can_err_cnt = 0;
|
||||
int can_overflow_cnt = 0;
|
||||
|
||||
// ********************* interrupt safe queue *********************
|
||||
|
||||
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
|
||||
bool ret = 0;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if (q->w_ptr != q->r_ptr) {
|
||||
*elem = q->elems[q->r_ptr];
|
||||
if ((q->r_ptr + 1U) == q->fifo_size) {
|
||||
q->r_ptr = 0;
|
||||
} else {
|
||||
q->r_ptr += 1U;
|
||||
}
|
||||
ret = 1;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
|
||||
bool ret = false;
|
||||
uint32_t next_w_ptr;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if ((q->w_ptr + 1U) == q->fifo_size) {
|
||||
next_w_ptr = 0;
|
||||
} else {
|
||||
next_w_ptr = q->w_ptr + 1U;
|
||||
}
|
||||
if (next_w_ptr != q->r_ptr) {
|
||||
q->elems[q->w_ptr] = *elem;
|
||||
q->w_ptr = next_w_ptr;
|
||||
ret = true;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
if (!ret) {
|
||||
can_overflow_cnt++;
|
||||
#ifdef DEBUG
|
||||
puts("can_push failed!\n");
|
||||
#endif
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint32_t can_slots_empty(can_ring *q) {
|
||||
uint32_t ret = 0;
|
||||
|
||||
ENTER_CRITICAL();
|
||||
if (q->w_ptr >= q->r_ptr) {
|
||||
ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr;
|
||||
} else {
|
||||
ret = q->r_ptr - q->w_ptr - 1U;
|
||||
}
|
||||
EXIT_CRITICAL();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void can_clear(can_ring *q) {
|
||||
ENTER_CRITICAL();
|
||||
q->w_ptr = 0;
|
||||
q->r_ptr = 0;
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
|
||||
// assign CAN numbering
|
||||
// bus num: Can bus number on ODB connector. Sent to/from USB
|
||||
// Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1)
|
||||
// cans: Look up MCU can interface from bus number
|
||||
// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc);
|
||||
// bus_lookup: Translates from 'can number' to 'bus number'.
|
||||
// can_num_lookup: Translates from 'bus number' to 'can number'.
|
||||
// can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward.
|
||||
|
||||
// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3
|
||||
CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3};
|
||||
uint8_t bus_lookup[] = {0,1,2};
|
||||
uint8_t can_num_lookup[] = {0,1,2,-1};
|
||||
int8_t can_forwarding[] = {-1,-1,-1,-1};
|
||||
uint32_t can_speed[] = {5000, 5000, 5000, 333};
|
||||
#define CAN_MAX 3U
|
||||
|
||||
#define CANIF_FROM_CAN_NUM(num) (cans[num])
|
||||
#define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2))
|
||||
#define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num])
|
||||
#define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num])
|
||||
|
||||
void process_can(uint8_t can_number);
|
||||
|
||||
bool can_set_speed(uint8_t can_number) {
|
||||
bool ret = true;
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
|
||||
ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number));
|
||||
return ret;
|
||||
}
|
||||
|
||||
void can_init_all(void) {
|
||||
bool ret = true;
|
||||
for (uint8_t i=0U; i < CAN_MAX; i++) {
|
||||
can_clear(can_queues[i]);
|
||||
ret &= can_init(i);
|
||||
}
|
||||
UNUSED(ret);
|
||||
}
|
||||
|
||||
void can_flip_buses(uint8_t bus1, uint8_t bus2){
|
||||
bus_lookup[bus1] = bus2;
|
||||
bus_lookup[bus2] = bus1;
|
||||
can_num_lookup[bus1] = bus2;
|
||||
can_num_lookup[bus2] = bus1;
|
||||
}
|
||||
|
||||
|
||||
// CAN error
|
||||
void can_sce(CAN_TypeDef *CAN) {
|
||||
ENTER_CRITICAL();
|
||||
|
||||
#ifdef DEBUG
|
||||
if (CAN==CAN1) puts("CAN1: ");
|
||||
if (CAN==CAN2) puts("CAN2: ");
|
||||
#ifdef CAN3
|
||||
if (CAN==CAN3) puts("CAN3: ");
|
||||
#endif
|
||||
puts("MSR:");
|
||||
puth(CAN->MSR);
|
||||
puts(" TSR:");
|
||||
puth(CAN->TSR);
|
||||
puts(" RF0R:");
|
||||
puth(CAN->RF0R);
|
||||
puts(" RF1R:");
|
||||
puth(CAN->RF1R);
|
||||
puts(" ESR:");
|
||||
puth(CAN->ESR);
|
||||
puts("\n");
|
||||
#endif
|
||||
|
||||
can_err_cnt += 1;
|
||||
llcan_clear_send(CAN);
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
|
||||
// ***************************** CAN *****************************
|
||||
|
||||
void process_can(uint8_t can_number) {
|
||||
if (can_number != 0xffU) {
|
||||
|
||||
ENTER_CRITICAL();
|
||||
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
|
||||
// check for empty mailbox
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
// add successfully transmitted message to my fifo
|
||||
if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
|
||||
can_txd_cnt += 1;
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
|
||||
CAN_FIFOMailBox_TypeDef to_push;
|
||||
to_push.RIR = CAN->sTxMailBox[0].TIR;
|
||||
to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
|
||||
to_push.RDLR = CAN->sTxMailBox[0].TDLR;
|
||||
to_push.RDHR = CAN->sTxMailBox[0].TDHR;
|
||||
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
|
||||
}
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
|
||||
#ifdef DEBUG
|
||||
puts("CAN TX ERROR!\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
|
||||
#ifdef DEBUG
|
||||
puts("CAN TX ARBITRATION LOST!\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
// clear interrupt
|
||||
// careful, this can also be cleared by requesting a transmission
|
||||
CAN->TSR |= CAN_TSR_RQCP0;
|
||||
}
|
||||
|
||||
if (can_pop(can_queues[bus_number], &to_send)) {
|
||||
can_tx_cnt += 1;
|
||||
// only send if we have received a packet
|
||||
CAN->sTxMailBox[0].TDLR = to_send.RDLR;
|
||||
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
|
||||
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
|
||||
CAN->sTxMailBox[0].TIR = to_send.RIR;
|
||||
|
||||
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
|
||||
usb_outep3_resume_if_paused();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
EXIT_CRITICAL();
|
||||
}
|
||||
}
|
||||
|
||||
void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int bus = GET_BUS(to_push);
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
|
||||
ignition_can_cnt = 0U; // reset counter
|
||||
|
||||
if (bus == 0) {
|
||||
// TODO: verify on all supported GM models that we can reliably detect ignition using only this signal,
|
||||
// since the 0x1F1 signal can briefly go low immediately after ignition
|
||||
if ((addr == 0x160) && (len == 5)) {
|
||||
// this message isn't all zeros when ignition is on
|
||||
ignition_cadillac = GET_BYTES_04(to_push) != 0;
|
||||
}
|
||||
// GM exception
|
||||
if ((addr == 0x1F1) && (len == 8)) {
|
||||
// Bit 5 is ignition "on"
|
||||
bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20) != 0);
|
||||
ignition_can = ignition_gm || ignition_cadillac;
|
||||
}
|
||||
// Tesla exception
|
||||
if ((addr == 0x348) && (len == 8)) {
|
||||
// GTW_status
|
||||
ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// CAN receive handlers
|
||||
// blink blue when we are receiving CAN messages
|
||||
void can_rx(uint8_t can_number) {
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
|
||||
while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
can_rx_cnt += 1;
|
||||
|
||||
// can is live
|
||||
pending_can_live = 1;
|
||||
|
||||
// add to my fifo
|
||||
CAN_FIFOMailBox_TypeDef to_push;
|
||||
to_push.RIR = CAN->sFIFOMailBox[0].RIR;
|
||||
to_push.RDTR = CAN->sFIFOMailBox[0].RDTR;
|
||||
to_push.RDLR = CAN->sFIFOMailBox[0].RDLR;
|
||||
to_push.RDHR = CAN->sFIFOMailBox[0].RDHR;
|
||||
|
||||
// modify RDTR for our API
|
||||
to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4);
|
||||
can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
|
||||
|
||||
// next
|
||||
CAN->RF0R |= CAN_RF0R_RFOM0;
|
||||
}
|
||||
}
|
||||
|
||||
bool can_tx_check_min_slots_free(uint32_t min) {
|
||||
return
|
||||
(can_slots_empty(&can_tx1_q) >= min) &&
|
||||
(can_slots_empty(&can_tx2_q) >= min) &&
|
||||
(can_slots_empty(&can_tx3_q) >= min);
|
||||
}
|
||||
|
||||
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
|
||||
if (!skip_tx_hook) {
|
||||
#ifdef DEBUG
|
||||
puts("TX CAN");
|
||||
puth2(bus_number);
|
||||
puts(" ");
|
||||
puth(to_push->RIR >> 21);
|
||||
puts("\n");
|
||||
#endif
|
||||
if (bus_number < BUS_MAX) {
|
||||
// add CAN packet to send queue
|
||||
// bus number isn't passed through
|
||||
to_push->RDTR &= 0xF;
|
||||
can_fwd_errs += can_push(can_queues[bus_number], to_push) ? 0U : 1U;
|
||||
process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void can_set_forwarding(int from, int to) {
|
||||
can_forwarding[from] = to;
|
||||
}
|
||||
|
||||
bool can_init(uint8_t can_number) {
|
||||
bool ret = false;
|
||||
|
||||
if (can_number != 0xffU) {
|
||||
CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
|
||||
ret &= can_set_speed(can_number);
|
||||
ret &= llcan_init(CAN);
|
||||
// in case there are queued up messages
|
||||
process_can(can_number);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
cd ..
|
||||
SDSU=1 scons -u
|
||||
cd smart_dsu
|
||||
|
||||
../../tests/gateway/enter_canloader.py ../obj/panda.bin.signed
|
|
@ -0,0 +1,633 @@
|
|||
// ********************* Includes *********************
|
||||
#include "../config.h"
|
||||
#include "libc.h"
|
||||
|
||||
#include "main_declarations.h"
|
||||
#include "critical.h"
|
||||
#include "faults.h"
|
||||
|
||||
#include "drivers/registers.h"
|
||||
#include "drivers/interrupts.h"
|
||||
#include "drivers/llcan.h"
|
||||
#include "drivers/llgpio.h"
|
||||
#include "drivers/adc.h"
|
||||
|
||||
#include "board.h"
|
||||
|
||||
#include "drivers/clock.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#include "gpio.h"
|
||||
#include "crc.h"
|
||||
|
||||
// uncomment for usb debugging via debug_console.py
|
||||
#define TGW_USB
|
||||
// #define DEBUG_CAN
|
||||
#define DEBUG_CTRL
|
||||
|
||||
#ifdef TGW_USB
|
||||
#include "drivers/uart.h"
|
||||
#include "drivers/usb.h"
|
||||
#else
|
||||
// no serial either
|
||||
void puts(const char *a) {
|
||||
UNUSED(a);
|
||||
}
|
||||
void puth(unsigned int i) {
|
||||
UNUSED(i);
|
||||
}
|
||||
void puth2(unsigned int i) {
|
||||
UNUSED(i);
|
||||
}
|
||||
#endif
|
||||
|
||||
#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
|
||||
uint32_t enter_bootloader_mode;
|
||||
|
||||
// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
|
||||
void __initialize_hardware_early(void) {
|
||||
early();
|
||||
}
|
||||
|
||||
#ifdef TGW_USB
|
||||
|
||||
#include "gateway/can.h"
|
||||
|
||||
// ********************* usb debugging *********************
|
||||
// TODO: neuter this if we are not debugging
|
||||
void debug_ring_callback(uart_ring *ring) {
|
||||
char rcv;
|
||||
while (getc(ring, &rcv) != 0) {
|
||||
(void)putc(ring, rcv);
|
||||
}
|
||||
}
|
||||
|
||||
int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;
|
||||
int ilen = 0;
|
||||
while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) {
|
||||
ilen++;
|
||||
}
|
||||
return ilen*0x10;
|
||||
}
|
||||
// send on serial, first byte to select the ring
|
||||
void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
uint8_t *usbdata8 = (uint8_t *)usbdata;
|
||||
uart_ring *ur = get_ring_by_number(usbdata8[0]);
|
||||
if ((len != 0) && (ur != NULL)) {
|
||||
if ((usbdata8[0] < 2U)) {
|
||||
for (int i = 1; i < len; i++) {
|
||||
while (!putc(ur, usbdata8[i])) {
|
||||
// wait
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// send on CAN
|
||||
void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
|
||||
UNUSED(usbdata);
|
||||
UNUSED(len);
|
||||
UNUSED(hardwired);
|
||||
}
|
||||
void usb_cb_ep3_out_complete() {
|
||||
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
|
||||
usb_outep3_resume_if_paused();
|
||||
}
|
||||
}
|
||||
|
||||
void usb_cb_enumeration_complete() {
|
||||
puts("USB enumeration complete\n");
|
||||
is_enumerated = 1;
|
||||
}
|
||||
|
||||
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {
|
||||
UNUSED(hardwired);
|
||||
unsigned int resp_len = 0;
|
||||
uart_ring *ur = NULL;
|
||||
switch (setup->b.bRequest) {
|
||||
// **** 0xd1: enter bootloader mode
|
||||
case 0xd1:
|
||||
// this allows reflashing of the bootstub
|
||||
// so it's blocked over wifi
|
||||
switch (setup->b.wValue.w) {
|
||||
case 0:
|
||||
// only allow bootloader entry on debug builds
|
||||
#ifdef ALLOW_DEBUG
|
||||
if (hardwired) {
|
||||
puts("-> entering bootloader\n");
|
||||
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case 1:
|
||||
puts("-> entering softloader\n");
|
||||
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
default:
|
||||
puts("Bootloader mode invalid\n");
|
||||
break;
|
||||
}
|
||||
break;
|
||||
// **** 0xd8: reset ST
|
||||
case 0xd8:
|
||||
NVIC_SystemReset();
|
||||
break;
|
||||
// **** 0xe0: uart read
|
||||
case 0xe0:
|
||||
ur = get_ring_by_number(setup->b.wValue.w);
|
||||
if (!ur) {
|
||||
break;
|
||||
}
|
||||
// read
|
||||
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
||||
getc(ur, (char*)&resp[resp_len])) {
|
||||
++resp_len;
|
||||
}
|
||||
break;
|
||||
// **** 0xf1: Clear CAN ring buffer.
|
||||
case 0xf1:
|
||||
if (setup->b.wValue.w == 0xFFFFU) {
|
||||
puts("Clearing CAN Rx queue\n");
|
||||
can_clear(&can_rx_q);
|
||||
} else if (setup->b.wValue.w < BUS_MAX) {
|
||||
puts("Clearing CAN Tx queue\n");
|
||||
can_clear(can_queues[setup->b.wValue.w]);
|
||||
} else {
|
||||
puts("Clearing CAN CAN ring buffer failed: wrong bus number\n");
|
||||
}
|
||||
break;
|
||||
// **** 0xf2: Clear UART ring buffer.
|
||||
case 0xf2:
|
||||
{
|
||||
uart_ring * rb = get_ring_by_number(setup->b.wValue.w);
|
||||
if (rb != NULL) {
|
||||
puts("Clearing UART queue.\n");
|
||||
clear_uart_buff(rb);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
puts("NO HANDLER ");
|
||||
puth(setup->b.bRequest);
|
||||
puts("\n");
|
||||
break;
|
||||
}
|
||||
return resp_len;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// ***************************** can port *****************************
|
||||
|
||||
// Toyota Checksum algorithm
|
||||
uint8_t toyota_checksum(int addr, uint8_t *dat, int len){
|
||||
int cksum = 0;
|
||||
for(int ii = 0; ii < (len - 1); ii++){
|
||||
cksum = (cksum + dat[ii]);
|
||||
}
|
||||
cksum += len;
|
||||
cksum += ((addr >> 8U) & 0xFF); // idh
|
||||
cksum += ((addr) & 0xFF); // idl
|
||||
return cksum & 0xFF;
|
||||
}
|
||||
|
||||
#define CAN_UPDATE 0xF0 //bootloader
|
||||
#define COUNTER_CYCLE 0xFU
|
||||
uint8_t counter = 0;
|
||||
|
||||
void CAN1_TX_IRQ_Handler(void) {
|
||||
process_can(0);
|
||||
}
|
||||
|
||||
void CAN2_TX_IRQ_Handler(void) {
|
||||
process_can(1);
|
||||
}
|
||||
|
||||
void CAN3_TX_IRQ_Handler(void) {
|
||||
process_can(2);
|
||||
}
|
||||
|
||||
#define MAX_TIMEOUT 50U
|
||||
uint32_t timeout = 0;
|
||||
uint32_t timeout_f10 = 0;
|
||||
uint32_t timeout_f11 = 0;
|
||||
|
||||
#define NO_FAULT 0U
|
||||
#define FAULT_BAD_CHECKSUM 1U
|
||||
#define FAULT_SEND 2U
|
||||
#define FAULT_SCE 3U
|
||||
#define FAULT_STARTUP 4U
|
||||
#define FAULT_TIMEOUT 5U
|
||||
#define FAULT_INVALID 6U
|
||||
#define STATE_AEB_CTRL 7U
|
||||
|
||||
uint8_t state = FAULT_STARTUP;
|
||||
uint8_t ctrl_mode = 0;
|
||||
bool send = 0;
|
||||
|
||||
//------------- BUS 1 - PTCAN ------------//
|
||||
|
||||
#define ACC_CTRL 0xF10
|
||||
bool enable_acc = 0;
|
||||
int acc_cmd = 0;
|
||||
|
||||
#define AEB_CTRL 0xF11
|
||||
bool enable_aeb_control = 0;
|
||||
int aeb_cmd = 0;
|
||||
|
||||
//------------- BUS 2 - DSU -------------//
|
||||
|
||||
#define DSU_ACC_CONTROL 0x343
|
||||
bool acc_cancel = 0;
|
||||
|
||||
#define DSU_AEB_CMD 0x344
|
||||
bool stock_aeb_active = 0;
|
||||
|
||||
void CAN1_RX0_IRQ_Handler(void) {
|
||||
while ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_fwd;
|
||||
to_fwd.RIR = CAN1->sFIFOMailBox[0].RIR | 1; // TXQ
|
||||
to_fwd.RDTR = CAN1->sFIFOMailBox[0].RDTR;
|
||||
to_fwd.RDLR = CAN1->sFIFOMailBox[0].RDLR;
|
||||
to_fwd.RDHR = CAN1->sFIFOMailBox[0].RDHR;
|
||||
|
||||
uint16_t address = CAN1->sFIFOMailBox[0].RIR >> 21;
|
||||
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN1 RX: ");
|
||||
puth(address);
|
||||
puts("\n");
|
||||
#endif
|
||||
|
||||
// CAN data buffer
|
||||
uint8_t dat[8];
|
||||
|
||||
switch (address) {
|
||||
case CAN_UPDATE:
|
||||
if (GET_BYTES_04(&CAN1->sFIFOMailBox[0]) == 0xdeadface) {
|
||||
if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x0ab00b1e) {
|
||||
enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
} else if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x02b00b1e) {
|
||||
enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
|
||||
NVIC_SystemReset();
|
||||
} else {
|
||||
puts("Failed entering Softloader or Bootloader\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
case ACC_CTRL:
|
||||
// send this EXACTLY how ACC_CONTROL is sent
|
||||
for (int i=0; i<8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i);
|
||||
}
|
||||
if (dat[8] == toyota_checksum(address, dat, 8)){
|
||||
enable_acc = 1; // TODO: set this somewhere else.. 1D2? do we need this?
|
||||
acc_cmd = (dat[0] << 8U) | dat[1]; // ACC_CMD
|
||||
acc_cancel = (dat[3] & 1U);
|
||||
// reset the timer
|
||||
timeout_f10 = 0;
|
||||
ctrl_mode |= 1; // set ACC_CTRL mode bit
|
||||
} else {
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
enable_acc = 0;
|
||||
acc_cmd = 0;
|
||||
}
|
||||
to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
|
||||
break;
|
||||
case AEB_CTRL:
|
||||
// send this EXACTLY how PRE_COLLISION2 is sent
|
||||
for (int i=0; i<8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i);
|
||||
}
|
||||
if (dat[8] == toyota_checksum(address, dat, 8)){
|
||||
// an emergency maneuver is being requested
|
||||
enable_aeb_control = 1;
|
||||
aeb_cmd = (dat[0] << 2U) | (dat[1] & 3U);
|
||||
// reset the timer
|
||||
timeout_f11 = 0;
|
||||
ctrl_mode |= (1 << 1U); // set AEB_CTRL mode bit
|
||||
state = STATE_AEB_CTRL;
|
||||
} else {
|
||||
enable_aeb_control = 0;
|
||||
aeb_cmd = 0;
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
}
|
||||
to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
|
||||
break;
|
||||
default:
|
||||
// FWD as-is
|
||||
break;
|
||||
}
|
||||
// send to CAN3
|
||||
can_send(&to_fwd, 2, false);
|
||||
// next
|
||||
can_rx(0);
|
||||
// CAN1->RF0R |= CAN_RF0R_RFOM0;
|
||||
}
|
||||
}
|
||||
|
||||
void CAN1_SCE_IRQ_Handler(void) {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN1);
|
||||
llcan_clear_send(CAN1);
|
||||
}
|
||||
|
||||
// CAN2 not used for now.. chip shortage means don't populate on the board
|
||||
|
||||
// void CAN2_RX0_IRQ_Handler(void) {
|
||||
// while ((CAN3->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
// uint16_t address = CAN3->sFIFOMailBox[0].RIR >> 21;
|
||||
// #ifdef DEBUG_CAN
|
||||
// puts("CAN3 RX: ");
|
||||
// puth(address);
|
||||
// puts("\n");
|
||||
// #else
|
||||
// UNUSED(address);
|
||||
// #endif
|
||||
|
||||
// // next
|
||||
// can_rx(1);
|
||||
// }
|
||||
// }
|
||||
|
||||
// void CAN2_SCE_IRQ_Handler(void) {
|
||||
// state = FAULT_SCE;
|
||||
// can_sce(CAN2);
|
||||
// llcan_clear_send(CAN2);
|
||||
// }
|
||||
|
||||
void CAN3_RX0_IRQ_Handler(void) {
|
||||
// From the DSU
|
||||
while ((CAN3->RF0R & CAN_RF0R_FMP0) != 0) {
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_fwd;
|
||||
to_fwd.RIR = CAN3->sFIFOMailBox[0].RIR | 1; // TXQ
|
||||
to_fwd.RDTR = CAN3->sFIFOMailBox[0].RDTR;
|
||||
to_fwd.RDLR = CAN3->sFIFOMailBox[0].RDLR;
|
||||
to_fwd.RDHR = CAN3->sFIFOMailBox[0].RDHR;
|
||||
|
||||
uint16_t address = CAN3->sFIFOMailBox[0].RIR >> 21;
|
||||
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN2 RX: ");
|
||||
puth(address);
|
||||
puts("\n");
|
||||
#endif
|
||||
|
||||
// CAN data buffer
|
||||
uint8_t dat[8];
|
||||
|
||||
switch (address) {
|
||||
case DSU_ACC_CONTROL: // ACC_CTRL
|
||||
for (int i=0; i<8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i);
|
||||
}
|
||||
if(dat[7] == toyota_checksum(address, dat, 8)) {
|
||||
// add permit_braking and recompute the checksum
|
||||
dat[2] &= 0x3F; // mask off the top 2 bits
|
||||
dat[2] |= (1 << 6U); // SET_ME_X01
|
||||
dat[3] |= (1 << 6U); // permit_braking
|
||||
dat[7] = toyota_checksum(address, dat, 8);
|
||||
if (enable_acc){
|
||||
// modify this before sending to the car only if requested
|
||||
dat[0] = (acc_cmd >> 8U);
|
||||
dat[1] = (acc_cmd & 0xFF);
|
||||
}
|
||||
to_fwd.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_fwd.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
|
||||
// reset the timer for seeing the DSU
|
||||
timeout = 0;
|
||||
state = NO_FAULT;
|
||||
} else {
|
||||
// bad checksum
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
#ifdef DEBUG_CAN
|
||||
for(int ii = 0; ii<8; ii++){
|
||||
puth2(dat[ii]);
|
||||
}
|
||||
puts("\n expected: ");
|
||||
puth2(toyota_checksum(address, dat, 8));
|
||||
puts(" got: ");
|
||||
puth2(dat[7]);
|
||||
puts("\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
case DSU_AEB_CMD: // AEB BRAKING
|
||||
for (int i=0; i<8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i);
|
||||
}
|
||||
uint16_t stock_aeb = ((dat[0] << 8U) | dat[1]) >> 6U;
|
||||
stock_aeb_active = (stock_aeb != 0);
|
||||
//DS1STAT2 bit 10
|
||||
//DS1STBK2 bit 13
|
||||
if(dat[7] == toyota_checksum(address, dat, 8)) {
|
||||
if (enable_aeb_control & !stock_aeb_active){
|
||||
// modify this message before sending to the car only if requested and stock AEB is NOT active
|
||||
dat[0] = (aeb_cmd >> 2U); // 10 bit msg
|
||||
dat[1] = (((aeb_cmd << 8U) & 3U) << 6U) | (2 << 3U) | (2 << 0U);
|
||||
dat[4] |= (1U << 6U); // BRKHLD
|
||||
dat[7] = toyota_checksum(address, dat, 8);
|
||||
}
|
||||
to_fwd.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_fwd.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
|
||||
} else {
|
||||
// bad checksum
|
||||
state = FAULT_BAD_CHECKSUM;
|
||||
#ifdef DEBUG_CAN
|
||||
for(int ii = 0; ii<8; ii++){
|
||||
puth2(dat[ii]);
|
||||
}
|
||||
puts("\n expected: ");
|
||||
puth2(toyota_checksum(address, dat, 8));
|
||||
puts(" got: ");
|
||||
puth2(dat[7]);
|
||||
puts("\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
default:
|
||||
// FWD as-is
|
||||
break;
|
||||
}
|
||||
// send to CAN1
|
||||
can_send(&to_fwd, 0, false);
|
||||
// next
|
||||
can_rx(2);
|
||||
}
|
||||
}
|
||||
|
||||
void CAN3_SCE_IRQ_Handler(void) {
|
||||
state = FAULT_SCE;
|
||||
can_sce(CAN3);
|
||||
llcan_clear_send(CAN3);
|
||||
}
|
||||
|
||||
void TIM3_IRQ_Handler(void) {
|
||||
|
||||
//send to EON. cap this to 50Hz
|
||||
if (send){
|
||||
if ((CAN1->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
|
||||
uint8_t dat[4];
|
||||
dat[0] = 0;
|
||||
dat[1] = ctrl_mode;
|
||||
dat[2] = ((state & 0xFU) << 4) | (counter);
|
||||
dat[3] = toyota_checksum(0x2FF, dat, 4);
|
||||
|
||||
CAN_FIFOMailBox_TypeDef to_send;
|
||||
to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
|
||||
to_send.RDTR = 4;
|
||||
to_send.RIR = (0x2FF << 21) | 1U;
|
||||
can_send(&to_send, 0, false);
|
||||
|
||||
counter += 1;
|
||||
counter &= COUNTER_CYCLE;
|
||||
}
|
||||
else {
|
||||
// old can packet hasn't sent!
|
||||
#ifdef DEBUG_CAN
|
||||
puts("CAN1 MISS1\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
send = !send;
|
||||
|
||||
// up timeouts
|
||||
if (timeout == MAX_TIMEOUT) {
|
||||
state = FAULT_TIMEOUT;
|
||||
} else {
|
||||
timeout += 1U;
|
||||
}
|
||||
if (timeout_f10 == MAX_TIMEOUT){
|
||||
enable_acc = 0;
|
||||
ctrl_mode &= 0xFE; // clear ACC ctrl mode bit
|
||||
} else {
|
||||
timeout_f10 += 1U;
|
||||
}
|
||||
if (timeout_f11 == MAX_TIMEOUT){
|
||||
enable_aeb_control = 0;
|
||||
ctrl_mode &= 0xFD; // clear AEB ctrl mode bit
|
||||
} else {
|
||||
timeout_f11 += 1U;
|
||||
}
|
||||
TIM3->SR = 0;
|
||||
|
||||
#ifdef DEBUG_CTRL
|
||||
puts("enable_acc: ");
|
||||
puth2(enable_acc);
|
||||
puts("\nenable_aeb_control: ");
|
||||
puth2(enable_aeb_control);
|
||||
puts("\nacc_cmd: ");
|
||||
puth(acc_cmd);
|
||||
puts("\naeb_cmd: ");
|
||||
puth(aeb_cmd);
|
||||
puts("\nstate: ");
|
||||
puth2(state);
|
||||
puts(" ctrl_mode: ");
|
||||
puth2(ctrl_mode);
|
||||
puts("\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
// ***************************** main code *****************************
|
||||
|
||||
|
||||
void gw(void) {
|
||||
// read/write
|
||||
// maybe we can implement the ADC and DAC here for pedal functionality?
|
||||
watchdog_feed();
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
// Init interrupt table
|
||||
init_interrupts(true);
|
||||
|
||||
REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
// REGISTER_INTERRUPT(CAN2_TX_IRQn, CAN2_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
// REGISTER_INTERRUPT(CAN2_RX0_IRQn, CAN2_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
// REGISTER_INTERRUPT(CAN2_SCE_IRQn, CAN2_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
|
||||
REGISTER_INTERRUPT(CAN3_TX_IRQn, CAN3_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
REGISTER_INTERRUPT(CAN3_RX0_IRQn, CAN3_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
REGISTER_INTERRUPT(CAN3_SCE_IRQn, CAN3_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
|
||||
|
||||
// Should run at around 732Hz (see init below)
|
||||
REGISTER_INTERRUPT(TIM3_IRQn, TIM3_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TIM3)
|
||||
|
||||
disable_interrupts();
|
||||
|
||||
// init devices
|
||||
clock_init();
|
||||
peripherals_init();
|
||||
detect_configuration();
|
||||
detect_board_type();
|
||||
|
||||
// init microsecond system timer
|
||||
// increments 1000000 times per second
|
||||
// generate an update to set the prescaler
|
||||
TIM2->PSC = 48-1;
|
||||
TIM2->CR1 = TIM_CR1_CEN;
|
||||
TIM2->EGR = TIM_EGR_UG;
|
||||
// use TIM2->CNT to read
|
||||
|
||||
// init board
|
||||
current_board->init();
|
||||
// enable USB
|
||||
#ifdef TGW_USB
|
||||
USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
|
||||
USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN;
|
||||
usb_init();
|
||||
#endif
|
||||
|
||||
// init can
|
||||
bool llcan_speed_set = llcan_set_speed(CAN1, 5000, false, false);
|
||||
if (!llcan_speed_set) {
|
||||
puts("Failed to set llcan1 speed");
|
||||
}
|
||||
// llcan_speed_set = llcan_set_speed(CAN2, 5000, false, false);
|
||||
// if (!llcan_speed_set) {
|
||||
// puts("Failed to set llcan2 speed");
|
||||
// }
|
||||
llcan_speed_set = llcan_set_speed(CAN3, 5000, false, false);
|
||||
if (!llcan_speed_set) {
|
||||
puts("Failed to set llcan3 speed");
|
||||
}
|
||||
|
||||
bool ret = llcan_init(CAN1);
|
||||
// ret = llcan_init(CAN2);
|
||||
ret = llcan_init(CAN3);
|
||||
UNUSED(ret);
|
||||
|
||||
// 48mhz / 65536 ~= 732
|
||||
timer_init(TIM3, 7);
|
||||
NVIC_EnableIRQ(TIM3_IRQn);
|
||||
|
||||
// TODO: figure out a function for these GPIOs
|
||||
// set_gpio_mode(GPIOB, 12, MODE_OUTPUT);
|
||||
// set_gpio_output_type(GPIOB, 12, OUTPUT_TYPE_PUSH_PULL);
|
||||
// set_gpio_output(GPIOB, 12, 1);
|
||||
|
||||
// set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
|
||||
// set_gpio_output_type(GPIOB, 13, OUTPUT_TYPE_PUSH_PULL);
|
||||
|
||||
watchdog_init();
|
||||
|
||||
puts("**** INTERRUPTS ON ****\n");
|
||||
enable_interrupts();
|
||||
|
||||
// main pedal loop
|
||||
while (1) {
|
||||
gw();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
|
@ -0,0 +1,17 @@
|
|||
// ******************** Prototypes ********************
|
||||
void puts(const char *a);
|
||||
void puth(unsigned int i);
|
||||
void puth2(unsigned int i);
|
||||
typedef struct board board;
|
||||
typedef struct harness_configuration harness_configuration;
|
||||
void can_flip_buses(uint8_t bus1, uint8_t bus2);
|
||||
void can_set_obd(uint8_t harness_orientation, bool obd);
|
||||
|
||||
// ********************* Globals **********************
|
||||
uint8_t hw_type = 0;
|
||||
const board *current_board;
|
||||
bool is_enumerated = 0;
|
||||
uint32_t heartbeat_counter = 0;
|
||||
uint32_t uptime_cnt = 0;
|
||||
bool siren_enabled = false;
|
||||
bool green_led_enabled = false;
|
|
@ -0,0 +1,11 @@
|
|||
#!/usr/bin/env sh
|
||||
set -e
|
||||
|
||||
DFU_UTIL="dfu-util"
|
||||
|
||||
cd ..
|
||||
SDSU=1 scons -u
|
||||
cd smart_dsu
|
||||
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed
|
||||
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.panda.bin
|
|
@ -267,12 +267,133 @@ void CAN1_SCE_IRQ_Handler(void) {
|
|||
|
||||
#endif
|
||||
|
||||
#ifdef GATEWAY
|
||||
|
||||
#include "drivers/llcan.h"
|
||||
#define CAN CAN1
|
||||
|
||||
#define CAN_BL_INPUT 0x1
|
||||
#define CAN_BL_OUTPUT 0x2
|
||||
|
||||
void CAN1_TX_IRQ_Handler(void) {
|
||||
// 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_IRQ_Handler(void) {
|
||||
while (CAN->RF0R & CAN_RF0R_FMP0) {
|
||||
if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
|
||||
uint8_t dat[8];
|
||||
for (int i = 0; i < 8; i++) {
|
||||
dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
|
||||
}
|
||||
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_IRQ_Handler(void) {
|
||||
llcan_clear_send(CAN);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
void soft_flasher_start(void) {
|
||||
#ifdef PEDAL
|
||||
REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
#endif
|
||||
#ifdef GATEWAY
|
||||
REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
|
||||
#endif
|
||||
|
||||
puts("\n\n\n************************ FLASHER START ************************\n");
|
||||
|
||||
|
@ -297,6 +418,17 @@ void soft_flasher_start(void) {
|
|||
llcan_init(CAN1);
|
||||
#endif
|
||||
|
||||
#ifdef GATEWAY
|
||||
//init can for bootloading
|
||||
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
|
||||
set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
|
||||
set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
|
||||
current_board->enable_can_transceiver(1, true);
|
||||
// init can
|
||||
llcan_set_speed(CAN1, 5000, false, false);
|
||||
llcan_init(CAN1);
|
||||
#endif
|
||||
|
||||
// A4,A5,A6,A7: setup SPI
|
||||
set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
|
||||
set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
|
||||
|
|
Loading…
Reference in New Issue