From bfc0eb2276406d8aea63ecf0b0df5af34cba1c44 Mon Sep 17 00:00:00 2001
From: Karlsson Winkels <k.winkels@fasttube.de>
Date: Wed, 4 Jun 2025 16:39:44 +0200
Subject: [PATCH] added filtering, changed control flow (untested)

---
 Software/Core/Src/main.c | 60 ++++++++++++++++++++++++++--------------
 1 file changed, 39 insertions(+), 21 deletions(-)

diff --git a/Software/Core/Src/main.c b/Software/Core/Src/main.c
index a76b745..08b8d59 100644
--- a/Software/Core/Src/main.c
+++ b/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 */
@@ -50,16 +51,18 @@
 /* Private variables ---------------------------------------------------------*/
 
 /* USER CODE BEGIN PV */
-unsigned int mscounter;
-unsigned int setup_complete;
+unsigned int tx_counter;
+bool adc_conv_cplt;
 
 FDCAN_HandleTypeDef *hMainCAN, *hPeriCAN;
 FDCAN_TxHeaderTypeDef txHeader;
 
-/* Declare buffer in D1 domain SRAM */
+// 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};
 
@@ -78,12 +81,9 @@ static void MX_NVIC_Init(void);
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
 
-void loop_1kHz() {
+void send_latest_can() {
 
-  if (!setup_complete)
-    return;
-
-  mscounter++;
+  tx_counter++;
 
   for (int di = 0; di < NUM_DIO_PINS; di++) {
     dio_values[di] = HAL_GPIO_ReadPin(
@@ -99,7 +99,7 @@ void loop_1kHz() {
     if (pktinfo->num_signals < 0)
       continue;
 
-    if (mscounter % pktinfo->period == 0) {
+    if (tx_counter % pktinfo->period == 0) {
 
       txHeader.Identifier = pktinfo->can_id;
       txHeader.DataLength = pktinfo->dlc;
@@ -115,7 +115,7 @@ void loop_1kHz() {
             break;
 
           case AIN:
-            value = signal->factor * adc_values[signal->channel];
+            value = signal->factor * filtered_values[signal->channel];
             break;
 
           case FIN:
@@ -137,13 +137,25 @@ void loop_1kHz() {
 
     }
 
-    if (mscounter >= 500) {
-      mscounter = 0;
+    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 */
@@ -242,8 +254,7 @@ int main(void)
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_1);
   HAL_TIM_IC_Start_IT(&htim8, TIM_CHANNEL_2);
 
-  mscounter = 0;
-  setup_complete = 1;
+  tx_counter = 0;
 
   /* USER CODE END 2 */
 
@@ -251,11 +262,18 @@ int main(void)
   /* USER CODE BEGIN WHILE */
   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 */
-    //HAL_SuspendTick();
-    //HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
   }
   /* USER CODE END 3 */
 }
@@ -333,13 +351,13 @@ static void MX_NVIC_Init(void)
 }
 
 /* USER CODE BEGIN 4 */
-/*void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
+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) {
 
@@ -348,7 +366,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
   if (htim != &htim6)
     return;
 
-  loop_1kHz();
+  // TODO: Make timer 10kHz and dont trigger a function with it, for exiting WFI in main
 
 };