118 lines
3.8 KiB
C
118 lines
3.8 KiB
C
// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
|
|
// Copyright (C) 2026 Pierre Barbier <pierrebarbier741@gmail.com>
|
|
// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
|
|
// 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"
|
|
#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
|
|
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) {
|
|
case CYCLE_COMPRESSION: {
|
|
if (state_g.cam_state != CAM_TRIGD) {
|
|
DEBUG_LOG("Cam miss detected, sync dynamite incoming\n\r");
|
|
state_g.cam_miss_ctr++;
|
|
if (state_g.cam_miss_ctr > MAX_CAM_MISS) {
|
|
state_g.sync_state = SYNC_NOT_OK;
|
|
state_g.crank_state = CYCLE_UNKNOWN;
|
|
break;
|
|
}
|
|
}
|
|
state_g.crank_state = CYCLE_COMBUSTION;
|
|
DEBUG_LOG("Crank state incremented\n\r");
|
|
state_g.cam_state = CAM_IDLE;
|
|
break;
|
|
}
|
|
case CYCLE_COMBUSTION:
|
|
case CYCLE_EXHAUST:
|
|
case CYCLE_INTAKE: {
|
|
if (state_g.cam_state == CAM_TRIGD) {
|
|
state_g.sync_state = SYNC_NOT_OK;
|
|
state_g.crank_state = CYCLE_UNKNOWN;
|
|
state_g.cam_state = CAM_IDLE;
|
|
break;
|
|
}
|
|
state_g.crank_state++;
|
|
DEBUG_LOG("Crank state incremented\n\r");
|
|
break;
|
|
}
|
|
case CYCLE_UNKNOWN: {
|
|
if (state_g.sync_state == SYNC_PENDING &&
|
|
state_g.cam_state == CAM_TRIGD) {
|
|
state_g.crank_state = CYCLE_COMBUSTION;
|
|
state_g.cam_state = CAM_IDLE;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
// SPARK SCHEDULE
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|