Merge pull request commaai#92 from commaai/pedal

Pedal

board/drivers/can.h

@@ -372,6 +372,8 @@ void can_rx(uint8_t can_number) {
   }
 }
 
+#ifndef CUSTOM_CAN_INTERRUPTS
+
 void CAN1_TX_IRQHandler() { process_can(0); }
 void CAN1_RX0_IRQHandler() { can_rx(0); }
 void CAN1_SCE_IRQHandler() { can_sce(CAN1); }
@@ -386,6 +388,8 @@ void CAN3_RX0_IRQHandler() { can_rx(2); }
 void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
 #endif
 
+#endif
+
 void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
   if (safety_tx_hook(to_push)) {
     if (bus_number < BUS_MAX) {

board/drivers/drivers.h

@@ -88,7 +88,7 @@ uint32_t adc_get(int channel);
 // ********************* DAC *********************
 
 void dac_init();
-uint32_t dac_set(int channel, uint32_t value);
+void dac_set(int channel, uint32_t value);
 
 
 // ********************* TIMER *********************

board/gpio.h

@@ -72,8 +72,15 @@ void clock_init() {
     RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
                    RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE;
   #else
-    RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
-                   RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE;
+    #ifdef PEDAL
+      // comma pedal has a 16mhz crystal
+      RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
+                     RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE;
+    #else
+      // NEO board has a 8mhz crystal
+      RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
+                     RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE;
+    #endif
   #endif
 
   // start PLL
@@ -132,8 +139,13 @@ void set_can_enable(CAN_TypeDef *CAN, int enabled) {
       // CAN1_EN
       set_gpio_output(GPIOC, 1, !enabled);
     #else
-      // CAN1_EN
-      set_gpio_output(GPIOB, 3, enabled);
+      #ifdef PEDAL
+        // CAN1_EN (not flipped)
+        set_gpio_output(GPIOB, 3, !enabled);
+      #else
+        // CAN1_EN
+        set_gpio_output(GPIOB, 3, enabled);
+      #endif
     #endif
   } else if (CAN == CAN2) {
     #ifdef PANDA
@@ -285,6 +297,14 @@ void gpio_init() {
   set_gpio_mode(GPIOC, 2, MODE_ANALOG);
   set_gpio_mode(GPIOC, 3, MODE_ANALOG);
 
+#ifdef PEDAL
+  // comma pedal has inputs on C0 and C1
+  set_gpio_mode(GPIOC, 0, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 1, MODE_ANALOG);
+  // DAC outputs on A4 and A5
+  //   apparently they don't need GPIO setup
+#endif
+
   // C8: FAN aka TIM3_CH4
   set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
 
@@ -443,9 +463,10 @@ void early() {
 
 
   if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
+  #ifdef PANDA
     set_esp_mode(ESP_DISABLED);
+  #endif
     set_led(LED_GREEN, 1);
-
     jump_to_bootloader();
   }
 

board/pedal/.gitignore

@@ -0,0 +1 @@
+obj/*

board/pedal/Makefile

@@ -0,0 +1,32 @@
+# :set noet
+PROJ_NAME = comma
+
+CFLAGS  = -O2 -Wall -std=gnu11
+CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m3
+CFLAGS += -msoft-float -DSTM32F2 -DSTM32F205xx
+CFLAGS += -I ../inc -I ../ -nostdlib
+CFLAGS += -T../stm32_flash.ld
+
+CC = arm-none-eabi-gcc
+OBJCOPY = arm-none-eabi-objcopy
+OBJDUMP = arm-none-eabi-objdump
+
+all: obj/$(PROJ_NAME).bin
+	#$(OBJDUMP) -d obj/$(PROJ_NAME).elf
+	dfu-util -d 0483:df11 -a 0 -s 0x08000000:leave -D $<
+
+obj/main.o: main.c ../*.h
+	mkdir -p obj
+	$(CC) $(CFLAGS) -o $@ -c $<
+
+obj/startup_stm32f205xx.o: ../startup_stm32f205xx.s
+	mkdir -p obj
+	$(CC) $(CFLAGS) -o $@ -c $<
+
+obj/$(PROJ_NAME).bin: obj/startup_stm32f205xx.o obj/main.o
+	$(CC) $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
+	$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf $@
+
+clean:
+	rm -f obj/*
+

board/pedal/README

@@ -0,0 +1,6 @@
+This is the firmware for the comma pedal. It borrows a lot from panda.
+
+The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "virtually" press the pedal.
+
+This is the open source software. Open source hardware coming soon.
+

board/pedal/main.c

@@ -0,0 +1,252 @@
+//#define DEBUG
+//#define CAN_LOOPBACK_MODE
+//#define USE_INTERNAL_OSC
+
+#define PEDAL
+
+#include "../config.h"
+
+#include "drivers/drivers.h"
+#include "drivers/llgpio.h"
+#include "gpio.h"
+
+#define CUSTOM_CAN_INTERRUPTS
+
+#include "libc.h"
+#include "safety.h"
+#include "drivers/adc.h"
+#include "drivers/uart.h"
+#include "drivers/dac.h"
+#include "drivers/can.h"
+#include "drivers/timer.h"
+
+#define CAN CAN1
+
+//#define PEDAL_USB
+
+#ifdef PEDAL_USB
+  #include "drivers/usb.h"
+#endif
+
+#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
+uint32_t enter_bootloader_mode;
+
+void __initialize_hardware_early() {
+  if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
+    enter_bootloader_mode = 0;
+    void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)0x1fff0004));
+    bootloader();
+
+    // LOOP
+    while(1);
+  }
+}
+
+// ********************* serial debugging *********************
+
+void debug_ring_callback(uart_ring *ring) {
+  char rcv;
+  while (getc(ring, &rcv)) {
+    putc(ring, rcv);
+  }
+}
+
+#ifdef PEDAL_USB
+
+int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) { return 0; }
+void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {}
+void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {}
+void usb_cb_enumeration_complete() {}
+
+int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
+  int resp_len = 0;
+  uart_ring *ur = NULL;
+  switch (setup->b.bRequest) {
+    // **** 0xe0: uart read
+    case 0xe0:
+      ur = get_ring_by_number(setup->b.wValue.w);
+      if (!ur) break;
+      if (ur == &esp_ring) uart_dma_drain();
+      // read
+      while ((resp_len < min(setup->b.wLength.w, MAX_RESP_LEN)) &&
+                         getc(ur, (char*)&resp[resp_len])) {
+        ++resp_len;
+      }
+      break;
+  }
+  return resp_len;
+}
+
+#endif
+
+// ***************************** honda can checksum *****************************
+
+int can_cksum(uint8_t *dat, int len, int addr, int idx) {
+  int i;
+  int s = 0;
+  for (i = 0; i < len; i++) {
+    s += (dat[i] >> 4); 
+    s += dat[i] & 0xF;
+  }
+  s += (addr>>0)&0xF;
+  s += (addr>>4)&0xF;
+  s += (addr>>8)&0xF;
+  s += idx;
+  s = 8-s;
+  return s&0xF;
+}
+
+// ***************************** can port *****************************
+
+// addresses to be used on CAN
+#define CAN_GAS_INPUT  0x200
+#define CAN_GAS_OUTPUT 0x201
+
+void CAN1_TX_IRQHandler() {
+  // clear interrupt
+  CAN->TSR |= CAN_TSR_RQCP0;
+}
+
+uint16_t gas_set = 0;
+uint32_t timeout = 0;
+uint32_t current_index = 0;
+
+void CAN1_RX0_IRQHandler() {
+  while (CAN->RF0R & CAN_RF0R_FMP0) {
+    #ifdef DEBUG
+      puts("CAN RX\n");
+    #endif
+    uint32_t address = CAN->sFIFOMailBox[0].RIR>>21;
+    if (address == CAN_GAS_INPUT) {
+      uint8_t *dat = (uint8_t *)&CAN->sFIFOMailBox[0].RDLR;
+      uint16_t value = (dat[0] << 8) | dat[1];
+      uint8_t index = (dat[2] >> 4) & 3;
+      if (can_cksum(dat, 2, CAN_GAS_INPUT, index) == (dat[2] & 0xF)) {
+        if (((current_index+1)&3) == index) {
+          // TODO: set and start timeout 
+          #ifdef DEBUG
+            puts("setting gas ");
+            puth(value);
+            puts("\n");
+          #endif
+          gas_set = value;
+          timeout = 0;
+        }
+        // TODO: better lockout? prevents same spam
+        current_index = index;
+      }
+    }
+    // next
+    CAN->RF0R |= CAN_RF0R_RFOM0;
+  }
+}
+
+void CAN1_SCE_IRQHandler() {
+  can_sce(CAN);
+}
+
+int pdl0 = 0, pdl1 = 0;
+int pkt_idx = 0;
+
+int led_value = 0;
+
+void TIM3_IRQHandler() {
+  #ifdef DEBUG
+    puth(TIM3->CNT);
+    puts(" ");
+    puth(pdl0);
+    puts(" ");
+    puth(pdl1);
+    puts("\n");
+  #endif
+
+  // check timer for sending the user pedal and clearing the CAN
+  if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
+    uint8_t *dat = (uint8_t *)&CAN->sTxMailBox[0].TDLR;
+    CAN->sTxMailBox[0].TDLR = (((pdl0>>8)&0xFF)<<0) |
+                              (((pdl0>>0)&0xFF)<<8) |
+                              (((pdl1>>8)&0xFF)<<16) |
+                              (((pdl1>>0)&0xFF)<<24);
+    CAN->sTxMailBox[0].TDHR = can_cksum(dat, 4, CAN_GAS_OUTPUT, pkt_idx) | (pkt_idx << 4);
+    CAN->sTxMailBox[0].TDTR = 5;  // len of packet is 4
+    CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1;
+    ++pkt_idx;
+    pkt_idx &= 3;
+  } else {
+    // old can packet hasn't sent!
+    // TODO: do something?
+    #ifdef DEBUG
+      puts("CAN MISS\n");
+    #endif
+  }
+
+
+  // blink the LED
+  set_led(LED_GREEN, led_value);
+  led_value = !led_value;
+
+  TIM3->SR = 0;
+
+  // up timeout for gas set
+  timeout++;
+}
+
+// ***************************** main code *****************************
+
+void pedal() {
+  // read/write
+  pdl0 = adc_get(ADCCHAN_ACCEL0);
+  pdl1 = adc_get(ADCCHAN_ACCEL1);
+
+  // write the pedal to the DAC
+  if (timeout < 10) {
+    dac_set(0, max(gas_set, pdl0));
+    dac_set(1, max(gas_set*2, pdl1));
+  } else {
+    dac_set(0, pdl0);
+    dac_set(1, pdl1);
+  }
+}
+
+int main() {
+  __disable_irq();
+
+  // init devices
+  clock_init();
+  periph_init();
+  gpio_init();
+
+#ifdef PEDAL_USB
+  // enable USB
+  usb_init();
+#endif
+
+  // pedal stuff
+  dac_init();
+  adc_init();
+
+  // init can
+  can_silent = ALL_CAN_LIVE;
+  can_init_all();
+
+  // 48mhz / 65536 ~= 732
+  timer_init(TIM3, 15);
+
+  // needed?
+  NVIC_EnableIRQ(CAN1_TX_IRQn);
+  NVIC_EnableIRQ(CAN1_RX0_IRQn);
+  NVIC_EnableIRQ(CAN1_SCE_IRQn);
+
+  NVIC_EnableIRQ(TIM3_IRQn);
+
+  puts("**** INTERRUPTS ON ****\n");
+  __enable_irq();
+
+  // main pedal loop
+  while (1) {
+    pedal();
+  }
+
+  return 0;
+}
+

tests/can_printer.py

@@ -28,7 +28,7 @@ def can_printer():
     if sec_since_boot() - lp > 0.1:
       dd = chr(27) + "[2J"
       dd += "%5.2f\n" % (sec_since_boot() - start)
-      for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))):
+      for k,v in sorted(zip(msgs.keys(), map(lambda x: str(x[-1]).encode("hex"), msgs.values()))):
         dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
       print(dd)
       lp = sec_since_boot()