Fixed critical ring buffer underflow bug

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 SPARK_ADVANCE 40
+#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/Inc/main.h

@@ -43,7 +43,7 @@ extern "C" {
 /* Exported constants --------------------------------------------------------*/
 /* USER CODE BEGIN EC */
 extern volatile global_state_t state_g;
-
+extern TIM_HandleTypeDef htim2;
 /* USER CODE END EC */
 
 /* Exported macro ------------------------------------------------------------*/
@@ -51,6 +51,8 @@ extern volatile global_state_t state_g;
 
 /* USER CODE END EM */
 
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+
 /* Exported functions prototypes ---------------------------------------------*/
 void Error_Handler(void);
 

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 */
@@ -330,6 +355,7 @@ static void MX_TIM2_Init(void)
   TIM_ClockConfigTypeDef sClockSourceConfig = {0};
   TIM_MasterConfigTypeDef sMasterConfig = {0};
   TIM_IC_InitTypeDef sConfigIC = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
 
   /* USER CODE BEGIN TIM2_Init 1 */
 
@@ -353,6 +379,10 @@ static void MX_TIM2_Init(void)
   {
     Error_Handler();
   }
+  if (HAL_TIM_OC_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
   sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
   sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
   if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
@@ -372,10 +402,20 @@ static void MX_TIM2_Init(void)
   {
     Error_Handler();
   }
+  sConfigOC.OCMode = TIM_OCMODE_TOGGLE;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_OC_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+  {
+    Error_Handler();
+  }
   /* USER CODE BEGIN TIM2_Init 2 */
   HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_1);
   HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2);
+  HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_3);
   /* USER CODE END TIM2_Init 2 */
+  HAL_TIM_MspPostInit(&htim2);
 
 }
 

STM32/CM7/Core/Src/ring_buffer.c

@@ -5,6 +5,7 @@
 #include <assert.h>
 #include <ring_buffer.h>
 #include <stdbool.h>
+#include <stdint.h>
 
 void ringBufferPush(volatile ring_buffer_t *rb, uint32_t value) {
   rb->buffer[rb->w_head] = value;
@@ -13,7 +14,8 @@ void ringBufferPush(volatile ring_buffer_t *rb, uint32_t value) {
 }
 
 uint32_t ringBufferRead(volatile ring_buffer_t *rb, uint8_t neg_idx) {
-  return rb->buffer[rb->w_head - 1 - neg_idx];
+  uint8_t idx = (uint8_t)(rb->w_head - 1u - neg_idx);
+  return rb->buffer[idx];
 }
 
 void ringBufferRevert(volatile ring_buffer_t *rb, uint8_t val) {

STM32/CM7/Core/Src/stm32h7xx_hal_msp.c

@@ -56,7 +56,9 @@
 /* USER CODE BEGIN 0 */
 
 /* USER CODE END 0 */
-/**
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+                    /**
   * Initializes the Global MSP.
   */
 void HAL_MspInit(void)
@@ -117,6 +119,32 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
 
 }
 
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(htim->Instance==TIM2)
+  {
+    /* USER CODE BEGIN TIM2_MspPostInit 0 */
+
+    /* USER CODE END TIM2_MspPostInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM2 GPIO Configuration
+    PA2     ------> TIM2_CH3
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_2;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    /* USER CODE BEGIN TIM2_MspPostInit 1 */
+
+    /* USER CODE END TIM2_MspPostInit 1 */
+  }
+
+}
 /**
   * @brief TIM_Base MSP De-Initialization
   * This function freeze the hardware resources used in this example
@@ -134,10 +162,11 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
     __HAL_RCC_TIM2_CLK_DISABLE();
 
     /**TIM2 GPIO Configuration
+    PA2     ------> TIM2_CH3
     PA1     ------> TIM2_CH2
     PA0     ------> TIM2_CH1
     */
-    HAL_GPIO_DeInit(GPIOA, TIM2_CAM_Pin|TIM2_CRANK_Pin);
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_2|TIM2_CAM_Pin|TIM2_CRANK_Pin);
 
     /* TIM2 interrupt DeInit */
     HAL_NVIC_DisableIRQ(TIM2_IRQn);

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) {

STM32/CM7/Core/Src/tasks/crank.c

@@ -8,20 +8,35 @@
 #include "macros.h"
 #include "main.h"
 #include "ring_buffer.h"
+#include "stm32h747xx.h"
+#include "stm32h7xx.h"
+#include "stm32h7xx_hal_tim.h"
 #include "tasks.h"
 #include <stdint.h>
 
+static inline void TIM2_CH3_SetOCMode(uint32_t oc_mode) {
+  MODIFY_REG(TIM2->CCMR2, TIM_CCMR2_OC3M | TIM_CCMR2_OC3M_3, oc_mode);
+}
+
 void crankHandler(void *argument) {
   for (;;) {
     osThreadFlagsWait(0x01, osFlagsWaitAny, osWaitForever);
     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;
+    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) {
@@ -64,17 +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
-    } 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;
+      }
     }
   }
 }

STM32/STM32.ioc

@@ -33,18 +33,19 @@ Mcu.Name=STM32H747XIHx
 Mcu.Package=TFBGA240
 Mcu.Pin0=PA14 (JTCK/SWCLK)
 Mcu.Pin1=PC15-OSC32_OUT (OSC32_OUT)
-Mcu.Pin10=VP_SYS_VS_Systick
-Mcu.Pin11=VP_SYS_M4_VS_Systick
-Mcu.Pin12=VP_TIM2_VS_ClockSourceINT
+Mcu.Pin10=VP_FREERTOS_M7_VS_CMSIS_V2
+Mcu.Pin11=VP_SYS_VS_Systick
+Mcu.Pin12=VP_SYS_M4_VS_Systick
+Mcu.Pin13=VP_TIM2_VS_ClockSourceINT
 Mcu.Pin2=PC14-OSC32_IN (OSC32_IN)
 Mcu.Pin3=PA13 (JTMS/SWDIO)
 Mcu.Pin4=PI12
 Mcu.Pin5=PH1-OSC_OUT (PH1)
 Mcu.Pin6=PH0-OSC_IN (PH0)
-Mcu.Pin7=PA1
-Mcu.Pin8=PA0
-Mcu.Pin9=VP_FREERTOS_M7_VS_CMSIS_V2
-Mcu.PinsNb=13
+Mcu.Pin7=PA2
+Mcu.Pin8=PA1
+Mcu.Pin9=PA0
+Mcu.PinsNb=14
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32H747XIHx
@@ -94,6 +95,11 @@ PA14\ (JTCK/SWCLK).GPIOParameters=PinAttribute
 PA14\ (JTCK/SWCLK).Locked=true
 PA14\ (JTCK/SWCLK).PinAttribute=CortexM7
 PA14\ (JTCK/SWCLK).Signal=DEBUG_JTCK-SWCLK
+PA2.GPIOParameters=GPIO_Speed,PinAttribute
+PA2.GPIO_Speed=GPIO_SPEED_FREQ_LOW
+PA2.Locked=true
+PA2.PinAttribute=CortexM7
+PA2.Signal=S_TIM2_CH3
 PC14-OSC32_IN\ (OSC32_IN).GPIOParameters=PinAttribute
 PC14-OSC32_IN\ (OSC32_IN).Locked=true
 PC14-OSC32_IN\ (OSC32_IN).Mode=LSE-External-Oscillator
@@ -245,14 +251,18 @@ SH.S_TIM2_CH1_ETR.0=TIM2_CH1,Input_Capture1_from_TI1
 SH.S_TIM2_CH1_ETR.ConfNb=1
 SH.S_TIM2_CH2.0=TIM2_CH2,Input_Capture2_from_TI2
 SH.S_TIM2_CH2.ConfNb=1
+SH.S_TIM2_CH3.0=TIM2_CH3,Output Compare3 CH3
+SH.S_TIM2_CH3.ConfNb=1
 SYS.userName=SYS_M7
 TIM2.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
 TIM2.Channel-Input_Capture2_from_TI2=TIM_CHANNEL_2
+TIM2.Channel-Output\ Compare3\ CH3=TIM_CHANNEL_3
 TIM2.ClockDivision=TIM_CLOCKDIVISION_DIV4
 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
+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_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

docs/ARCHITECTURE.md

@@ -0,0 +1,34 @@
+# NeoECU Firmware Architecture
+
+## AMP
+NeoECU features an AMP (Asymmetric Multi Processing) architecture making use of both the `CM7` and `CM4` core of the STM32H747 MCU powering NeoECU
+
+### CM7
+The Cortex M7 is in charge of initial boot and more importantly all of the engine control. The core uses the following software stack:
+- STM32_HAL
+- FreeRTOS
+- CMSISv2
+This fairly low-level software stack, paired with strict coding rules, allows for stable and deterministic engine control. The key coding rules are:
+- No heap allocation
+- Pointer indirection limited to one level
+Further coding rules, guidelines and conventions can be found in CODING.md
+
+### CM4
+The Cortex M4 handles all of the telemetry, and other vehicle functions. It will be build around ZephyrRTOS to allow for a higher level of abstraction and a complete I/O stack out of the box. The code written for this core will not follow the same rules as for the CM7 as anything run on the CM4 should be non critical to core engine function.
+
+## Engine Control
+As mentioned previously, all core engine control is handled on the Cortex M7, this is to isolate critical engine control from heavier telemetry operations that are non critical to core engine function.
+
+### Input Signals
+The engine provides two key input signals:
+- Crank pulse, triggered every 180 deg
+- Cam pulse, triggered every 720 deg and slightly dephased
+These are both registered using a hardware interrupt using `TIM2` as a low jitter time source.
+These signals can have missing pulses or extra triggers which need to be accounted for.
+
+### Time Base
+`TIM2` is the main timebase used to control the engine, it provides 4 channels that are used as follows:
+- CH_1: Crank input compare
+- CH_2: Cam input compare
+- CH_3: Undefined
+- CH_4: Undefined

docs/CODING.md

@@ -0,0 +1,20 @@
+# NeoECU Firmware Coding Guidelines
+
+## Naming Conventions
+Functions : camelCase
+Variables : python_case
+Constants : UPPER_CASE
+Macros : UPPER_CASE
+Types : python_case_t
+
+## Performance Standard
+Don't use heap at all if possible avoid mallocs at all cost.
+Don't make heavy computation in ISR delegate everything to a task that you'll wake.
+Don't copy big chunks of memory, use pointers if you need to read a buffer, even if simply passing the buffer as a parameter compiles it will have huge overhead.
+
+## Readability Standard
+Avoid more than one layer of pointer indirection if possible.
+Don't use void* they are unclear and make mistakes more likely.
+Avoid magic numbers and indexes, use macros and constants to define them clearly.
+Define all macros in macros.h and all global variables and constants in glocal_state.h.
+Use one c file per feature, avoid big monoliths that become hard to jump around.