add cp filter

Core/Src/cp.c

@@ -1,15 +1,18 @@
 #include "cp.h"
 #include "adc.h"
 #include "board.h"
+#include "debug.h"
 #include "tim.h"
 #include <stdint.h>
 #include <stdlib.h>
 
 #define MAX_DUTY 450
+#define FILTER_ORDER 100
 
 static int32_t cp_voltage_mv = 0;
 static uint8_t cp_duty = 0;
 CP_State_t fake_cp_state = EV_STATE_ACQUIRING;
+CP_State_t cp_state_buffer = EV_STATE_ACQUIRING;
 
 #define VREFINT_CAL_ADDR ((uint16_t*)0x1FFFF7BA) // для STM32F1!
 
@@ -93,7 +96,63 @@ CP_State_t CP_GetState(void) {
     }
 }
 
-void CP_Loop(void) {
-    (void)CP_GetState();
+CP_State_t CP_GetFilteredState(void) {
+    return cp_state_buffer;
+}
+
+void CP_FilterState(void) {
+
+    static CP_State_t pending_state = EV_STATE_ACQUIRING;
+    static uint8_t stable_count = 0u;
+    CP_State_t current_state = CP_GetState();
+
+    /* Keep last accepted state while CP is still acquiring. */
+    if (current_state == EV_STATE_ACQUIRING) {
+        pending_state = EV_STATE_ACQUIRING;
+        stable_count = 0u;
+        return;
+    }
+
+    if (current_state != pending_state) {
+        pending_state = current_state;
+        stable_count = 1u;
+        return;
+    }
+
+    if (stable_count < FILTER_ORDER) {
+        stable_count++;
+    }
+
+    if (stable_count >= FILTER_ORDER) {
+        cp_state_buffer = pending_state;
+    }
+}
+
+void CP_Loop(void) {
+    static uint8_t initialized = 0;
+    static CP_State_t prev_state = EV_STATE_ACQUIRING;
+    static uint8_t prev_duty = 0;
+
+    CP_FilterState();
+
+    CP_State_t current_state = CP_GetFilteredState();
+    uint8_t current_duty = cp_duty;
+
+    if (!initialized) {
+        prev_state = current_state;
+        prev_duty = current_duty;
+        initialized = 1;
+        return;
+    }
+
+    if (current_state != prev_state) {
+        log_printf(LOG_INFO, "CP state changed: %d -> %d\n", prev_state, current_state);
+        prev_state = current_state;
+    }
+
+    if (current_duty != prev_duty) {
+        log_printf(LOG_INFO, "CP duty changed: %u -> %u\n", prev_duty, current_duty);
+        prev_duty = current_duty;
+    }
 }