/* injector_controller.c
STM32 HAL-based sequential / batch injector controller with TPS divider handling.
Integrated CID authoritative phase lock: Bank A first after CID, then B, C, ...
-------------------------------------------------------------------------------
Requirements (CubeMX):
- htim_us : free-running @1 MHz (micros())
- htim_res : basic TIM for µs one-shot residual scheduling (interrupt)
- htim_pw : basic TIM for µs one-shot PW off scheduling (interrupt)
- hadc1 : ADC1 with TPS channel configured
- EXTI : CRK_PIN and CID_PIN EXTI interrupts enabled
-------------------------------------------------------------------------------
*/
#include "stm32f1xx_hal.h"
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
// ----------------- User-configurable pin / peripheral mapping -----------------
#define INJ_A_PORT GPIOA
#define INJ_A_PIN GPIO_PIN_0
#define INJ_B_PORT GPIOA
#define INJ_B_PIN GPIO_PIN_1
#define INJ_C_PORT GPIOA
#define INJ_C_PIN GPIO_PIN_2
#define TPS_ADC_CHANNEL ADC_CHANNEL_3 // PA3 (through divider)
#define CID_PIN GPIO_PIN_3 // PB3 (cam / cylinder ID)
#define CID_PORT GPIOB
#define CRK_PIN GPIO_PIN_4 // PB4 (crank tooth)
#define CRK_PORT GPIOB
// TIM / ADC handles (configure in CubeMX and define in main.c)
extern TIM_HandleTypeDef htim_us; // free-running @1 MHz -> micros()
extern TIM_HandleTypeDef htim_res; // residual wait one-shot (µs)
extern TIM_HandleTypeDef htim_pw; // injector pulse width off timer (µs)
extern ADC_HandleTypeDef hadc1; // ADC1 configured for TPS channel
// ----------------- Divider / ADC constants -----------------
#define ADC_MAX 4095.0f
#define VREF 3.3f
// Divider: Rtop=12k, Rbot=18k -> ADC sees 0..3.0V for TPS 0..5.0V
#define DIV_RATIO ((12.0f + 18.0f) / 18.0f) // 30/18 = 1.6666667
// ----------------- Other Constants -----------------
#define TEETH_PER_REV 58
#define TEETH_PER_720 (TEETH_PER_REV * 2) // 116
#define DEG_PER_TOOTH (360.0f / (float)TEETH_PER_REV) // ~6.2068966
#define STARTUP_TIME_MS 120000U // 2 minutes
#define STARTUP_PW_US 5000U // 5 ms = 5000 us during startup/batch mode
#define MIN_PW_US 200U // safety minimum
#define MAX_PW_US 10000U // safety max
#define MIN_RESIDUAL_US 10U // ignore micro waits below this
// CID sync loss timeout (ms) — if no CID seen in this time, clear cid_synced
#define CID_LOSS_TIMEOUT_MS 2000U
// Bank angles (deg) relative to CID / 0 point
static const float bank_angles_deg[3] = { 0.0f, 240.0f, 480.0f };
// TPS map (voltage in V -> pulse width in ms) 100 points
#define TPS_BINS_N 100
static const float tps_volts[TPS_BINS_N] = {
0.050,0.100,0.150,0.200,0.250,0.300,0.350,0.400,0.450,0.500,
0.550,0.600,0.650,0.700,0.750,0.800,0.850,0.900,0.950,1.000,
1.050,1.100,1.150,1.200,1.250,1.300,1.350,1.400,1.450,1.500,
1.550,1.600,1.650,1.700,1.750,1.800,1.850,1.900,1.950,2.000,
2.050,2.100,2.150,2.200,2.250,2.300,2.350,2.400,2.450,2.500,
2.550,2.600,2.650,2.700,2.750,2.800,2.850,2.900,2.950,3.000,
3.050,3.100,3.150,3.200,3.250,3.300,3.350,3.400,3.450,3.500,
3.550,3.600,3.650,3.700,3.750,3.800,3.850,3.900,3.950,4.000,
4.050,4.100,4.150,4.200,4.250,4.300,4.350,4.400,4.450,4.500,
4.550,4.600,4.650,4.700,4.750,4.800,4.850,4.900,4.950
};
static const float tps_pw_ms[TPS_BINS_N] = {
1.5,1.5,1.6,1.6,1.7,1.7,1.8,1.8,1.9,1.9,
2.0,2.0,2.1,2.1,2.2,2.2,2.3,2.3,2.4,2.4,
2.5,2.5,2.6,2.6,2.7,2.7,2.8,2.8,2.9,2.9,
3.0,3.0,3.1,3.1,3.2,3.2,3.3,3.3,3.4,3.4,
3.5,3.5,3.6,3.6,3.7,3.7,3.8,3.8,3.9,3.9,
4.0,4.0,4.1,4.1,4.2,4.2,4.3,4.3,4.4,4.4,
4.5,4.5,4.6,4.6,4.7,4.7,4.8,4.8,4.9,4.9,
5.0,5.0,5.1,5.1,5.2,5.2,5.3,5.3,5.4,5.4,
5.5,5.5,5.6,5.6,5.7,5.7,5.8,5.8,5.9,5.9,
6.0,6.0,6.1,6.1,6.2,6.2,6.3,6.3,6.4,6.4
};
// ----------------- Runtime / state -----------------
volatile uint32_t tooth_in_rev = 0; // 0..57 counted between missing-gap resync
volatile int toothAfterCID = 0; // 0..115 (counts teeth since last CID)
volatile bool cid_seen = false;
volatile uint32_t last_cid_us = 0;
volatile uint32_t rev_time_us = 80000; // measured 1 rev time (360°) in microseconds; initial guess
volatile float time_per_degree_us = 222.222f; // rev_time_us/360
int bank_integer_tooth[3]; // integer tooth indices after CID (0..115)
uint32_t bank_residual_us[3]; // fractional wait in microseconds
uint32_t bank_pulse_us[3]; // during sequential mode from TPS map
volatile bool sequential_mode = false;
// scheduling helpers
volatile int scheduled_bank_for_residual = -1; // which bank is waiting in residual timer
volatile int scheduled_bank_for_pw_off = -1; // which bank to turn OFF on pw timer
volatile bool startup_batch_active = false; // true when we've fired all injectors in startup and waiting to turn all off
// ----------------- Extra sequencing state -----------------
volatile bool cid_synced = false; // set true once CID reference has been used to align banks
volatile int expected_next_bank = 0; // 0=A,1=B,2=C : which bank is allowed next when cid_synced==true
// CID loss watchdog
static uint32_t last_cid_tick_ms = 0;
// ----------------- Helpers -----------------
static inline uint32_t micros(void) {
return __HAL_TIM_GET_COUNTER(&htim_us);
}
// linear interpolate TPS table to get pulse width in microseconds
static uint32_t get_pw_from_tps_voltage(float v) {
if (v <= tps_volts[0]) return (uint32_t)(tps_pw_ms[0]*1000.0f + 0.5f);
if (v >= tps_volts[TPS_BINS_N-1]) return (uint32_t)(tps_pw_ms[TPS_BINS_N-1]*1000.0f + 0.5f);
int i;
for (i=0;i<TPS_BINS_N-1;i++){
if (v <= tps_volts[i+1]) break;
}
float t = (v - tps_volts[i]) / (tps_volts[i+1] - tps_volts[i]);
float pw_ms = tps_pw_ms[i] + t * (tps_pw_ms[i+1] - tps_pw_ms[i]);
uint32_t pw_us = (uint32_t)(pw_ms * 1000.0f + 0.5f);
if (pw_us < MIN_PW_US) pw_us = MIN_PW_US;
if (pw_us > MAX_PW_US) pw_us = MAX_PW_US;
return pw_us;
}
// ----------------- TPS ADC read (with divider reconstruction) -----------------
static float read_tps_voltage(void) {
ADC_ChannelConfTypeDef sConfig = {0};
sConfig.Channel = TPS_ADC_CHANNEL;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES_5;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
HAL_ADC_Start(&hadc1);
if (HAL_ADC_PollForConversion(&hadc1, 10) == HAL_OK) {
uint32_t raw = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
float v_adc = ((float)raw / ADC_MAX) * VREF; // voltage at ADC pin (after divider)
float v_tps = v_adc * DIV_RATIO; // reconstruct TPS (0..5V)
return v_tps;
}
// fallback low value
return tps_volts[0];
}
// ----------------- Injector I/O -----------------
static inline void injector_on(int bank) {
if (bank==0) HAL_GPIO_WritePin(INJ_A_PORT, INJ_A_PIN, GPIO_PIN_SET);
else if (bank==1) HAL_GPIO_WritePin(INJ_B_PORT, INJ_B_PIN, GPIO_PIN_SET);
else HAL_GPIO_WritePin(INJ_C_PORT, INJ_C_PIN, GPIO_PIN_SET);
}
static inline void injector_off(int bank) {
if (bank==0) HAL_GPIO_WritePin(INJ_A_PORT, INJ_A_PIN, GPIO_PIN_RESET);
else if (bank==1) HAL_GPIO_WritePin(INJ_B_PORT, INJ_B_PIN, GPIO_PIN_RESET);
else HAL_GPIO_WritePin(INJ_C_PORT, INJ_C_PIN, GPIO_PIN_RESET);
}
static inline void injector_all_off(void) {
HAL_GPIO_WritePin(INJ_A_PORT, INJ_A_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(INJ_B_PORT, INJ_B_PIN, GPIO_PIN_RESET);
HAL_GPIO_WritePin(INJ_C_PORT, INJ_C_PIN, GPIO_PIN_RESET);
}
// ----------------- Scheduling math -----------------
static void compute_bank_targets_from_CID(void) {
float deg_per_tooth = DEG_PER_TOOTH;
for (int b=0;b<3;b++){
float angle = bank_angles_deg[b]; // 0/240/480
if (angle < 0.0f) while(angle<0.0f) angle += 720.0f;
if (angle >= 720.0f) angle = fmodf(angle, 720.0f);
float tooth_index = angle / deg_per_tooth; // fractional teeth e.g. 38.6667
int integer_tooth = (int)floorf(tooth_index + 1e-6f);
float frac = tooth_index - (float)integer_tooth;
float residual_deg = frac * deg_per_tooth;
uint32_t residual_us = (uint32_t)roundf(residual_deg * time_per_degree_us);
bank_integer_tooth[b] = integer_tooth;
bank_residual_us[b] = residual_us;
}
}
// ----------------- Timers / callbacks -----------------
static void start_residual_timer_us(uint32_t us, int bank) {
// guard: avoid double-scheduling same bank
if (scheduled_bank_for_residual == bank) return;
if (us < MIN_RESIDUAL_US) {
// immediate ON
scheduled_bank_for_residual = -1;
injector_on(bank);
// Advance expected_next_bank here only when injector actually turns ON in residual callback.
// But in this immediate-case we already turned it ON so advance now:
if (cid_synced) {
expected_next_bank = (expected_next_bank + 1) % 3;
}
scheduled_bank_for_pw_off = bank;
__HAL_TIM_SET_COUNTER(&htim_pw, 0);
__HAL_TIM_SET_AUTORELOAD(&htim_pw, bank_pulse_us[bank] - 1);
HAL_TIM_Base_Start_IT(&htim_pw);
return;
}
scheduled_bank_for_residual = bank;
__HAL_TIM_SET_COUNTER(&htim_res, 0);
__HAL_TIM_SET_AUTORELOAD(&htim_res, us - 1);
HAL_TIM_Base_Start_IT(&htim_res);
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
if (htim == &htim_res) {
HAL_TIM_Base_Stop_IT(&htim_res);
int b = scheduled_bank_for_residual;
scheduled_bank_for_residual = -1;
if (b >= 0 && b < 3) {
injector_on(b);
// Immediately advance expected_next_bank so next valid bank is allowed
if (cid_synced) {
expected_next_bank = (expected_next_bank + 1) % 3;
}
scheduled_bank_for_pw_off = b;
__HAL_TIM_SET_COUNTER(&htim_pw, 0);
__HAL_TIM_SET_AUTORELOAD(&htim_pw, bank_pulse_us[b] - 1);
HAL_TIM_Base_Start_IT(&htim_pw);
}
} else if (htim == &htim_pw) {
HAL_TIM_Base_Stop_IT(&htim_pw);
// Special-case: if startup_batch_active==true then turn ALL injectors off
if (startup_batch_active) {
injector_all_off();
startup_batch_active = false;
scheduled_bank_for_pw_off = -1;
return;
}
int b = scheduled_bank_for_pw_off;
scheduled_bank_for_pw_off = -1;
if (b >= 0 && b < 3) injector_off(b);
}
}
// ----------------- EXTI ISRs (Crank tooth + CID) -----------------
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
uint32_t now_us = micros();
if (GPIO_Pin == CRK_PIN) {
static uint32_t last_tooth_us = 0;
uint32_t dt = (last_tooth_us==0) ? 0 : (now_us - last_tooth_us);
last_tooth_us = now_us;
tooth_in_rev++;
if (tooth_in_rev >= TEETH_PER_REV) tooth_in_rev = 0;
float avg_tooth_us = (rev_time_us / (float)TEETH_PER_REV);
if (dt > (uint32_t)(avg_tooth_us * 2.2f) && dt > 2000u) {
// missing-gap detected: reset tooth count in rev
tooth_in_rev = 0;
}
if (cid_seen) {
toothAfterCID++;
if (toothAfterCID >= TEETH_PER_720) toothAfterCID = 0;
// Only schedule the bank that matches expected_next_bank when CID is synced.
// If CID not yet synced, allow scheduling of any bank (legacy behavior).
for (int b=0;b<3;b++) {
if (toothAfterCID == bank_integer_tooth[b]) {
if (!cid_synced || (b == expected_next_bank)) {
start_residual_timer_us(bank_residual_us[b], b);
}
}
}
}
return;
}
if (GPIO_Pin == CID_PIN) {
uint32_t now = micros();
if (last_cid_us != 0) {
uint32_t period = now - last_cid_us; // µs between CID pulses (720°)
uint32_t new_rev_time_us = period / 2; // 360°
// low-pass / smoothing
rev_time_us = (uint32_t)(0.85f * (float)rev_time_us + 0.15f * (float)new_rev_time_us);
time_per_degree_us = (float)rev_time_us / 360.0f;
}
last_cid_us = now;
cid_seen = true;
toothAfterCID = 0;
// Compute bank tooth targets/residuals based on this CID reference
compute_bank_targets_from_CID();
// Mark as synced and force Bank A to be the next bank to fire.
cid_synced = true;
expected_next_bank = 0; // Bank A is first after CID
last_cid_tick_ms = HAL_GetTick();
if (!sequential_mode) {
// startup batch mode: fire all banks for STARTUP_PW_US once
injector_on(0); injector_on(1); injector_on(2);
startup_batch_active = true;
// start single PW timer to turn all off
__HAL_TIM_SET_COUNTER(&htim_pw, 0);
__HAL_TIM_SET_AUTORELOAD(&htim_pw, STARTUP_PW_US - 1);
HAL_TIM_Base_Start_IT(&htim_pw);
} else {
// sequential mode: compute pulse widths from TPS
float tps_v = read_tps_voltage(); // returns 0..~5V
uint32_t pw = get_pw_from_tps_voltage(tps_v);
bank_pulse_us[0] = bank_pulse_us[1] = bank_pulse_us[2] = pw;
// schedule Bank A as the first bank after CID using its residual offset
start_residual_timer_us(bank_residual_us[0], 0);
}
}
}
// ----------------- Mode switch (startup timer) -----------------
static void check_startup_timeout(void) {
static uint32_t boot_ms = 0;
if (boot_ms == 0) boot_ms = HAL_GetTick();
if (!sequential_mode && (HAL_GetTick() - boot_ms >= STARTUP_TIME_MS)) {
sequential_mode = true;
// Will switch to sequential on next CID sync
}
// CID loss watchdog: clear cid_synced if no CID seen recently
if (cid_synced) {
uint32_t now_ms = HAL_GetTick();
if ((now_ms - last_cid_tick_ms) > CID_LOSS_TIMEOUT_MS) {
cid_synced = false;
// optional: reset expected_next_bank to 0 so first seen bank will be allowed
expected_next_bank = 0;
}
}
}
// ----------------- Initialization helper -----------------
void injector_controller_init(void) {
injector_all_off();
cid_seen = false;
cid_synced = false;
sequential_mode = false;
startup_batch_active = false;
scheduled_bank_for_residual = -1;
scheduled_bank_for_pw_off = -1;
expected_next_bank = 0;
last_cid_tick_ms = HAL_GetTick();
// start microsecond timer (configure htim_us @1MHz in CubeMX)
HAL_TIM_Base_Start(&htim_us);
compute_bank_targets_from_CID();
}
// ----------------- Main loop (call from main.c) -----------------
int main(void) {
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_ADC1_Init();
MX_TIM_US_Init(); // htim_us @1MHz
MX_TIM_RES_Init(); // htim_res basic TIM for µs one-shot
MX_TIM_PW_Init(); // htim_pw basic TIM for µs one-shot
MX_EXTI_Init(); // Configure PB3 and PB4 as EXTI
injector_controller_init();
while (1) {
check_startup_timeout();
HAL_Delay(10);
}
}