add cp filter

Core/Inc/cp.h

@@ -16,11 +16,15 @@ typedef enum {
     EV_STATE_ACQUIRING = 6,
 } CP_State_t;
 
+extern CP_State_t cp_state_buffer;
+
 void CP_Init(void);
 void CP_SetDuty(uint8_t percentage);
 uint8_t CP_GetDuty(void);
 int32_t CP_GetVoltage(void);
 CP_State_t CP_GetState(void);
+CP_State_t CP_GetFilteredState(void);
+void CP_FilterState(void);
 void CP_Loop(void);
 
 #endif

Core/Inc/main.h

@@ -43,7 +43,7 @@ extern "C" {
 /* USER CODE BEGIN EC */
 #define FW_VERSION_MAJOR 1
 #define FW_VERSION_MINOR 0
-#define FW_VERSION_PATCH 15
+#define FW_VERSION_PATCH 16
 /* USER CODE END EC */
 
 /* Exported macro ------------------------------------------------------------*/

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) {
+CP_State_t CP_GetFilteredState(void) {
-    (void)CP_GetState();
+    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;
     }

Debug/Core/Src/cp.cyclo

@@ -1,7 +1,9 @@
-../Core/Src/cp.c:16:16:CP_ReadVoltageMv	1
+../Core/Src/cp.c:19:16:CP_ReadVoltageMv	1
-../Core/Src/cp.c:33:6:CP_Init	1
+../Core/Src/cp.c:36:6:CP_Init	1
-../Core/Src/cp.c:50:6:CP_SetDuty	1
+../Core/Src/cp.c:53:6:CP_SetDuty	1
-../Core/Src/cp.c:63:9:CP_GetDuty	1
+../Core/Src/cp.c:66:9:CP_GetDuty	1
-../Core/Src/cp.c:67:9:CP_GetVoltage	1
+../Core/Src/cp.c:70:9:CP_GetVoltage	1
-../Core/Src/cp.c:72:12:CP_GetState	12
+../Core/Src/cp.c:75:12:CP_GetState	12
-../Core/Src/cp.c:96:6:CP_Loop	1
+../Core/Src/cp.c:99:12:CP_GetFilteredState	1
+../Core/Src/cp.c:103:6:CP_FilterState	5
+../Core/Src/cp.c:131:6:CP_Loop	4

Debug/Core/Src/serial.cyclo

@@ -1,18 +1,18 @@
-../Core/Src/serial.c:59:22:uart3_log_hal_error	3
+../Core/Src/serial.c:58:22:uart3_log_hal_error	3
-../Core/Src/serial.c:79:22:uart3_arm_rx_or_log	3
+../Core/Src/serial.c:78:22:uart3_arm_rx_or_log	3
-../Core/Src/serial.c:94:15:CCS_RxEventCallback	4
+../Core/Src/serial.c:93:15:CCS_RxEventCallback	4
-../Core/Src/serial.c:117:15:HAL_UART_ErrorCallback	5
+../Core/Src/serial.c:116:15:HAL_UART_ErrorCallback	5
-../Core/Src/serial.c:135:6:CCS_SerialLoop	43
+../Core/Src/serial.c:134:6:CCS_SerialLoop	43
-../Core/Src/serial.c:301:6:CCS_Init	1
+../Core/Src/serial.c:297:6:CCS_Init	1
-../Core/Src/serial.c:315:26:crc16_ibm	3
+../Core/Src/serial.c:311:26:crc16_ibm	3
-../Core/Src/serial.c:330:17:CCS_BuildPacket	4
+../Core/Src/serial.c:326:17:CCS_BuildPacket	4
-../Core/Src/serial.c:346:13:CCS_SendPacket	2
+../Core/Src/serial.c:342:13:CCS_SendPacket	2
-../Core/Src/serial.c:354:13:CCS_SendResetReason	1
+../Core/Src/serial.c:350:13:CCS_SendResetReason	1
-../Core/Src/serial.c:358:6:CCS_SendEmergencyStop	1
+../Core/Src/serial.c:354:6:CCS_SendEmergencyStop	1
-../Core/Src/serial.c:362:6:CCS_SendStart	1
+../Core/Src/serial.c:358:6:CCS_SendStart	1
-../Core/Src/serial.c:366:13:CCS_CalculateEnergy	2
+../Core/Src/serial.c:362:13:CCS_CalculateEnergy	2
-../Core/Src/serial.c:381:13:send_state	3
+../Core/Src/serial.c:377:13:send_state	3
-../Core/Src/serial.c:409:26:expected_payload_len	2
+../Core/Src/serial.c:405:26:expected_payload_len	2
-../Core/Src/serial.c:425:22:apply_command	13
+../Core/Src/serial.c:421:22:apply_command	13
-../Core/Src/serial.c:497:25:process_received_packet	8
+../Core/Src/serial.c:493:25:process_received_packet	8
-../Core/Src/serial.c:550:13:CCS_UART3_Watchdog	3
+../Core/Src/serial.c:546:13:CCS_UART3_Watchdog	3