Merge branch 'integration-changes'

Hardware/~sensor-node.kicad_pcb.lck

@@ -1 +0,0 @@
-{"hostname":"nagata","username":"oskar"}

Hardware/~sensor-node.kicad_sch.lck

@@ -1 +0,0 @@
-{"hostname":"nagata","username":"oskar"}

Software/Core/Inc/main.h

@@ -47,6 +47,7 @@ extern "C" {
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 
+void Error_Handler_Led(uint8_t err);
 /* USER CODE END EM */
 
 /* Exported functions prototypes ---------------------------------------------*/

Software/Core/Inc/mappings.h

@@ -4,6 +4,13 @@
 // CHOOSE ONE, comment the other or comment both and use -D SN_FRONT
 //#define SN_FRONT
 #define SN_REAR
+// #define DEBUG
+
+#if (defined(SN_FRONT) && defined(SN_REAR)) || \
+    (defined(SN_FRONT) && defined(DEBUG))   || \
+    (defined(SN_REAR) && defined(DEBUG))
+#error "Choose only one of #define SN_FRONT, #define SN_REAR, or #define DEBUG!"
+#endif
 
 #include <stdint.h>
 #include "main.h"
@@ -14,12 +21,12 @@ static struct {
   GPIO_TypeDef* port;
   uint16_t pin;
 } DIO_PIN_MAP[NUM_DIO_PINS] = {
-/* 0 */ { .port = D1_IC_GPIO_Port,  .pin = D1_IC_Pin},
-/* 1 */ { .port = D2_GPIO_Port,     .pin = D2_Pin},
-/* 2 */ { .port = D3_GPIO_Port,     .pin = D3_Pin},
-/* 3 */ { .port = D4_IC_GPIO_Port,  .pin = D4_IC_Pin},
-/* 4 */ { .port = D5_GPIO_Port,     .pin = D5_Pin},
-/* 5 */ { .port = D6_GPIO_Port,     .pin = D6_Pin},
+/* 0 */ { .port = D5_GPIO_Port,     .pin = D5_Pin},
+/* 1 */ { .port = D6_GPIO_Port,     .pin = D6_Pin},
+/* 2 */ { .port = D4_IC_GPIO_Port,  .pin = D4_IC_Pin},
+/* 3 */ { .port = D3_GPIO_Port,     .pin = D3_Pin},
+/* 4 */ { .port = D2_GPIO_Port,     .pin = D2_Pin},
+/* 5 */ { .port = D1_IC_GPIO_Port,  .pin = D1_IC_Pin},
 };
 
 typedef enum {
@@ -95,10 +102,10 @@ typedef struct {
 static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
     {
         .can_id = 0x0D1, .dlc = 3, .num_signals = 7, .period = 100,  .signals = {
-            { .type = DIN, .channel = R5, .start =  0, .length =  1, .factor = 1., .name = "LS L" },
-            { .type = DIN, .channel = L9, .start =  1, .length =  1, .factor = 1., .name = "LS R" },
-            { .type = DIN, .channel = LA, .start =  2, .length =  1, .factor = 1., .name = "SDC M pre BOTS" },
-            { .type = DIN, .channel = LC, .start =  3, .length =  1, .factor = 1., .name = "SDC M post INS" },
+            { .type = DIN, .channel = L9, .start =  0, .length =  1, .factor = 1., .name = "LS L" },
+            { .type = DIN, .channel = LA, .start =  1, .length =  1, .factor = 1., .name = "LS R" },
+            { .type = DIN, .channel = R5, .start =  2, .length =  1, .factor = 1., .name = "SDC M pre BOTS" },
+            { .type = DIN, .channel = R6, .start =  3, .length =  1, .factor = 1., .name = "SDC M post INS" },
             { .type = DIN, .channel = R7, .start =  4, .length =  1, .factor = 1., .name = "SDC M post BSPD" },
 
             { .type = AIN, .channel = R0, .start =  8, .length =  8, .factor = ((512./2)/ADQ), .name = "BDTS FL" },
@@ -109,7 +116,7 @@ static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
         .can_id = 0x0D3, .dlc = 7, .num_signals = 6, .period = 10, .signals = {
             { .type = AIN, .channel = L1, .start =  0, .length =  8, .factor = (256./ADQ), .name = "APPS 1" },
             { .type = AIN, .channel = L2, .start =  8, .length =  8, .factor = (256./ADQ), .name = "APPS 2" },
-            { .type = AIN, .channel = RA, .start = 16, .length =  8, .factor = ((512./2)/ADQ), .name = "BP F" },
+            { .type = AIN, .channel = L5, .start = 16, .length =  8, .factor = ((512./2)/ADQ), .name = "BP F" },
             { .type = AIN, .channel = R9, .start = 24, .length = 12, .factor = 1., .name = "SAS" },
             { .type = FIN, .channel = R8, .start = 40, .length =  8, .factor = 1., .name = "WSS FL" },
             { .type = FIN, .channel = LB, .start = 48, .length =  8, .factor = 1., .name = "WSS FR" },
@@ -117,7 +124,7 @@ static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
     },
     {
         .can_id = 0x0D5, .dlc = 3, .num_signals = 2, .period = 10, .signals = {
-            { .type = AIN, .channel = RC, .start =  0, .length = 12, .factor = 1., .name = "DS FL" },
+            { .type = AIN, .channel = L3, .start =  0, .length = 12, .factor = 1., .name = "DS FL" },
             { .type = AIN, .channel = L4, .start = 12, .length = 12, .factor = 1., .name = "DS FR" },
         }
     },
@@ -129,6 +136,15 @@ static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
     }
 };
 
+static uint8_t DIO_LATCHING[NUM_DIO_PINS] = {
+    1, 1, 0, 0, 0, 0,
+};
+
+#define CAN_PWM_DC_ID 0x0DA // UNUSED
+#define CAN_PWM_CONF_ID 0x0DB // UNUSED
+#define CAN_PWM_BASE_ID CAN_PWM_DC_ID
+#define CAN_PWM_FILTER_MASK 0x7FE // Match both
+
 #endif
 
 #ifdef SN_REAR
@@ -143,20 +159,20 @@ static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
             { .type = AIN, .channel = R0, .start =  8, .length =  8, .factor = ((512./2)/ADQ), .name = "BDTS RL" },
             { .type = AIN, .channel = L8, .start = 16, .length =  8, .factor = ((512./2)/ADQ), .name = "BDTS RR" },
 
-            { .type = AIN, .channel = R9, .start = 24, .length =  8, .factor = (256./ADQ), .name = "WT BAT" },
-            { .type = AIN, .channel = RB, .start = 32, .length =  8, .factor = (256./ADQ), .name = "WT DT" },
-            { .type = AIN, .channel = RD, .start = 40, .length =  8, .factor = (256./ADQ), .name = "WP BAT" },
-            { .type = AIN, .channel = RF, .start = 48, .length =  8, .factor = (256./ADQ), .name = "WP DT" },
+            { .type = AIN, .channel = L3, .start = 24, .length =  8, .factor = (256./ADQ), .name = "WT BAT" },
+            { .type = AIN, .channel = R9, .start = 32, .length =  8, .factor = (256./ADQ), .name = "WT DT" },
+            { .type = AIN, .channel = L5, .start = 40, .length =  8, .factor = (256./ADQ), .name = "WP BAT" },
+            { .type = AIN, .channel = RB, .start = 48, .length =  8, .factor = (256./ADQ), .name = "WP DT" },
         }
     },
     {
         .can_id = 0x0D4, .dlc = 8, .num_signals = 7, .period = 10, .signals = {
             { .type = AIN, .channel = L1, .start =  0, .length =  8, .factor = ((25.6/.1)/ADQ), .name = "EBS APS 1" },
             { .type = AIN, .channel = L2, .start =  8, .length =  8, .factor = ((25.6/.1)/ADQ), .name = "EBS APS 2" },
-            { .type = AIN, .channel = RA, .start = 16, .length =  8, .factor = ((512./2)/ADQ), .name = "BP F" },
+            { .type = AIN, .channel = RA, .start = 16, .length =  8, .factor = ((512./2)/ADQ), .name = "BP R" },
             { .type = FIN, .channel = R8, .start = 24, .length =  8, .factor = 1., .name = "WSS RL" },
             { .type = FIN, .channel = LB, .start = 32, .length =  8, .factor = 1., .name = "WSS RR" },
-            { .type = AIN, .channel = RC, .start = 40, .length = 12, .factor = 1., .name = "DS RL" },
+            { .type = AIN, .channel = L7, .start = 40, .length = 12, .factor = 1., .name = "DS RL" },
             { .type = AIN, .channel = L4, .start = 52, .length = 12, .factor = 1., .name = "DS RR" },
         }
     },
@@ -171,8 +187,66 @@ static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
     }
 };
 
+static uint8_t DIO_LATCHING[NUM_DIO_PINS] = {
+    0, 0, 0, 0, 0, 0,
+};
+
 #define CAN_PWM_DC_ID 0x0DC
 #define CAN_PWM_CONF_ID 0x0DD
+#define CAN_PWM_BASE_ID CAN_PWM_DC_ID
+#define CAN_PWM_FILTER_MASK 0x7FE // Match both 0x0DC and 0x0DD
+
+#endif
+
+#ifdef DEBUG
+
+static can_pkt_t CAN_SIGNAL_MAP[NUM_TX_PKT] = {
+    {
+        .can_id = 0x044, .dlc = 6, .num_signals = 9, .period = 100,  .signals = {
+            { .type = DIN, .channel = L9, .start =  0, .length =  1, .factor = 1., .name = "" },
+            { .type = DIN, .channel = LA, .start =  1, .length =  1, .factor = 1., .name = "" },
+            { .type = DIN, .channel = LC, .start =  2, .length =  1, .factor = 1., .name = "" },
+            { .type = DIN, .channel = R5, .start =  3, .length =  1, .factor = 1., .name = "" },
+            { .type = DIN, .channel = R6, .start =  4, .length =  1, .factor = 1., .name = "" },
+            { .type = DIN, .channel = R7, .start =  5, .length =  1, .factor = 1., .name = "" },
+
+            { .type = AIN, .channel = L1, .start =  6+12*0, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L2, .start =  6+12*1, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L3, .start =  6+12*2, .length =  12, .factor = 1., .name = "" },
+        }
+    },
+    {
+        .can_id = 0x045, .dlc = 8, .num_signals = 5, .period = 100,  .signals = {
+            { .type = AIN, .channel = L4, .start =  12*0, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L5, .start =  12*1, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L6, .start =  12*2, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L7, .start =  12*3, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = L8, .start =  12*4, .length =  12, .factor = 1., .name = "" },
+        }
+    },
+    {
+        .can_id = 0x046, .dlc = 8, .num_signals = 5, .period = 100,  .signals = {
+            { .type = AIN, .channel = R9, .start =  12*0, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = RA, .start =  12*1, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = RB, .start =  12*2, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = RC, .start =  12*3, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = RD, .start =  12*4, .length =  12, .factor = 1., .name = "" },
+        }
+    },
+    {
+        .can_id = 0x047, .dlc = 5, .num_signals = 3, .period = 100,  .signals = {
+            { .type = AIN, .channel = RE, .start =  12*0, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = RF, .start =  12*1, .length =  12, .factor = 1., .name = "" },
+            { .type = AIN, .channel = R0, .start =  12*2, .length =  12, .factor = 1., .name = "" },
+
+        }
+    },
+};
+
+#define CAN_PWM_DC_ID 0x0DA // UNUSED
+#define CAN_PWM_CONF_ID 0x0DB // UNUSED
+#define CAN_PWM_BASE_ID CAN_PWM_DC_ID
+#define CAN_PWM_FILTER_MASK 0x7FE // Match both
 
 #endif
 

Software/Core/Inc/stm32h7xx_it.h

@@ -62,6 +62,7 @@ void FDCAN2_IT0_IRQHandler(void);
 void FDCAN1_IT1_IRQHandler(void);
 void FDCAN2_IT1_IRQHandler(void);
 void TIM8_CC_IRQHandler(void);
+void TIM6_DAC_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */

Software/Core/Src/main.c

@@ -28,6 +28,7 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include <string.h>
+#include <stdbool.h>
 
 #include "mappings.h"
 /* USER CODE END Includes */
@@ -40,6 +41,7 @@
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
 #define TIM_BASE_FREQ 96000000UL
+#define WSS_HISTORY_SIZE 10
 /* USER CODE END PD */
 
 /* Private macro -------------------------------------------------------------*/
@@ -50,16 +52,27 @@
 /* Private variables ---------------------------------------------------------*/
 
 /* USER CODE BEGIN PV */
-/* Declare buffer in D1 domain SRAM */
+unsigned int tx_counter;
+bool adc_conv_cplt;
+
+FDCAN_HandleTypeDef *hMainCAN, *hPeriCAN;
+FDCAN_TxHeaderTypeDef txHeader;
+
+// Declare buffer in AXI SRAM
 static uint16_t adc_values[NUM_ADC_PINS];
 static uint8_t  dio_values[NUM_DIO_PINS];
 
+static uint16_t filtered_values[NUM_ADC_PINS];
+
 // See mappings.h pwm_tim_t
 TIM_HandleTypeDef* PWM_TIM_MAP[3] = {&htim1, &htim4, &htim3};
 
 static uint8_t  pwm_ch_active[8];
 
 static uint16_t wss_flanks[2];
+static uint16_t wss_flanks_avg[2];
+static uint8_t wss_flanks_history[2][WSS_HISTORY_SIZE];
+static uint8_t wss_flanks_history_idx[2];  // index of the oldest entry (next to be overwritten)
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -72,6 +85,105 @@ static void MX_NVIC_Init(void);
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
 
+void send_latest_can() {
+
+  tx_counter++;
+
+  for (int di = 0; di < NUM_DIO_PINS; di++) {
+    uint8_t value = HAL_GPIO_ReadPin(
+      DIO_PIN_MAP[di].port,
+      DIO_PIN_MAP[di].pin
+    );
+
+    if (DIO_LATCHING[di]) {
+      dio_values[di] |= value;
+    } else {
+      dio_values[di] = value;
+    }
+  }
+
+  for (int pi = 0; pi < NUM_TX_PKT; pi++) {
+
+    can_pkt_t* pktinfo = &(CAN_SIGNAL_MAP[pi]);
+
+    if (pktinfo->num_signals < 0)
+      continue;
+
+    if (tx_counter % pktinfo->period == 0) {
+
+      txHeader.Identifier = pktinfo->can_id;
+      txHeader.DataLength = pktinfo->dlc;
+      uint64_t txData = 0;
+
+      for (int si = 0; si < pktinfo->num_signals; si++) {
+
+        can_signal_t* signal = &(pktinfo->signals[si]);
+        uint16_t value = 0;
+        switch (signal->type) {
+          case DIN:
+            value = dio_values[signal->channel];
+            dio_values[signal->channel] = 0;  // will be overwritten with real value at start of 1kHz loop
+            break;
+
+          case AIN:
+            value = signal->factor * filtered_values[signal->channel];
+            break;
+
+          case FIN:
+            uint8_t oldest_hist_idx = wss_flanks_history_idx[signal->channel];
+            // subtract oldest entry from avg
+            wss_flanks_avg[signal->channel] -= wss_flanks_history[signal->channel][oldest_hist_idx];
+            // overwrite oldest history entry with new value and reset counter
+            wss_flanks_history[signal->channel][oldest_hist_idx] = wss_flanks[signal->channel];
+            wss_flanks[signal->channel] = 0;
+            // add new counter to avg
+            wss_flanks_avg[signal->channel] += wss_flanks_history[signal->channel][oldest_hist_idx];
+            // increase / wrap around index
+            wss_flanks_history_idx[signal->channel]++;
+            if(wss_flanks_history_idx[signal->channel] >= WSS_HISTORY_SIZE) {
+              wss_flanks_history_idx[signal->channel] = 0;
+            }
+            // value = wss_flanks_avg[signal->channel] * (1000 / pktinfo->period / WSS_HISTORY_SIZE);
+            value = wss_flanks_avg[signal->channel];
+            break;
+
+          default:
+            break;
+        }
+
+        uint16_t mask = 0xFFFF >> (16 - signal->length);
+        txData |= ((uint64_t) (value & mask)) << (signal->start);
+
+      }
+
+      if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK) {
+        if(HAL_FDCAN_GetError(hMainCAN) != HAL_FDCAN_ERROR_FIFO_FULL) {
+          Error_Handler_Led(1);
+        }
+      }
+    }
+
+    if (tx_counter >= 256) {
+      tx_counter = 0;
+      HAL_GPIO_TogglePin(STATUS_G_GPIO_Port, STATUS_G_Pin);
+    }
+  }
+}
+
+void filter_adc() {
+  for (size_t i = 0; i < NUM_TX_PKT; i++){    
+    float alpha = 1 / CAN_SIGNAL_MAP[i].period; // TODO: check if it smoothes too much
+
+    for (size_t j = 0; i < CAN_SIGNAL_MAP[i].num_signals; j++) {
+      if(CAN_SIGNAL_MAP[i].signals[j].type != AIN)
+        continue;
+      
+      uint8_t ch_index = CAN_SIGNAL_MAP[i].signals[j].channel;
+      filtered_values[ch_index] = (alpha * adc_values[ch_index]) + ((1 - alpha) * filtered_values[ch_index]);
+    }
+  }
+}
+
 /* USER CODE END 0 */
 
 /**
@@ -117,21 +229,19 @@ int main(void)
   MX_NVIC_Init();
   /* USER CODE BEGIN 2 */
   
-  FDCAN_HandleTypeDef* hMainCAN = &hfdcan2;
-  //FDCAN_HandleTypeDef* hPeriCAN = &hfdcan1;
+  hMainCAN = &hfdcan1;
+  hPeriCAN = &hfdcan2;
 
   if (HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(2);
 
   if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_values, NUM_ADC_PINS) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(2);
 
-  HAL_TIM_Base_Start(&htim6);
+  HAL_TIM_Base_Start_IT(&htim6);
 
   // CAN TX PREP
 
-  FDCAN_TxHeaderTypeDef txHeader;
-
   // Prep the tx frame
   txHeader.ErrorStateIndicator = FDCAN_ESI_PASSIVE;
   txHeader.BitRateSwitch = FDCAN_BRS_OFF;
@@ -143,25 +253,25 @@ int main(void)
   txHeader.DataLength = 8;
 
   if (HAL_FDCAN_ActivateNotification(hMainCAN, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(3);
 
   if (HAL_FDCAN_ConfigGlobalFilter(hMainCAN, FDCAN_REJECT, FDCAN_REJECT,
                                     FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(3);
 
   FDCAN_FilterTypeDef filter;
   filter.IdType = FDCAN_STANDARD_ID;
   filter.FilterIndex = 0;
   filter.FilterType = FDCAN_FILTER_MASK;
   filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
-  filter.FilterID1 = 0x0DD;
-  filter.FilterID2 = 0x7FE; // Match 0x0DC and 0x0DD
+  filter.FilterID1 = CAN_PWM_BASE_ID;
+  filter.FilterID2 = CAN_PWM_FILTER_MASK;
 
   if (HAL_FDCAN_ConfigFilter(hMainCAN, &filter) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(5);
 
   if (HAL_FDCAN_Start(hMainCAN) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(5);
 
   // Init all channels as stopped
   memset(pwm_ch_active, 0, 8);
@@ -170,77 +280,30 @@ int main(void)
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_1);
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_2);
 
+  tx_counter = 0;
+
+  memset(wss_flanks, 0, sizeof(wss_flanks));
+  memset(wss_flanks_avg, 0, sizeof(wss_flanks));
+  memset(wss_flanks_history, 0, sizeof(wss_flanks_history));
+  memset(wss_flanks_history_idx, 0, sizeof(wss_flanks_history_idx));
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
-  for (unsigned int mscounter = 0; 1; mscounter++)
+  while(1)
   {
+    if(adc_conv_cplt) {
+      adc_conv_cplt = false;
+
+      filter_adc();
+
+      send_latest_can();
+    }
+
+    HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
-    for (int di = 0; di < NUM_DIO_PINS; di++) {
-      dio_values[di] = HAL_GPIO_ReadPin(
-        DIO_PIN_MAP[di].port,
-        DIO_PIN_MAP[di].pin
-      );
-    }
-
-    for (int pi = 0; pi < NUM_TX_PKT; pi++) {
-
-      can_pkt_t* pktinfo = &(CAN_SIGNAL_MAP[pi]);
-
-      if (pktinfo->num_signals < 0)
-        continue;
-
-      if (mscounter % pktinfo->period == 0) {
-
-        txHeader.Identifier = pktinfo->can_id;
-        txHeader.DataLength = pktinfo->dlc;
-        uint64_t txData = 0;
-
-        for (int si = 0; si < pktinfo->num_signals; si++) {
-
-          can_signal_t* signal = &(pktinfo->signals[si]);
-          uint16_t value = 0;
-          switch (signal->type) {
-            case DIN:
-              value = dio_values[signal->channel];
-              break;
-
-            case AIN:
-              value = signal->factor * adc_values[signal->channel];
-              break;
-
-            case FIN:
-              value = wss_flanks[signal->channel];
-              wss_flanks[signal->channel] = 0;
-              break;
-
-            default:
-              break;
-          }
-
-          uint16_t mask = 0xFFFF >> (16 - signal->length);
-          txData |= ((uint64_t) (value & mask)) << (signal->start);
-
-        }
-
-        if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK)
-          Error_Handler();
-
-      }
-
-      if (mscounter >= 500) {
-        mscounter = 0;
-        HAL_GPIO_TogglePin(STATUS_G_GPIO_Port, STATUS_G_Pin);
-      }
-
-    }
-
-    // TODO: Move all this into a 1kHz timer callback!
-    HAL_Delay(1);
-
   }
   /* USER CODE END 3 */
 }
@@ -284,7 +347,7 @@ void SystemClock_Config(void)
   RCC_OscInitStruct.PLL.PLLFRACN = 0;
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
   {
-    Error_Handler();
+    Error_Handler_Led(4);
   }
 
   /** Initializes the CPU, AHB and APB buses clocks
@@ -302,7 +365,7 @@ void SystemClock_Config(void)
 
   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
   {
-    Error_Handler();
+    Error_Handler_Led(4);
   }
 }
 
@@ -322,10 +385,21 @@ void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
 {
   if (hadc->Instance == ADC1)
   {
-    __asm volatile ("NOP");
+    adc_conv_cplt = true;
   }
 }
 
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
+
+  //HAL_GPIO_TogglePin(STATUS_B_GPIO_Port, STATUS_B_Pin);
+
+  if (htim != &htim6)
+    return;
+
+  // TODO: Make timer 10kHz and dont trigger a function with it, for exiting WFI in main
+
+};
+
 /*
  * Input capture timing calculations:
  * wheel speed = 8000 rpm * (11/48) = 30.55 rot/s
@@ -371,9 +445,6 @@ void SetCCR(TIM_TypeDef* Instance, unsigned int ch, uint8_t dc) {
 
 void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *handle, uint32_t RxFifo0ITs)
 {
-  FDCAN_HandleTypeDef* hMainCAN = &hfdcan2;
-  //FDCAN_HandleTypeDef* hPeriCAN = &hfdcan1;
-
   if (handle != hMainCAN || (RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == RESET)
     return; // TODO: handle Peripheral CAN
   
@@ -430,6 +501,54 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *handle, uint32_t RxFifo0ITs)
 
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
+{
+  HAL_ResumeTick();
+}
+
+void Error_Handler_Led(uint8_t err) {
+  HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_RESET);
+  switch (err)
+  {
+  case 0: // red: general error
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    break;
+
+  case 1: // red blue: in 1 kHz loop
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    break;
+
+  case 2: // blue: ADC calibration or DMA start
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    break;
+
+  case 3: // green blue: CAN notify and config gloabl filter
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+  case 4: //red green: clock
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+  case 5: // red green blue: CAN config filter and start
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+    default:
+    break;
+  }
+
+  __disable_irq();
+  while (1);
+}
+
 /* USER CODE END 4 */
 
 /**

Software/Core/Src/stm32h7xx_it.c

@@ -59,6 +59,7 @@ extern DMA_HandleTypeDef hdma_adc1;
 extern ADC_HandleTypeDef hadc1;
 extern FDCAN_HandleTypeDef hfdcan1;
 extern FDCAN_HandleTypeDef hfdcan2;
+extern TIM_HandleTypeDef htim6;
 extern TIM_HandleTypeDef htim8;
 /* USER CODE BEGIN EV */
 
@@ -300,6 +301,20 @@ void TIM8_CC_IRQHandler(void)
   /* USER CODE END TIM8_CC_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM6 global interrupt, DAC1_CH1 and DAC1_CH2 underrun error interrupts.
+  */
+void TIM6_DAC_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM6_DAC_IRQn 0 */
+
+  /* USER CODE END TIM6_DAC_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim6);
+  /* USER CODE BEGIN TIM6_DAC_IRQn 1 */
+
+  /* USER CODE END TIM6_DAC_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */

Software/Core/Src/tim.c

@@ -217,9 +217,9 @@ void MX_TIM6_Init(void)
 
   /* USER CODE END TIM6_Init 1 */
   htim6.Instance = TIM6;
-  htim6.Init.Prescaler = 4800-1;
+  htim6.Init.Prescaler = 480-1;
   htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim6.Init.Period = 2000-1;
+  htim6.Init.Period = 200-1;
   htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
   if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
   {
@@ -334,6 +334,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
   /* USER CODE END TIM6_MspInit 0 */
     /* TIM6 clock enable */
     __HAL_RCC_TIM6_CLK_ENABLE();
+
+    /* TIM6 interrupt Init */
+    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
   /* USER CODE BEGIN TIM6_MspInit 1 */
 
   /* USER CODE END TIM6_MspInit 1 */
@@ -502,6 +506,9 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
   /* USER CODE END TIM6_MspDeInit 0 */
     /* Peripheral clock disable */
     __HAL_RCC_TIM6_CLK_DISABLE();
+
+    /* TIM6 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn);
   /* USER CODE BEGIN TIM6_MspDeInit 1 */
 
   /* USER CODE END TIM6_MspDeInit 1 */

Software/debug_dbc_entries.txt

@@ -0,0 +1,29 @@
+BO_ 68 Sensornode_Debug_1: 6 Sensornode_F
+   SG_ Sensornode_Debug_L9 : 0|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_LA : 1|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_LC : 2|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_R5 : 3|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_R6 : 4|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_R7 : 5|1@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L1 : 6|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L2 : 18|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L3 : 30|12@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_ 69 Sensornode_Debug_2: 8 Sensornode_F
+   SG_ Sensornode_Debug_L4 : 0|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L5 : 12|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L6 : 24|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L7 : 36|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_L8 : 48|12@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_ 70 Sensornode_Debug_3: 8 Sensornode_F
+   SG_ Sensornode_Debug_R9 : 0|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_RA : 12|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_RB : 24|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_RC : 36|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_RD : 48|12@1+ (1,0) [0|1] "" Vector__XXX
+
+BO_ 71 Sensornode_Debug_4: 5 Sensornode_F
+   SG_ Sensornode_Debug_RE : 0|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_RF : 12|12@1+ (1,0) [0|1] "" Vector__XXX
+   SG_ Sensornode_Debug_R0 : 24|12@1+ (1,0) [0|1] "" Vector__XXX

Software/sensor-node.ioc

@@ -276,6 +276,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.TIM6_DAC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.TIM8_CC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA0.GPIOParameters=GPIO_Label
@@ -633,8 +634,8 @@ TIM4.Period=255
 TIM4.Prescaler=19
 TIM6.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_DISABLE
 TIM6.IPParameters=Prescaler,Period,TIM_MasterOutputTrigger,AutoReloadPreload
-TIM6.Period=2000-1
-TIM6.Prescaler=4800-1
+TIM6.Period=200-1
+TIM6.Prescaler=480-1
 TIM6.TIM_MasterOutputTrigger=TIM_TRGO_UPDATE
 TIM8.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
 TIM8.Channel-Input_Capture2_from_TI2=TIM_CHANNEL_2

pre-commit.sh

@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+
+[ ! find . -type f -iname "*.lck" ]