Try removing a lot of noise to fix input capture readings (#675)

* Try removing a lot of noise

* try to fix compile errors

* try to fix compile errors part 2

* compile errors...

* fix compile errors

* remove extra return statement

* applied formatter

* add changeset

---------

Co-authored-by: Jorge <jorgecaru3@gmail.com>
Co-authored-by: Jorge Sáez <jorgeesg82@gmail.com>

Author: 3 people

.changesets/fix-ic-noise.md

@@ -0,0 +1,2 @@
+release: patch
+summary: try to fix input capture by reducing noise in interrupt callback and add hardware filtering when configuring the input capture

Inc/HALAL/Models/TimerDomain/TimerDomain.hpp

@@ -218,6 +218,20 @@ enum class PWM_Frequency_Mode {
     SPEED,
 };
 
+struct InputCaptureInfo {
+    uint8_t channel_rising;
+    uint8_t channel_falling;
+
+    uint32_t value_rising;
+    uint32_t value_falling;
+    uint32_t period;
+
+    float duty_cycle;
+    uint32_t frequency;
+};
+
+static ST_LIB::InputCaptureInfo input_capture_info_dummy;
+
 constexpr std::array<uint8_t, 25> create_timer_idxmap() {
     std::array<uint8_t, 25> result{};
 
@@ -267,17 +281,6 @@ struct TimerDomain {
     static constexpr std::size_t max_instances = 16;
     static constexpr std::size_t input_capture_channels = 4;
 
-    struct InputCaptureInfo {
-        uint8_t channel_rising;
-        uint8_t channel_falling;
-
-        uint32_t value_rising;
-        uint32_t value_falling;
-        uint32_t period;
-
-        float duty_cycle;
-        uint32_t frequency;
-    };
     /* 2x as big as necessary but this makes indexing easier & faster */
     static InputCaptureInfo* input_capture_info[max_instances][input_capture_channels];
     static InputCaptureInfo input_capture_info_backing[max_instances][input_capture_channels];
@@ -746,6 +749,24 @@ struct TimerDomain {
             Scheduler_global_timer = cmsis_timers[timer_idxmap[SCHEDULER_TIMER_DOMAIN]];
             rcc_enable_timer(Scheduler_global_timer);
 
+            input_capture_info_dummy = ST_LIB::InputCaptureInfo{
+                .channel_rising = 0xFF,  // any value that isn't possible here
+                .channel_falling = 0xFF, // any value that isn't possible here
+
+                .value_rising = 0,
+                .value_falling = 0,
+                .period = 0,
+
+                .duty_cycle = 0,
+                .frequency = 0,
+            };
+
+            for (uint32_t inst = 0; inst < TimerDomain::max_instances; inst++) {
+                for (uint32_t ch = 0; ch < TimerDomain::input_capture_channels; ch++) {
+                    input_capture_info[inst][ch] = &input_capture_info_dummy;
+                }
+            }
+
             TimerDomain::callbacks[0] = TIM_Default_Callback;
             TimerDomain::callbacks[1] = TIM_Default_Callback;
             TimerDomain::callbacks[2] = TIM_Default_Callback;

Inc/HALAL/Services/InputCapture/InputCapture.hpp

@@ -26,7 +26,7 @@ class InputCapture {
     friend struct TimerWrapper<dev>;
 
     TimerWrapper<dev>* timer = nullptr;
-    TimerDomain::InputCaptureInfo* info = nullptr;
+    ST_LIB::InputCaptureInfo* info = nullptr;
     bool is_on = false;
     bool is_initialized = false;
     InputCapture(TimerWrapper<dev>* tim) : timer(tim) {}
@@ -59,7 +59,8 @@ class InputCapture {
             .ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING,
             .ICSelection = TIM_ICSELECTION_DIRECTTI,
             .ICPrescaler = TIM_ICPSC_DIV1,
-            .ICFilter = 0,
+            /* filter out with hardware some noise */
+            .ICFilter = 0x03,
         };
         timer->template config_input_compare_channel<pin_rising.channel>(&sConfigIC);
 
@@ -73,7 +74,6 @@ class InputCapture {
     void turn_on(void) {
         if (is_initialized == false) {
             init();
-            return;
         }
         if (is_on)
             return;

Src/HALAL/Models/TimerDomain/TimerDomain.cpp

@@ -24,27 +24,26 @@ TIM_HandleTypeDef htim24;
 void (*TimerDomain::callbacks[TimerDomain::max_instances])(void*) = {nullptr};
 void* TimerDomain::callback_data[TimerDomain::max_instances] = {nullptr};
 
-TimerDomain::InputCaptureInfo input_capture_info_dummy = {
-    .channel_rising = 0xFF,  // any value that isn't possible here
-    .channel_falling = 0xFF, // any value that isn't possible here
-};
-
-TimerDomain::InputCaptureInfo* TimerDomain::input_capture_info[max_instances]
-                                                              [input_capture_channels] = {
-                                                                  {&input_capture_info_dummy}
-};
-TimerDomain::InputCaptureInfo TimerDomain::input_capture_info_backing[max_instances]
-                                                                     [input_capture_channels];
+ST_LIB::InputCaptureInfo* TimerDomain::input_capture_info[max_instances][input_capture_channels];
+ST_LIB::InputCaptureInfo TimerDomain::input_capture_info_backing[max_instances]
+                                                                [input_capture_channels];
 
 static void TIM_IC_CaptureCallback(const uint32_t timer_idx, uint32_t channel) {
     TIM_HandleTypeDef* htim = TimerDomain::hal_handles[timer_idx];
 
-    TimerDomain::InputCaptureInfo* info = TimerDomain::input_capture_info[timer_idx][channel];
+    ST_LIB::InputCaptureInfo* info = TimerDomain::input_capture_info[timer_idx][channel];
+    uint32_t current = (*(((volatile uint32_t*)&htim->Instance->CCR1) + channel));
     if (info->channel_rising == channel) {
         // NOTE: CCR1 - CCR4 are contiguous
         // NOTE: CCxIF flag is cleared by software by reading the captured data in CCRx
-        uint32_t current = (*(((volatile uint32_t*)&htim->Instance->CCR1) + channel));
-        uint32_t period = current - info->value_rising;
+        uint32_t period;
+        if (current >= info->value_rising) {
+            period = current - info->value_rising;
+        } else {
+            // counter wrap around
+            uint32_t max_count = htim->Instance->ARR - 1;
+            period = (current + max_count) - info->value_rising;
+        }
 
         if ((period != 0) && (info->value_falling < period)) {
             uint32_t ref_clock =
@@ -55,8 +54,14 @@ static void TIM_IC_CaptureCallback(const uint32_t timer_idx, uint32_t channel) {
         }
         info->value_rising = current;
     } else if (info->channel_falling == channel) {
-        uint32_t falling_value =
-            *(((volatile uint32_t*)&htim->Instance->CCR1) + channel) - info->value_rising;
+        uint32_t falling_value;
+        if (current >= info->value_rising) {
+            falling_value = current - info->value_rising;
+        } else {
+            uint32_t max_count = htim->Instance->ARR - 1;
+            falling_value = (current + max_count) - info->value_rising;
+        }
+
         if (falling_value < info->period)
             info->value_falling = falling_value;
     } else [[unlikely]] {