diff --git a/STM32/CM7/Core/Inc/macros.h b/STM32/CM7/Core/Inc/macros.h index 88518e7..d76b5da 100644 --- a/STM32/CM7/Core/Inc/macros.h +++ b/STM32/CM7/Core/Inc/macros.h @@ -2,7 +2,7 @@ // Copyright (C) 2026 Pierre Barbier // Copyright (C) 2026 Association Exergie // SPDX-License-Identifier: GPL-3.0-or-later - +#include "global_state.h" #ifdef DEBUG #include "SEGGER_RTT.h" #define DEBUG_LOG(fmt, ...) SEGGER_RTT_printf(0, fmt "\n", ##__VA_ARGS__) @@ -13,3 +13,5 @@ #endif #define CRANK(num) ringBufferRead(&state_g.crank_RB, num) #define CAM(num) ringBufferRead(&state_g.cam_RB, num) +#define D1A CRANK(0)-CRANK(1) +#define D1B CRANK(1)-CRANK(2) diff --git a/STM32/CM7/Core/Src/tasks/crank.c b/STM32/CM7/Core/Src/tasks/crank.c index 1ed9823..8d74158 100644 --- a/STM32/CM7/Core/Src/tasks/crank.c +++ b/STM32/CM7/Core/Src/tasks/crank.c @@ -4,6 +4,7 @@ // SPDX-License-Identifier: GPL-3.0-or-later #include "cmsis_os2.h" +#include "global_state.h" #include "macros.h" #include "main.h" #include "ring_buffer.h" @@ -16,6 +17,12 @@ void crankHandler(void *argument) { DEBUG_LOG("Crank pulse detected at: %lu\n\r", ringBufferRead(&state_g.crank_RB, 0)); // FILTER + float delta_percentage = ((float)(D1A - D1B)) / D1B; + if ( delta_percentage> 0.4||delta_percentage < -0.4) { + state_g.sync_state = SYNC_NOT_OK; + state_g.crank_state = CYCLE_UNKNOWN; + continue; + } // INCREMENT SWITCH switch (state_g.crank_state) { case CYCLE_COMPRESSION: {