improvements

board/SConscript

@@ -75,6 +75,7 @@ c_bstub_sources = [
   ["hal", "inc/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c"],
   ["system", "inc/system_stm32f4xx.c"],
   ["it", "inc/stm32f4xx_it.c"],
+  ["util", "util.c"],
 ]
 
 def get_version(builder, build_type):

board/bldc/bldc.c

@@ -64,7 +64,7 @@ volatile uint32_t buzzerTimer = 0;
 static uint8_t  buzzerPrev  = 0;
 static uint8_t  buzzerIdx   = 0;
 
-uint8_t        enable       = 0;        // initially motors are disabled for SAFETY
+uint8_t        enable_motors = 0;        // initially motors are disabled for SAFETY
 static uint8_t enableFin    = 0;
 
 static const uint16_t pwm_res  = 64000000 / 2 / PWM_FREQ; // = 2000 ; TODO: should change to SystemCoreClock ? Needs testing
@@ -113,13 +113,13 @@ void DMA2_Stream0_IRQHandler(void) {
 
   // Disable PWM when current limit is reached (current chopping)
   // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
-  if(ABS(curL_DC) > curDC_max || enable == 0) {
+  if(ABS(curL_DC) > curDC_max || enable_motors == 0) {
     LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
   } else {
     LEFT_TIM->BDTR |= TIM_BDTR_MOE;
   }
 
-  if(ABS(curR_DC)  > curDC_max || enable == 0) {
+  if(ABS(curR_DC)  > curDC_max || enable_motors == 0) {
     RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
   } else {
     RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
@@ -161,7 +161,7 @@ void DMA2_Stream0_IRQHandler(void) {
   OverrunFlag = true;
 
   /* Make sure to stop BOTH motors in case of an error */
-  enableFin = enable && !rtY_Left.z_errCode && !rtY_Right.z_errCode;
+  enableFin = enable_motors && !rtY_Left.z_errCode && !rtY_Right.z_errCode;
 
   // ========================= LEFT MOTOR ============================
     uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN);

board/bootstub.c

@@ -19,6 +19,11 @@
 #include "obj/cert.h"
 #include "obj/gitversion.h"
 
+#include "drivers/llbxcan.h"
+#include "drivers/llflash.h"
+#include "provision.h"
+#include "util.h"
+
 #include "flasher.h"
 
 void __initialize_hardware_early(void) {
@@ -46,10 +51,10 @@ int main(void) {
   SystemClock_Config();
   MX_GPIO_Init();
 
-  HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET);   // Activate Latch
+  out_enable(POWERSWITCH, true);
-  HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
+  out_enable(LED_RED, false);
-  HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
+  out_enable(LED_GREEN, false);
-  HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
+  out_enable(LED_BLUE, false);
 
   if (enter_bootloader_mode == ENTER_SOFTLOADER_MAGIC) {
     enter_bootloader_mode = 0;

board/comms.h

@@ -141,7 +141,7 @@ void CAN2_RX0_IRQHandler(void) {
     } else if ((address == BROADCAST_ADDR) || (address == FALLBACK_ADDR) || (address == ECU_ADDR)) { // Process UBS and OBD2 requests
       process_ubs(address, GET_MAILBOX_BYTES_04(&CAN2->sFIFOMailBox[0]));
     }
-    HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_RESET);
+    out_enable(LED_BLUE, true);
     // next
     CAN2->RF0R |= CAN_RF0R_RFOM0;
   }

board/defines.h

@@ -142,6 +142,13 @@
 
 #define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * ((int)(!(pred))))]))
 
+#define LED_RED     0
+#define LED_GREEN   1
+#define LED_BLUE    2
+#define IGNITION    3
+#define POWERSWITCH 4
+#define TRANSCEIVER 5
+
 typedef struct {
   uint32_t rrB;
   uint32_t rrC;

board/flasher.h

@@ -1,7 +1,3 @@
-#include "drivers/llbxcan.h"
-#include "drivers/llflash.h"
-#include "provision.h"
-
 typedef union {
   uint16_t w;
   struct BW {
@@ -57,7 +53,7 @@ int can_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp) {
         flash_unlock();
         resp[1] = 0xff;
       }
-      HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
+      out_enable(LED_GREEN, true);
       unlocked = true;
       prog_ptr = (uint32_t *)APP_START_ADDRESS;
       break;
@@ -103,12 +99,12 @@ int can_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp) {
 }
 
 void flash_data(void *data, int len) {
-  HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
+  out_enable(LED_RED, false);
   for (int i = 0; i < len/4; i++) {
     flash_write_word(prog_ptr, *(uint32_t*)(data+(i*4)));
     prog_ptr++;
   }
-  HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_RESET);
+  out_enable(LED_RED, true);
 }
 
 int prep_data(uint8_t *data, uint8_t *data_out) {
@@ -241,7 +237,7 @@ void soft_flasher_start(void) {
 
   enter_bootloader_mode = 0;
 
-  HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, GPIO_PIN_RESET); // Enable transceiver by pulling STBY pin LOW (Normal mode)
+  out_enable(TRANSCEIVER, true);
 
   __HAL_RCC_CAN1_CLK_ENABLE(); // Also needed for CAN2, dumb...
   __HAL_RCC_CAN2_CLK_ENABLE();
@@ -251,15 +247,15 @@ void soft_flasher_start(void) {
   llcan_init(CAN);
 
   // green LED on for flashing
-  HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
+  out_enable(LED_GREEN, true);
 
   uint64_t cnt = 0;
 
   for (cnt=0;;cnt++) {
     // blink the green LED fast
-    HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
+    out_enable(LED_GREEN, false);
     delay(500000);
-    HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);
+    out_enable(LED_GREEN, true);
     delay(500000);
   }
 }

board/main.c

@@ -42,7 +42,7 @@ extern int16_t speedAvgAbs;             // Average measured speed in absolute
 extern volatile int pwml;               // global variable for pwm left. -1000 to 1000
 extern volatile int pwmr;               // global variable for pwm right. -1000 to 1000
 
-extern uint8_t enable;                  // global variable for motor enable
+extern uint8_t enable_motors;                  // global variable for motor enable
 
 extern int16_t batVoltage;              // global variable for battery voltage
 
@@ -65,6 +65,7 @@ static uint32_t buzzerTimer_prev = 0;
 
 const uint8_t crc_poly = 0xD5U;  // standard crc8
 
+
 int main(void) {
   HAL_Init();
   HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
@@ -86,22 +87,21 @@ int main(void) {
 
   HAL_ADC_Start(&hadc);
 
-  HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_SET);   // Activate Latch
+  out_enable(POWERSWITCH, true);
-
+  out_enable(IGNITION, true);
-  HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, GPIO_PIN_SET); // Set ignition pin HIGH (ON)
+  out_enable(TRANSCEIVER, true);
-  HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, GPIO_PIN_RESET); // Enable transceiver by pulling STBY pin LOW (Normal mode)
 
   // Reset LEDs upon startup
-  HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, GPIO_PIN_SET);
+  out_enable(LED_RED, false);
-  HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
+  out_enable(LED_GREEN, false);
-  HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
+  out_enable(LED_BLUE, false);
 
   __HAL_RCC_CAN1_CLK_ENABLE(); // Also needed for CAN2, dumb...
   __HAL_RCC_CAN2_CLK_ENABLE();
   llcan_set_speed(CAN2, 5000, false, false);
   llcan_init(CAN2);
 
-  poweronMelody();
+  //poweronMelody();
 
   ignition = 1;
 
@@ -117,22 +117,22 @@ int main(void) {
 
     if (ignition == 0) {
       cmdL = cmdR = 0;
-      enable = 0;
+      enable_motors = 0;
     }
-    if (!enable) {
+    if (!enable_motors) {
       cmdL = 0;
       cmdR = 0;
     }
 
-    if (ignition == 1 && enable == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (ABS(cmdL) < 50 && ABS(cmdR) < 50)) {
+    if (ignition == 1 && enable_motors == 0 && (!rtY_Left.z_errCode && !rtY_Right.z_errCode) && (ABS(cmdL) < 50 && ABS(cmdR) < 50)) {
-      beepShort(6); // make 2 beeps indicating the motor enable
+     // beepShort(6); // make 2 beeps indicating the motor enable
-      beepShort(4);
+      //beepShort(4);
       HAL_Delay(100);
       cmdL = cmdR = 0;
-      enable = 1; // enable motors
+      enable_motors = 1; // enable motors
     }
 
-    HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !ignition);
+    out_enable(LED_GREEN, ignition);
 
     pwml = CLAMP((int)cmdL, -1000, 1000);
     pwmr = -CLAMP((int)cmdR, -1000, 1000);
@@ -146,29 +146,45 @@ int main(void) {
     batVoltageCalib = batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC;
 
     if (main_loop_counter % 2 == 0) { // runs at ~100Hz
-      uint8_t dat[8];
+      if (ignition) { // Send msg only with ignition on
-      uint16_t speedL = rtY_Left.n_mot;
+        uint8_t dat[8];
-      uint16_t speedR = -(rtY_Right.n_mot); // Invert speed sign for the right wheel
+        uint16_t speedL = rtY_Left.n_mot;
-      dat[0] = (speedL >> 8U) & 0xFFU;
+        uint16_t speedR = -(rtY_Right.n_mot); // Invert speed sign for the right wheel
-      dat[1] = speedL & 0xFFU;
+        dat[0] = (speedL >> 8U) & 0xFFU;
-      dat[2] = (speedR >> 8U) & 0xFFU;
+        dat[1] = speedL & 0xFFU;
-      dat[3] = speedR & 0xFFU;
+        dat[2] = (speedR >> 8U) & 0xFFU;
-      dat[4] = rtY_Left.a_elecAngle;
+        dat[3] = speedR & 0xFFU;
-      dat[5] = rtY_Right.a_elecAngle;
+        dat[4] = rtY_Left.a_elecAngle;
-      dat[6] = (batVoltageCalib >> 8U) & 0xFFU;
+        dat[5] = rtY_Right.a_elecAngle;
-      dat[7] = batVoltageCalib & 0xFFU;
+        dat[6] = rtY_Left.z_errCode;
+        dat[7] = rtY_Right.z_errCode;
 
-      // Calibrate chip temp
+        // speed_L(2), speed_R(2), hall_angle_L(1), hall_angle_R(1), left mot error(1), right motor error(1)
-      // dat[4] = (board_temp_adcFilt >> 8U) & 0xFFU;
+        can_send_msg(0x201U, ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);
-      // dat[5] = board_temp_adcFilt & 0xFFU;
+      }
-      // dat[6] = (board_temp_deg_c >> 8U) & 0xFFU;
+    }
-      // dat[7] = board_temp_deg_c & 0xFFU;
 
-      can_send_msg(0x201U, ((dat[7] << 24U) | (dat[6] << 16U) | (dat[5]<< 8U) | dat[4]), ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 8U);
+    if (main_loop_counter % 20 == 0) { // Runs at ~10Hz
+      uint8_t dat[2];
+      dat[0] = ignition;
+      dat[1] = enable_motors;
+
+      // ignition(1), enable_motors(1)
+      can_send_msg(0x202U, 0x0U, ((dat[1] << 8U) | dat[0]), 2U);
     }
 
     if (main_loop_counter % 200 == 0) { // Runs at ~1Hz
-      HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
+      uint8_t dat[4];
+      dat[0] = (board_temp_deg_c >> 8U) & 0xFFU;
+      dat[1] = board_temp_deg_c & 0xFFU;
+      dat[2] = (batVoltageCalib >> 8U) & 0xFFU;
+      dat[3] = batVoltageCalib & 0xFFU;
+
+      // MCU temp(2), battery voltage(2)
+      can_send_msg(0x203U, 0x0U, ((dat[3] << 24U) | (dat[2] << 16U) | (dat[1] << 8U) | dat[0]), 4U);
+
+      // Reset LED after CAN RX
+      out_enable(LED_BLUE, false);
     }
 
     poweroffPressCheck();
@@ -176,7 +192,7 @@ int main(void) {
     if ((TEMP_POWEROFF_ENABLE && board_temp_deg_c >= TEMP_POWEROFF && speedAvgAbs < 20) || (batVoltage < BAT_DEAD && speedAvgAbs < 20)) {  // poweroff before mainboard burns OR low bat 3
       poweroff();
     } else if (rtY_Left.z_errCode || rtY_Right.z_errCode) {                                           // 1 beep (low pitch): Motor error, disable motors
-      enable = 0;
+      enable_motors = 0;
       beepCount(1, 24, 1);
     } else if (TEMP_WARNING_ENABLE && board_temp_deg_c >= TEMP_WARNING) {                             // 5 beeps (low pitch): Mainboard temperature warning
       beepCount(5, 24, 1);

board/util.c

@@ -15,7 +15,7 @@ extern uint8_t buzzerCount;             // global variable for the buzzer counts
 extern uint8_t buzzerFreq;              // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
 extern uint8_t buzzerPattern;           // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
 
-extern uint8_t enable;                  // global variable for motor enable
+extern uint8_t enable_motors;                  // global variable for motor enable
 extern uint8_t ignition;                // global variable for ignition on SBU2 line
 
 //------------------------------------------------------------------------
@@ -78,6 +78,29 @@ void BLDC_Init(void) {
   BLDC_controller_initialize(rtM_Right);
 }
 
+void out_enable(uint8_t led, bool enabled) {
+  switch(led) {
+    case LED_RED:
+      HAL_GPIO_WritePin(LED_RED_PORT, LED_RED_PIN, !enabled);
+      break;
+    case LED_GREEN:
+      HAL_GPIO_WritePin(LED_GREEN_PORT, LED_GREEN_PIN, !enabled);
+      break;
+    case LED_BLUE:
+      HAL_GPIO_WritePin(LED_BLUE_PORT, LED_BLUE_PIN, !enabled);
+      break;
+    case IGNITION:
+      HAL_GPIO_WritePin(IGNITION_PORT, IGNITION_PIN, enabled);
+      break;
+    case POWERSWITCH:
+      HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, enabled);
+      break;
+    case TRANSCEIVER:
+      HAL_GPIO_WritePin(CAN_STBY_PORT, CAN_STBY_PIN, !enabled);
+      break;
+  }
+}
+
 void poweronMelody(void) {
     buzzerCount = 0;
     for (int i = 8; i >= 0; i--) {
@@ -131,14 +154,14 @@ void calcAvgSpeed(void) {
 }
 
 void poweroff(void) {
-  enable = 0;
+  enable_motors = 0;
   buzzerCount = 0;
   buzzerPattern = 0;
   for (int i = 0; i < 8; i++) {
     buzzerFreq = (uint8_t)i;
     HAL_Delay(100);
   }
-  HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
+  out_enable(POWERSWITCH, true);
   while(1) {
     // Temporarily, to see that we went to power off but can't switch the latch
     HAL_GPIO_TogglePin(LED_RED_PORT, LED_RED_PIN);

board/util.h

@@ -3,11 +3,13 @@
 #define UTIL_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 // Initialization Functions
 void BLDC_Init(void);
 
 // General Functions
+void out_enable(uint8_t led, bool enabled);
 void poweronMelody(void);
 void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern);
 void beepLong(uint8_t freq);
@@ -24,6 +26,5 @@ void filtLowPass32(int32_t u, uint16_t coef, int32_t *y);
 void rateLimiter16(int16_t u, int16_t rate, int16_t *y);
 
 uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly);
-void delay(uint32_t a);
 
 #endif