Skip to content

Commit

Permalink
more formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@fregely
fregely committed Jan 30, 2025
1 parent edb0993 commit 64184c9
Showing 1 changed file with 107 additions and 111 deletions.
218 changes: 107 additions & 111 deletions Core/Src/fault.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@
#include "cerberus_conf.h"
#include "state_machine.h"

#define FAULT_HANDLE_QUEUE_SIZE 16
#define NUM_OF_FAULTS 18UL
#define SEND_FAULT_TIME 500 /* in millis */
#define FAULT_HANDLE_QUEUE_SIZE 16
#define NUM_OF_FAULTS 18UL
#define SEND_FAULT_TIME 500 /* in millis */

osMessageQueueId_t fault_handle_high_priority_queue;
osMessageQueueId_t fault_handle_low_priority_queue;
Expand All @@ -24,141 +24,137 @@ fault_sev_t* severity_levels = NULL;

osStatus_t queue_fault(fault_data_t* fault_data)
{
if (!fault_handle_high_priority_queue || !fault_handle_low_priority_queue)
return -1;
if (!fault_handle_high_priority_queue || !fault_handle_low_priority_queue)
return -1;

if (fault_data->severity <= DEFCON3) {
osStatus_t status = osMessageQueuePut(fault_handle_high_priority_queue, fault_data, 0U, 0U);
} else {
osStatus_t status = osMessageQueuePut(fault_handle_low_priority_queue, fault_data, 0U, 0U);
}
if (fault_data->severity <= DEFCON3) {
osStatus_t status = osMessageQueuePut(fault_handle_high_priority_queue, fault_data, 0U, 0U);
} else {
osStatus_t status = osMessageQueuePut(fault_handle_low_priority_queue, fault_data, 0U, 0U);
}

return status;
return status;
}

osThreadId_t fault_handle;
const osThreadAttr_t fault_handle_attributes = {
.name = "FaultHandler",
.stack_size = 64 * 16,
.priority = (osPriority_t)osPriorityRealtime7,
.name = "FaultHandler",
.stack_size = 64 * 16,
.priority = (osPriority_t)osPriorityRealtime7,
};

void vFaultHandler(void* pv_params)
{
// Create Timers Array
if (timers == NULL) {
timers = calloc(NUM_OF_FAULTS, sizeof(osTimerId_t));
}

// Create Severity Levels Array (all DEFCON_NONE initially)
if (severity_levels == NULL) {
severity_levels = calloc(NUM_OF_FAULTS, sizeof(fault_sev_t));
for (int i = 0; i < NUM_OF_FAULTS; i++) {
severity_levels[i] = DEFCON_NONE;
}
// Create Timers Array
if (timers == NULL) {
timers = calloc(NUM_OF_FAULTS, sizeof(osTimerId_t));
}

// Create Severity Levels Array (all DEFCON_NONE initially)
if (severity_levels == NULL) {
severity_levels = calloc(NUM_OF_FAULTS, sizeof(fault_sev_t));
for (int i = 0; i < NUM_OF_FAULTS; i++) {
severity_levels[i] = DEFCON_NONE;
}

fault_data_t fault_data;
fault_handle_low_priority_queue
= osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE / 2, sizeof(fault_data_t), NULL);
fault_handle_low_priority_queue
= osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE / 2, sizeof(fault_data_t), NULL);
for (;;) {
if (osMessageQueueGet(high_priority_queue, &fault_data, NULL,
pdMS_TO_TICKS(SEND_FAULT_TIME))
== osOK) {
// Process high-priority fault
process_fault(fault_data);
} // If no high-priority fault, check low-priority queue
else if (osMessageQueueGet(low_priority_queue, &fault_data, NULL,
pdMS_TO_TICKS(SEND_FAULT_TIME))
== osOK) {
process_fault(fault_data);
}
}

fault_data_t fault_data;
fault_handle_low_priority_queue
= osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE / 2, sizeof(fault_data_t), NULL);
fault_handle_low_priority_queue
= osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE / 2, sizeof(fault_data_t), NULL);
for (;;) {
if (osMessageQueueGet(high_priority_queue, &fault_data, NULL,
pdMS_TO_TICKS(SEND_FAULT_TIME)) == osOK) {
// Process high-priority fault
process_fault(fault_data);
} else if (osMessageQueueGet(low_priority_queue, &fault_data, NULL,
pdMS_TO_TICKS(SEND_FAULT_TIME)) == osOK) {
// Process low-priority fault
process_fault(fault_data);
}
}
}



void process_fault(fault_data_t fault_data)
{
// Set Fault
uint32_t* fault_id = m alloc(sizeof(uint32_t));
*fault_id = (uint32_t)fault_data.id;
faults |= *fault_id;

uint32_t index = (uint32_t)log2(*fault_id);

// Create Timers
if (!timers[index]) {
timers[index] = osTimerNew(clearFault, osTimerOnce, fault_id, NULL);
}

if (osTimerStart(timers[index], 4000) != osOK) {
return;
}

// Get New Maximum Severity Level
severity_levels[index] = fault_data.severity;
max_severity_level = getMaxSeverity();

switch (fault_data.severity) {
case DEFCON1: /* Highest(1st) Priority */
fault();
break;
case DEFCON2:
fault();
break;
case DEFCON3:
fault();
break;
case DEFCON4:
break;
case DEFCON5: /* Lowest Priority */
break;
case DEFCON_NONE:
break;
default:
break;
}

// Send Can Message (even if a new fault was not received)
can_msg_t msg;
msg.id = CANID_FAULT_MSG;
msg.len = 8;

memcpy(msg.data, &faults, sizeof(faults));
memcpy(msg.data + sizeof(faults), &max_severity_level, sizeof(max_severity_level));

queue_can_msg(msg);
// Set Fault
uint32_t* fault_id = malloc(sizeof(uint32_t));
*fault_id = (uint32_t)fault_data.id;
faults |= *fault_id;

uint32_t index = (uint32_t)log2(*fault_id);

// Create Timers
if (!timers[index]) {
timers[index] = osTimerNew(clearFault, osTimerOnce, fault_id, NULL);
}

if (osTimerStart(timers[index], 4000) != osOK) {
return;
}

// Get New Maximum Severity Level
severity_levels[index] = fault_data.severity;
max_severity_level = getMaxSeverity();

switch (fault_data.severity) {
case DEFCON1: /* Highest(1st) Priority */
fault();
break;
case DEFCON2:
fault();
break;
case DEFCON3:
fault();
break;
case DEFCON4:
break;
case DEFCON5: /* Lowest Priority */
break;
case DEFCON_NONE:
break;
default:
break;
}

// Send Can Message (even if a new fault was not received)
can_msg_t msg;
msg.id = CANID_FAULT_MSG;
msg.len = 8;

memcpy(msg.data, &faults, sizeof(faults));
memcpy(msg.data + sizeof(faults), &max_severity_level, sizeof(max_severity_level));

queue_can_msg(msg);
}

void clearFault(void* args)
{
uint32_t* fault_id = (uint32_t*)args;
uint32_t* fault_id = (uint32_t*)args;

// Remove this timer's fault from total faults
faults &= ~(*fault_id);
// Remove this timer's fault from total faults
faults &= ~(*fault_id);

// Remove this timer's severity from total severity
severity_levels[(uint32_t)log2(*fault_id)] = DEFCON_NONE;
max_severity_level = getMaxSeverity();
// Remove this timer's severity from total severity
severity_levels[(uint32_t)log2(*fault_id)] = DEFCON_NONE;
max_severity_level = getMaxSeverity();

// unfault car if all critical faults are cleared
if (max_severity_level > DEFCON3) {
set_ready_mode();
}
// unfault car if all critical faults are cleared
if (max_severity_level > DEFCON3) {
set_ready_mode();
}

free(fault_id);
free(fault_id);
}

fault_sev_t getMaxSeverity()
{
int max = DEFCON_NONE;
for (int i = 0; i < NUM_OF_FAULTS; i++) {
if (severity_levels[i] < max) {
max = severity_levels[i];
}
int max = DEFCON_NONE;
for (int i = 0; i < NUM_OF_FAULTS; i++) {
if (severity_levels[i] < max) {
max = severity_levels[i];
}
return (fault_sev_t)max;
}
return (fault_sev_t)max;
}

0 comments on commit 64184c9

Please sign in to comment.