root
ca13734bf0
Add kernel (Cortex-M0/M3/M4, Tricore, S32K, RISC-V ports), drivers, middleware (CAN stack, diagnostics, safety), applications, board support, build/test tooling, and documentation.
367 lines
11 KiB
C
367 lines
11 KiB
C
/**
|
|
* @file engine_control_task.c
|
|
* @brief Main engine control task
|
|
*/
|
|
|
|
#include "engine_control.h"
|
|
#include "fuel_injection.h"
|
|
#include "ignition_control.h"
|
|
#include "adc_driver.h"
|
|
#include "gpio_driver.h"
|
|
#include "pwm_driver.h"
|
|
#include "dtc_manager.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
/* Engine Control State */
|
|
typedef struct {
|
|
EngineState_t state;
|
|
EngineSensorData_t sensors;
|
|
EngineActuatorData_t actuators;
|
|
EngineFaultCode_t faults[20];
|
|
uint8_t fault_count;
|
|
uint32_t engine_run_time;
|
|
Mutex_t data_mutex;
|
|
bool initialized;
|
|
} EngineControlState_t;
|
|
|
|
static EngineControlState_t engine_control;
|
|
|
|
/* PID Controller Structure */
|
|
typedef struct {
|
|
PidGains_t gains;
|
|
float integral;
|
|
float previous_error;
|
|
float output;
|
|
} PidController_t;
|
|
|
|
static PidController_t fuel_pid;
|
|
static PidController_t idle_pid;
|
|
static PidController_t boost_pid;
|
|
|
|
/* Initialize Engine Control */
|
|
KernelStatus_t engine_control_init(void) {
|
|
if (engine_control.initialized) {
|
|
return KERNEL_ERROR;
|
|
}
|
|
|
|
/* Initialize state */
|
|
memset(&engine_control, 0, sizeof(EngineControlState_t));
|
|
engine_control.state = ENGINE_STATE_OFF;
|
|
engine_control.fault_count = 0;
|
|
|
|
/* Initialize PID controllers */
|
|
fuel_pid.gains = fuel_pid_gains;
|
|
fuel_pid.integral = 0;
|
|
fuel_pid.previous_error = 0;
|
|
fuel_pid.output = 0;
|
|
|
|
idle_pid.gains = idle_pid_gains;
|
|
idle_pid.integral = 0;
|
|
idle_pid.previous_error = 0;
|
|
idle_pid.output = 0;
|
|
|
|
boost_pid.gains = boost_pid_gains;
|
|
boost_pid.integral = 0;
|
|
boost_pid.previous_error = 0;
|
|
boost_pid.output = 0;
|
|
|
|
/* Create mutex */
|
|
mutex_create(&engine_control.data_mutex, false);
|
|
|
|
/* Initialize sensors */
|
|
adc_init(0, &(AdcConfig_t){
|
|
.resolution = ADC_RESOLUTION_12BIT,
|
|
.mode = ADC_MODE_SCAN,
|
|
.trigger_source = ADC_TRIGGER_TIMER,
|
|
.reference = ADC_REFERENCE_VDD,
|
|
.channel_count = 8,
|
|
.channels = {
|
|
{.channel = 0, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Coolant temp */
|
|
{.channel = 1, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Intake temp */
|
|
{.channel = 2, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Manifold pressure */
|
|
{.channel = 3, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Throttle position */
|
|
{.channel = 4, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Accelerator pedal */
|
|
{.channel = 5, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Oil pressure */
|
|
{.channel = 6, .sampling_time = ADC_SAMPLING_28_5_CYCLES}, /* Fuel pressure */
|
|
{.channel = 7, .sampling_time = ADC_SAMPLING_28_5_CYCLES} /* Battery voltage */
|
|
}
|
|
});
|
|
|
|
/* Initialize actuators */
|
|
pwm_init(0, &(PwmConfig_t){
|
|
.frequency_hz = 1000,
|
|
.alignment = PWM_ALIGNMENT_EDGE,
|
|
.channel_count = 4,
|
|
.channels = {
|
|
{.channel = 0, .duty_cycle = 0}, /* Injector 1 */
|
|
{.channel = 1, .duty_cycle = 0}, /* Injector 2 */
|
|
{.channel = 2, .duty_cycle = 0}, /* Idle air control */
|
|
{.channel = 3, .duty_cycle = 0} /* Boost control */
|
|
}
|
|
});
|
|
|
|
engine_control.initialized = true;
|
|
return KERNEL_OK;
|
|
}
|
|
|
|
/* Start Engine */
|
|
KernelStatus_t engine_control_start(void) {
|
|
if (!engine_control.initialized) {
|
|
return KERNEL_ERROR;
|
|
}
|
|
|
|
engine_control.state = ENGINE_STATE_CRANKING;
|
|
|
|
/* Start PWM outputs */
|
|
pwm_start(0);
|
|
|
|
/* Start ADC conversions */
|
|
adc_start_conversion(0);
|
|
|
|
return KERNEL_OK;
|
|
}
|
|
|
|
/* Stop Engine */
|
|
KernelStatus_t engine_control_stop(void) {
|
|
if (!engine_control.initialized) {
|
|
return KERNEL_ERROR;
|
|
}
|
|
|
|
/* Stop actuators */
|
|
pwm_stop(0);
|
|
|
|
/* Stop ADC */
|
|
adc_stop_conversion(0);
|
|
|
|
engine_control.state = ENGINE_STATE_OFF;
|
|
|
|
return KERNEL_OK;
|
|
}
|
|
|
|
/* Main Engine Control Task */
|
|
void engine_control_task(void* parameters) {
|
|
(void)parameters;
|
|
|
|
TickType_t last_wake_time = kernel_get_tick_count();
|
|
|
|
while (1) {
|
|
/* Wait for next control period */
|
|
kernel_delay(ENGINE_CONTROL_PERIOD_MS);
|
|
|
|
switch (engine_control.state) {
|
|
case ENGINE_STATE_CRANKING:
|
|
/* Check RPM for engine start */
|
|
if (engine_control.sensors.rpm > 400) {
|
|
engine_control.state = ENGINE_STATE_RUNNING;
|
|
}
|
|
break;
|
|
|
|
case ENGINE_STATE_RUNNING:
|
|
/* Check for idle condition */
|
|
if (engine_control.sensors.rpm < ENGINE_IDLE_RPM + 50 &&
|
|
engine_control.sensors.accelerator_pedal < 5) {
|
|
engine_control.state = ENGINE_STATE_IDLE;
|
|
}
|
|
|
|
/* Check for acceleration */
|
|
if (engine_control.sensors.accelerator_pedal > 80) {
|
|
engine_control.state = ENGINE_STATE_ACCELERATING;
|
|
}
|
|
break;
|
|
|
|
case ENGINE_STATE_IDLE:
|
|
/* Idle speed control */
|
|
idle_pid_control();
|
|
|
|
/* Check if leaving idle */
|
|
if (engine_control.sensors.accelerator_pedal > 5) {
|
|
engine_control.state = ENGINE_STATE_RUNNING;
|
|
}
|
|
break;
|
|
|
|
case ENGINE_STATE_ACCELERATING:
|
|
/* Acceleration enrichment */
|
|
engine_control.actuators.injector_pulse_width *= 1.2f;
|
|
|
|
/* Check if still accelerating */
|
|
if (engine_control.sensors.accelerator_pedal < 80) {
|
|
engine_control.state = ENGINE_STATE_RUNNING;
|
|
}
|
|
break;
|
|
|
|
case ENGINE_STATE_DECELERATING:
|
|
/* Deceleration fuel cutoff */
|
|
if (engine_control.sensors.rpm > 1500 &&
|
|
engine_control.sensors.accelerator_pedal < 2) {
|
|
engine_control.actuators.injector_pulse_width = 0;
|
|
}
|
|
|
|
/* Check if still decelerating */
|
|
if (engine_control.sensors.accelerator_pedal > 2) {
|
|
engine_control.state = ENGINE_STATE_RUNNING;
|
|
}
|
|
break;
|
|
|
|
case ENGINE_STATE_FAULT:
|
|
/* Handle faults */
|
|
handle_engine_faults();
|
|
break;
|
|
|
|
case ENGINE_STATE_LIMP_HOME:
|
|
/* Limp home mode */
|
|
engine_control.actuators.injector_pulse_width =
|
|
fuel_map_values[0][3]; /* Fixed fuel */
|
|
engine_control.actuators.ignition_advance =
|
|
ignition_map_values[0][3]; /* Fixed timing */
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
/* Update engine run time */
|
|
engine_control.engine_run_time += ENGINE_CONTROL_PERIOD_MS;
|
|
|
|
/* Check for faults */
|
|
check_engine_faults();
|
|
|
|
last_wake_time = kernel_get_tick_count();
|
|
}
|
|
}
|
|
|
|
/* Idle Speed Control */
|
|
static void idle_pid_control(void) {
|
|
int32_t rpm_error = ENGINE_IDLE_RPM - engine_control.sensors.rpm;
|
|
|
|
/* PID calculation */
|
|
idle_pid.integral += rpm_error * (IDLE_CONTROL_PERIOD_MS / 1000.0f);
|
|
|
|
/* Limit integral */
|
|
if (idle_pid.integral > idle_pid.gains.integral_limit) {
|
|
idle_pid.integral = idle_pid.gains.integral_limit;
|
|
} else if (idle_pid.integral < -idle_pid.gains.integral_limit) {
|
|
idle_pid.integral = -idle_pid.gains.integral_limit;
|
|
}
|
|
|
|
float derivative = (rpm_error - idle_pid.previous_error) /
|
|
(IDLE_CONTROL_PERIOD_MS / 1000.0f);
|
|
|
|
idle_pid.output = idle_pid.gains.kp * rpm_error +
|
|
idle_pid.gains.ki * idle_pid.integral +
|
|
idle_pid.gains.kd * derivative;
|
|
|
|
/* Limit output */
|
|
if (idle_pid.output > idle_pid.gains.output_limit) {
|
|
idle_pid.output = idle_pid.gains.output_limit;
|
|
} else if (idle_pid.output < 0) {
|
|
idle_pid.output = 0;
|
|
}
|
|
|
|
/* Update actuator */
|
|
engine_control.actuators.idle_air_control = (uint16_t)idle_pid.output;
|
|
|
|
idle_pid.previous_error = rpm_error;
|
|
}
|
|
|
|
/* Check Engine Faults */
|
|
static void check_engine_faults(void) {
|
|
/* Check coolant temperature */
|
|
if (engine_control.sensors.coolant_temp > ENGINE_MAX_COOLANT_TEMP) {
|
|
add_engine_fault(ENGINE_FAULT_OVERHEAT);
|
|
engine_control.state = ENGINE_STATE_FAULT;
|
|
}
|
|
|
|
/* Check oil pressure */
|
|
if (engine_control.sensors.oil_pressure < ENGINE_MIN_OIL_PRESSURE &&
|
|
engine_control.sensors.rpm > 1000) {
|
|
add_engine_fault(ENGINE_FAULT_LOW_OIL_PRESSURE);
|
|
engine_control.state = ENGINE_STATE_FAULT;
|
|
}
|
|
|
|
/* Check battery voltage */
|
|
if (engine_control.sensors.battery_voltage < 9.0f ||
|
|
engine_control.sensors.battery_voltage > 16.0f) {
|
|
add_engine_fault(ENGINE_FAULT_NONE); // Just log the condition
|
|
}
|
|
}
|
|
|
|
/* Add Engine Fault */
|
|
static void add_engine_fault(EngineFaultCode_t fault) {
|
|
if (engine_control.fault_count < 20) {
|
|
/* Check if fault already exists */
|
|
for (uint8_t i = 0; i < engine_control.fault_count; i++) {
|
|
if (engine_control.faults[i] == fault) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
engine_control.faults[engine_control.fault_count] = fault;
|
|
engine_control.fault_count++;
|
|
|
|
/* Add DTC */
|
|
DtcCode_t dtc_code = {
|
|
.high_byte = 0x00, /* Powertrain */
|
|
.middle_byte = 0x00,
|
|
.low_byte = fault
|
|
};
|
|
dtc_manager_add_dtc(&dtc_code, 3); /* Medium severity */
|
|
}
|
|
}
|
|
|
|
/* Handle Engine Faults */
|
|
static void handle_engine_faults(void) {
|
|
for (uint8_t i = 0; i < engine_control.fault_count; i++) {
|
|
switch (engine_control.faults[i]) {
|
|
case ENGINE_FAULT_OVERHEAT:
|
|
/* Enable cooling fan */
|
|
engine_control.actuators.cooling_fan_duty = 100;
|
|
|
|
/* Reduce power */
|
|
engine_control.actuators.injector_pulse_width *= 0.5f;
|
|
break;
|
|
|
|
case ENGINE_FAULT_LOW_OIL_PRESSURE:
|
|
/* Immediate engine shutdown */
|
|
engine_control_stop();
|
|
break;
|
|
|
|
default:
|
|
/* Enter limp home mode */
|
|
engine_control.state = ENGINE_STATE_LIMP_HOME;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Get Engine State */
|
|
EngineState_t engine_control_get_state(void) {
|
|
return engine_control.state;
|
|
}
|
|
|
|
/* Get Sensor Data */
|
|
KernelStatus_t engine_control_get_sensor_data(EngineSensorData_t* data) {
|
|
if (data == NULL) {
|
|
return KERNEL_ERROR;
|
|
}
|
|
|
|
mutex_lock(&engine_control.data_mutex, 100);
|
|
memcpy(data, &engine_control.sensors, sizeof(EngineSensorData_t));
|
|
mutex_unlock(&engine_control.data_mutex);
|
|
|
|
return KERNEL_OK;
|
|
}
|
|
|
|
/* Get Actuator Data */
|
|
KernelStatus_t engine_control_get_actuator_data(EngineActuatorData_t* data) {
|
|
if (data == NULL) {
|
|
return KERNEL_ERROR;
|
|
}
|
|
|
|
mutex_lock(&engine_control.data_mutex, 100);
|
|
memcpy(data, &engine_control.actuators, sizeof(EngineActuatorData_t));
|
|
mutex_unlock(&engine_control.data_mutex);
|
|
|
|
return KERNEL_OK;
|
|
}
|