Add full automotive RTOS project
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.
This commit is contained in:
root
@@ -0,0 +1,185 @@
|
||||
/**
|
||||
* @file body_control_task.c
|
||||
* @brief Body control module main task
|
||||
*/
|
||||
|
||||
#include "kernel.h"
|
||||
#include "gpio_driver.h"
|
||||
#include "can_driver.h"
|
||||
#include "door_control.h"
|
||||
#include "lighting_control.h"
|
||||
#include <string.h>
|
||||
|
||||
/* Body Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
uint8_t door_status[4]; /* 0=closed, 1=open, 2=locked, 3=unlocked */
|
||||
uint8_t light_status[8]; /* 0=off, 1=on, 2=auto */
|
||||
bool alarm_active;
|
||||
uint16_t interior_temp;
|
||||
Mutex_t data_mutex;
|
||||
} BodyControlState_t;
|
||||
|
||||
static BodyControlState_t body_control;
|
||||
|
||||
/* Initialize Body Control */
|
||||
KernelStatus_t body_control_init(void) {
|
||||
if (body_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&body_control, 0, sizeof(BodyControlState_t));
|
||||
|
||||
/* Initialize door status */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
body_control.door_status[i] = 2; /* Locked */
|
||||
}
|
||||
|
||||
/* Initialize light status */
|
||||
for (int i = 0; i < 8; i++) {
|
||||
body_control.light_status[i] = 2; /* Auto */
|
||||
}
|
||||
|
||||
mutex_create(&body_control.data_mutex, false);
|
||||
|
||||
body_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Body Control Task */
|
||||
void body_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Process body control functions */
|
||||
process_door_control();
|
||||
process_lighting_control();
|
||||
process_can_messages();
|
||||
process_alarm_system();
|
||||
|
||||
/* 50ms period */
|
||||
kernel_delay(50);
|
||||
}
|
||||
}
|
||||
|
||||
/* Process Door Control */
|
||||
static void process_door_control(void) {
|
||||
/* Check door switches */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
bool door_open = gpio_read(0, i);
|
||||
bool door_locked = gpio_read(1, i);
|
||||
|
||||
mutex_lock(&body_control.data_mutex, 100);
|
||||
|
||||
if (door_open) {
|
||||
body_control.door_status[i] = 1; /* Open */
|
||||
} else if (door_locked) {
|
||||
body_control.door_status[i] = 2; /* Locked */
|
||||
} else {
|
||||
body_control.door_status[i] = 3; /* Unlocked */
|
||||
}
|
||||
|
||||
mutex_unlock(&body_control.data_mutex);
|
||||
}
|
||||
}
|
||||
|
||||
/* Process Lighting Control */
|
||||
static void process_lighting_control(void) {
|
||||
/* Read light switch position */
|
||||
bool headlights_on = gpio_read(2, 0);
|
||||
bool auto_mode = gpio_read(2, 1);
|
||||
|
||||
/* Read ambient light sensor */
|
||||
uint16_t ambient_light = adc_read_single(3);
|
||||
|
||||
mutex_lock(&body_control.data_mutex, 100);
|
||||
|
||||
if (auto_mode) {
|
||||
/* Automatic headlights */
|
||||
if (ambient_light < 100) {
|
||||
body_control.light_status[0] = 1; /* Headlights on */
|
||||
} else {
|
||||
body_control.light_status[0] = 0; /* Headlights off */
|
||||
}
|
||||
} else {
|
||||
/* Manual control */
|
||||
body_control.light_status[0] = headlights_on ? 1 : 0;
|
||||
}
|
||||
|
||||
mutex_unlock(&body_control.data_mutex);
|
||||
}
|
||||
|
||||
/* Process CAN Messages */
|
||||
static void process_can_messages(void) {
|
||||
CanMessage_t message;
|
||||
|
||||
/* Check for body control messages */
|
||||
if (can_receive_message(&message, 0) == KERNEL_OK) {
|
||||
/* Process message based on ID */
|
||||
switch (message.id.id) {
|
||||
case 0x100: /* Door control command */
|
||||
handle_door_command(&message);
|
||||
break;
|
||||
|
||||
case 0x101: /* Lighting command */
|
||||
handle_lighting_command(&message);
|
||||
break;
|
||||
|
||||
case 0x102: /* Alarm command */
|
||||
handle_alarm_command(&message);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Handle Door Command */
|
||||
static void handle_door_command(const CanMessage_t* message) {
|
||||
uint8_t door = message->data[0];
|
||||
uint8_t action = message->data[1];
|
||||
|
||||
if (door < 4) {
|
||||
switch (action) {
|
||||
case 0: /* Lock */
|
||||
gpio_write(1, door, true);
|
||||
break;
|
||||
case 1: /* Unlock */
|
||||
gpio_write(1, door, false);
|
||||
break;
|
||||
case 2: /* Open */
|
||||
gpio_write(3, door, true);
|
||||
break;
|
||||
case 3: /* Close */
|
||||
gpio_write(3, door, false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Process Alarm System */
|
||||
static void process_alarm_system(void) {
|
||||
/* Check if alarm is armed */
|
||||
static bool alarm_armed = true;
|
||||
|
||||
if (alarm_armed && !body_control.alarm_active) {
|
||||
/* Check for intrusion */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
if (body_control.door_status[i] == 1) {
|
||||
/* Door opened while armed */
|
||||
body_control.alarm_active = true;
|
||||
|
||||
/* Send alarm message */
|
||||
CanMessage_t alarm_msg;
|
||||
alarm_msg.id.id = 0x200;
|
||||
alarm_msg.length = 2;
|
||||
alarm_msg.data[0] = i; /* Door number */
|
||||
alarm_msg.data[1] = 1; /* Alarm active */
|
||||
|
||||
can_send_message(&alarm_msg, 100);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,168 @@
|
||||
/**
|
||||
* @file door_control.c
|
||||
* @brief Door control module
|
||||
*/
|
||||
|
||||
#include "kernel.h"
|
||||
#include "gpio_driver.h"
|
||||
#include "can_driver.h"
|
||||
#include <string.h>
|
||||
|
||||
/* Door Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
uint8_t door_position[4]; /* 0=closed, 1=opening, 2=open, 3=closing */
|
||||
uint8_t door_lock[4]; /* 0=unlocked, 1=locked */
|
||||
uint32_t door_timer[4];
|
||||
bool window_position[4]; /* 0=down, 1=up */
|
||||
Mutex_t mutex;
|
||||
} DoorControlState_t;
|
||||
|
||||
static DoorControlState_t door_control;
|
||||
|
||||
/* Initialize Door Control */
|
||||
KernelStatus_t door_control_init(void) {
|
||||
if (door_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&door_control, 0, sizeof(DoorControlState_t));
|
||||
|
||||
/* Initialize door positions */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
door_control.door_position[i] = 0; /* Closed */
|
||||
door_control.door_lock[i] = 1; /* Locked */
|
||||
door_control.window_position[i] = 0; /* Down */
|
||||
}
|
||||
|
||||
mutex_create(&door_control.mutex, false);
|
||||
|
||||
door_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Open Door */
|
||||
KernelStatus_t door_open(uint8_t door) {
|
||||
if (!door_control.initialized || door >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
/* Check if door is locked */
|
||||
if (door_control.door_lock[door] == 1) {
|
||||
mutex_unlock(&door_control.mutex);
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Start opening */
|
||||
door_control.door_position[door] = 1; /* Opening */
|
||||
door_control.door_timer[door] = kernel_get_tick_count();
|
||||
|
||||
/* Activate door motor */
|
||||
gpio_write(3, door, true);
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Close Door */
|
||||
KernelStatus_t door_close(uint8_t door) {
|
||||
if (!door_control.initialized || door >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
/* Start closing */
|
||||
door_control.door_position[door] = 3; /* Closing */
|
||||
door_control.door_timer[door] = kernel_get_tick_count();
|
||||
|
||||
/* Reverse door motor */
|
||||
gpio_write(3, door, false);
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Lock Door */
|
||||
KernelStatus_t door_lock(uint8_t door) {
|
||||
if (!door_control.initialized || door >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
door_control.door_lock[door] = 1;
|
||||
gpio_write(1, door, true); /* Lock */
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Unlock Door */
|
||||
KernelStatus_t door_unlock(uint8_t door) {
|
||||
if (!door_control.initialized || door >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
door_control.door_lock[door] = 0;
|
||||
gpio_write(1, door, false); /* Unlock */
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Control Window */
|
||||
KernelStatus_t window_control(uint8_t door, bool up) {
|
||||
if (!door_control.initialized || door >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
door_control.window_position[door] = up ? 1 : 0;
|
||||
gpio_write(4, door, up); /* Window motor */
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Door Control Task */
|
||||
void door_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Process door operations */
|
||||
mutex_lock(&door_control.mutex, 100);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
/* Check if door is moving */
|
||||
if (door_control.door_position[i] == 1) {
|
||||
/* Check if opening complete */
|
||||
if ((kernel_get_tick_count() - door_control.door_timer[i]) > 3000) {
|
||||
door_control.door_position[i] = 2; /* Open */
|
||||
gpio_write(3, i, false); /* Stop motor */
|
||||
}
|
||||
} else if (door_control.door_position[i] == 3) {
|
||||
/* Check if closing complete */
|
||||
if ((kernel_get_tick_count() - door_control.door_timer[i]) > 3000) {
|
||||
door_control.door_position[i] = 0; /* Closed */
|
||||
gpio_write(3, i, false); /* Stop motor */
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mutex_unlock(&door_control.mutex);
|
||||
|
||||
/* 100ms period */
|
||||
kernel_delay(100);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,182 @@
|
||||
/**
|
||||
* @file lighting_control.c
|
||||
* @brief Lighting control module
|
||||
*/
|
||||
|
||||
#include "kernel.h"
|
||||
#include "gpio_driver.h"
|
||||
#include "pwm_driver.h"
|
||||
#include "can_driver.h"
|
||||
#include <string.h>
|
||||
|
||||
/* Lighting Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
uint8_t light_state[10]; /* 0=off, 1=on, 2=auto */
|
||||
uint16_t light_intensity[10]; /* 0-10000 (100.00%) */
|
||||
uint8_t ambient_light_level;
|
||||
bool daytime_running_active;
|
||||
Mutex_t mutex;
|
||||
} LightingControlState_t;
|
||||
|
||||
/* Light Types */
|
||||
typedef enum {
|
||||
LIGHT_HEADLIGHT_LOW = 0,
|
||||
LIGHT_HEADLIGHT_HIGH = 1,
|
||||
LIGHT_FOG_FRONT = 2,
|
||||
LIGHT_FOG_REAR = 3,
|
||||
LIGHT_TURN_LEFT = 4,
|
||||
LIGHT_TURN_RIGHT = 5,
|
||||
LIGHT_BRAKE = 6,
|
||||
LIGHT_REVERSE = 7,
|
||||
LIGHT_INTERIOR = 8,
|
||||
LIGHT_DAYTIME_RUNNING = 9
|
||||
} LightType_t;
|
||||
|
||||
static LightingControlState_t lighting_control;
|
||||
|
||||
/* Initialize Lighting Control */
|
||||
KernelStatus_t lighting_control_init(void) {
|
||||
if (lighting_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&lighting_control, 0, sizeof(LightingControlState_t));
|
||||
|
||||
/* Initialize lights */
|
||||
for (int i = 0; i < 10; i++) {
|
||||
lighting_control.light_state[i] = 0; /* Off */
|
||||
lighting_control.light_intensity[i] = 0;
|
||||
}
|
||||
|
||||
lighting_control.daytime_running_active = true;
|
||||
|
||||
mutex_create(&lighting_control.mutex, false);
|
||||
|
||||
lighting_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Set Light State */
|
||||
KernelStatus_t light_set_state(LightType_t light, uint8_t state) {
|
||||
if (!lighting_control.initialized || light >= 10) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&lighting_control.mutex, 100);
|
||||
|
||||
lighting_control.light_state[light] = state;
|
||||
|
||||
/* Update GPIO */
|
||||
switch (light) {
|
||||
case LIGHT_HEADLIGHT_LOW:
|
||||
gpio_write(5, 0, state == 1);
|
||||
break;
|
||||
case LIGHT_HEADLIGHT_HIGH:
|
||||
gpio_write(5, 1, state == 1);
|
||||
break;
|
||||
case LIGHT_FOG_FRONT:
|
||||
gpio_write(5, 2, state == 1);
|
||||
break;
|
||||
case LIGHT_FOG_REAR:
|
||||
gpio_write(5, 3, state == 1);
|
||||
break;
|
||||
case LIGHT_TURN_LEFT:
|
||||
gpio_write(5, 4, state == 1);
|
||||
break;
|
||||
case LIGHT_TURN_RIGHT:
|
||||
gpio_write(5, 5, state == 1);
|
||||
break;
|
||||
case LIGHT_BRAKE:
|
||||
gpio_write(5, 6, state == 1);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
mutex_unlock(&lighting_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Set Light Intensity */
|
||||
KernelStatus_t light_set_intensity(LightType_t light, uint16_t intensity) {
|
||||
if (!lighting_control.initialized || light >= 10 || intensity > 10000) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&lighting_control.mutex, 100);
|
||||
|
||||
lighting_control.light_intensity[light] = intensity;
|
||||
|
||||
/* Update PWM for dimmable lights */
|
||||
switch (light) {
|
||||
case LIGHT_INTERIOR:
|
||||
pwm_set_duty_cycle(2, 0, intensity);
|
||||
break;
|
||||
case LIGHT_DAYTIME_RUNNING:
|
||||
pwm_set_duty_cycle(2, 1, intensity);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
mutex_unlock(&lighting_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Turn Signal Control */
|
||||
KernelStatus_t light_turn_signal(LightType_t direction, uint8_t flashes) {
|
||||
if (!lighting_control.initialized ||
|
||||
(direction != LIGHT_TURN_LEFT && direction != LIGHT_TURN_RIGHT)) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Flash turn signal */
|
||||
for (uint8_t i = 0; i < flashes; i++) {
|
||||
light_set_state(direction, 1);
|
||||
kernel_delay(500);
|
||||
light_set_state(direction, 0);
|
||||
kernel_delay(500);
|
||||
}
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Lighting Control Task */
|
||||
void lighting_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Read ambient light sensor */
|
||||
uint16_t ambient = adc_read_single(4);
|
||||
|
||||
mutex_lock(&lighting_control.mutex, 100);
|
||||
|
||||
/* Update ambient light level */
|
||||
lighting_control.ambient_light_level = ambient / 41; /* 0-100 */
|
||||
|
||||
/* Automatic headlight control */
|
||||
if (lighting_control.light_state[LIGHT_HEADLIGHT_LOW] == 2) {
|
||||
/* Auto mode */
|
||||
if (lighting_control.ambient_light_level < 30) {
|
||||
gpio_write(5, 0, true); /* Turn on headlights */
|
||||
lighting_control.light_state[LIGHT_HEADLIGHT_LOW] = 1;
|
||||
} else {
|
||||
gpio_write(5, 0, false); /* Turn off headlights */
|
||||
lighting_control.light_state[LIGHT_HEADLIGHT_LOW] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Daytime running lights */
|
||||
if (lighting_control.daytime_running_active) {
|
||||
pwm_set_duty_cycle(2, 1, 5000); /* 50% intensity */
|
||||
}
|
||||
|
||||
mutex_unlock(&lighting_control.mutex);
|
||||
|
||||
/* 100ms period */
|
||||
kernel_delay(100);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
* @file brake_control.h
|
||||
* @brief Brake control module interface
|
||||
*/
|
||||
|
||||
#ifndef BRAKE_CONTROL_H
|
||||
#define BRAKE_CONTROL_H
|
||||
|
||||
#include "kernel.h"
|
||||
|
||||
/* Brake Control Parameters */
|
||||
#define BRAKE_CONTROL_PERIOD_MS 5
|
||||
#define ABS_CONTROL_PERIOD_MS 1
|
||||
#define WHEEL_SPEED_SENSOR_COUNT 4
|
||||
#define MAX_BRAKE_PRESSURE 20000 /* kPa */
|
||||
#define ABS_SLIP_THRESHOLD 0.2f
|
||||
#define ABS_DECELERATION_THRESHOLD -10.0f /* m/s² */
|
||||
|
||||
/* Brake States */
|
||||
typedef enum {
|
||||
BRAKE_STATE_IDLE = 0,
|
||||
BRAKE_STATE_NORMAL_BRAKING = 1,
|
||||
BRAKE_STATE_ABS_ACTIVE = 2,
|
||||
BRAKE_STATE_EMERGENCY = 3,
|
||||
BRAKE_STATE_FAULT = 4
|
||||
} BrakeState_t;
|
||||
|
||||
/* Wheel Speed Data */
|
||||
typedef struct {
|
||||
float wheel_speed[WHEEL_SPEED_SENSOR_COUNT]; /* km/h */
|
||||
float wheel_acceleration[WHEEL_SPEED_SENSOR_COUNT]; /* m/s² */
|
||||
uint16_t wheel_sensor_raw[WHEEL_SPEED_SENSOR_COUNT];
|
||||
bool sensor_fault[WHEEL_SPEED_SENSOR_COUNT];
|
||||
} WheelSpeedData_t;
|
||||
|
||||
/* Brake System Data */
|
||||
typedef struct {
|
||||
uint16_t brake_pedal_position;
|
||||
uint16_t brake_pressure;
|
||||
float vehicle_speed;
|
||||
float vehicle_deceleration;
|
||||
bool abs_active;
|
||||
bool brake_light;
|
||||
uint8_t abs_fault_code;
|
||||
} BrakeSystemData_t;
|
||||
|
||||
/* Brake Control Functions */
|
||||
KernelStatus_t brake_control_init(void);
|
||||
KernelStatus_t brake_control_start(void);
|
||||
KernelStatus_t brake_control_stop(void);
|
||||
KernelStatus_t brake_control_get_data(BrakeSystemData_t* data);
|
||||
KernelStatus_t brake_control_get_wheel_data(WheelSpeedData_t* data);
|
||||
BrakeState_t brake_control_get_state(void);
|
||||
void brake_control_task(void* parameters);
|
||||
void abs_control_task(void* parameters);
|
||||
|
||||
#endif /* BRAKE_CONTROL_H */
|
||||
@@ -0,0 +1,174 @@
|
||||
/**
|
||||
* @file abs_control.c
|
||||
* @brief Anti-lock Braking System control
|
||||
*/
|
||||
|
||||
#include "brake_control.h"
|
||||
#include "gpio_driver.h"
|
||||
#include <math.h>
|
||||
|
||||
/* ABS Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
bool abs_active;
|
||||
float reference_speed;
|
||||
float slip[WHEEL_SPEED_SENSOR_COUNT];
|
||||
uint8_t control_phase; /* 0=increase, 1=hold, 2=decrease */
|
||||
uint32_t phase_timer;
|
||||
Mutex_t mutex;
|
||||
} AbsControlState_t;
|
||||
|
||||
static AbsControlState_t abs_control;
|
||||
|
||||
/* Initialize ABS */
|
||||
KernelStatus_t abs_control_init(void) {
|
||||
if (abs_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
abs_control.abs_active = false;
|
||||
abs_control.reference_speed = 0;
|
||||
abs_control.control_phase = 0;
|
||||
abs_control.phase_timer = 0;
|
||||
|
||||
for (int i = 0; i < WHEEL_SPEED_SENSOR_COUNT; i++) {
|
||||
abs_control.slip[i] = 0;
|
||||
}
|
||||
|
||||
mutex_create(&abs_control.mutex, false);
|
||||
|
||||
abs_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* ABS Control Task */
|
||||
void abs_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Wait for next ABS control period */
|
||||
kernel_delay(ABS_CONTROL_PERIOD_MS);
|
||||
|
||||
WheelSpeedData_t wheels;
|
||||
brake_control_get_wheel_data(&wheels);
|
||||
|
||||
/* Check if ABS should be active */
|
||||
if (brake_control_get_state() == BRAKE_STATE_NORMAL_BRAKING &&
|
||||
check_abs_activation(&wheels)) {
|
||||
|
||||
abs_control.abs_active = true;
|
||||
control_abs(&wheels);
|
||||
} else {
|
||||
abs_control.abs_active = false;
|
||||
abs_control.control_phase = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Check ABS Activation Conditions */
|
||||
static bool check_abs_activation(const WheelSpeedData_t* wheels) {
|
||||
/* Calculate reference speed (maximum wheel speed) */
|
||||
float max_speed = 0;
|
||||
for (int i = 0; i < WHEEL_SPEED_SENSOR_COUNT; i++) {
|
||||
if (wheels->wheel_speed[i] > max_speed) {
|
||||
max_speed = wheels->wheel_speed[i];
|
||||
}
|
||||
}
|
||||
|
||||
abs_control.reference_speed = max_speed;
|
||||
|
||||
/* Calculate slip for each wheel */
|
||||
for (int i = 0; i < WHEEL_SPEED_SENSOR_COUNT; i++) {
|
||||
if (max_speed > 5.0f) { /* Only calculate above 5 km/h */
|
||||
abs_control.slip[i] = (max_speed - wheels->wheel_speed[i]) / max_speed;
|
||||
} else {
|
||||
abs_control.slip[i] = 0;
|
||||
}
|
||||
|
||||
/* Check if slip exceeds threshold */
|
||||
if (abs_control.slip[i] > ABS_SLIP_THRESHOLD) {
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Check for excessive deceleration */
|
||||
if (wheels->wheel_acceleration[i] < ABS_DECELERATION_THRESHOLD) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ABS Control Algorithm */
|
||||
static void control_abs(const WheelSpeedData_t* wheels) {
|
||||
mutex_lock(&abs_control.mutex, 100);
|
||||
|
||||
/* Simple ABS control algorithm */
|
||||
switch (abs_control.control_phase) {
|
||||
case 0: /* Increase pressure */
|
||||
if (abs_control.slip[0] > ABS_SLIP_THRESHOLD) {
|
||||
abs_control.control_phase = 2; /* Switch to decrease */
|
||||
abs_control.phase_timer = kernel_get_tick_count();
|
||||
}
|
||||
break;
|
||||
|
||||
case 1: /* Hold pressure */
|
||||
if ((kernel_get_tick_count() - abs_control.phase_timer) > 10) {
|
||||
abs_control.control_phase = 0; /* Switch to increase */
|
||||
}
|
||||
break;
|
||||
|
||||
case 2: /* Decrease pressure */
|
||||
if (abs_control.slip[0] < ABS_SLIP_THRESHOLD * 0.5f) {
|
||||
abs_control.control_phase = 1; /* Switch to hold */
|
||||
abs_control.phase_timer = kernel_get_tick_count();
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
mutex_unlock(&abs_control.mutex);
|
||||
|
||||
/* Control brake pressure valves */
|
||||
control_pressure_valves();
|
||||
}
|
||||
|
||||
/* Control Pressure Valves */
|
||||
static void control_pressure_valves(void) {
|
||||
switch (abs_control.control_phase) {
|
||||
case 0: /* Increase pressure */
|
||||
gpio_write(0, 0, false); /* Inlet valve open */
|
||||
gpio_write(0, 1, false); /* Outlet valve closed */
|
||||
break;
|
||||
|
||||
case 1: /* Hold pressure */
|
||||
gpio_write(0, 0, true); /* Inlet valve closed */
|
||||
gpio_write(0, 1, false); /* Outlet valve closed */
|
||||
break;
|
||||
|
||||
case 2: /* Decrease pressure */
|
||||
gpio_write(0, 0, true); /* Inlet valve closed */
|
||||
gpio_write(0, 1, true); /* Outlet valve open */
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get ABS Status */
|
||||
bool abs_control_is_active(void) {
|
||||
return abs_control.abs_active;
|
||||
}
|
||||
|
||||
/* Get Slip Values */
|
||||
KernelStatus_t abs_control_get_slip(float* slip, uint8_t* count) {
|
||||
if (slip == NULL || count == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&abs_control.mutex, 100);
|
||||
for (int i = 0; i < WHEEL_SPEED_SENSOR_COUNT; i++) {
|
||||
slip[i] = abs_control.slip[i];
|
||||
}
|
||||
*count = WHEEL_SPEED_SENSOR_COUNT;
|
||||
mutex_unlock(&abs_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
@@ -0,0 +1,174 @@
|
||||
/**
|
||||
* @file brake_task.c
|
||||
* @brief Main brake control task
|
||||
*/
|
||||
|
||||
#include "brake_control.h"
|
||||
#include "adc_driver.h"
|
||||
#include "gpio_driver.h"
|
||||
#include "pwm_driver.h"
|
||||
#include "dtc_manager.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
/* Brake Control State */
|
||||
typedef struct {
|
||||
BrakeState_t state;
|
||||
BrakeSystemData_t data;
|
||||
WheelSpeedData_t wheels;
|
||||
uint16_t target_pressure;
|
||||
uint16_t actual_pressure;
|
||||
Mutex_t data_mutex;
|
||||
bool initialized;
|
||||
} BrakeControlState_t;
|
||||
|
||||
static BrakeControlState_t brake_control;
|
||||
|
||||
/* Initialize Brake Control */
|
||||
KernelStatus_t brake_control_init(void) {
|
||||
if (brake_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&brake_control, 0, sizeof(BrakeControlState_t));
|
||||
brake_control.state = BRAKE_STATE_IDLE;
|
||||
|
||||
mutex_create(&brake_control.data_mutex, false);
|
||||
|
||||
/* Initialize ADC for brake sensors */
|
||||
adc_init(1, &(AdcConfig_t){
|
||||
.resolution = ADC_RESOLUTION_12BIT,
|
||||
.mode = ADC_MODE_CONTINUOUS,
|
||||
.channel_count = 2,
|
||||
.channels = {
|
||||
{.channel = 0}, /* Brake pedal position */
|
||||
{.channel = 1} /* Brake pressure */
|
||||
}
|
||||
});
|
||||
|
||||
/* Initialize PWM for brake pressure control */
|
||||
pwm_init(1, &(PwmConfig_t){
|
||||
.frequency_hz = 2000,
|
||||
.channel_count = 1,
|
||||
.channels = {
|
||||
{.channel = 0, .duty_cycle = 0} /* Brake pressure valve */
|
||||
}
|
||||
});
|
||||
|
||||
brake_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Brake Control Task */
|
||||
void brake_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Wait for next brake control period */
|
||||
kernel_delay(BRAKE_CONTROL_PERIOD_MS);
|
||||
|
||||
/* Read brake sensors */
|
||||
read_brake_sensors();
|
||||
|
||||
/* Process brake logic */
|
||||
process_brake_logic();
|
||||
|
||||
/* Check for faults */
|
||||
check_brake_faults();
|
||||
}
|
||||
}
|
||||
|
||||
/* Read Brake Sensors */
|
||||
static void read_brake_sensors(void) {
|
||||
uint16_t brake_pedal = 0;
|
||||
uint16_t brake_pressure = 0;
|
||||
|
||||
adc_read_channel(1, 0, &brake_pedal, 10);
|
||||
adc_read_channel(1, 1, &brake_pressure, 10);
|
||||
|
||||
mutex_lock(&brake_control.data_mutex, 100);
|
||||
brake_control.data.brake_pedal_position = brake_pedal;
|
||||
brake_control.data.brake_pressure = brake_pressure;
|
||||
mutex_unlock(&brake_control.data_mutex);
|
||||
}
|
||||
|
||||
/* Process Brake Logic */
|
||||
static void process_brake_logic(void) {
|
||||
mutex_lock(&brake_control.data_mutex, 100);
|
||||
|
||||
/* Check brake pedal position */
|
||||
if (brake_control.data.brake_pedal_position > 100) {
|
||||
brake_control.state = BRAKE_STATE_NORMAL_BRAKING;
|
||||
brake_control.data.brake_light = true;
|
||||
|
||||
/* Calculate target pressure based on pedal position */
|
||||
brake_control.target_pressure =
|
||||
(brake_control.data.brake_pedal_position - 100) * 200;
|
||||
|
||||
/* Limit pressure */
|
||||
if (brake_control.target_pressure > MAX_BRAKE_PRESSURE) {
|
||||
brake_control.target_pressure = MAX_BRAKE_PRESSURE;
|
||||
}
|
||||
} else {
|
||||
brake_control.state = BRAKE_STATE_IDLE;
|
||||
brake_control.data.brake_light = false;
|
||||
brake_control.target_pressure = 0;
|
||||
}
|
||||
|
||||
/* Check for emergency braking */
|
||||
if (brake_control.data.brake_pedal_position > 900 &&
|
||||
brake_control.data.vehicle_deceleration < -8.0f) {
|
||||
brake_control.state = BRAKE_STATE_EMERGENCY;
|
||||
|
||||
/* Maximum braking force */
|
||||
brake_control.target_pressure = MAX_BRAKE_PRESSURE;
|
||||
}
|
||||
|
||||
mutex_unlock(&brake_control.data_mutex);
|
||||
|
||||
/* Update brake pressure control */
|
||||
update_brake_pressure();
|
||||
}
|
||||
|
||||
/* Update Brake Pressure */
|
||||
static void update_brake_pressure(void) {
|
||||
/* Simple PID pressure control */
|
||||
int32_t pressure_error = brake_control.target_pressure -
|
||||
brake_control.actual_pressure;
|
||||
|
||||
/* Calculate valve duty cycle */
|
||||
uint16_t valve_duty = 0;
|
||||
|
||||
if (pressure_error > 100) {
|
||||
valve_duty = 10000; /* Full open */
|
||||
} else if (pressure_error > 0) {
|
||||
valve_duty = (uint16_t)((pressure_error * 10000) / 100);
|
||||
} else {
|
||||
valve_duty = 0; /* Closed */
|
||||
}
|
||||
|
||||
/* Update PWM */
|
||||
pwm_set_duty_cycle(1, 0, valve_duty);
|
||||
|
||||
/* Update actual pressure */
|
||||
brake_control.actual_pressure += pressure_error / 10;
|
||||
}
|
||||
|
||||
/* Check Brake Faults */
|
||||
static void check_brake_faults(void) {
|
||||
/* Check wheel speed sensors */
|
||||
for (int i = 0; i < WHEEL_SPEED_SENSOR_COUNT; i++) {
|
||||
if (brake_control.wheels.sensor_fault[i]) {
|
||||
brake_control.state = BRAKE_STATE_FAULT;
|
||||
brake_control.data.abs_fault_code = i + 1;
|
||||
|
||||
/* Add DTC */
|
||||
DtcCode_t dtc_code = {
|
||||
.high_byte = 0x01, /* Chassis */
|
||||
.middle_byte = 0x00,
|
||||
.low_byte = i + 1
|
||||
};
|
||||
dtc_manager_add_dtc(&dtc_code, 4); /* High severity */
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,319 @@
|
||||
/**
|
||||
* @file display_task.c
|
||||
* @brief Dashboard display task
|
||||
*/
|
||||
|
||||
#include "kernel.h"
|
||||
#include "can_driver.h"
|
||||
#include "spi_driver.h"
|
||||
#include "gpio_driver.h"
|
||||
#include "engine_control.h"
|
||||
#include "brake_control.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
/* Display State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
uint16_t display_buffer[1024]; /* Display frame buffer */
|
||||
uint8_t current_screen;
|
||||
bool backlight_on;
|
||||
uint8_t backlight_intensity;
|
||||
Mutex_t mutex;
|
||||
} DisplayState_t;
|
||||
|
||||
static DisplayState_t display;
|
||||
|
||||
/* Dashboard Data */
|
||||
typedef struct {
|
||||
uint16_t speed;
|
||||
uint16_t rpm;
|
||||
int16_t coolant_temp;
|
||||
uint16_t fuel_level;
|
||||
uint16_t odometer;
|
||||
uint16_t trip_meter;
|
||||
bool turn_left;
|
||||
bool turn_right;
|
||||
bool high_beam;
|
||||
bool check_engine;
|
||||
bool abs_warning;
|
||||
bool oil_pressure_warning;
|
||||
bool battery_warning;
|
||||
} DashboardData_t;
|
||||
|
||||
static DashboardData_t dashboard_data;
|
||||
|
||||
/* Initialize Display */
|
||||
KernelStatus_t display_init(void) {
|
||||
if (display.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&display, 0, sizeof(DisplayState_t));
|
||||
display.current_screen = 0;
|
||||
display.backlight_on = true;
|
||||
display.backlight_intensity = 100;
|
||||
|
||||
mutex_create(&display.mutex, false);
|
||||
|
||||
/* Initialize SPI for display */
|
||||
spi_init(0, &(SpiConfig_t){
|
||||
.mode = SPI_MODE_0,
|
||||
.clock_speed = SPI_CLOCK_8MHZ,
|
||||
.data_order = SPI_DATA_ORDER_MSB_FIRST,
|
||||
.data_size = 8,
|
||||
.use_dma = true,
|
||||
.enable_hardware_cs = false,
|
||||
.cs_polarity = SPI_CS_ACTIVE_LOW,
|
||||
.cs_port = 0,
|
||||
.cs_pin = 15
|
||||
});
|
||||
|
||||
display.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Dashboard Display Task */
|
||||
void display_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Update dashboard data */
|
||||
update_dashboard_data();
|
||||
|
||||
/* Render display */
|
||||
render_display();
|
||||
|
||||
/* Update display */
|
||||
update_display_hardware();
|
||||
|
||||
/* 50ms refresh rate */
|
||||
kernel_delay(50);
|
||||
}
|
||||
}
|
||||
|
||||
/* Update Dashboard Data */
|
||||
static void update_dashboard_data(void) {
|
||||
/* Get data from CAN bus */
|
||||
CanMessage_t message;
|
||||
|
||||
while (can_receive_message(&message, 0) == KERNEL_OK) {
|
||||
switch (message.id.id) {
|
||||
case 0x300: /* Engine data */
|
||||
dashboard_data.rpm = (message.data[0] << 8) | message.data[1];
|
||||
dashboard_data.coolant_temp = (int16_t)((message.data[2] << 8) |
|
||||
message.data[3]);
|
||||
dashboard_data.check_engine = message.data[4] & 0x01;
|
||||
dashboard_data.oil_pressure_warning = message.data[4] & 0x02;
|
||||
break;
|
||||
|
||||
case 0x301: /* Vehicle speed */
|
||||
dashboard_data.speed = (message.data[0] << 8) | message.data[1];
|
||||
dashboard_data.odometer = (message.data[2] << 16) |
|
||||
(message.data[3] << 8) |
|
||||
message.data[4];
|
||||
break;
|
||||
|
||||
case 0x302: /* Fuel level */
|
||||
dashboard_data.fuel_level = message.data[0];
|
||||
dashboard_data.battery_warning = message.data[1] & 0x01;
|
||||
break;
|
||||
|
||||
case 0x303: /* Turn signals */
|
||||
dashboard_data.turn_left = message.data[0] & 0x01;
|
||||
dashboard_data.turn_right = message.data[0] & 0x02;
|
||||
dashboard_data.high_beam = message.data[0] & 0x04;
|
||||
break;
|
||||
|
||||
case 0x304: /* ABS status */
|
||||
dashboard_data.abs_warning = message.data[0] & 0x01;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Render Display */
|
||||
static void render_display(void) {
|
||||
mutex_lock(&display.mutex, 100);
|
||||
|
||||
/* Clear display buffer */
|
||||
memset(display.display_buffer, 0, sizeof(display.display_buffer));
|
||||
|
||||
/* Draw speedometer */
|
||||
draw_speedometer();
|
||||
|
||||
/* Draw tachometer */
|
||||
draw_tachometer();
|
||||
|
||||
/* Draw fuel gauge */
|
||||
draw_fuel_gauge();
|
||||
|
||||
/* Draw temperature gauge */
|
||||
draw_temperature_gauge();
|
||||
|
||||
/* Draw warning indicators */
|
||||
draw_warning_indicators();
|
||||
|
||||
/* Draw odometer */
|
||||
draw_odometer();
|
||||
|
||||
mutex_unlock(&display.mutex);
|
||||
}
|
||||
|
||||
/* Draw Speedometer */
|
||||
static void draw_speedometer(void) {
|
||||
/* Draw circular gauge */
|
||||
int center_x = 100;
|
||||
int center_y = 100;
|
||||
int radius = 80;
|
||||
|
||||
/* Draw arc */
|
||||
for (int angle = 0; angle < 270; angle++) {
|
||||
int x = center_x + (int)(radius * cos(angle * M_PI / 180));
|
||||
int y = center_y + (int)(radius * sin(angle * M_PI / 180));
|
||||
|
||||
if (x >= 0 && x < 240 && y >= 0 && y < 320) {
|
||||
display.display_buffer[y * 240 + x] = 0xFFFF; /* White */
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw speed needle */
|
||||
float speed_angle = (dashboard_data.speed * 270.0f) / 240.0f; /* 240 km/h max */
|
||||
int needle_x = center_x + (int)((radius - 10) * cos(speed_angle * M_PI / 180));
|
||||
int needle_y = center_y + (int)((radius - 10) * sin(speed_angle * M_PI / 180));
|
||||
|
||||
/* Draw line from center to needle tip */
|
||||
draw_line(center_x, center_y, needle_x, needle_y, 0xF800); /* Red */
|
||||
|
||||
/* Draw speed text */
|
||||
char speed_text[10];
|
||||
snprintf(speed_text, sizeof(speed_text), "%d km/h", dashboard_data.speed);
|
||||
draw_text(60, 200, speed_text, 0xFFFF);
|
||||
}
|
||||
|
||||
/* Draw Tachometer */
|
||||
static void draw_tachometer(void) {
|
||||
int center_x = 300;
|
||||
int center_y = 100;
|
||||
int radius = 60;
|
||||
|
||||
/* Draw arc */
|
||||
for (int angle = 0; angle < 270; angle++) {
|
||||
int x = center_x + (int)(radius * cos(angle * M_PI / 180));
|
||||
int y = center_y + (int)(radius * sin(angle * M_PI / 180));
|
||||
|
||||
if (x >= 0 && x < 480 && y >= 0 && y < 320) {
|
||||
display.display_buffer[y * 480 + x] = 0xFFFF;
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw RPM needle */
|
||||
float rpm_angle = (dashboard_data.rpm * 270.0f) / 8000.0f; /* 8000 RPM max */
|
||||
int needle_x = center_x + (int)((radius - 10) * cos(rpm_angle * M_PI / 180));
|
||||
int needle_y = center_y + (int)((radius - 10) * sin(rpm_angle * M_PI / 180));
|
||||
|
||||
draw_line(center_x, center_y, needle_x, needle_y, 0x07E0); /* Green */
|
||||
|
||||
/* Draw RPM text */
|
||||
char rpm_text[10];
|
||||
snprintf(rpm_text, sizeof(rpm_text), "%d RPM", dashboard_data.rpm);
|
||||
draw_text(260, 200, rpm_text, 0xFFFF);
|
||||
}
|
||||
|
||||
/* Draw Warning Indicators */
|
||||
static void draw_warning_indicators(void) {
|
||||
/* Check engine light */
|
||||
if (dashboard_data.check_engine) {
|
||||
draw_text(20, 280, "CHECK ENGINE", 0xF800); /* Red */
|
||||
}
|
||||
|
||||
/* ABS warning */
|
||||
if (dashboard_data.abs_warning) {
|
||||
draw_text(20, 300, "ABS", 0xF800);
|
||||
}
|
||||
|
||||
/* Oil pressure warning */
|
||||
if (dashboard_data.oil_pressure_warning) {
|
||||
draw_text(100, 300, "OIL", 0xF800);
|
||||
}
|
||||
|
||||
/* Battery warning */
|
||||
if (dashboard_data.battery_warning) {
|
||||
draw_text(150, 300, "BAT", 0xF800);
|
||||
}
|
||||
|
||||
/* Turn signals */
|
||||
if (dashboard_data.turn_left) {
|
||||
draw_text(400, 280, "<--", 0x07E0); /* Green */
|
||||
}
|
||||
if (dashboard_data.turn_right) {
|
||||
draw_text(440, 280, "-->", 0x07E0);
|
||||
}
|
||||
|
||||
/* High beam */
|
||||
if (dashboard_data.high_beam) {
|
||||
draw_text(400, 300, "HIGH", 0x001F); /* Blue */
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw Line */
|
||||
static void draw_line(int x1, int y1, int x2, int y2, uint16_t color) {
|
||||
int dx = abs(x2 - x1);
|
||||
int dy = abs(y2 - y1);
|
||||
int sx = (x1 < x2) ? 1 : -1;
|
||||
int sy = (y1 < y2) ? 1 : -1;
|
||||
int err = dx - dy;
|
||||
|
||||
while (1) {
|
||||
if (x1 >= 0 && x1 < 480 && y1 >= 0 && y1 < 320) {
|
||||
display.display_buffer[y1 * 480 + x1] = color;
|
||||
}
|
||||
|
||||
if (x1 == x2 && y1 == y2) {
|
||||
break;
|
||||
}
|
||||
|
||||
int e2 = 2 * err;
|
||||
if (e2 > -dy) {
|
||||
err -= dy;
|
||||
x1 += sx;
|
||||
}
|
||||
if (e2 < dx) {
|
||||
err += dx;
|
||||
y1 += sy;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Draw Text */
|
||||
static void draw_text(int x, int y, const char* text, uint16_t color) {
|
||||
/* Simple 8x8 font rendering */
|
||||
while (*text) {
|
||||
char c = *text++;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if (font_bitmap[(uint8_t)c][i] & (1 << j)) {
|
||||
int px = x + j;
|
||||
int py = y + i;
|
||||
|
||||
if (px >= 0 && px < 480 && py >= 0 && py < 320) {
|
||||
display.display_buffer[py * 480 + px] = color;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
x += 8;
|
||||
}
|
||||
}
|
||||
|
||||
/* Update Display Hardware */
|
||||
static void update_display_hardware(void) {
|
||||
mutex_lock(&display.mutex, 100);
|
||||
|
||||
/* Send frame buffer to display via SPI */
|
||||
spi_write(0, (uint8_t*)display.display_buffer, sizeof(display.display_buffer), 100);
|
||||
|
||||
mutex_unlock(&display.mutex);
|
||||
}
|
||||
@@ -0,0 +1,200 @@
|
||||
/**
|
||||
* @file gauge_control.c
|
||||
* @brief Analog gauge control for dashboard
|
||||
*/
|
||||
|
||||
#include "kernel.h"
|
||||
#include "pwm_driver.h"
|
||||
#include "adc_driver.h"
|
||||
#include "can_driver.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
/* Gauge Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
uint16_t speed_gauge_position;
|
||||
uint16_t rpm_gauge_position;
|
||||
uint16_t fuel_gauge_position;
|
||||
uint16_t temp_gauge_position;
|
||||
uint16_t target_positions[4];
|
||||
Mutex_t mutex;
|
||||
} GaugeControlState_t;
|
||||
|
||||
static GaugeControlState_t gauge_control;
|
||||
|
||||
/* Initialize Gauge Control */
|
||||
KernelStatus_t gauge_control_init(void) {
|
||||
if (gauge_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
memset(&gauge_control, 0, sizeof(GaugeControlState_t));
|
||||
|
||||
/* Initialize PWM for gauges */
|
||||
pwm_init(3, &(PwmConfig_t){
|
||||
.frequency_hz = 100, /* 100 Hz for smooth gauge movement */
|
||||
.channel_count = 4,
|
||||
.channels = {
|
||||
{.channel = 0, .duty_cycle = 0}, /* Speedometer */
|
||||
{.channel = 1, .duty_cycle = 0}, /* Tachometer */
|
||||
{.channel = 2, .duty_cycle = 0}, /* Fuel gauge */
|
||||
{.channel = 3, .duty_cycle = 0} /* Temperature gauge */
|
||||
}
|
||||
});
|
||||
|
||||
mutex_create(&gauge_control.mutex, false);
|
||||
|
||||
gauge_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Gauge Control Task */
|
||||
void gauge_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Read CAN messages for gauge data */
|
||||
CanMessage_t message;
|
||||
|
||||
while (can_receive_message(&message, 0) == KERNEL_OK) {
|
||||
process_gauge_message(&message);
|
||||
}
|
||||
|
||||
/* Smooth gauge movement */
|
||||
smooth_gauge_movement();
|
||||
|
||||
/* 10ms update rate */
|
||||
kernel_delay(10);
|
||||
}
|
||||
}
|
||||
|
||||
/* Process Gauge Message */
|
||||
static void process_gauge_message(const CanMessage_t* message) {
|
||||
mutex_lock(&gauge_control.mutex, 100);
|
||||
|
||||
switch (message->id.id) {
|
||||
case 0x300: /* Engine data */
|
||||
/* RPM: 0-8000 RPM maps to 0-10000 duty cycle */
|
||||
gauge_control.target_positions[1] =
|
||||
((message->data[0] << 8) | message->data[1]) * 10000 / 8000;
|
||||
|
||||
/* Temperature: -40 to 120°C maps to 0-10000 */
|
||||
int16_t temp = (message->data[2] << 8) | message->data[3];
|
||||
gauge_control.target_positions[3] =
|
||||
(temp + 40) * 10000 / 160;
|
||||
break;
|
||||
|
||||
case 0x301: /* Vehicle speed */
|
||||
/* Speed: 0-240 km/h maps to 0-10000 */
|
||||
gauge_control.target_positions[0] =
|
||||
((message->data[0] << 8) | message->data[1]) * 10000 / 240;
|
||||
break;
|
||||
|
||||
case 0x302: /* Fuel level */
|
||||
/* Fuel: 0-100% maps to 0-10000 */
|
||||
gauge_control.target_positions[2] = message->data[0] * 100;
|
||||
break;
|
||||
}
|
||||
|
||||
mutex_unlock(&gauge_control.mutex);
|
||||
}
|
||||
|
||||
/* Smooth Gauge Movement */
|
||||
static void smooth_gauge_movement(void) {
|
||||
mutex_lock(&gauge_control.mutex, 100);
|
||||
|
||||
/* Smooth movement for each gauge */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
uint16_t current = 0;
|
||||
uint16_t target = gauge_control.target_positions[i];
|
||||
|
||||
/* Get current position */
|
||||
switch (i) {
|
||||
case 0:
|
||||
current = gauge_control.speed_gauge_position;
|
||||
break;
|
||||
case 1:
|
||||
current = gauge_control.rpm_gauge_position;
|
||||
break;
|
||||
case 2:
|
||||
current = gauge_control.fuel_gauge_position;
|
||||
break;
|
||||
case 3:
|
||||
current = gauge_control.temp_gauge_position;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Calculate new position with smoothing */
|
||||
int32_t delta = target - current;
|
||||
uint16_t new_position = current + (delta / 10); /* 10% movement per update */
|
||||
|
||||
/* Update position */
|
||||
switch (i) {
|
||||
case 0:
|
||||
gauge_control.speed_gauge_position = new_position;
|
||||
break;
|
||||
case 1:
|
||||
gauge_control.rpm_gauge_position = new_position;
|
||||
break;
|
||||
case 2:
|
||||
gauge_control.fuel_gauge_position = new_position;
|
||||
break;
|
||||
case 3:
|
||||
gauge_control.temp_gauge_position = new_position;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Update PWM output */
|
||||
pwm_set_duty_cycle(3, i, new_position);
|
||||
}
|
||||
|
||||
mutex_unlock(&gauge_control.mutex);
|
||||
}
|
||||
|
||||
/* Calibrate Gauges */
|
||||
KernelStatus_t gauge_calibrate(uint8_t gauge, uint16_t min_position,
|
||||
uint16_t max_position) {
|
||||
if (!gauge_control.initialized || gauge >= 4) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&gauge_control.mutex, 100);
|
||||
|
||||
/* Set gauge to minimum position */
|
||||
pwm_set_duty_cycle(3, gauge, min_position);
|
||||
kernel_delay(1000); /* Wait 1 second */
|
||||
|
||||
/* Set gauge to maximum position */
|
||||
pwm_set_duty_cycle(3, gauge, max_position);
|
||||
kernel_delay(1000); /* Wait 1 second */
|
||||
|
||||
/* Return to zero */
|
||||
pwm_set_duty_cycle(3, gauge, min_position);
|
||||
|
||||
mutex_unlock(&gauge_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Self-Test Gauges */
|
||||
KernelStatus_t gauge_self_test(void) {
|
||||
if (!gauge_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
/* Perform gauge sweep */
|
||||
for (uint16_t position = 0; position <= 10000; position += 100) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
pwm_set_duty_cycle(3, i, position);
|
||||
}
|
||||
kernel_delay(10);
|
||||
}
|
||||
|
||||
/* Return to zero */
|
||||
for (int i = 0; i < 4; i++) {
|
||||
pwm_set_duty_cycle(3, i, 0);
|
||||
}
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
/**
|
||||
* @file engine_control.h
|
||||
* @brief Engine control module interface
|
||||
*/
|
||||
|
||||
#ifndef ENGINE_CONTROL_H
|
||||
#define ENGINE_CONTROL_H
|
||||
|
||||
#include "kernel.h"
|
||||
#include "engine_parameters.h"
|
||||
|
||||
/* Engine Control Functions */
|
||||
KernelStatus_t engine_control_init(void);
|
||||
KernelStatus_t engine_control_start(void);
|
||||
KernelStatus_t engine_control_stop(void);
|
||||
KernelStatus_t engine_control_get_sensor_data(EngineSensorData_t* data);
|
||||
KernelStatus_t engine_control_get_actuator_data(EngineActuatorData_t* data);
|
||||
KernelStatus_t engine_control_set_actuator_data(const EngineActuatorData_t* data);
|
||||
EngineState_t engine_control_get_state(void);
|
||||
KernelStatus_t engine_control_get_faults(EngineFaultCode_t* faults,
|
||||
uint8_t* count);
|
||||
KernelStatus_t engine_control_clear_faults(void);
|
||||
void engine_control_task(void* parameters);
|
||||
void fuel_injection_task(void* parameters);
|
||||
void ignition_control_task(void* parameters);
|
||||
void sensor_reading_task(void* parameters);
|
||||
|
||||
#endif /* ENGINE_CONTROL_H */
|
||||
@@ -0,0 +1,177 @@
|
||||
/**
|
||||
* @file engine_parameters.h
|
||||
* @brief Engine control parameters and calibration data
|
||||
*/
|
||||
|
||||
#ifndef ENGINE_PARAMETERS_H
|
||||
#define ENGINE_PARAMETERS_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* Engine Operating Parameters */
|
||||
#define ENGINE_MAX_RPM 6500
|
||||
#define ENGINE_IDLE_RPM 800
|
||||
#define ENGINE_REDLINE_RPM 6000
|
||||
#define ENGINE_MAX_TORQUE_RPM 4000
|
||||
#define ENGINE_MAX_POWER_RPM 5500
|
||||
|
||||
/* Temperature Limits */
|
||||
#define ENGINE_MAX_COOLANT_TEMP 120 /* °C */
|
||||
#define ENGINE_MIN_COOLANT_TEMP -40 /* °C */
|
||||
#define ENGINE_OPTIMAL_TEMP 90 /* °C */
|
||||
#define ENGINE_MAX_OIL_TEMP 150 /* °C */
|
||||
#define ENGINE_MAX_INTAKE_TEMP 80 /* °C */
|
||||
|
||||
/* Pressure Limits */
|
||||
#define ENGINE_MAX_MANIFOLD_PRESSURE 250 /* kPa */
|
||||
#define ENGINE_MIN_OIL_PRESSURE 100 /* kPa */
|
||||
#define ENGINE_MAX_FUEL_PRESSURE 500 /* kPa */
|
||||
|
||||
/* Fuel System Parameters */
|
||||
#define FUEL_STOICHIOMETRIC_RATIO 14.7f
|
||||
#define FUEL_MAX_INJECTION_TIME 20.0f /* ms */
|
||||
#define FUEL_MIN_INJECTION_TIME 0.5f /* ms */
|
||||
#define FUEL_INJECTOR_FLOW_RATE 250.0f /* cc/min */
|
||||
|
||||
/* Ignition Parameters */
|
||||
#define IGNITION_MAX_ADVANCE 45.0f /* degrees BTDC */
|
||||
#define IGNITION_MIN_ADVANCE -10.0f /* degrees ATDC */
|
||||
#define IGNITION_BASE_ADVANCE 10.0f /* degrees BTDC */
|
||||
#define IGNITION_DWELL_TIME 3.5f /* ms */
|
||||
|
||||
/* Control Loop Parameters */
|
||||
#define ENGINE_CONTROL_PERIOD_MS 1 /* 1ms control loop */
|
||||
#define FUEL_CONTROL_PERIOD_MS 10 /* 10ms fuel update */
|
||||
#define IGNITION_CONTROL_PERIOD_MS 5 /* 5ms ignition update */
|
||||
#define SENSOR_READ_PERIOD_MS 2 /* 2ms sensor reading */
|
||||
|
||||
/* PID Controller Gains */
|
||||
typedef struct {
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
float integral_limit;
|
||||
float output_limit;
|
||||
} PidGains_t;
|
||||
|
||||
/* Fuel Control PID */
|
||||
static const PidGains_t fuel_pid_gains = {
|
||||
.kp = 0.5f,
|
||||
.ki = 0.1f,
|
||||
.kd = 0.05f,
|
||||
.integral_limit = 100.0f,
|
||||
.output_limit = 100.0f
|
||||
};
|
||||
|
||||
/* Idle Control PID */
|
||||
static const PidGains_t idle_pid_gains = {
|
||||
.kp = 0.8f,
|
||||
.ki = 0.2f,
|
||||
.kd = 0.1f,
|
||||
.integral_limit = 50.0f,
|
||||
.output_limit = 100.0f
|
||||
};
|
||||
|
||||
/* Boost Control PID */
|
||||
static const PidGains_t boost_pid_gains = {
|
||||
.kp = 0.3f,
|
||||
.ki = 0.05f,
|
||||
.kd = 0.02f,
|
||||
.integral_limit = 200.0f,
|
||||
.output_limit = 250.0f
|
||||
};
|
||||
|
||||
/* Engine State Enumeration */
|
||||
typedef enum {
|
||||
ENGINE_STATE_OFF = 0,
|
||||
ENGINE_STATE_CRANKING = 1,
|
||||
ENGINE_STATE_RUNNING = 2,
|
||||
ENGINE_STATE_IDLE = 3,
|
||||
ENGINE_STATE_ACCELERATING = 4,
|
||||
ENGINE_STATE_DECELERATING = 5,
|
||||
ENGINE_STATE_FAULT = 6,
|
||||
ENGINE_STATE_LIMP_HOME = 7
|
||||
} EngineState_t;
|
||||
|
||||
/* Engine Sensor Data */
|
||||
typedef struct {
|
||||
uint16_t rpm;
|
||||
uint16_t vehicle_speed;
|
||||
int16_t coolant_temp;
|
||||
int16_t intake_air_temp;
|
||||
int16_t oil_temp;
|
||||
uint16_t manifold_pressure;
|
||||
uint16_t oil_pressure;
|
||||
uint16_t fuel_pressure;
|
||||
uint16_t throttle_position;
|
||||
uint16_t accelerator_pedal;
|
||||
float mass_air_flow;
|
||||
float lambda;
|
||||
float battery_voltage;
|
||||
} EngineSensorData_t;
|
||||
|
||||
/* Engine Actuator Data */
|
||||
typedef struct {
|
||||
uint16_t injector_pulse_width;
|
||||
float ignition_advance;
|
||||
uint16_t idle_air_control;
|
||||
uint16_t boost_control;
|
||||
uint16_t fuel_pump_duty;
|
||||
uint16_t cooling_fan_duty;
|
||||
} EngineActuatorData_t;
|
||||
|
||||
/* Engine Fault Codes */
|
||||
typedef enum {
|
||||
ENGINE_FAULT_NONE = 0,
|
||||
ENGINE_FAULT_COOLANT_TEMP_SENSOR = 1,
|
||||
ENGINE_FAULT_INTAKE_TEMP_SENSOR = 2,
|
||||
ENGINE_FAULT_MANIFOLD_PRESSURE_SENSOR = 3,
|
||||
ENGINE_FAULT_MAF_SENSOR = 4,
|
||||
ENGINE_FAULT_OXYGEN_SENSOR = 5,
|
||||
ENGINE_FAULT_KNOCK_SENSOR = 6,
|
||||
ENGINE_FAULT_CRANKSHAFT_SENSOR = 7,
|
||||
ENGINE_FAULT_CAMSHAFT_SENSOR = 8,
|
||||
ENGINE_FAULT_INJECTOR_1 = 9,
|
||||
ENGINE_FAULT_INJECTOR_2 = 10,
|
||||
ENGINE_FAULT_INJECTOR_3 = 11,
|
||||
ENGINE_FAULT_INJECTOR_4 = 12,
|
||||
ENGINE_FAULT_IGNITION_COIL = 13,
|
||||
ENGINE_FAULT_FUEL_PUMP = 14,
|
||||
ENGINE_FAULT_OVERHEAT = 15,
|
||||
ENGINE_FAULT_LOW_OIL_PRESSURE = 16
|
||||
} EngineFaultCode_t;
|
||||
|
||||
/* Lookup Tables */
|
||||
typedef struct {
|
||||
const uint16_t* rpm_points;
|
||||
const uint16_t* load_points;
|
||||
const uint16_t* values;
|
||||
uint8_t rpm_count;
|
||||
uint8_t load_count;
|
||||
} LookupTable2D_t;
|
||||
|
||||
/* Fuel Map (injection time in microseconds) */
|
||||
static const uint16_t fuel_map_rpm[] = {0, 500, 1000, 2000, 3000, 4000, 5000, 6000};
|
||||
static const uint16_t fuel_map_load[] = {0, 20, 40, 60, 80, 100};
|
||||
static const uint16_t fuel_map_values[][8] = {
|
||||
{0, 1000, 800, 700, 650, 600, 550, 500},
|
||||
{1000, 1500, 1300, 1200, 1100, 1000, 950, 900},
|
||||
{2000, 2500, 2200, 2000, 1800, 1700, 1600, 1500},
|
||||
{3000, 3500, 3200, 3000, 2800, 2600, 2400, 2200},
|
||||
{4000, 4500, 4200, 4000, 3800, 3600, 3400, 3200},
|
||||
{5000, 5500, 5200, 5000, 4800, 4600, 4400, 4200}
|
||||
};
|
||||
|
||||
/* Ignition Advance Map (degrees BTDC) */
|
||||
static const uint16_t ignition_map_rpm[] = {0, 500, 1000, 2000, 3000, 4000, 5000, 6000};
|
||||
static const uint16_t ignition_map_load[] = {0, 20, 40, 60, 80, 100};
|
||||
static const int16_t ignition_map_values[][8] = {
|
||||
{10, 12, 15, 18, 20, 22, 25, 28},
|
||||
{10, 12, 15, 18, 20, 22, 25, 28},
|
||||
{8, 10, 13, 16, 18, 20, 23, 26},
|
||||
{6, 8, 11, 14, 16, 18, 21, 24},
|
||||
{4, 6, 9, 12, 14, 16, 19, 22},
|
||||
{2, 4, 7, 10, 12, 14, 17, 20}
|
||||
};
|
||||
|
||||
#endif /* ENGINE_PARAMETERS_H */
|
||||
@@ -0,0 +1,366 @@
|
||||
/**
|
||||
* @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;
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
/**
|
||||
* @file fuel_injection.c
|
||||
* @brief Fuel injection control
|
||||
*/
|
||||
|
||||
#include "engine_control.h"
|
||||
#include "fuel_injection.h"
|
||||
#include "adc_driver.h"
|
||||
#include "pwm_driver.h"
|
||||
#include <math.h>
|
||||
|
||||
/* Fuel Injection State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
float fuel_pressure;
|
||||
float injection_time;
|
||||
uint16_t injector_duty;
|
||||
uint8_t injection_mode; /* 0=sequential, 1=batch, 2=simultaneous */
|
||||
Mutex_t mutex;
|
||||
} FuelInjectionState_t;
|
||||
|
||||
static FuelInjectionState_t fuel_injection;
|
||||
|
||||
/* Initialize Fuel Injection */
|
||||
KernelStatus_t fuel_injection_init(void) {
|
||||
if (fuel_injection.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
fuel_injection.fuel_pressure = 0;
|
||||
fuel_injection.injection_time = 0;
|
||||
fuel_injection.injector_duty = 0;
|
||||
fuel_injection.injection_mode = 0; /* Sequential */
|
||||
|
||||
mutex_create(&fuel_injection.mutex, false);
|
||||
|
||||
fuel_injection.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Fuel Injection Control Task */
|
||||
void fuel_injection_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Wait for next fuel update period */
|
||||
kernel_delay(FUEL_CONTROL_PERIOD_MS);
|
||||
|
||||
EngineSensorData_t sensors;
|
||||
engine_control_get_sensor_data(&sensors);
|
||||
|
||||
/* Calculate required fuel */
|
||||
float required_fuel = calculate_fuel_requirement(&sensors);
|
||||
|
||||
/* Apply corrections */
|
||||
required_fuel = apply_fuel_corrections(required_fuel, &sensors);
|
||||
|
||||
/* Convert to injector pulse width */
|
||||
uint16_t pulse_width = (uint16_t)(required_fuel * 1000); /* Convert to microseconds */
|
||||
|
||||
/* Limit pulse width */
|
||||
if (pulse_width > (uint16_t)(FUEL_MAX_INJECTION_TIME * 1000)) {
|
||||
pulse_width = (uint16_t)(FUEL_MAX_INJECTION_TIME * 1000);
|
||||
}
|
||||
|
||||
/* Update injector PWM */
|
||||
mutex_lock(&fuel_injection.mutex, 100);
|
||||
fuel_injection.injection_time = required_fuel;
|
||||
fuel_injection.injector_duty = calculate_injector_duty(pulse_width,
|
||||
sensors.rpm);
|
||||
mutex_unlock(&fuel_injection.mutex);
|
||||
|
||||
/* Update PWM output */
|
||||
pwm_set_duty_cycle(0, 0, fuel_injection.injector_duty); /* Injector 1 */
|
||||
pwm_set_duty_cycle(0, 1, fuel_injection.injector_duty); /* Injector 2 */
|
||||
}
|
||||
}
|
||||
|
||||
/* Calculate Fuel Requirement */
|
||||
static float calculate_fuel_requirement(const EngineSensorData_t* sensors) {
|
||||
/* Basic fuel calculation using speed-density method */
|
||||
float air_mass = 0;
|
||||
|
||||
if (sensors->mass_air_flow > 0) {
|
||||
/* Use MAF sensor if available */
|
||||
air_mass = sensors->mass_air_flow / (sensors->rpm / 2);
|
||||
} else {
|
||||
/* Speed-density calculation */
|
||||
float air_density = 1.225f; /* kg/m³ at sea level */
|
||||
float engine_displacement = 2.0f; /* 2.0L engine */
|
||||
float volumetric_efficiency = 0.85f;
|
||||
|
||||
air_mass = air_density * engine_displacement *
|
||||
volumetric_efficiency * sensors->manifold_pressure / 101.3f;
|
||||
}
|
||||
|
||||
/* Calculate fuel mass for stoichiometric mixture */
|
||||
float fuel_mass = air_mass / FUEL_STOICHIOMETRIC_RATIO;
|
||||
|
||||
/* Convert to injection time */
|
||||
float injection_time = (fuel_mass * 1000000) / FUEL_INJECTOR_FLOW_RATE;
|
||||
|
||||
return injection_time;
|
||||
}
|
||||
|
||||
/* Apply Fuel Corrections */
|
||||
static float apply_fuel_corrections(float base_fuel, const EngineSensorData_t* sensors) {
|
||||
float corrected_fuel = base_fuel;
|
||||
|
||||
/* Lambda correction */
|
||||
if (sensors->lambda > 0) {
|
||||
float lambda_error = 1.0f - sensors->lambda;
|
||||
corrected_fuel *= (1.0f + lambda_error * 0.5f);
|
||||
}
|
||||
|
||||
/* Coolant temperature correction */
|
||||
if (sensors->coolant_temp < 70) {
|
||||
float enrichment = (70 - sensors->coolant_temp) * 0.01f;
|
||||
corrected_fuel *= (1.0f + enrichment);
|
||||
}
|
||||
|
||||
/* Acceleration enrichment */
|
||||
if (sensors->accelerator_pedal > 80) {
|
||||
corrected_fuel *= 1.2f;
|
||||
}
|
||||
|
||||
/* Battery voltage correction */
|
||||
if (sensors->battery_voltage < 12.0f) {
|
||||
corrected_fuel *= (12.0f / sensors->battery_voltage);
|
||||
}
|
||||
|
||||
return corrected_fuel;
|
||||
}
|
||||
|
||||
/* Calculate Injector Duty Cycle */
|
||||
static uint16_t calculate_injector_duty(uint16_t pulse_width_us, uint16_t rpm) {
|
||||
/* Calculate period in microseconds */
|
||||
uint32_t period_us = (60000000UL) / rpm; /* 2 revolutions per cycle */
|
||||
|
||||
/* Calculate duty cycle */
|
||||
uint32_t duty = (pulse_width_us * PWM_MAX_DUTY_CYCLE) / period_us;
|
||||
|
||||
if (duty > PWM_MAX_DUTY_CYCLE) {
|
||||
duty = PWM_MAX_DUTY_CYCLE;
|
||||
}
|
||||
|
||||
return (uint16_t)duty;
|
||||
}
|
||||
|
||||
/* Get Fuel Injection Data */
|
||||
KernelStatus_t fuel_injection_get_data(float* pressure, float* injection_time,
|
||||
uint16_t* duty_cycle) {
|
||||
if (pressure == NULL || injection_time == NULL || duty_cycle == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&fuel_injection.mutex, 100);
|
||||
*pressure = fuel_injection.fuel_pressure;
|
||||
*injection_time = fuel_injection.injection_time;
|
||||
*duty_cycle = fuel_injection.injector_duty;
|
||||
mutex_unlock(&fuel_injection.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
/**
|
||||
* @file ignition_control.c
|
||||
* @brief Ignition timing control
|
||||
*/
|
||||
|
||||
#include "engine_control.h"
|
||||
#include "ignition_control.h"
|
||||
#include "gpio_driver.h"
|
||||
#include <math.h>
|
||||
|
||||
/* Ignition Control State */
|
||||
typedef struct {
|
||||
bool initialized;
|
||||
float ignition_advance;
|
||||
float dwell_time;
|
||||
uint8_t ignition_mode;
|
||||
bool knock_detected;
|
||||
uint32_t knock_count;
|
||||
Mutex_t mutex;
|
||||
} IgnitionControlState_t;
|
||||
|
||||
static IgnitionControlState_t ignition_control;
|
||||
|
||||
/* Initialize Ignition Control */
|
||||
KernelStatus_t ignition_control_init(void) {
|
||||
if (ignition_control.initialized) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
ignition_control.ignition_advance = IGNITION_BASE_ADVANCE;
|
||||
ignition_control.dwell_time = IGNITION_DWELL_TIME;
|
||||
ignition_control.ignition_mode = 0;
|
||||
ignition_control.knock_detected = false;
|
||||
ignition_control.knock_count = 0;
|
||||
|
||||
mutex_create(&ignition_control.mutex, false);
|
||||
|
||||
ignition_control.initialized = true;
|
||||
return KERNEL_OK;
|
||||
}
|
||||
|
||||
/* Ignition Control Task */
|
||||
void ignition_control_task(void* parameters) {
|
||||
(void)parameters;
|
||||
|
||||
while (1) {
|
||||
/* Wait for next ignition update period */
|
||||
kernel_delay(IGNITION_CONTROL_PERIOD_MS);
|
||||
|
||||
EngineSensorData_t sensors;
|
||||
engine_control_get_sensor_data(&sensors);
|
||||
|
||||
/* Calculate ignition advance */
|
||||
float advance = calculate_ignition_advance(&sensors);
|
||||
|
||||
/* Apply knock correction */
|
||||
if (ignition_control.knock_detected) {
|
||||
advance -= 5.0f; /* Retard timing */
|
||||
ignition_control.knock_detected = false;
|
||||
ignition_control.knock_count++;
|
||||
}
|
||||
|
||||
/* Limit advance */
|
||||
if (advance > IGNITION_MAX_ADVANCE) {
|
||||
advance = IGNITION_MAX_ADVANCE;
|
||||
} else if (advance < IGNITION_MIN_ADVANCE) {
|
||||
advance = IGNITION_MIN_ADVANCE;
|
||||
}
|
||||
|
||||
/* Update ignition control */
|
||||
mutex_lock(&ignition_control.mutex, 100);
|
||||
ignition_control.ignition_advance = advance;
|
||||
mutex_unlock(&ignition_control.mutex);
|
||||
|
||||
/* Update actuator */
|
||||
EngineActuatorData_t actuators;
|
||||
engine_control_get_actuator_data(&actuators);
|
||||
actuators.ignition_advance = advance;
|
||||
engine_control_set_actuator_data(&actuators);
|
||||
}
|
||||
}
|
||||
|
||||
/* Calculate Ignition Advance */
|
||||
static float calculate_ignition_advance(const EngineSensorData_t* sensors) {
|
||||
float advance = IGNITION_BASE_ADVANCE;
|
||||
|
||||
/* RPM correction */
|
||||
if (sensors->rpm < 1000) {
|
||||
advance += 5.0f; /* More advance at low RPM */
|
||||
} else if (sensors->rpm > 5000) {
|
||||
advance -= 3.0f; /* Less advance at high RPM */
|
||||
}
|
||||
|
||||
/* Load correction */
|
||||
if (sensors->manifold_pressure < 40) {
|
||||
advance += 2.0f; /* More advance at light load */
|
||||
} else if (sensors->manifold_pressure > 80) {
|
||||
advance -= 4.0f; /* Less advance at high load */
|
||||
}
|
||||
|
||||
/* Temperature correction */
|
||||
if (sensors->coolant_temp < 0) {
|
||||
advance += 3.0f; /* More advance when cold */
|
||||
} else if (sensors->coolant_temp > 100) {
|
||||
advance -= 5.0f; /* Less advance when hot */
|
||||
}
|
||||
|
||||
return advance;
|
||||
}
|
||||
|
||||
/* Detect Knock */
|
||||
void ignition_control_detect_knock(void) {
|
||||
/* Read knock sensor */
|
||||
uint16_t knock_signal = adc_read_single(7);
|
||||
|
||||
/* Check for knock */
|
||||
if (knock_signal > 200) {
|
||||
ignition_control.knock_detected = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get Ignition Data */
|
||||
KernelStatus_t ignition_control_get_data(float* advance, float* dwell) {
|
||||
if (advance == NULL || dwell == NULL) {
|
||||
return KERNEL_ERROR;
|
||||
}
|
||||
|
||||
mutex_lock(&ignition_control.mutex, 100);
|
||||
*advance = ignition_control.ignition_advance;
|
||||
*dwell = ignition_control.dwell_time;
|
||||
mutex_unlock(&ignition_control.mutex);
|
||||
|
||||
return KERNEL_OK;
|
||||
}
|
||||
Reference in New Issue
Block a user