diff --git a/STM32/CM7/Core/Inc/global_state.h b/STM32/CM7/Core/Inc/global_state.h index 66e70dd..0840d00 100644 --- a/STM32/CM7/Core/Inc/global_state.h +++ b/STM32/CM7/Core/Inc/global_state.h @@ -3,10 +3,12 @@ // SPDX-License-Identifier: GPL-3.0-or-later #pragma once #include +#include #include #define MAX_CAM_MISS 2 #define SPARK_ADVANCE 20 +#define INJECTION_PHASE 0 typedef enum { SYNC_OK = 0, SYNC_PENDING = 1, SYNC_NOT_OK = 2 } sync_state_t; @@ -20,6 +22,12 @@ typedef enum { typedef enum { CAM_IDLE = 0, CAM_TRIGD = 1 } cam_state_t; +typedef enum { + SPARK_IDLE = 0, + SPARK_CHARGING = 1, + SPARK_NONE = 2 +} spark_state_t; + typedef struct { crank_state_t crank_state; cam_state_t cam_state; @@ -27,4 +35,6 @@ typedef struct { sync_state_t sync_state; ring_buffer_t crank_RB; ring_buffer_t cam_RB; + spark_state_t next_spark_state; + spark_state_t current_spark_state; } global_state_t; diff --git a/STM32/CM7/Core/Inc/macros.h b/STM32/CM7/Core/Inc/macros.h index f09fa86..07800bc 100644 --- a/STM32/CM7/Core/Inc/macros.h +++ b/STM32/CM7/Core/Inc/macros.h @@ -5,7 +5,6 @@ #include "global_state.h" #ifdef DEBUG #include "SEGGER_RTT.h" -#define INJECTION_PHASE 0 #define DEBUG_LOG(fmt, ...) SEGGER_RTT_printf(0, fmt "\n", ##__VA_ARGS__) #else #define DEBUG_LOG(fmt, ...) \ @@ -14,5 +13,3 @@ #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/main.c b/STM32/CM7/Core/Src/main.c index 12f8b09..4e560ce 100644 --- a/STM32/CM7/Core/Src/main.c +++ b/STM32/CM7/Core/Src/main.c @@ -11,9 +11,13 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ +#include "global_state.h" +#include "macros.h" #include "ring_buffer.h" +#include "stm32h7xx_hal_tim.h" #include "tasks.h" #include +#include #ifdef DEBUG #include "SEGGER_RTT.h" #endif @@ -101,6 +105,25 @@ extern void camHandler(void *argument); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ + +void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim) { + if (htim->Instance == TIM2 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3) { + if (state_g.next_spark_state == SPARK_IDLE && + state_g.current_spark_state == SPARK_CHARGING) { + DEBUG_LOG("Releasing spark"); + } else if (state_g.next_spark_state == SPARK_CHARGING && + state_g.current_spark_state == SPARK_IDLE) { + DEBUG_LOG("Charging spark"); + osThreadFlagsSet(crankTaskHandle, 0x02); + } + + if (state_g.next_spark_state != SPARK_NONE) { + state_g.current_spark_state = state_g.next_spark_state; + state_g.next_spark_state = SPARK_NONE; + } + } +} + void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) { if (htim->Instance == TIM2) { switch (htim->Channel) { @@ -164,6 +187,8 @@ int main(void) state_g.crank_state = CYCLE_UNKNOWN; state_g.cam_state = CAM_IDLE; state_g.cam_miss_ctr = 0; + state_g.next_spark_state = SPARK_NONE; + state_g.current_spark_state = SPARK_IDLE; /* USER CODE END Init */ /* Configure the system clock */ @@ -377,7 +402,7 @@ static void MX_TIM2_Init(void) { Error_Handler(); } - sConfigOC.OCMode = TIM_OCMODE_FORCED_INACTIVE; + sConfigOC.OCMode = TIM_OCMODE_TOGGLE; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; diff --git a/STM32/CM7/Core/Src/tasks/cam.c b/STM32/CM7/Core/Src/tasks/cam.c index d282aba..89b487b 100644 --- a/STM32/CM7/Core/Src/tasks/cam.c +++ b/STM32/CM7/Core/Src/tasks/cam.c @@ -4,9 +4,9 @@ // SPDX-License-Identifier: GPL-3.0-or-later #include "cmsis_os2.h" +#include "global_state.h" #include "macros.h" #include "main.h" -#include "global_state.h" #include "ring_buffer.h" #include "tasks.h" @@ -18,10 +18,10 @@ void camHandler(void *argument) { ringBufferRead(&state_g.cam_RB, 0)); // FILTERS - if (CAM_TRIGD) { - ringBufferRevert(&state_g.cam_RB,1); - return; - } + // if (CAM_TRIGD) { + // ringBufferRevert(&state_g.cam_RB,1); + // return; + // } state_g.cam_state = CAM_TRIGD; state_g.cam_miss_ctr = 0; switch (state_g.sync_state) { diff --git a/STM32/CM7/Core/Src/tasks/crank.c b/STM32/CM7/Core/Src/tasks/crank.c index 8eb3b14..dbe1932 100644 --- a/STM32/CM7/Core/Src/tasks/crank.c +++ b/STM32/CM7/Core/Src/tasks/crank.c @@ -24,11 +24,19 @@ 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; + if (state_g.sync_state == SYNC_OK) { + uint32_t d1a = CRANK(0) - CRANK(1); + uint32_t d1b = CRANK(1) - CRANK(2); + int32_t delta_percentage = + ((int64_t)d1a - (int64_t)d1b) * 100 / (int64_t)d1b; + DEBUG_LOG("%ld\n\r", delta_percentage); + if (delta_percentage > 40 || delta_percentage < -40) { + // state_g.sync_state = SYNC_NOT_OK; + // state_g.crank_state = CYCLE_UNKNOWN; + // ringBufferRevert(&state_g.crank_RB, 1); + state_g.crank_RB.w_head -= 1; + continue; + } } // INCREMENT SWITCH switch (state_g.crank_state) { @@ -71,19 +79,39 @@ void crankHandler(void *argument) { } // SPARK SCHEDULE - if (state_g.crank_state == CYCLE_COMPRESSION && - state_g.sync_state == SYNC_OK) { - DEBUG_LOG("Spark schedule reached, congrats\n\r"); - uint32_t d1a = CRANK(0) - CRANK(1); - // TODO: Map interpolation rather than SPARK_ADVANCE CONSTANT - uint32_t d_spark = d1a * (45 - SPARK_ADVANCE) / 180; - uint32_t t_spark = CRANK(0) + d_spark; - // TODO: schedule spark - // __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark); - // TIM2_CH3_SetOCMode(t_spark); - } else if (state_g.crank_state == CYCLE_EXHAUST) { - uint32_t t_injection = CRANK(0) + (45+INJECTION_PHASE) * (CRANK(4) - CRANK(0)) / 720; - //TODO Schedule injection + if (state_g.sync_state == SYNC_OK) { + switch (state_g.crank_state) { + case CYCLE_COMPRESSION: { + DEBUG_LOG("Spark schedule reached, congrats\n\r"); + uint32_t d1a = CRANK(0) - CRANK(1); + // TODO: Map interpolation rather than SPARK_ADVANCE CONSTANT + uint32_t d_spark = d1a * (45 - SPARK_ADVANCE) / 180; + uint32_t t_spark = CRANK(0) + d_spark; + // TODO: schedule spark + if (state_g.current_spark_state != SPARK_CHARGING) { + uint32_t force_charge_time = __HAL_TIM_GET_COUNTER(&htim2) + 2000; + // TIM2_CH3_SetOCMode(TIM_OCMODE_ACTIVE); + __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, force_charge_time); + + state_g.next_spark_state = SPARK_CHARGING; + osThreadFlagsWait(0x02, osFlagsWaitAny, osWaitForever); + } + DEBUG_LOG("Spark scheduled for: %lu , current time: %lu\n\r", t_spark, + __HAL_TIM_GET_COUNTER(&htim2)); + // TIM2_CH3_SetOCMode(TIM_OCMODE_INACTIVE); + __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark); + state_g.next_spark_state = SPARK_IDLE; + break; + } + case CYCLE_EXHAUST: { + uint32_t t_injection = + CRANK(0) + (45 + INJECTION_PHASE) * (CRANK(4) - CRANK(0)) / 720; + // TODO: Schedule injection + break; + } + default: + break; + } } } } diff --git a/STM32/STM32.ioc b/STM32/STM32.ioc index 81170be..30bfa25 100644 --- a/STM32/STM32.ioc +++ b/STM32/STM32.ioc @@ -95,7 +95,8 @@ PA14\ (JTCK/SWCLK).GPIOParameters=PinAttribute PA14\ (JTCK/SWCLK).Locked=true PA14\ (JTCK/SWCLK).PinAttribute=CortexM7 PA14\ (JTCK/SWCLK).Signal=DEBUG_JTCK-SWCLK -PA2.GPIOParameters=PinAttribute +PA2.GPIOParameters=GPIO_Speed,PinAttribute +PA2.GPIO_Speed=GPIO_SPEED_FREQ_LOW PA2.Locked=true PA2.PinAttribute=CortexM7 PA2.Signal=S_TIM2_CH3 @@ -261,7 +262,7 @@ TIM2.ICFilter_CH1=10 TIM2.ICFilter_CH2=10 TIM2.ICPolarity_CH1=TIM_INPUTCHANNELPOLARITY_FALLING TIM2.IPParameters=ClockDivision,Prescaler,Channel-Input_Capture1_from_TI1,ICPolarity_CH1,Channel-Input_Capture2_from_TI2,ICFilter_CH2,ICFilter_CH1,Channel-Output Compare3 CH3,OCMode_3 -TIM2.OCMode_3=TIM_OCMODE_FORCED_INACTIVE +TIM2.OCMode_3=TIM_OCMODE_TOGGLE TIM2.Prescaler=0 VP_FREERTOS_M7_VS_CMSIS_V2.Mode=CMSIS_V2 VP_FREERTOS_M7_VS_CMSIS_V2.Signal=FREERTOS_M7_VS_CMSIS_V2