Broken spark implementation

STM32/CM7/Core/Inc/global_state.h

@@ -3,10 +3,12 @@
 // SPDX-License-Identifier: GPL-3.0-or-later
 #pragma once
 #include <ring_buffer.h>
+#include <stdbool.h>
 #include <stdint.h>
 
 #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;

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)

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 <assert.h>
+#include <stdbool.h>
 #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;

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) {
+    // if (CAM_TRIGD) {
-        ringBufferRevert(&state_g.cam_RB,1);
+    //     ringBufferRevert(&state_g.cam_RB,1);
-        return;
+    //     return;
-    }
+    // }
     state_g.cam_state = CAM_TRIGD;
     state_g.cam_miss_ctr = 0;
     switch (state_g.sync_state) {

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 (state_g.sync_state == SYNC_OK) {
-    if (delta_percentage > 0.4 || delta_percentage < -0.4) {
+      uint32_t d1a = CRANK(0) - CRANK(1);
-      state_g.sync_state = SYNC_NOT_OK;
+      uint32_t d1b = CRANK(1) - CRANK(2);
-      state_g.crank_state = CYCLE_UNKNOWN;
+      int32_t delta_percentage =
-      continue;
+          ((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 &&
+    if (state_g.sync_state == SYNC_OK) {
-        state_g.sync_state == SYNC_OK) {
+      switch (state_g.crank_state) {
-      DEBUG_LOG("Spark schedule reached, congrats\n\r");
+      case CYCLE_COMPRESSION: {
-      uint32_t d1a = CRANK(0) - CRANK(1);
+        DEBUG_LOG("Spark schedule reached, congrats\n\r");
-      // TODO: Map interpolation rather than SPARK_ADVANCE CONSTANT
+        uint32_t d1a = CRANK(0) - CRANK(1);
-      uint32_t d_spark = d1a * (45 - SPARK_ADVANCE) / 180;
+        // TODO: Map interpolation rather than SPARK_ADVANCE CONSTANT
-      uint32_t t_spark = CRANK(0) + d_spark;
+        uint32_t d_spark = d1a * (45 - SPARK_ADVANCE) / 180;
-      // TODO: schedule spark
+        uint32_t t_spark = CRANK(0) + d_spark;
-      // __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, t_spark);
+        // TODO: schedule spark
-      // TIM2_CH3_SetOCMode(t_spark);
+        if (state_g.current_spark_state != SPARK_CHARGING) {
-    } else if (state_g.crank_state == CYCLE_EXHAUST) {
+          uint32_t force_charge_time = __HAL_TIM_GET_COUNTER(&htim2) + 2000;
-        uint32_t t_injection = CRANK(0) + (45+INJECTION_PHASE) * (CRANK(4) - CRANK(0)) / 720;
+          // TIM2_CH3_SetOCMode(TIM_OCMODE_ACTIVE);
-        //TODO Schedule injection
+          __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;
+      }
     }
   }
 }

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