Merge pull request #97 from commaai/pedal_improvements

Pedal improvements
dragonpilot-community · Mar 5, 2018 · 23de8d4 · 23de8d4
2 parents 1b77026 + e2f73d2
commit 23de8d4
Show file tree

Hide file tree

Showing 2 changed files with 83 additions and 20 deletions.
diff --git a/board/pedal/README b/board/pedal/README
@@ -4,3 +4,25 @@ The comma pedal is a gas pedal interceptor for Honda/Acura. It allows you to "vi
 
 This is the open source software. Open source hardware coming soon.
 
+== Test Plan ==
+
+* Startup
+** Confirm STATE_FAULT_STARTUP
+* Timeout
+** Send value
+** Confirm value is output
+** Stop sending messages
+** Confirm value is passthru after 100ms
+** Confirm STATE_FAULT_TIMEOUT
+* Random values
+** Send random 6 byte messages
+** Confirm random values cause passthru
+** Confirm STATE_FAULT_BAD_CHECKSUM
+* Same message lockout
+** Send same message repeated
+** Confirm timeout behavior
+* Don't set enable
+** Confirm no output
+* Set enable and values
+** Confirm output
+
diff --git a/board/pedal/main.c b/board/pedal/main.c
@@ -107,10 +107,22 @@ void CAN1_TX_IRQHandler() {
   CAN->TSR |= CAN_TSR_RQCP0;
 }
 
-uint16_t gas_set = 0;
+// two independent values
+uint16_t gas_set_0 = 0;
+uint16_t gas_set_1 = 0;
+
+#define MAX_TIMEOUT 10
 uint32_t timeout = 0;
 uint32_t current_index = 0;
 
+#define NO_FAULT 0
+#define FAULT_BAD_CHECKSUM 1
+#define FAULT_SEND 2
+#define FAULT_SCE 3
+#define FAULT_STARTUP 4
+#define FAULT_TIMEOUT 5
+uint8_t state = FAULT_STARTUP;
+
 void CAN1_RX0_IRQHandler() {
   while (CAN->RF0R & CAN_RF0R_FMP0) {
     #ifdef DEBUG
@@ -119,21 +131,33 @@ void CAN1_RX0_IRQHandler() {
     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)) {
+      uint8_t *dat2 = (uint8_t *)&CAN->sFIFOMailBox[0].RDHR;
+      uint16_t value_0 = (dat[0] << 8) | dat[1];
+      uint16_t value_1 = (dat[2] << 8) | dat[3];
+      uint8_t enable = (dat2[0] >> 7) & 1;
+      uint8_t index = (dat2[1] >> 4) & 3;
+      if (can_cksum(dat, 5, CAN_GAS_INPUT, index) == (dat2[1] & 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;
+          if (enable) {
+            gas_set_0 = value_0;
+            gas_set_1 = value_1;
+          } else {
+            // clear the fault state if values are 0
+            if (value_0 == 0 && value_1 == 0) state = NO_FAULT;
+            gas_set_0 = gas_set_1 = 0;
+          }
+          // clear the timeout
           timeout = 0;
         }
-        // TODO: better lockout? prevents same spam
         current_index = index;
+      } else {
+        // wrong checksum = fault
+        state = FAULT_BAD_CHECKSUM;
       }
     }
     // next
@@ -142,6 +166,7 @@ void CAN1_RX0_IRQHandler() {
 }
 
 void CAN1_SCE_IRQHandler() {
+  state = FAULT_SCE;
   can_sce(CAN);
 }
 
@@ -162,33 +187,39 @@ void TIM3_IRQHandler() {
 
   // check timer for sending the user pedal and clearing the CAN
   if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
-    uint8_t *dat = (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
+    uint8_t dat[8];
+    dat[0] = (pdl0>>8)&0xFF;
+    dat[1] = (pdl0>>0)&0xFF;
+    dat[2] = (pdl1>>8)&0xFF;
+    dat[3] = (pdl1>>0)&0xFF;
+    dat[4] = state;
+    dat[5] = can_cksum(dat, 5, CAN_GAS_OUTPUT, pkt_idx);
+    CAN->sTxMailBox[0].TDLR = dat[0] | (dat[1]<<8) | (dat[2]<<16) | (dat[3]<<24);
+    CAN->sTxMailBox[0].TDHR = dat[4] | (dat[5]<<8);
+    CAN->sTxMailBox[0].TDTR = 6;  // len of packet is 5
     CAN->sTxMailBox[0].TIR = (CAN_GAS_OUTPUT << 21) | 1;
     ++pkt_idx;
     pkt_idx &= 3;
   } else {
     // old can packet hasn't sent!
-    // TODO: do something?
+    state = FAULT_SEND;
     #ifdef DEBUG
       puts("CAN MISS\n");
     #endif
   }
 
-
   // blink the LED
   set_led(LED_GREEN, led_value);
   led_value = !led_value;
 
   TIM3->SR = 0;
 
   // up timeout for gas set
-  timeout++;
+  if (timeout == MAX_TIMEOUT) {
+    state = FAULT_TIMEOUT;
+  } else {
+    timeout += 1;
+  }
 }
 
 // ***************************** main code *****************************
@@ -199,13 +230,16 @@ void pedal() {
   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));
+  if (timeout < MAX_TIMEOUT && state == NO_FAULT) {
+    dac_set(0, max(gas_set_0, pdl0));
+    dac_set(1, max(gas_set_1, pdl1));
   } else {
     dac_set(0, pdl0);
     dac_set(1, pdl1);
   }
+
+  // feed the watchdog
+  IWDG->KR = 0xAAAA;
 }
 
 int main() {
@@ -239,6 +273,13 @@ int main() {
 
   NVIC_EnableIRQ(TIM3_IRQn);
 
+  // setup watchdog
+  IWDG->KR = 0x5555;
+  IWDG->PR = 0;          // divider /4
+  // 0 = 0.125 ms, let's have a 50ms watchdog
+  IWDG->RLR = 400 - 1;
+  IWDG->KR = 0xCCCC;
+
   puts("**** INTERRUPTS ON ****\n");
   __enable_irq();