From 25836178befcbcc1c5ed7454b1fe0be5222257cd Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Wed, 4 Sep 2024 21:17:08 -0400 Subject: [PATCH] restore order --- Core/Src/bms.c | 30 ++-- Core/Src/can_handler.c | 38 ++--- Core/Src/fault.c | 22 +-- Core/Src/monitor.c | 306 +++++++++++++++++++++++++---------------- shell.nix | 15 ++ 5 files changed, 255 insertions(+), 156 deletions(-) create mode 100644 shell.nix diff --git a/Core/Src/bms.c b/Core/Src/bms.c index 77ddfd2..166ae55 100644 --- a/Core/Src/bms.c +++ b/Core/Src/bms.c @@ -12,19 +12,23 @@ osMessageQueueId_t bms_monitor_queue; -bms_t* bms; +bms_t *bms; void bms_fault_callback(); osThreadId_t bms_monitor_handle; -const osThreadAttr_t bms_monitor_attributes - = { .name = "BMSCANMonitor", .stack_size = 64 * 8, .priority = (osPriority_t)osPriorityHigh2 }; +const osThreadAttr_t bms_monitor_attributes = { + .name = "BMSCANMonitor", + .stack_size = 64 * 8, + .priority = (osPriority_t)osPriorityHigh2 +}; void bms_fault_callback() { fault_data_t fault_data - = { .id = BMS_CAN_MONITOR_FAULT, .severity = DEFCON1 }; /*TO-DO: update severity*/ + = { .id = BMS_CAN_MONITOR_FAULT, + .severity = DEFCON1 }; /*TO-DO: update severity*/ fault_data.diag = "Failing To Receive CAN Messages from Shepherd"; osTimerStart(bms->bms_monitor_timer, BMS_CAN_MONITOR_DELAY); queue_fault(&fault_data); @@ -36,22 +40,28 @@ void bms_init() assert(bms); /*TO-DO: specify timer attributes*/ - bms->bms_monitor_timer = osTimerNew(&bms_fault_callback, osTimerOnce, NULL, NULL); + bms->bms_monitor_timer = + osTimerNew(&bms_fault_callback, osTimerOnce, NULL, NULL); - bms_monitor_queue = osMessageQueueNew(CAN_QUEUE_SIZE, sizeof(can_msg_t), NULL); + bms_monitor_queue = + osMessageQueueNew(CAN_QUEUE_SIZE, sizeof(can_msg_t), NULL); assert(bms_monitor_queue); } -void vBMSCANMonitor(void* pv_params) +void vBMSCANMonitor(void *pv_params) { can_msg_t msg_from_queue; osTimerStart(bms->bms_monitor_timer, BMS_CAN_MONITOR_DELAY); for (;;) { - if (osOK == osMessageQueueGet(bms_monitor_queue, &msg_from_queue, NULL, osWaitForever)) { + if (osOK == osMessageQueueGet(bms_monitor_queue, + &msg_from_queue, NULL, + osWaitForever)) { /*TO-DO: fix duration (ticks)*/ - osTimerStart(bms->bms_monitor_timer, BMS_CAN_MONITOR_DELAY); - bms->dcl = (uint16_t)((msg_from_queue.data[1] << 8) & msg_from_queue.data[0]); + osTimerStart(bms->bms_monitor_timer, + BMS_CAN_MONITOR_DELAY); + bms->dcl = (uint16_t)((msg_from_queue.data[1] << 8) & + msg_from_queue.data[0]); // serial_print("BMS DCL %d", bms->dcl); } } diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c index 01b9d72..9d06085 100644 --- a/Core/Src/can_handler.c +++ b/Core/Src/can_handler.c @@ -25,13 +25,14 @@ #define CAN_MSG_QUEUE_SIZE 50 /* messages */ static osMessageQueueId_t can_outbound_queue; -can_t* can1; +can_t *can1; /* Relevant Info for Initializing CAN 1 */ -static uint32_t id_list[] = { DTI_CANID_ERPM, DTI_CANID_CURRENTS, DTI_CANID_TEMPS_FAULT, - DTI_CANID_ID_IQ, DTI_CANID_SIGNALS, BMS_DCL_MSG }; +static uint32_t id_list[] = { DTI_CANID_ERPM, DTI_CANID_CURRENTS, + DTI_CANID_TEMPS_FAULT, DTI_CANID_ID_IQ, + DTI_CANID_SIGNALS, BMS_DCL_MSG }; -void init_can1(CAN_HandleTypeDef* hcan) +void init_can1(CAN_HandleTypeDef *hcan) { assert(hcan); @@ -39,20 +40,21 @@ void init_can1(CAN_HandleTypeDef* hcan) can1 = malloc(sizeof(can_t)); assert(can1); - can1->hcan = hcan; - can1->id_list = id_list; + can1->hcan = hcan; + can1->id_list = id_list; can1->id_list_len = sizeof(id_list) / sizeof(uint32_t); assert(!can_init(can1)); - can_outbound_queue = osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL); + can_outbound_queue = + osMessageQueueNew(CAN_MSG_QUEUE_SIZE, sizeof(can_msg_t), NULL); } /* Callback to be called when we get a CAN message */ -void can1_callback(CAN_HandleTypeDef* hcan) +void can1_callback(CAN_HandleTypeDef *hcan) { fault_data_t fault_data = { - .id = CAN_ROUTING_FAULT, + .id = CAN_ROUTING_FAULT, .severity = DEFCON2, }; @@ -60,14 +62,15 @@ void can1_callback(CAN_HandleTypeDef* hcan) can_msg_t new_msg; /* Read in CAN message */ - if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, new_msg.data) != HAL_OK) { + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, + new_msg.data) != HAL_OK) { fault_data.diag = "Failed to read CAN Msg"; queue_fault(&fault_data); return; } new_msg.len = rx_header.DLC; - new_msg.id = rx_header.StdId; + new_msg.id = rx_header.StdId; // TODO: Switch to hash map switch (new_msg.id) { @@ -90,21 +93,24 @@ void can1_callback(CAN_HandleTypeDef* hcan) osThreadId_t can_dispatch_handle; const osThreadAttr_t can_dispatch_attributes = { - .name = "CanDispatch", + .name = "CanDispatch", .stack_size = 128 * 8, - .priority = (osPriority_t)osPriorityRealtime5, + .priority = (osPriority_t)osPriorityRealtime5, }; -void vCanDispatch(void* pv_params) +void vCanDispatch(void *pv_params) { - fault_data_t fault_data = { .id = CAN_DISPATCH_FAULT, .severity = DEFCON1 }; + fault_data_t fault_data = { .id = CAN_DISPATCH_FAULT, + .severity = DEFCON1 }; can_msg_t msg_from_queue; HAL_StatusTypeDef msg_status; for (;;) { /* Send CAN message */ - if (osOK == osMessageQueueGet(can_outbound_queue, &msg_from_queue, NULL, osWaitForever)) { + if (osOK == osMessageQueueGet(can_outbound_queue, + &msg_from_queue, NULL, + osWaitForever)) { msg_status = can_send_msg(can1, &msg_from_queue); if (msg_status == HAL_ERROR) { fault_data.diag = "Failed to send CAN message"; diff --git a/Core/Src/fault.c b/Core/Src/fault.c index a415dc7..7e84e4e 100644 --- a/Core/Src/fault.c +++ b/Core/Src/fault.c @@ -14,12 +14,12 @@ osMessageQueueId_t fault_handle_queue; osThreadId_t fault_handle; const osThreadAttr_t fault_handle_attributes = { - .name = "FaultHandler", + .name = "FaultHandler", .stack_size = 52 * 8, - .priority = (osPriority_t)osPriorityRealtime1, + .priority = (osPriority_t)osPriorityRealtime1, }; -int queue_fault(fault_data_t* fault_data) +int queue_fault(fault_data_t *fault_data) { if (!fault_handle_queue) return -1; @@ -28,30 +28,34 @@ int queue_fault(fault_data_t* fault_data) return 0; } -void vFaultHandler(void* pv_params) +void vFaultHandler(void *pv_params) { fault_data_t fault_data; osStatus_t status; - const state_req_t fault_request = { .id = FUNCTIONAL, .state.functional = FAULTED }; - fault_handle_queue = osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE, sizeof(fault_data_t), NULL); + const state_req_t fault_request = { .id = FUNCTIONAL, + .state.functional = FAULTED }; + fault_handle_queue = osMessageQueueNew(FAULT_HANDLE_QUEUE_SIZE, + sizeof(fault_data_t), NULL); for (;;) { /* Wait until a message is in the queue, send messages when they are in the queue */ - status = osMessageQueueGet(fault_handle_queue, &fault_data, NULL, osWaitForever); + status = osMessageQueueGet(fault_handle_queue, &fault_data, + NULL, osWaitForever); if (status == osOK) { uint32_t fault_id = (uint32_t)fault_data.id; endian_swap(&fault_id, 4); uint8_t defcon = (uint8_t)fault_data.severity; can_msg_t msg; - msg.id = CANID_FAULT_MSG; + msg.id = CANID_FAULT_MSG; msg.len = 8; uint8_t msg_data[8]; memcpy(msg_data, &fault_id, 4); memcpy(msg_data + 4, &defcon, 1); memcpy(msg.data, msg_data, msg.len); queue_can_msg(msg); - printf("\r\nFault Handler! Diagnostic Info:\t%s\r\n\r\n", fault_data.diag); + printf("\r\nFault Handler! Diagnostic Info:\t%s\r\n\r\n", + fault_data.diag); switch (fault_data.severity) { case DEFCON1: /* Highest(1st) Priority */ queue_state_transition(fault_request); diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 73d92d2..9ba4635 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -25,21 +25,22 @@ #define PEDAL_DIFF_THRESH 30 #define PEDAL_FAULT_TIME 500 /* ms */ -static bool tsms = false; +static bool tsms = false; static bool brake_state = false; osThreadId lv_monitor_handle; const osThreadAttr_t lv_monitor_attributes = { - .name = "LVMonitor", + .name = "LVMonitor", .stack_size = 32 * 8, - .priority = (osPriority_t)osPriorityHigh4, + .priority = (osPriority_t)osPriorityHigh4, }; -void vLVMonitor(void* pv_params) +void vLVMonitor(void *pv_params) { - mpu_t* mpu = (mpu_t*)pv_params; - fault_data_t fault_data = { .id = LV_MONITOR_FAULT, .severity = DEFCON5 }; - can_msg_t msg = { .id = CANID_LV_MONITOR, .len = 4, .data = { 0 } }; + mpu_t *mpu = (mpu_t *)pv_params; + fault_data_t fault_data = { .id = LV_MONITOR_FAULT, + .severity = DEFCON5 }; + can_msg_t msg = { .id = CANID_LV_MONITOR, .len = 4, .data = { 0 } }; uint32_t v_int; @@ -58,7 +59,8 @@ void vLVMonitor(void* pv_params) // endian_swap(&v_int, sizeof(v_int)); memcpy(msg.data, &v_int, msg.len); if (queue_can_msg(msg)) { - fault_data.diag = "Failed to send steering buttons can message"; + fault_data.diag = + "Failed to send steering buttons can message"; queue_fault(&fault_data); } @@ -68,9 +70,9 @@ void vLVMonitor(void* pv_params) osThreadId_t temp_monitor_handle; const osThreadAttr_t temp_monitor_attributes = { - .name = "TempMonitor", + .name = "TempMonitor", .stack_size = 32 * 8, - .priority = (osPriority_t)osPriorityHigh1, + .priority = (osPriority_t)osPriorityHigh1, }; bool get_brake_state() @@ -78,16 +80,19 @@ bool get_brake_state() return brake_state; } -void vTempMonitor(void* pv_params) +void vTempMonitor(void *pv_params) { - fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT, .severity = DEFCON5 }; - can_msg_t temp_msg = { .id = CANID_TEMP_SENSOR, .len = 4, .data = { 0 } }; + fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT, + .severity = DEFCON5 }; + can_msg_t temp_msg = { .id = CANID_TEMP_SENSOR, + .len = 4, + .data = { 0 } }; - mpu_t* mpu = (mpu_t*)pv_params; + mpu_t *mpu = (mpu_t *)pv_params; for (;;) { /* Take measurement */ - uint16_t temp = 0; + uint16_t temp = 0; uint16_t humidity = 0; if (read_temp_sensor(mpu, &temp, &humidity)) { fault_data.diag = "Failed to get temp"; @@ -114,25 +119,30 @@ void vTempMonitor(void* pv_params) osThreadId_t pedals_monitor_handle; const osThreadAttr_t pedals_monitor_attributes = { - .name = "PedalMonitor", + .name = "PedalMonitor", .stack_size = 128 * 8, - .priority = (osPriority_t)osPriorityRealtime1, + .priority = (osPriority_t)osPriorityRealtime1, }; -void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t* diff_timer, - nertimer_t* sc_timer, nertimer_t* oc_timer, fault_data_t* fault_data) +void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, + nertimer_t *diff_timer, nertimer_t *sc_timer, + nertimer_t *oc_timer, fault_data_t *fault_data) { /* Fault - open circuit (Max ADC value + some a lil bit) */ - if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) - && is_timer_active(oc_timer)) { + if ((accel_1 > (MAX_ADC_VAL_12b - 20) || + accel_2 > (MAX_ADC_VAL_12b - 20)) && + is_timer_active(oc_timer)) { if (is_timer_expired(oc_timer)) { - if ((accel_1 == MAX_ADC_VAL_12b || accel_2 == MAX_ADC_VAL_12b)) { - fault_data->diag = "Pedal open circuit fault - max acceleration value "; + if ((accel_1 == MAX_ADC_VAL_12b || + accel_2 == MAX_ADC_VAL_12b)) { + fault_data->diag = + "Pedal open circuit fault - max acceleration value "; queue_fault(fault_data); } } - } else if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) - && !is_timer_active(oc_timer)) { + } else if ((accel_1 > (MAX_ADC_VAL_12b - 20) || + accel_2 > (MAX_ADC_VAL_12b - 20)) && + !is_timer_active(oc_timer)) { start_timer(oc_timer, PEDAL_FAULT_TIME); } else { cancel_timer(oc_timer); @@ -142,51 +152,65 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t* diff_timer if ((accel_1 < 500 || accel_2 < 500) && is_timer_active(sc_timer)) { if (is_timer_expired(sc_timer)) { if ((accel_1 < 500 || accel_2 < 500)) { - fault_data->diag = "Pedal short circuit fault - no acceleration value "; + fault_data->diag = + "Pedal short circuit fault - no acceleration value "; queue_fault(fault_data); } } - } else if ((accel_1 < 500 || accel_2 < 500) && !is_timer_active(sc_timer)) { + } else if ((accel_1 < 500 || accel_2 < 500) && + !is_timer_active(sc_timer)) { start_timer(sc_timer, PEDAL_FAULT_TIME); } else { cancel_timer(sc_timer); } /* Normalize pedal values */ - uint16_t accel_1_norm = accel_1 - ACCEL1_OFFSET <= 0 ? 0 - : (uint16_t)(accel_1 - ACCEL1_OFFSET) * 100 - / (ACCEL1_MAX_VAL - ACCEL1_OFFSET); - uint16_t accel_2_norm = accel_2 - ACCEL2_OFFSET <= 0 ? 0 - : (uint16_t)(accel_2 - ACCEL2_OFFSET) * 100 - / (ACCEL2_MAX_VAL - ACCEL2_OFFSET); + uint16_t accel_1_norm = + accel_1 - ACCEL1_OFFSET <= 0 ? + 0 : + (uint16_t)(accel_1 - ACCEL1_OFFSET) * 100 / + (ACCEL1_MAX_VAL - ACCEL1_OFFSET); + uint16_t accel_2_norm = + accel_2 - ACCEL2_OFFSET <= 0 ? + 0 : + (uint16_t)(accel_2 - ACCEL2_OFFSET) * 100 / + (ACCEL2_MAX_VAL - ACCEL2_OFFSET); /* Fault - difference between pedal sensing values */ - if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) && is_timer_active(diff_timer)) { + if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) && + is_timer_active(diff_timer)) { /* starting diff timer */ if (is_timer_expired(diff_timer)) { - if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH)) { - fault_data->diag = "Pedal fault - pedal values are too different "; + if ((abs(accel_1_norm - accel_2_norm) > + PEDAL_DIFF_THRESH)) { + fault_data->diag = + "Pedal fault - pedal values are too different "; queue_fault(fault_data); } } - } else if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) - && !is_timer_active(diff_timer)) { + } else if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) && + !is_timer_active(diff_timer)) { start_timer(diff_timer, PEDAL_FAULT_TIME); } else { cancel_timer(diff_timer); } } -void vPedalsMonitor(void* pv_params) +void vPedalsMonitor(void *pv_params) { - const uint8_t num_samples = 10; + const uint8_t num_samples = 10; static const uint8_t buffer_size = 10; enum { ACCELPIN_2, ACCELPIN_1, BRAKEPIN_1, BRAKEPIN_2 }; static pedals_t sensor_data; - fault_data_t fault_data = { .id = ONBOARD_PEDAL_FAULT, .severity = DEFCON1 }; - can_msg_t accel_pedals_msg = { .id = CANID_PEDALS_ACCEL_MSG, .len = 8, .data = { 0 } }; - can_msg_t brake_pedals_msg = { .id = CANID_PEDALS_BRAKE_MSG, .len = 8, .data = { 0 } }; + fault_data_t fault_data = { .id = ONBOARD_PEDAL_FAULT, + .severity = DEFCON1 }; + can_msg_t accel_pedals_msg = { .id = CANID_PEDALS_ACCEL_MSG, + .len = 8, + .data = { 0 } }; + can_msg_t brake_pedals_msg = { .id = CANID_PEDALS_BRAKE_MSG, + .len = 8, + .data = { 0 } }; uint32_t adc_data[4]; bool is_braking = false; @@ -199,30 +223,35 @@ void vPedalsMonitor(void* pv_params) cancel_timer(&oc_timer_accelerator); /* Handle ADC Data for two input accelerator value and two input brake value*/ - mpu_t* mpu = (mpu_t*)pv_params; + mpu_t *mpu = (mpu_t *)pv_params; - uint8_t counter = 0; + uint8_t counter = 0; static int index = 0; for (;;) { read_pedals(mpu, adc_data); /* Evaluate Pedal Faulting Conditions */ - eval_pedal_fault(adc_data[ACCELPIN_1], adc_data[ACCELPIN_2], &diff_timer_accelerator, - &sc_timer_accelerator, &oc_timer_accelerator, &fault_data); + eval_pedal_fault(adc_data[ACCELPIN_1], adc_data[ACCELPIN_2], + &diff_timer_accelerator, &sc_timer_accelerator, + &oc_timer_accelerator, &fault_data); // eval_pedal_fault(adc_data[BRAKEPIN_1], adc_data[BRAKEPIN_1], &diff_timer_brake, // &sc_timer_brake, &oc_timer_brake, &fault_data); /* Offset adjusted per pedal sensor, clamp to be above 0 */ - uint16_t accel_val1 = (int16_t)adc_data[ACCELPIN_1] - ACCEL1_OFFSET <= 0 - ? 0 - : (uint16_t)(adc_data[ACCELPIN_1] - ACCEL1_OFFSET) * 100 - / (ACCEL1_MAX_VAL - ACCEL1_OFFSET); + uint16_t accel_val1 = + (int16_t)adc_data[ACCELPIN_1] - ACCEL1_OFFSET <= 0 ? + 0 : + (uint16_t)(adc_data[ACCELPIN_1] - + ACCEL1_OFFSET) * + 100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET); // printf("Accel 1: %d\r\n", max_pedal1); - uint16_t accel_val2 = (int16_t)adc_data[ACCELPIN_2] - ACCEL2_OFFSET <= 0 - ? 0 - : (uint16_t)(adc_data[ACCELPIN_2] - ACCEL2_OFFSET) * 100 - / (ACCEL2_MAX_VAL - ACCEL2_OFFSET); + uint16_t accel_val2 = + (int16_t)adc_data[ACCELPIN_2] - ACCEL2_OFFSET <= 0 ? + 0 : + (uint16_t)(adc_data[ACCELPIN_2] - + ACCEL2_OFFSET) * + 100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET); // printf("Accel 2: %d\r\n",max_pedal2); uint16_t accel_val = (uint16_t)(accel_val1 + accel_val2) / 2; @@ -238,7 +267,8 @@ void vPedalsMonitor(void* pv_params) static float buffer[10] = { 0 }; - uint16_t brake_avg = (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2; + uint16_t brake_avg = + (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2; // Add the new value to the buffer buffer[index] = brake_avg; @@ -252,7 +282,7 @@ void vPedalsMonitor(void* pv_params) } float average_brake = sum / buffer_size; - is_braking = average_brake > PEDAL_BRAKE_THRESH; + is_braking = average_brake > PEDAL_BRAKE_THRESH; brake_state = is_braking; osMessageQueuePut(brakelight_signal, &is_braking, 0U, 0U); @@ -260,13 +290,17 @@ void vPedalsMonitor(void* pv_params) // osMessageQueuePut(break_state_queue, &is_braking, 0U, 0U); /* Low Pass Filter */ - sensor_data.accelerator_value = (sensor_data.accelerator_value + (accel_val)) / num_samples; - sensor_data.brake_value - = average_brake / 10.0; // still divide by 10 since we multiple by 10 on other end + sensor_data.accelerator_value = + (sensor_data.accelerator_value + (accel_val)) / + num_samples; + sensor_data.brake_value = + average_brake / + 10.0; // still divide by 10 since we multiple by 10 on other end /* Publish to Onboard Pedals Queue */ // printf("Accel pedal queue %d", sensor_data.accelerator_value); - osStatus_t check = osMessageQueuePut(pedal_data_queue, &sensor_data, 0U, 0U); + osStatus_t check = osMessageQueuePut(pedal_data_queue, + &sensor_data, 0U, 0U); if (check != 0) { fault_data.diag = "Failed to push pedal data to queue"; @@ -277,16 +311,22 @@ void vPedalsMonitor(void* pv_params) counter += 1; if (counter >= - 5) { + 5) { counter = 0; - endian_swap(&adc_data[ACCELPIN_1], sizeof(adc_data[ACCELPIN_1])); - endian_swap(&adc_data[ACCELPIN_2], sizeof(adc_data[ACCELPIN_2])); - memcpy(accel_pedals_msg.data, &adc_data, accel_pedals_msg.len); + endian_swap(&adc_data[ACCELPIN_1], + sizeof(adc_data[ACCELPIN_1])); + endian_swap(&adc_data[ACCELPIN_2], + sizeof(adc_data[ACCELPIN_2])); + memcpy(accel_pedals_msg.data, &adc_data, + accel_pedals_msg.len); queue_can_msg(accel_pedals_msg); - endian_swap(&adc_data[BRAKEPIN_1], sizeof(adc_data[BRAKEPIN_1])); - endian_swap(&adc_data[BRAKEPIN_2], sizeof(adc_data[BRAKEPIN_2])); - memcpy(brake_pedals_msg.data, adc_data + 2, brake_pedals_msg.len); + endian_swap(&adc_data[BRAKEPIN_1], + sizeof(adc_data[BRAKEPIN_1])); + endian_swap(&adc_data[BRAKEPIN_2], + sizeof(adc_data[BRAKEPIN_2])); + memcpy(brake_pedals_msg.data, adc_data + 2, + brake_pedals_msg.len); queue_can_msg(brake_pedals_msg); } @@ -296,26 +336,30 @@ void vPedalsMonitor(void* pv_params) osThreadId_t imu_monitor_handle; const osThreadAttr_t imu_monitor_attributes = { - .name = "IMUMonitor", + .name = "IMUMonitor", .stack_size = 32 * 8, - .priority = (osPriority_t)osPriorityHigh, + .priority = (osPriority_t)osPriorityHigh, }; -void vIMUMonitor(void* pv_params) +void vIMUMonitor(void *pv_params) { const uint8_t num_samples = 10; static imu_data_t sensor_data; fault_data_t fault_data = { .id = IMU_FAULT, .severity = DEFCON5 }; - can_msg_t imu_accel_msg = { .id = CANID_IMU_ACCEL, .len = 6, .data = { 0 } }; - can_msg_t imu_gyro_msg = { .id = CANID_IMU_GYRO, .len = 6, .data = { 0 } }; + can_msg_t imu_accel_msg = { .id = CANID_IMU_ACCEL, + .len = 6, + .data = { 0 } }; + can_msg_t imu_gyro_msg = { .id = CANID_IMU_GYRO, + .len = 6, + .data = { 0 } }; - mpu_t* mpu = (mpu_t*)pv_params; + mpu_t *mpu = (mpu_t *)pv_params; for (;;) { // serial_print("IMU Task\r\n"); /* Take measurement */ uint16_t accel_data[3] = { 0 }; - uint16_t gyro_data[3] = { 0 }; + uint16_t gyro_data[3] = { 0 }; if (read_accel(mpu, accel_data)) { fault_data.diag = "Failed to get IMU acceleration"; queue_fault(&fault_data); @@ -327,12 +371,18 @@ void vIMUMonitor(void* pv_params) } /* Run values through LPF of sample size */ - sensor_data.accel_x = (sensor_data.accel_x + accel_data[0]) / num_samples; - sensor_data.accel_y = (sensor_data.accel_y + accel_data[1]) / num_samples; - sensor_data.accel_z = (sensor_data.accel_z + accel_data[2]) / num_samples; - sensor_data.gyro_x = (sensor_data.gyro_x + gyro_data[0]) / num_samples; - sensor_data.gyro_y = (sensor_data.gyro_y + gyro_data[1]) / num_samples; - sensor_data.gyro_z = (sensor_data.gyro_z + gyro_data[2]) / num_samples; + sensor_data.accel_x = + (sensor_data.accel_x + accel_data[0]) / num_samples; + sensor_data.accel_y = + (sensor_data.accel_y + accel_data[1]) / num_samples; + sensor_data.accel_z = + (sensor_data.accel_z + accel_data[2]) / num_samples; + sensor_data.gyro_x = + (sensor_data.gyro_x + gyro_data[0]) / num_samples; + sensor_data.gyro_y = + (sensor_data.gyro_y + gyro_data[1]) / num_samples; + sensor_data.gyro_z = + (sensor_data.gyro_z + gyro_data[2]) / num_samples; /* Publish to IMU Queue */ osMessageQueuePut(imu_queue, &sensor_data, 0U, 0U); @@ -365,17 +415,18 @@ void vIMUMonitor(void* pv_params) osThreadId_t tsms_monitor_handle; const osThreadAttr_t tsms_monitor_attributes = { - .name = "TsmsMonitor", + .name = "TsmsMonitor", .stack_size = 32 * 8, - .priority = (osPriority_t)osPriorityHigh, + .priority = (osPriority_t)osPriorityHigh, }; -void vTsmsMonitor(void* pv_params) +void vTsmsMonitor(void *pv_params) { - fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, .severity = DEFCON5 }; - pdu_t* pdu = (pdu_t*)pv_params; + fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, + .severity = DEFCON5 }; + pdu_t *pdu = (pdu_t *)pv_params; - bool tsms_status = false; + bool tsms_status = false; nertimer_t tsms_debounce_timer = { .active = false }; for (;;) { @@ -384,11 +435,13 @@ void vTsmsMonitor(void* pv_params) printf("Checking pdu"); // Timer has not been started, and there is a change in TSMS, so start the timer - if (tsms != tsms_status && !is_timer_active(&tsms_debounce_timer)) { + if (tsms != tsms_status && + !is_timer_active(&tsms_debounce_timer)) { start_timer(&tsms_debounce_timer, 500); } // During debouncing, the tsms reading changes, so end the debounce period - else if (tsms == tsms_status && !is_timer_expired(&tsms_debounce_timer)) { + else if (tsms == tsms_status && + !is_timer_expired(&tsms_debounce_timer)) { cancel_timer(&tsms_debounce_timer); } // The TSMS reading has been consistent thorughout the debounce period @@ -408,16 +461,17 @@ void vTsmsMonitor(void* pv_params) osThreadId_t fusing_monitor_handle; const osThreadAttr_t fusing_monitor_attributes = { - .name = "FusingMonitor", + .name = "FusingMonitor", .stack_size = 64 * 8, - .priority = (osPriority_t)osPriorityAboveNormal1, + .priority = (osPriority_t)osPriorityAboveNormal1, }; -void vFusingMonitor(void* pv_params) +void vFusingMonitor(void *pv_params) { - fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, .severity = DEFCON5 }; - can_msg_t fuse_msg = { .id = CANID_FUSE, .len = 2, .data = { 0 } }; - pdu_t* pdu = (pdu_t*)pv_params; + fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, + .severity = DEFCON5 }; + can_msg_t fuse_msg = { .id = CANID_FUSE, .len = 2, .data = { 0 } }; + pdu_t *pdu = (pdu_t *)pv_params; uint16_t fuse_buf; bool fuses[MAX_FUSES] = { 0 }; @@ -435,8 +489,9 @@ void vFusingMonitor(void* pv_params) } for (fuse_t fuse = 0; fuse < MAX_FUSES; fuse++) { - fuse_buf |= fuses[fuse] - << fuse; /* Sets the bit at position `fuse` to the state of the fuse */ + fuse_buf |= + fuses[fuse] + << fuse; /* Sets the bit at position `fuse` to the state of the fuse */ } // serial_print("Fuses:\t%X\r\n", fuse_buf); @@ -460,16 +515,19 @@ void vFusingMonitor(void* pv_params) osThreadId_t shutdown_monitor_handle; const osThreadAttr_t shutdown_monitor_attributes = { - .name = "ShutdownMonitor", + .name = "ShutdownMonitor", .stack_size = 64 * 8, - .priority = (osPriority_t)osPriorityHigh2, + .priority = (osPriority_t)osPriorityHigh2, }; -void vShutdownMonitor(void* pv_params) +void vShutdownMonitor(void *pv_params) { - fault_data_t fault_data = { .id = SHUTDOWN_MONITOR_FAULT, .severity = DEFCON5 }; - can_msg_t shutdown_msg = { .id = CANID_SHUTDOWN_LOOP, .len = 2, .data = { 0 } }; - pdu_t* pdu = (pdu_t*)pv_params; + fault_data_t fault_data = { .id = SHUTDOWN_MONITOR_FAULT, + .severity = DEFCON5 }; + can_msg_t shutdown_msg = { .id = CANID_SHUTDOWN_LOOP, + .len = 2, + .data = { 0 } }; + pdu_t *pdu = (pdu_t *)pv_params; bool shutdown_loop[MAX_SHUTDOWN_STAGES] = { 0 }; uint16_t shutdown_buf; @@ -486,10 +544,11 @@ void vShutdownMonitor(void* pv_params) queue_fault(&fault_data); } - for (shutdown_stage_t stage = 0; stage < MAX_SHUTDOWN_STAGES; stage++) { - shutdown_buf - |= shutdown_loop[stage] - << stage; /* Sets the bit at position `stage` to the state of the stage */ + for (shutdown_stage_t stage = 0; stage < MAX_SHUTDOWN_STAGES; + stage++) { + shutdown_buf |= + shutdown_loop[stage] + << stage; /* Sets the bit at position `stage` to the state of the stage */ } // serial_print("Shutdown status:\t%X\r\n", shutdown_buf); @@ -514,17 +573,18 @@ void vShutdownMonitor(void* pv_params) osThreadId steeringio_buttons_monitor_handle; const osThreadAttr_t steeringio_buttons_monitor_attributes = { - .name = "SteeringIOButtonsMonitor", + .name = "SteeringIOButtonsMonitor", .stack_size = 200 * 8, - .priority = (osPriority_t)osPriorityAboveNormal1, + .priority = (osPriority_t)osPriorityAboveNormal1, }; -void vSteeringIOButtonsMonitor(void* pv_params) +void vSteeringIOButtonsMonitor(void *pv_params) { button_data_t buttons; - steeringio_t* wheel = (steeringio_t*)pv_params; - can_msg_t msg = { .id = 0x680, .len = 8, .data = { 0 } }; - fault_data_t fault_data = { .id = BUTTONS_MONITOR_FAULT, .severity = DEFCON5 }; + steeringio_t *wheel = (steeringio_t *)pv_params; + can_msg_t msg = { .id = 0x680, .len = 8, .data = { 0 } }; + fault_data_t fault_data = { .id = BUTTONS_MONITOR_FAULT, + .severity = DEFCON5 }; for (;;) { uint8_t button_1 = !HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4); @@ -543,8 +603,10 @@ void vSteeringIOButtonsMonitor(void* pv_params) // serial_print("\r\n"); - uint8_t button_data = (button_1 << 7) | (button_2 << 6) | (button_3 << 5) | (button_4 << 4) - | (button_5 << 3) | (button_6 << 2) | (button_7 << 1) | (button_8); + uint8_t button_data = (button_1 << 7) | (button_2 << 6) | + (button_3 << 5) | (button_4 << 4) | + (button_5 << 3) | (button_6 << 2) | + (button_7 << 1) | (button_8); buttons.data[0] = button_data; @@ -553,7 +615,8 @@ void vSteeringIOButtonsMonitor(void* pv_params) /* Set the first byte to be the first 8 buttons with each bit representing the pin status */ msg.data[0] = button_data; if (queue_can_msg(msg)) { - fault_data.diag = "Failed to send steering buttons can message"; + fault_data.diag = + "Failed to send steering buttons can message"; queue_fault(&fault_data); } @@ -568,20 +631,21 @@ bool get_tsms() osThreadId brakelight_monitor_handle; const osThreadAttr_t brakelight_monitor_attributes = { - .name = "BrakelightMonitor", + .name = "BrakelightMonitor", .stack_size = 32 * 8, - .priority = (osPriority_t)osPriorityHigh, + .priority = (osPriority_t)osPriorityHigh, }; -void vBrakelightMonitor(void* pv_params) +void vBrakelightMonitor(void *pv_params) { - pdu_t* pdu = (pdu_t*)pv_params; + pdu_t *pdu = (pdu_t *)pv_params; bool state; osStatus_t status; for (;;) { - status = osMessageQueueGet(brakelight_signal, &state, NULL, osWaitForever); + status = osMessageQueueGet(brakelight_signal, &state, NULL, + osWaitForever); if (!status) { write_brakelight(pdu, state); } diff --git a/shell.nix b/shell.nix new file mode 100644 index 0000000..4b35928 --- /dev/null +++ b/shell.nix @@ -0,0 +1,15 @@ +{ + pkgs ? import { }, +}: +pkgs.mkShell { + # nativeBuildInputs is usually what you want -- tools you need to run + nativeBuildInputs = with pkgs.buildPackages; [ + probe-rs-tools + linuxKernel.packages.linux_6_6.usbip + python3 + llvmPackages_18.clang-tools + ]; + shellHook = '' + source ../ner-venv/bin/activate + ''; +}