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;
+    }
 }
 

Core/Src/main.c

@@ -97,7 +97,7 @@ void ED_Delay(uint32_t Delay)
 
   while ((HAL_GetTick() - tickstart) < wait){
     CCS_SerialLoop();
-//    CP_Loop();
+    CP_Loop();
     CONN_Task();
     LED_Task();
     SC_Task();

Core/Src/serial.c

@@ -46,7 +46,6 @@ static uint8_t everest_timed_out = 0;
 static uint32_t last_everest_timeout_log_tick = 0;
 static uint32_t uart3_last_packet_tick = 0;
 static uint32_t uart3_last_reinit_tick = 0;
-static CP_State_t cp_state_buffer = EV_STATE_ACQUIRING;
 
 CCS_State_t CCS_State;
 CCS_EvInfo_t CCS_EvInfo;
@@ -144,9 +143,6 @@ void CCS_SerialLoop(void) {
 
     CCS_UART3_Watchdog();
 
-    /* Read CP once per loop and use buffered value below. */
-    cp_state_buffer = CP_GetState();
-
     if (CONN.connControl != CMD_NONE) {
         last_cmd = CONN.connControl;
     }