653 lines
17 KiB
C
653 lines
17 KiB
C
// ********************* 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 "smart_dsu/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;
|
|
bool op_ctrl_mode = 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 DSU_ACC_CONTROL: // hack to allow the firmware to support stock OP
|
|
to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
|
|
op_ctrl_mode = 1;
|
|
// reset timer for op_ctrl_mode
|
|
TIM2->CNT = 0;
|
|
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[7] == 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[7] == 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
|
|
if (op_ctrl_mode){
|
|
to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
|
|
// op_ctrl_mode = 0;
|
|
// for now, just set this flag until the next reboot
|
|
} else {
|
|
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;
|
|
|
|
// check TIM2 for op_ctrl_mode timer
|
|
uint32_t op_ts = TIM2->CNT;
|
|
if (op_ts > 100000){
|
|
op_ctrl_mode = 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;
|
|
}
|