diff --git a/Apps/Inc/CAN_Queue.h b/Apps/Inc/CAN_Queue.h index 5425b4a20..250e2d501 100644 --- a/Apps/Inc/CAN_Queue.h +++ b/Apps/Inc/CAN_Queue.h @@ -1,8 +1,9 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CAN_Queue.h - * @brief Queue that holds all CAN messages that Task_CANBusConsumer needs to send. - * + * @brief Queue that holds all CAN messages that Task_CANBusConsumer needs to + * send. + * * @defgroup CAN_Queue * @addtogroup CAN_Queue * @{ @@ -21,5 +22,4 @@ ErrorStatus CAN_Queue_Pend(CANDATA_t *message); #endif - /* @} */ diff --git a/Apps/Inc/FaultState.h b/Apps/Inc/FaultState.h index 62a4f8ed7..3f2198479 100644 --- a/Apps/Inc/FaultState.h +++ b/Apps/Inc/FaultState.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file FaultState.h - * @brief - * + * @brief + * * @defgroup FaultState * @addtogroup FaultState * @{ @@ -10,10 +10,8 @@ #ifndef __FAULT_STATE_H #define __FAULT_STATE_H - void EnterFaultState(void); #endif - /* @} */ diff --git a/Apps/Inc/MedianFilter.h b/Apps/Inc/MedianFilter.h index 5dd629571..d54ed71f4 100644 --- a/Apps/Inc/MedianFilter.h +++ b/Apps/Inc/MedianFilter.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file MedianFilter.h - * @brief - * + * @brief + * * @defgroup MedianFilter * @addtogroup MedianFilter * @{ @@ -10,10 +10,10 @@ /* * This file implements a median filter. - * - * In order to use it in another file, you must import it in + * + * In order to use it in another file, you must import it in * a particular way. - * + * * 1. Define your data type, like so * #define MEDIAN_FILTER_TYPE int * 2. Define your filter depth, like so @@ -24,23 +24,23 @@ * #define MEDIAN_FILTER_NAME my_fifo * 4. Import this file * #include "MedianFilter.h" - * + * * This file includes some defaults, but they might not work for * your case! - * + * * Also, this file undef's everything at the end, so you can import * multiple times if you need. - * + * * If MEDIAN_FILTER_NAME == my_filter, then your new data structure will be * called my_filter_t. - * + * * NOTE: importantly, this does not currently support usage from * header files. That is, all these types/functions are statically * declared, so there cannot be a non-static fifo at the moment. */ // The header guard only guard the import, -// since this file can be imported multiple times +// since this file can be imported multiple times #ifndef MEDIAN_FILTER_H #define MEDIAN_FILTER_H #include @@ -68,133 +68,135 @@ #endif // Utility definitions -#define _CONCAT(A, B) A ## B +#define _CONCAT(A, B) A##B #define CONCAT(A, B) _CONCAT(A, B) // some shorthand -#define MF_TYPE MEDIAN_FILTER_TYPE -#define MF_DEPTH MEDIAN_FILTER_DEPTH +#define MF_TYPE MEDIAN_FILTER_TYPE +#define MF_DEPTH MEDIAN_FILTER_DEPTH #define MF_CHANNELS MEDIAN_FILTER_CHANNELS -#define MF_NAME MEDIAN_FILTER_NAME +#define MF_NAME MEDIAN_FILTER_NAME // Type names #define MEDIAN_FILTER_STRUCT_NAME CONCAT(MF_NAME, _s) #define MEDIAN_FILTER_TYPE_NAME CONCAT(MF_NAME, _t) // more shorthand -#define MF_STRUCT_NAME MEDIAN_FILTER_STRUCT_NAME -#define MF_TYPE_NAME MEDIAN_FILTER_TYPE_NAME +#define MF_STRUCT_NAME MEDIAN_FILTER_STRUCT_NAME +#define MF_TYPE_NAME MEDIAN_FILTER_TYPE_NAME // The actual structure typedef struct MF_STRUCT_NAME { - MF_TYPE raw[MF_CHANNELS][MF_DEPTH]; - MF_TYPE filtered[MF_CHANNELS]; - uint32_t index; + MF_TYPE raw[MF_CHANNELS][MF_DEPTH]; + MF_TYPE filtered[MF_CHANNELS]; + uint32_t index; } MF_TYPE_NAME; // Define some names for our functions -#define MEDIAN CONCAT(MF_NAME, _median) -#define INIT CONCAT(MF_NAME, _init) -#define GET CONCAT(MF_NAME, _get) -#define PUT CONCAT(MF_NAME, _put) -#define GETSINGLE CONCAT(MF_NAME, _getSingle) +#define MEDIAN CONCAT(MF_NAME, _median) +#define INIT CONCAT(MF_NAME, _init) +#define GET CONCAT(MF_NAME, _get) +#define PUT CONCAT(MF_NAME, _put) +#define GETSINGLE CONCAT(MF_NAME, _getSingle) /** - * @brief Helper function to find the median of an array of MF_TYPE with length MF_DEPTH. - * DO NOT call this function directly. - * + * @brief Helper function to find the median of an array of MF_TYPE with length + * MF_DEPTH. DO NOT call this function directly. + * * @param channel the channel in the median filter to find the median of */ -static inline MF_TYPE __attribute__((unused)) -MEDIAN (MF_TYPE *channel) { - static MF_TYPE sorted[MF_DEPTH]; - - // copy channels into temporary array - memcpy(sorted, channel, MF_DEPTH * sizeof(MF_TYPE)); - - // sort temporary array - for (uint32_t i = 0; i < MF_DEPTH; ++i) { - MF_TYPE min = sorted[i]; - uint32_t minIdx = i; - for (uint32_t j = i + 1; j < MF_DEPTH; ++j) { - if (sorted[j] < min) { - min = sorted[j]; - minIdx = j; - } - } - sorted[minIdx] = sorted[i]; - sorted[i] = min; +static inline MF_TYPE __attribute__((unused)) MEDIAN(MF_TYPE *channel) { + static MF_TYPE sorted[MF_DEPTH]; + + // copy channels into temporary array + memcpy(sorted, channel, MF_DEPTH * sizeof(MF_TYPE)); + + // sort temporary array + for (uint32_t i = 0; i < MF_DEPTH; ++i) { + MF_TYPE min = sorted[i]; + uint32_t minIdx = i; + for (uint32_t j = i + 1; j < MF_DEPTH; ++j) { + if (sorted[j] < min) { + min = sorted[j]; + minIdx = j; + } } + sorted[minIdx] = sorted[i]; + sorted[i] = min; + } - // return median - return sorted[MF_DEPTH >> 1]; + // return median + return sorted[MF_DEPTH >> 1]; } /** * @brief Initialize a new median filter - * + * * If the type of the filter is myfilter_t, then this function * will be called myfilter_init(). - * + * * @param filter a pointer to the median filter to initialize * @param low a value that is below the range of expected values * @param high a value that is above the range of expected values */ static inline void __attribute__((unused)) -INIT (MF_TYPE_NAME *filter, MF_TYPE low, MF_TYPE high) { - // intialize the filter with alternating low and high values, so it will be stable at startup - for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { - for (uint32_t i = 0; i < MF_DEPTH - 1; ++i) { - filter->raw[channel][i] = (i & 1) ? high : low; - } - filter->filtered[channel] = MEDIAN(filter->raw[channel]); +INIT(MF_TYPE_NAME *filter, MF_TYPE low, MF_TYPE high) { + // intialize the filter with alternating low and high values, so it will be + // stable at startup + for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { + for (uint32_t i = 0; i < MF_DEPTH - 1; ++i) { + filter->raw[channel][i] = (i & 1) ? high : low; } + filter->filtered[channel] = MEDIAN(filter->raw[channel]); + } - filter->index = MF_DEPTH - 1; + filter->index = MF_DEPTH - 1; } /** - * @brief update the median filter by giving it a new set of values for all channels - * + * @brief update the median filter by giving it a new set of values for all + * channels + * * @param filter a pointer to the median filter - * @param channels a complete set of new values for all channels to add to the median filter - * + * @param channels a complete set of new values for all channels to add to the + * median filter + * */ static inline void __attribute__((unused)) -PUT (MF_TYPE_NAME *filter, MF_TYPE *channels) { - // put the new data into the filter - for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { - filter->raw[channel][filter->index] = channels[channel]; - } - (filter->index) = (filter->index + 1) % MF_DEPTH; - - // update the list of filtered values - for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { - filter->filtered[channel] = MEDIAN(filter->raw[channel]); - } +PUT(MF_TYPE_NAME *filter, MF_TYPE *channels) { + // put the new data into the filter + for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { + filter->raw[channel][filter->index] = channels[channel]; + } + (filter->index) = (filter->index + 1) % MF_DEPTH; + + // update the list of filtered values + for (uint32_t channel = 0; channel < MF_CHANNELS; ++channel) { + filter->filtered[channel] = MEDIAN(filter->raw[channel]); + } } /** * @brief get a complete set of filtered values for all channels - * + * * @param filter a pointer to the median filter * @param dest a pointer to a buffer to store all of the filtered values */ static inline void __attribute__((unused)) -GET (MF_TYPE_NAME *filter, MF_TYPE *dest) { - memcpy(dest, filter->filtered, sizeof(MF_TYPE) * MF_CHANNELS); +GET(MF_TYPE_NAME *filter, MF_TYPE *dest) { + memcpy(dest, filter->filtered, sizeof(MF_TYPE) * MF_CHANNELS); } /** * @brief get a filtered value for a single channel in the median filter - * + * * @param filter a pointer to the median filter * @param channel the channel to read * @return the filtered value */ static inline MF_TYPE __attribute__((unused)) -GETSINGLE (MF_TYPE_NAME *filter, uint32_t channel) { - return filter->filtered[channel]; +GETSINGLE(MF_TYPE_NAME *filter, uint32_t channel) { + return filter->filtered[channel]; } // undef everything, so this file can be included multiple times @@ -216,6 +218,6 @@ GETSINGLE (MF_TYPE_NAME *filter, uint32_t channel) { #undef INIT #undef GET #undef PUT -#undef GETSINGLE +#undef GETSINGLE /* @} */ \ No newline at end of file diff --git a/Apps/Inc/ReadCarCAN.h b/Apps/Inc/ReadCarCAN.h index 93b68a8c5..a8a8d97d0 100644 --- a/Apps/Inc/ReadCarCAN.h +++ b/Apps/Inc/ReadCarCAN.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file ReadCarCAN.h - * @brief - * + * @brief + * * @defgroup ReadCarCAN * @addtogroup ReadCarCAN * @{ @@ -11,21 +11,19 @@ #ifndef __READ_CAR_CAN_H #define __READ_CAR_CAN_H -#include "os.h" -#include "common.h" -#include "Tasks.h" - #include "CANbus.h" +#include "Tasks.h" +#include "common.h" +#include "os.h" #define SAT_BUF_LENGTH 5 /** * @brief Returns whether regen braking / charging is enabled or not * @return Whether regen braking / charging is enabled or not -*/ + */ bool ChargeEnable_Get(); #endif - /* @} */ diff --git a/Apps/Inc/ReadTritium.h b/Apps/Inc/ReadTritium.h old mode 100755 new mode 100644 index fcb6465be..a6525e3ae --- a/Apps/Inc/ReadTritium.h +++ b/Apps/Inc/ReadTritium.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file ReadTritium.h - * @brief - * + * @brief + * * @defgroup ReadTritium * @addtogroup ReadTritium * @{ @@ -11,31 +11,32 @@ #ifndef __READ_TRITIUM_H #define __READ_TRITIUM_H -#include "os.h" -#include "common.h" #include "Tasks.h" +#include "common.h" +#include "os.h" #define CAR_STOPPED 0 /** * Motor Error States - * Read messages from motor in ReadTritium and trigger appropriate error messages as needed based on bits - * + * Read messages from motor in ReadTritium and trigger appropriate error + * messages as needed based on bits + * */ -typedef enum{ - T_HARDWARE_OVER_CURRENT_ERR = (1<<0), - T_SOFTWARE_OVER_CURRENT_ERR = (1<<1), - T_DC_BUS_OVERVOLT_ERR = (1<<2), - T_HALL_SENSOR_ERR = (1<<3), - T_WATCHDOG_LAST_RESET_ERR = (1<<4), - T_CONFIG_READ_ERR = (1<<5), - T_UNDER_VOLTAGE_LOCKOUT_ERR = (1<<6), - T_DESAT_FAULT_ERR = (1<<7), - T_MOTOR_OVER_SPEED_ERR = (1<<8), - T_INIT_FAIL = (1<<9), //motor controller fails to restart or initialize - T_NONE = 0x00, +typedef enum { + T_HARDWARE_OVER_CURRENT_ERR = (1 << 0), + T_SOFTWARE_OVER_CURRENT_ERR = (1 << 1), + T_DC_BUS_OVERVOLT_ERR = (1 << 2), + T_HALL_SENSOR_ERR = (1 << 3), + T_WATCHDOG_LAST_RESET_ERR = (1 << 4), + T_CONFIG_READ_ERR = (1 << 5), + T_UNDER_VOLTAGE_LOCKOUT_ERR = (1 << 6), + T_DESAT_FAULT_ERR = (1 << 7), + T_MOTOR_OVER_SPEED_ERR = (1 << 8), + T_INIT_FAIL = (1 << 9), // motor controller fails to restart or initialize + T_NONE = 0x00, } tritium_error_code_t; -#define NUM_TRITIUM_ERRORS 9 //9 errors, and 1 entry for no error +#define NUM_TRITIUM_ERRORS 9 // 9 errors, and 1 entry for no error /** * @brief Returns the current error status of the tritium controller @@ -48,5 +49,4 @@ void MotorController_Restart(); #endif - /* @} */ diff --git a/Apps/Inc/SendTritium.h b/Apps/Inc/SendTritium.h index a132d64a7..cc5f6277f 100644 --- a/Apps/Inc/SendTritium.h +++ b/Apps/Inc/SendTritium.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file SendTritium.h - * @brief - * + * @brief + * * @defgroup SendTritium * @addtogroup SendTritium * @{ @@ -16,37 +16,36 @@ #define MOTOR_MSG_PERIOD 100 #define FSM_PERIOD 100 -#define DEBOUNCE_PERIOD 2 // in units of FSM_PERIOD -#define MOTOR_MSG_COUNTER_THRESHOLD (MOTOR_MSG_PERIOD)/(FSM_PERIOD) +#define DEBOUNCE_PERIOD 2 // in units of FSM_PERIOD +#define MOTOR_MSG_COUNTER_THRESHOLD (MOTOR_MSG_PERIOD) / (FSM_PERIOD) #define FOREACH_Gear(GEAR) \ - GEAR(FORWARD_GEAR) \ - GEAR(NEUTRAL_GEAR) \ - GEAR(REVERSE_GEAR) \ + GEAR(FORWARD_GEAR) \ + GEAR(NEUTRAL_GEAR) \ + GEAR(REVERSE_GEAR) typedef enum GEAR_ENUM { - FOREACH_Gear(GENERATE_ENUM) - NUM_GEARS, + FOREACH_Gear(GENERATE_ENUM) NUM_GEARS, } Gear_t; // State Names -typedef enum{ - FORWARD_DRIVE, - NEUTRAL_DRIVE, - REVERSE_DRIVE, - RECORD_VELOCITY, - POWERED_CRUISE, - COASTING_CRUISE, - BRAKE_STATE, - ONEPEDAL, - ACCELERATE_CRUISE +typedef enum { + FORWARD_DRIVE, + NEUTRAL_DRIVE, + REVERSE_DRIVE, + RECORD_VELOCITY, + POWERED_CRUISE, + COASTING_CRUISE, + BRAKE_STATE, + ONEPEDAL, + ACCELERATE_CRUISE } TritiumStateName_t; // State Struct for FSM -typedef struct TritiumState{ - TritiumStateName_t name; - void (*stateHandler)(void); - void (*stateDecider)(void); +typedef struct TritiumState { + TritiumStateName_t name; + void (*stateHandler)(void); + void (*stateDecider)(void); } TritiumState_t; // Getter functions for local variables in SendTritium.c @@ -81,5 +80,4 @@ EXPOSE_SETTER(float, velocitySetpoint) #endif - /* @} */ diff --git a/Apps/Inc/Tasks.h b/Apps/Inc/Tasks.h index 1f80eac47..42ce3e6b2 100644 --- a/Apps/Inc/Tasks.h +++ b/Apps/Inc/Tasks.h @@ -1,20 +1,19 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Tasks.h - * @brief - * + * @brief + * * @defgroup Tasks * @addtogroup Tasks * @{ */ - #ifndef __TASKS_H #define __TASKS_H #include "common.h" -#include "os.h" #include "config.h" +#include "os.h" /** * Task initialization macro @@ -27,34 +26,33 @@ /** * Priority Definitions */ -#define TASK_FAULT_STATE_PRIO 1 -#define TASK_INIT_PRIO 2 -#define TASK_READ_TRITIUM_PRIO 3 -#define TASK_SEND_TRITIUM_PRIO 4 -#define TASK_READ_CAR_CAN_PRIO 5 -#define TASK_UPDATE_DISPLAY_PRIO 6 -#define TASK_SEND_CAR_CAN_PRIO 8 -#define TASK_TELEMETRY_PRIO 9 -#define TASK_DEBUG_DUMP_PRIO 10 -#define TASK_COMMAND_LINE_PRIO 11 +#define TASK_FAULT_STATE_PRIO 1 +#define TASK_INIT_PRIO 2 +#define TASK_READ_TRITIUM_PRIO 3 +#define TASK_SEND_TRITIUM_PRIO 4 +#define TASK_READ_CAR_CAN_PRIO 5 +#define TASK_UPDATE_DISPLAY_PRIO 6 +#define TASK_SEND_CAR_CAN_PRIO 8 +#define TASK_TELEMETRY_PRIO 9 +#define TASK_DEBUG_DUMP_PRIO 10 +#define TASK_COMMAND_LINE_PRIO 11 /** * Stack Sizes */ -#define DEFAULT_STACK_SIZE 256 -#define WATERMARK_STACK_LIMIT DEFAULT_STACK_SIZE/2 - -#define TASK_FAULT_STATE_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_INIT_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_SEND_TRITIUM_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_READ_CAR_CAN_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_UPDATE_DISPLAY_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_READ_TRITIUM_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_SEND_CAR_CAN_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_TELEMETRY_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_DEBUG_DUMP_STACK_SIZE DEFAULT_STACK_SIZE -#define TASK_COMMAND_LINE_STACK_SIZE DEFAULT_STACK_SIZE +#define DEFAULT_STACK_SIZE 256 +#define WATERMARK_STACK_LIMIT DEFAULT_STACK_SIZE / 2 +#define TASK_FAULT_STATE_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_INIT_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_SEND_TRITIUM_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_READ_CAR_CAN_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_UPDATE_DISPLAY_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_READ_TRITIUM_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_SEND_CAR_CAN_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_TELEMETRY_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_DEBUG_DUMP_STACK_SIZE DEFAULT_STACK_SIZE +#define TASK_COMMAND_LINE_STACK_SIZE DEFAULT_STACK_SIZE /** * Task Prototypes @@ -75,7 +73,7 @@ void Task_SendCarCAN(void* p_arg); void Task_Telemetry(void* p_arg); -void Task_DebugDump(void *p_arg); +void Task_DebugDump(void* p_arg); void Task_CommandLine(void* p_arg); @@ -93,7 +91,6 @@ extern OS_TCB Telemetry_TCB; extern OS_TCB DebugDump_TCB; extern OS_TCB CommandLine_TCB; - /** * Stacks */ @@ -124,61 +121,59 @@ extern OS_SEM FaultState_Sem4; */ void TaskSwHook_Init(void); - /** * Global Variables */ - /** * OS Error States - * + * * Stores error data to indicate which part of the code * an error is coming from. */ -typedef enum{ - OS_NONE_LOC = 0x000, - OS_ARRAY_LOC = 0x001, - OS_READ_CAN_LOC = 0x002, - OS_READ_TRITIUM_LOC = 0x004, - OS_SEND_CAN_LOC = 0x008, - OS_SEND_TRITIUM_LOC = 0x010, - OS_UPDATE_VEL_LOC = 0x020, - OS_CONTACTOR_LOC = 0x080, - OS_MINIONS_LOC = 0x100, - OS_MAIN_LOC = 0x200, - OS_CANDRIVER_LOC = 0x400, - OS_MOTOR_CONNECTION_LOC = 0x800, - OS_DISPLAY_LOC = 0x1000 +typedef enum { + OS_NONE_LOC = 0x000, + OS_ARRAY_LOC = 0x001, + OS_READ_CAN_LOC = 0x002, + OS_READ_TRITIUM_LOC = 0x004, + OS_SEND_CAN_LOC = 0x008, + OS_SEND_TRITIUM_LOC = 0x010, + OS_UPDATE_VEL_LOC = 0x020, + OS_CONTACTOR_LOC = 0x080, + OS_MINIONS_LOC = 0x100, + OS_MAIN_LOC = 0x200, + OS_CANDRIVER_LOC = 0x400, + OS_MOTOR_CONNECTION_LOC = 0x800, + OS_DISPLAY_LOC = 0x1000 } os_error_loc_t; /** * Fault Enum - * + * * Different fault states that need to be handled by the FaultState task */ -typedef enum{ - FAULT_NONE = 0x00, // No fault - FAULT_OS = 0x01, // for OS faults - FAULT_UNREACH = 0x02, // for unreachable conditions - FAULT_TRITIUM = 0x04, // for errors sent from the tritium - FAULT_READBPS = 0x08, // for unsuccessfully reading from BPS CAN - FAULT_DISPLAY = 0x10, // for display faults - FAULT_BPS = 0x20, // for if BPS trips +typedef enum { + FAULT_NONE = 0x00, // No fault + FAULT_OS = 0x01, // for OS faults + FAULT_UNREACH = 0x02, // for unreachable conditions + FAULT_TRITIUM = 0x04, // for errors sent from the tritium + FAULT_READBPS = 0x08, // for unsuccessfully reading from BPS CAN + FAULT_DISPLAY = 0x10, // for display faults + FAULT_BPS = 0x20, // for if BPS trips } fault_bitmap_t; /** * Task trace - * + * * Stores the last TASK_TRACE_LENGTH tasks that were run * The most recent task is at tasks[index], the one before at tasks[index-1], * wrapping back around at the beginnning - * + * */ #define TASK_TRACE_LENGTH 8 typedef struct { - OS_TCB *tasks[TASK_TRACE_LENGTH]; - uint32_t index; + OS_TCB* tasks[TASK_TRACE_LENGTH]; + uint32_t index; } task_trace_t; extern task_trace_t PrevTasks; @@ -194,16 +189,18 @@ extern os_error_loc_t OSErrLocBitmap; * @param OS_err_loc Where OS error occured (driver level) * @param err OS Error that occurred */ -void _assertOSError(uint16_t OS_err_loc, OS_ERR err); //TODO: This should be changed to enforce only enum usage +void _assertOSError( + uint16_t OS_err_loc, + OS_ERR err); // TODO: This should be changed to enforce only enum usage #if DEBUG == 1 -#define assertOSError(OS_err_loc,err) \ - if (err != OS_ERR_NONE) { \ - printf("Error asserted at %s, line %d: %d\n\r", __FILE__, __LINE__, err); \ - } \ - _assertOSError(OS_err_loc,err); +#define assertOSError(OS_err_loc, err) \ + if (err != OS_ERR_NONE) { \ + printf("Error asserted at %s, line %d: %d\n\r", __FILE__, __LINE__, err); \ + } \ + _assertOSError(OS_err_loc, err); #else -#define assertOSError(OS_err_loc,err) _assertOSError(OS_err_loc,err); +#define assertOSError(OS_err_loc, err) _assertOSError(OS_err_loc, err); #endif #endif diff --git a/Apps/Inc/UpdateDisplay.h b/Apps/Inc/UpdateDisplay.h index a2204518f..452650053 100644 --- a/Apps/Inc/UpdateDisplay.h +++ b/Apps/Inc/UpdateDisplay.h @@ -2,12 +2,12 @@ * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file UpdateDisplay.h * @brief Function prototypes for the display application. - * + * * This contains function prototypes relevant to the UpdateDisplay * application. Call assertUpdateDisplayError after calling any of the * functions in this application. - * - * + * + * * @defgroup UpdateDisplay * @addtogroup UpdateDisplay * @{ @@ -16,37 +16,33 @@ #ifndef __UPDATE_DISPLAY_H #define __UPDATE_DISPLAY_H -#include "os.h" -#include "common.h" -#include "Tasks.h" - -#include "Display.h" #include "Contactors.h" +#include "Display.h" +#include "Tasks.h" +#include "common.h" +#include "os.h" /** * Error types */ -typedef enum{ - UPDATEDISPLAY_ERR_NONE = 0, - UPDATEDISPLAY_ERR_FIFO_PUT =-1, // Error putting command in fifo - UPDATEDISPLAY_ERR_FIFO_POP =-2, // Error popping command from fifo - UPDATEDISPLAY_ERR_PARSE_COMP =-3, // Error parsing component/val in SetComponent - UPDATEDISPLAY_ERR_NO_CHANGE =-4, // No change in component value +typedef enum { + UPDATEDISPLAY_ERR_NONE = 0, + UPDATEDISPLAY_ERR_FIFO_PUT = -1, // Error putting command in fifo + UPDATEDISPLAY_ERR_FIFO_POP = -2, // Error popping command from fifo + UPDATEDISPLAY_ERR_PARSE_COMP = + -3, // Error parsing component/val in SetComponent + UPDATEDISPLAY_ERR_NO_CHANGE = -4, // No change in component value } UpdateDisplayError_t; /** * For display elements with three states */ -typedef enum{ - STATE_0 =0, - STATE_1 =1, - STATE_2 =2 -} TriState_t; +typedef enum { STATE_0 = 0, STATE_1 = 1, STATE_2 = 2 } TriState_t; // For cruise control and regen #define DISP_DISABLED STATE_0 -#define DISP_ENABLED STATE_1 // Able to be used -#define DISP_ACTIVE STATE_2 // Actively being used right now +#define DISP_ENABLED STATE_1 // Able to be used +#define DISP_ACTIVE STATE_2 // Actively being used right now // For gear changes #define DISP_NEUTRAL STATE_0 @@ -83,7 +79,8 @@ UpdateDisplayError_t UpdateDisplay_SetSBPV(uint32_t mv); /** * @brief Sets the velocity of the vehicle on the display - * @param mphTenths velocity of the vehicle in tenths of mph (1 digit of precision) + * @param mphTenths velocity of the vehicle in tenths of mph (1 digit of + * precision) * @returns UpdateDisplayError_t */ UpdateDisplayError_t UpdateDisplay_SetVelocity(uint32_t mphTenths); diff --git a/Apps/Inc/common.h b/Apps/Inc/common.h index a8475f8c3..04ee0abcb 100644 --- a/Apps/Inc/common.h +++ b/Apps/Inc/common.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file common.h - * @brief - * + * @brief + * * @defgroup common * @addtogroup common * @{ @@ -12,12 +12,13 @@ #define __COMMON_H #include -#include +#include #include +#include #include -#include #include -#include +#include + #include "config.h" /* Used for generating ENUMS */ @@ -25,30 +26,26 @@ #define GENERATE_STRING(STRING) #STRING, /** - * Used for creating getter functions (returns the value based on given inputs) - * - * GETTER is used for creating the function and EXPOSE_GETTER is used for creating the declaration in the header file + * Used for creating getter functions (returns the value based on given inputs) + * + * GETTER is used for creating the function and EXPOSE_GETTER is used for + * creating the declaration in the header file */ #define GETTER(type, name) \ - type get_##name(void){ \ - return name; \ - } \ + type get_##name(void) { return name; } -#define EXPOSE_GETTER(type, name) \ - type get_##name(void); \ +#define EXPOSE_GETTER(type, name) type get_##name(void); /** - * Used for creating setter functions (sets the value based on given inputs) - * - * SETTER is used for creating the function and EXPOSE_SETTER is used for creating the declaration in the header file + * Used for creating setter functions (sets the value based on given inputs) + * + * SETTER is used for creating the function and EXPOSE_SETTER is used for + * creating the declaration in the header file */ #define SETTER(type, name) \ - void set_##name(type val){ \ - name = val; \ - } \ + void set_##name(type val) { name = val; } -#define EXPOSE_SETTER(type, name) \ - void set_##name(type val); \ +#define EXPOSE_SETTER(type, name) void set_##name(type val); typedef void (*callback_t)(void); @@ -58,9 +55,9 @@ void print_float(char *str, float f); * @brief Meters per second to rpm conversion * @param velocity_mps velocity in meters per second * @returns rpm -*/ -inline float mpsToRpm(float velocity_mps){ - return (velocity_mps * 60) / WHEEL_CIRCUMFERENCE; + */ +inline float mpsToRpm(float velocity_mps) { + return (velocity_mps * 60) / WHEEL_CIRCUMFERENCE; } #endif diff --git a/Apps/Inc/fifo.h b/Apps/Inc/fifo.h index 2f821c91d..e10c2faa2 100644 --- a/Apps/Inc/fifo.h +++ b/Apps/Inc/fifo.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file fifo.h - * @brief - * + * @brief + * * @defgroup fifo * @addtogroup fifo * @{ @@ -10,10 +10,10 @@ /* * This file implements a fifo. - * - * In order to use it in another file, you must import it in + * + * In order to use it in another file, you must import it in * a particular way. - * + * * 1. Define your data type, like so * #define FIFO_TYPE int * 2. Define your fifo size, like so @@ -22,23 +22,23 @@ * #define FIFO_NAME my_fifo * 4. Import this file * #include "fifo.h" - * + * * This file includes some defaults, but they might not work for * your case! - * + * * Also, this file undef's everything at the end, so you can import * multiple times if you need. - * + * * If FIFO_NAME == my_fifo, then your new data structure will be * called my_fifo_t. - * + * * NOTE: importantly, this does not currently support usage from * header files. That is, all these types/functions are statically * declared, so there cannot be a non-static fifo at the moment. */ // The header guard only guard the import, -// since this file can be imported multiple times +// since this file can be imported multiple times #ifndef __FIFO_H #define __FIFO_H #include @@ -60,7 +60,7 @@ #endif // Utility definitions -#define _CONCAT(A, B) A ## B +#define _CONCAT(A, B) A##B #define CONCAT(A, B) _CONCAT(A, B) // Type names @@ -69,165 +69,157 @@ // The actual structure typedef struct FIFO_STRUCT_NAME { - FIFO_TYPE buffer[FIFO_SIZE]; - int put; - int get; + FIFO_TYPE buffer[FIFO_SIZE]; + int put; + int get; } FIFO_TYPE_NAME; // Define some names for our functions #define IS_EMPTY CONCAT(FIFO_NAME, _is_empty) -#define IS_FULL CONCAT(FIFO_NAME, _is_full) -#define GET CONCAT(FIFO_NAME, _get) -#define PUT CONCAT(FIFO_NAME, _put) -#define NEW CONCAT(FIFO_NAME, _new) -#define PEEK CONCAT(FIFO_NAME, _peek) -#define POPBACK CONCAT(FIFO_NAME, _popback) -#define RENEW CONCAT(FIFO_NAME, _renew) +#define IS_FULL CONCAT(FIFO_NAME, _is_full) +#define GET CONCAT(FIFO_NAME, _get) +#define PUT CONCAT(FIFO_NAME, _put) +#define NEW CONCAT(FIFO_NAME, _new) +#define PEEK CONCAT(FIFO_NAME, _peek) +#define POPBACK CONCAT(FIFO_NAME, _popback) +#define RENEW CONCAT(FIFO_NAME, _renew) /** * @brief Initialize a new fifo - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_new(). - * + * * @return an empty fifo */ -static inline FIFO_TYPE_NAME __attribute__((unused)) -NEW () { - FIFO_TYPE_NAME fifo; - memset(&fifo, 0, sizeof(fifo)); - return fifo; +static inline FIFO_TYPE_NAME __attribute__((unused)) NEW() { + FIFO_TYPE_NAME fifo; + memset(&fifo, 0, sizeof(fifo)); + return fifo; } /** * @brief Initialize a fifo by reference. - * + * * This is going to be faster than _new(). * This does not erase all of the contents of the fifo, but * rather marks it as "empty". */ -static inline void __attribute__((unused)) -RENEW (FIFO_TYPE_NAME * fifo) { - if(fifo != NULL) { - fifo->get = fifo->put; - } +static inline void __attribute__((unused)) RENEW(FIFO_TYPE_NAME *fifo) { + if (fifo != NULL) { + fifo->get = fifo->put; + } } /** * @brief Determine whether the fifo is empty. - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_is_empty(). - * + * * @param fifo A pointer to the fifo * @return true If empty * @return false If not empty */ -static bool -IS_EMPTY (FIFO_TYPE_NAME *fifo) { - return fifo->put == fifo->get; -} +static bool IS_EMPTY(FIFO_TYPE_NAME *fifo) { return fifo->put == fifo->get; } /** * @brief Determine whether the fifo is full. - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifio_is_full(). - * + * * @param fifo A pointer to the fifo * @return true If full * @return false If not full */ -static bool -IS_FULL (FIFO_TYPE_NAME *fifo) { - return (fifo->put + 1) % FIFO_SIZE == fifo->get; +static bool IS_FULL(FIFO_TYPE_NAME *fifo) { + return (fifo->put + 1) % FIFO_SIZE == fifo->get; } /** * @brief Get the next element from the fifo - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_get(). - * + * * @param fifo A pointer to the fifo * @param elem A pointer to an element to use for storage * @return true if successful * @return false if unsuccessful */ -static bool __attribute__((unused)) -GET (FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { - if(!IS_EMPTY(fifo)) { - *elem = fifo->buffer[fifo->get]; - fifo->get = (fifo->get + 1) % FIFO_SIZE; - return true; - } - - return false; +static bool __attribute__((unused)) GET(FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { + if (!IS_EMPTY(fifo)) { + *elem = fifo->buffer[fifo->get]; + fifo->get = (fifo->get + 1) % FIFO_SIZE; + return true; + } + + return false; } /** * @brief Put an element into the fifo - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_put(). - * + * * @param fifo A pointer to the fifo * @param elem The element to put * @return true if successful * @return false if unsuccessful */ -static bool __attribute__((unused)) -PUT (FIFO_TYPE_NAME *fifo, FIFO_TYPE elem) { - if(!IS_FULL(fifo)) { - fifo->buffer[fifo->put] = elem; - fifo->put = (fifo->put + 1) % FIFO_SIZE; - return true; - } - - return false; +static bool __attribute__((unused)) PUT(FIFO_TYPE_NAME *fifo, FIFO_TYPE elem) { + if (!IS_FULL(fifo)) { + fifo->buffer[fifo->put] = elem; + fifo->put = (fifo->put + 1) % FIFO_SIZE; + return true; + } + + return false; } /** * @brief Peek into the fifo - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_peek(). - * + * * @param fifo A pointer to the fifo * @param elem A pointer to space where the next element will be put * @return true if successful * @return false if unsuccessful */ static bool __attribute__((unused)) -PEEK (FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { - if(!IS_EMPTY(fifo)) { - *elem = fifo->buffer[fifo->get]; - return true; - } +PEEK(FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { + if (!IS_EMPTY(fifo)) { + *elem = fifo->buffer[fifo->get]; + return true; + } - return false; + return false; } /** * @brief Take the last element of the fifo (most recent) - * + * * If the type of the fifo is myfifo_t, then this function * will be called myfifo_popback(). - * + * * @param fifo A pointer to the fifo * @param elem A pointer to space to put the element * @return true if successful * @return false if unsuccessful */ static bool __attribute__((unused)) -POPBACK (FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { - if(!IS_EMPTY(fifo)) { - fifo->put = (fifo->put + FIFO_SIZE - 1) % FIFO_SIZE; - *elem = fifo->buffer[fifo->put]; - return true; - } - - return false; +POPBACK(FIFO_TYPE_NAME *fifo, FIFO_TYPE *elem) { + if (!IS_EMPTY(fifo)) { + fifo->put = (fifo->put + FIFO_SIZE - 1) % FIFO_SIZE; + *elem = fifo->buffer[fifo->put]; + return true; + } + + return false; } #undef IS_EMPTY diff --git a/Apps/Src/CAN_Queue.c b/Apps/Src/CAN_Queue.c index 6e61a8b5a..86044f24a 100644 --- a/Apps/Src/CAN_Queue.c +++ b/Apps/Src/CAN_Queue.c @@ -1,14 +1,15 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CAN_Queue.c - * @brief - * + * @brief + * */ #include "CAN_Queue.h" -#include "os.h" + #include "CANbus.h" #include "Tasks.h" +#include "os.h" // fifo #define FIFO_TYPE CANDATA_t @@ -21,49 +22,46 @@ static OS_SEM canFifo_Sem4; static OS_MUTEX canFifo_Mutex; void CAN_Queue_Init(void) { - OS_ERR err; - CPU_TS ticks; - OSMutexCreate(&canFifo_Mutex, "CAN queue mutex", &err); - assertOSError(OS_SEND_CAN_LOC, err); - OSSemCreate(&canFifo_Sem4, - "CAN queue semaphore", - 0, - &err); - assertOSError(OS_SEND_CAN_LOC, err); - OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); - assertOSError(OS_SEND_CAN_LOC, err); - CAN_fifo_renew(&canFifo); - OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); - assertOSError(OS_SEND_CAN_LOC, err); + OS_ERR err; + CPU_TS ticks; + OSMutexCreate(&canFifo_Mutex, "CAN queue mutex", &err); + assertOSError(OS_SEND_CAN_LOC, err); + OSSemCreate(&canFifo_Sem4, "CAN queue semaphore", 0, &err); + assertOSError(OS_SEND_CAN_LOC, err); + OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); + assertOSError(OS_SEND_CAN_LOC, err); + CAN_fifo_renew(&canFifo); + OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); + assertOSError(OS_SEND_CAN_LOC, err); } ErrorStatus CAN_Queue_Post(CANDATA_t message) { - OS_ERR err; - CPU_TS ticks; - OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); - assertOSError(OS_SEND_CAN_LOC, err); - bool success = CAN_fifo_put(&canFifo, message); - OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); - assertOSError(OS_SEND_CAN_LOC, err); + OS_ERR err; + CPU_TS ticks; + OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); + assertOSError(OS_SEND_CAN_LOC, err); + bool success = CAN_fifo_put(&canFifo, message); + OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); + assertOSError(OS_SEND_CAN_LOC, err); - if (success) { - OSSemPost(&canFifo_Sem4, OS_OPT_POST_1, &err); - assertOSError(OS_SEND_CAN_LOC, err); - } + if (success) { + OSSemPost(&canFifo_Sem4, OS_OPT_POST_1, &err); + assertOSError(OS_SEND_CAN_LOC, err); + } - return success ? SUCCESS : ERROR; + return success ? SUCCESS : ERROR; } ErrorStatus CAN_Queue_Pend(CANDATA_t *message) { - OS_ERR err; - CPU_TS ticks; - - OSSemPend(&canFifo_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); - assertOSError(OS_SEND_CAN_LOC, err); - OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); - assertOSError(OS_SEND_CAN_LOC, err); - bool result = CAN_fifo_get(&canFifo, message); - OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); - assertOSError(OS_SEND_CAN_LOC, err); - return result ? SUCCESS : ERROR; + OS_ERR err; + CPU_TS ticks; + + OSSemPend(&canFifo_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); + assertOSError(OS_SEND_CAN_LOC, err); + OSMutexPend(&canFifo_Mutex, 0, OS_OPT_POST_NONE, &ticks, &err); + assertOSError(OS_SEND_CAN_LOC, err); + bool result = CAN_fifo_get(&canFifo, message); + OSMutexPost(&canFifo_Mutex, OS_OPT_POST_NONE, &err); + assertOSError(OS_SEND_CAN_LOC, err); + return result ? SUCCESS : ERROR; } diff --git a/Apps/Src/CommandLine.c b/Apps/Src/CommandLine.c index 79b5f480f..1ed242bdf 100644 --- a/Apps/Src/CommandLine.c +++ b/Apps/Src/CommandLine.c @@ -2,20 +2,21 @@ #include #include #include -#include "os.h" -#include "Tasks.h" + #include "BSP_UART.h" #include "CANbus.h" #include "Contactors.h" #include "Minions.h" #include "Pedals.h" +#include "Tasks.h" +#include "os.h" -#define MAX_BUFFER_SIZE 128 // defined from BSP_UART_Read function +#define MAX_BUFFER_SIZE 128 // defined from BSP_UART_Read function // Represents a command that the command line understands struct Command { - const char *name; - bool (*action)(void); + const char *name; + bool (*action)(void); }; static bool cmd_help(void); @@ -34,322 +35,296 @@ static bool cmd_Minions_Write(void); static bool cmd_Pedals_Read(void); - const struct Command cmdline_commands[] = { - {.name = "help", .action = cmd_help}, - {.name = "CANbus_Send", .action = cmd_CANbus_Send}, - {.name = "CANbus_Read", .action = cmd_CANbus_Read}, - {.name = "Contactors_Get", .action = cmd_Contactors_Get}, - {.name = "Contactors_Set", .action = cmd_Contactors_Set}, - {.name = "Minions_Read", .action = cmd_Minions_Read}, - {.name = "Minions_Write", .action = cmd_Minions_Write}, - {.name = "Pedals_Read", .action = cmd_Pedals_Read}, - {.name = NULL, .action = NULL} -}; + {.name = "help", .action = cmd_help}, + {.name = "CANbus_Send", .action = cmd_CANbus_Send}, + {.name = "CANbus_Read", .action = cmd_CANbus_Read}, + {.name = "Contactors_Get", .action = cmd_Contactors_Get}, + {.name = "Contactors_Set", .action = cmd_Contactors_Set}, + {.name = "Minions_Read", .action = cmd_Minions_Read}, + {.name = "Minions_Write", .action = cmd_Minions_Write}, + {.name = "Pedals_Read", .action = cmd_Pedals_Read}, + {.name = NULL, .action = NULL}}; static char input[MAX_BUFFER_SIZE]; -char *save; // Save pointer for strtok_r +char *save; // Save pointer for strtok_r char *help = { - "LHRS Controls Command Line:\n\r" - " For help, enter [help]\n\r" - " Format is: cmd [param, ...]\n\r" - " Commands and their params are as follows:\n\r" - " CANbus_Send (non)blocking motor/car 'string' - Sends a CAN\n\r" - "message with the string data as is on the determined line\n\r" - " CANbus_Read (non)blocking motor/car - Reads a CAN message\n\r" - "on the detemined line\n\r" - " Contactors_Get array_c/array_p/motor_c - Gets the status of\n\r" - "determined contactor\n\r" - " Contactors_Set array_c/array_p/motor_c on/off (non)blocking -\n\r" - "Sets the determined contactor\n\r" - " Contactors_Enable array_c/array_p/motor_c - Enables the determined\n\r" - "contactor\n\r" - " Contactors_Disable array_c/array_p/motor_c - Disables the determined\n\r" - "contactor\n\r" - " Minions_Read 'input' - Reads the current status of the input\n\r" - " Minions_Write `output` on/off - Sets the current state of the output\n\r" - " Pedals_Read accel/brake - Reads the current status of the pedal\n\r" -}; - -static inline bool isWhiteSpace(char character){ - switch (character) { - case 0x09: - case 0x0A: - case 0x0C: - case 0x0D: - case 0x20: return true; - default: return false; - } + "LHRS Controls Command Line:\n\r" + " For help, enter [help]\n\r" + " Format is: cmd [param, ...]\n\r" + " Commands and their params are as follows:\n\r" + " CANbus_Send (non)blocking motor/car 'string' - Sends a CAN\n\r" + "message with the string data as is on the determined line\n\r" + " CANbus_Read (non)blocking motor/car - Reads a CAN message\n\r" + "on the detemined line\n\r" + " Contactors_Get array_c/array_p/motor_c - Gets the status of\n\r" + "determined contactor\n\r" + " Contactors_Set array_c/array_p/motor_c on/off (non)blocking -\n\r" + "Sets the determined contactor\n\r" + " Contactors_Enable array_c/array_p/motor_c - Enables the determined\n\r" + "contactor\n\r" + " Contactors_Disable array_c/array_p/motor_c - Disables the " + "determined\n\r" + "contactor\n\r" + " Minions_Read 'input' - Reads the current status of the input\n\r" + " Minions_Write `output` on/off - Sets the current state of the " + "output\n\r" + " Pedals_Read accel/brake - Reads the current status of the pedal\n\r"}; + +static inline bool isWhiteSpace(char character) { + switch (character) { + case 0x09: + case 0x0A: + case 0x0C: + case 0x0D: + case 0x20: + return true; + default: + return false; + } } static bool executeCommand(char *input) { - // The first word in the input should be a valid command - char *command = strtok_r(input, " ", &save); - // Iterate through all valid commands and check if the input matches (exits if the action is NULL (acts as a sentinal)) - for (int i=0; cmdline_commands[i].action; i++) { - if (!strcmp(command, cmdline_commands[i].name)) { - return cmdline_commands[i].action(); // Execute the command - } - } - return false; // Didn't find a valid command! + // The first word in the input should be a valid command + char *command = strtok_r(input, " ", &save); + // Iterate through all valid commands and check if the input matches (exits if + // the action is NULL (acts as a sentinal)) + for (int i = 0; cmdline_commands[i].action; i++) { + if (!strcmp(command, cmdline_commands[i].name)) { + return cmdline_commands[i].action(); // Execute the command + } + } + return false; // Didn't find a valid command! } // *********** Command line (shell) ************ -void Task_CommandLine(void* p_arg) { - OS_ERR err; - - // output welcome/help screen - printf(help); - - while(1){ - printf("> "); - BSP_UART_Read(UART_2, input); - printf("\n\r"); - - if (!executeCommand(input)) { // If command failed, error - printf("Bad cmd. Please try again\n\r"); - } - } - - // Delay of 1 seconds - OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); - if (err != OS_ERR_NONE){ - assertOSError(OS_NONE_LOC, err); +void Task_CommandLine(void *p_arg) { + OS_ERR err; + + // output welcome/help screen + printf(help); + + while (1) { + printf("> "); + BSP_UART_Read(UART_2, input); + printf("\n\r"); + + if (!executeCommand(input)) { // If command failed, error + printf("Bad cmd. Please try again\n\r"); } + } + + // Delay of 1 seconds + OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); + if (err != OS_ERR_NONE) { + assertOSError(OS_NONE_LOC, err); + } } // Function Implementations // ------------------------------------------------------------------------ static inline bool cmd_help(void) { - printf(help); - return true; + printf(help); + return true; } -/* This has not been tested and not sure if this implementation is correct (not sure -if we want to just transmit string data from serial) */ -static bool cmd_CANbus_Send(void){ - char *data = strtok_r(NULL, " ", &save); - CANDATA_t msg = {.ID=0x0582, .idx=0}; // this would change in the future (don't assume char as data) - for(int i = 0; i < 8 && i < strlen(data); i++){ - msg.data[i] = data[i]; - } - - char *blockInput = strtok_r(NULL, " ", &save); - bool blocking; - if(strcmp(blockInput, "blocking") == 0){ - blocking = CAN_BLOCKING; - } - else if(strcmp(blockInput, "nonblocking") == 0){ - blocking = CAN_NON_BLOCKING; - } - else{ - return false; - } - - char *busInput = strtok_r(NULL, " ", &save); - CAN_t bus; - if(strcmp(busInput, "motor") == 0){ - bus = CAN_1; - } - else if(strcmp(busInput, "car") == 0){ - bus = CAN_3; - } - else{ - return false; - } - - if(CANbus_Send(msg, blocking, bus)){ - printf("msg sent on %s (%s)\n\r", "can", blockInput); - }else{ - printf("msg sent failed\n\r"); - } - return true; +/* This has not been tested and not sure if this implementation is correct (not +sure if we want to just transmit string data from serial) */ +static bool cmd_CANbus_Send(void) { + char *data = strtok_r(NULL, " ", &save); + CANDATA_t msg = { + .ID = 0x0582, + .idx = 0}; // this would change in the future (don't assume char as data) + for (int i = 0; i < 8 && i < strlen(data); i++) { + msg.data[i] = data[i]; + } + + char *blockInput = strtok_r(NULL, " ", &save); + bool blocking; + if (strcmp(blockInput, "blocking") == 0) { + blocking = CAN_BLOCKING; + } else if (strcmp(blockInput, "nonblocking") == 0) { + blocking = CAN_NON_BLOCKING; + } else { + return false; + } + + char *busInput = strtok_r(NULL, " ", &save); + CAN_t bus; + if (strcmp(busInput, "motor") == 0) { + bus = CAN_1; + } else if (strcmp(busInput, "car") == 0) { + bus = CAN_3; + } else { + return false; + } + + if (CANbus_Send(msg, blocking, bus)) { + printf("msg sent on %s (%s)\n\r", "can", blockInput); + } else { + printf("msg sent failed\n\r"); + } + return true; } -static bool cmd_CANbus_Read(void){ - CANDATA_t msg; - - char *blockInput = strtok_r(NULL, " ", &save); - bool blocking; - if(strcmp(blockInput, "blocking") == 0){ - blocking = CAN_BLOCKING; - } - else if(strcmp(blockInput, "nonblocking") == 0){ - blocking = CAN_NON_BLOCKING; - } - else{ - return false; - } - - char *busInput = strtok_r(NULL, " ", &save); - CAN_t bus; - if(strcmp(busInput, "motor") == 0){ - bus = CAN_1; - } - else if(strcmp(busInput, "car") == 0){ - bus = CAN_3; - } - else{ - return false; - } - - if(CANbus_Read(&msg, blocking, bus) == SUCCESS){ - printf("msg recieved on %s (%s)\n\r", busInput, blockInput); - printf("ID: %d, Data: ", msg.ID); - for(int i = 0; i < 8; i++){ - printf("[%d] %x \n\r", i, msg.data[i]); - } - }else{ - printf("read failed on %s (%s)\n\r", busInput, blockInput); - } - return true; +static bool cmd_CANbus_Read(void) { + CANDATA_t msg; + + char *blockInput = strtok_r(NULL, " ", &save); + bool blocking; + if (strcmp(blockInput, "blocking") == 0) { + blocking = CAN_BLOCKING; + } else if (strcmp(blockInput, "nonblocking") == 0) { + blocking = CAN_NON_BLOCKING; + } else { + return false; + } + + char *busInput = strtok_r(NULL, " ", &save); + CAN_t bus; + if (strcmp(busInput, "motor") == 0) { + bus = CAN_1; + } else if (strcmp(busInput, "car") == 0) { + bus = CAN_3; + } else { + return false; + } + + if (CANbus_Read(&msg, blocking, bus) == SUCCESS) { + printf("msg recieved on %s (%s)\n\r", busInput, blockInput); + printf("ID: %d, Data: ", msg.ID); + for (int i = 0; i < 8; i++) { + printf("[%d] %x \n\r", i, msg.data[i]); + } + } else { + printf("read failed on %s (%s)\n\r", busInput, blockInput); + } + return true; } -static bool cmd_Contactors_Get(void){ - char *contactorInput = strtok_r(NULL, " ", &save); - contactor_t contactor; - if(strcmp(contactorInput, "array_c") == 0){ - contactor = ARRAY_CONTACTOR; - } - else if(strcmp(contactorInput, "array_p") == 0){ - contactor = ARRAY_PRECHARGE; - } - else if(strcmp(contactorInput, "motor_c") == 0){ - contactor = MOTOR_CONTACTOR; - } - else{ - return false; - } - - printf("%s state: %s\n\r", contactorInput, Contactors_Get(contactor) == ON ? "on" : "off"); - return true; +static bool cmd_Contactors_Get(void) { + char *contactorInput = strtok_r(NULL, " ", &save); + contactor_t contactor; + if (strcmp(contactorInput, "array_c") == 0) { + contactor = ARRAY_CONTACTOR; + } else if (strcmp(contactorInput, "array_p") == 0) { + contactor = ARRAY_PRECHARGE; + } else if (strcmp(contactorInput, "motor_c") == 0) { + contactor = MOTOR_CONTACTOR; + } else { + return false; + } + + printf("%s state: %s\n\r", contactorInput, + Contactors_Get(contactor) == ON ? "on" : "off"); + return true; } -static bool cmd_Contactors_Set(void){ - char *contactorInput = strtok_r(NULL, " ", &save); - contactor_t contactor; - if(strcmp(contactorInput, "array_c") == 0){ - contactor = ARRAY_CONTACTOR; - } - else if(strcmp(contactorInput, "array_p") == 0){ - contactor = ARRAY_PRECHARGE; - } - else if(strcmp(contactorInput, "motor_c") == 0){ - contactor = MOTOR_CONTACTOR; - } - else{ - return false; - } - - char *stateInput = strtok_r(NULL, " ", &save); - bool state; - if(strcmp(stateInput, "on") == 0){ - state = true; - } - else if(strcmp(stateInput, "off") == 0){ - state = false; - } - else{ - return false; - } - - char *blockingInput = strtok_r(NULL, " ", &save); - bool blocking; - if(strcmp(blockingInput, "blocking") == 0){ - blocking = true; - } - else if(strcmp(blockingInput, "nonblocking") == 0){ - blocking = false; - } - else{ - return false; - } - - if(Contactors_Set(contactor, state, blocking)){ - printf("%s set to %s (%s)\n\r", contactorInput, stateInput, blockingInput); - }else{ - printf("set failed\n\r"); - } - return true; +static bool cmd_Contactors_Set(void) { + char *contactorInput = strtok_r(NULL, " ", &save); + contactor_t contactor; + if (strcmp(contactorInput, "array_c") == 0) { + contactor = ARRAY_CONTACTOR; + } else if (strcmp(contactorInput, "array_p") == 0) { + contactor = ARRAY_PRECHARGE; + } else if (strcmp(contactorInput, "motor_c") == 0) { + contactor = MOTOR_CONTACTOR; + } else { + return false; + } + + char *stateInput = strtok_r(NULL, " ", &save); + bool state; + if (strcmp(stateInput, "on") == 0) { + state = true; + } else if (strcmp(stateInput, "off") == 0) { + state = false; + } else { + return false; + } + + char *blockingInput = strtok_r(NULL, " ", &save); + bool blocking; + if (strcmp(blockingInput, "blocking") == 0) { + blocking = true; + } else if (strcmp(blockingInput, "nonblocking") == 0) { + blocking = false; + } else { + return false; + } + + if (Contactors_Set(contactor, state, blocking)) { + printf("%s set to %s (%s)\n\r", contactorInput, stateInput, blockingInput); + } else { + printf("set failed\n\r"); + } + return true; } -static bool cmd_Minions_Read(void){ - char *pinInput = strtok_r(NULL, " ", &save); - pin_t pin; - if(strcmp(pinInput, "ign_1") == 0){ - pin = IGN_1; - } - else if(strcmp(pinInput, "ign_2") == 0){ - pin = IGN_2; - } - else if(strcmp(pinInput, "regen_sw") == 0){ - pin = REGEN_SW; - } - else if(strcmp(pinInput, "for_sw") == 0){ - pin = FOR_SW; - } - else if(strcmp(pinInput, "rev_sw") == 0){ - pin = REV_SW; - } - else if(strcmp(pinInput, "cruz_en") == 0){ - pin = CRUZ_EN; - } - else if(strcmp(pinInput, "cruz_st") == 0){ - pin = CRUZ_ST; - } - else if(strcmp(pinInput, "brakelight") == 0){ - pin = BRAKELIGHT; - } - else{ - return false; - } - - printf("%s is %s\n\r", pinInput, Minions_Read(pin) ? "on" : "off"); - return true; +static bool cmd_Minions_Read(void) { + char *pinInput = strtok_r(NULL, " ", &save); + pin_t pin; + if (strcmp(pinInput, "ign_1") == 0) { + pin = IGN_1; + } else if (strcmp(pinInput, "ign_2") == 0) { + pin = IGN_2; + } else if (strcmp(pinInput, "regen_sw") == 0) { + pin = REGEN_SW; + } else if (strcmp(pinInput, "for_sw") == 0) { + pin = FOR_SW; + } else if (strcmp(pinInput, "rev_sw") == 0) { + pin = REV_SW; + } else if (strcmp(pinInput, "cruz_en") == 0) { + pin = CRUZ_EN; + } else if (strcmp(pinInput, "cruz_st") == 0) { + pin = CRUZ_ST; + } else if (strcmp(pinInput, "brakelight") == 0) { + pin = BRAKELIGHT; + } else { + return false; + } + + printf("%s is %s\n\r", pinInput, Minions_Read(pin) ? "on" : "off"); + return true; } -static bool cmd_Minions_Write(void){ - char *pinInput = strtok_r(NULL, " ", &save); - pin_t pin; - if(strcmp(pinInput, "brakelight") == 0){ - pin = BRAKELIGHT; - } - else{ - return false; - } - - char *stateInput = strtok_r(NULL, " ", &save); - bool state; - if(strcmp(stateInput, "on") == 0){ - state = true; - } - else if(strcmp(stateInput, "off") == 0){ - state = false; - } - else{ - return false; - } - - Minions_Write(pin, state); - printf("%s set to %s\n\r", pinInput, stateInput); - return true; +static bool cmd_Minions_Write(void) { + char *pinInput = strtok_r(NULL, " ", &save); + pin_t pin; + if (strcmp(pinInput, "brakelight") == 0) { + pin = BRAKELIGHT; + } else { + return false; + } + + char *stateInput = strtok_r(NULL, " ", &save); + bool state; + if (strcmp(stateInput, "on") == 0) { + state = true; + } else if (strcmp(stateInput, "off") == 0) { + state = false; + } else { + return false; + } + + Minions_Write(pin, state); + printf("%s set to %s\n\r", pinInput, stateInput); + return true; } -static bool cmd_Pedals_Read(void){ - char *pedalInput = strtok_r(NULL, " ", &save); - pedal_t pedal; - if(strcmp(pedalInput, "accel") == 0){ - pedal = ACCELERATOR; - } - else if(strcmp(pedalInput, "brake") == 0){ - pedal = BRAKE; - } - else{ - return false; - } - - printf("%s: %d\n\r", pedalInput, Pedals_Read(pedal)); - return true; +static bool cmd_Pedals_Read(void) { + char *pedalInput = strtok_r(NULL, " ", &save); + pedal_t pedal; + if (strcmp(pedalInput, "accel") == 0) { + pedal = ACCELERATOR; + } else if (strcmp(pedalInput, "brake") == 0) { + pedal = BRAKE; + } else { + return false; + } + + printf("%s: %d\n\r", pedalInput, Pedals_Read(pedal)); + return true; } diff --git a/Apps/Src/DebugDump.c b/Apps/Src/DebugDump.c index 7013178b1..c584e7e51 100644 --- a/Apps/Src/DebugDump.c +++ b/Apps/Src/DebugDump.c @@ -1,129 +1,108 @@ -#include "os.h" -#include "Tasks.h" -#include "bsp.h" -#include "CANbus.h" +#include + #include "CAN_Queue.h" -#include "Pedals.h" -#include "Minions.h" +#include "CANbus.h" #include "Contactors.h" -#include "common.h" -#include -#include "Tasks.h" +#include "Minions.h" +#include "Pedals.h" #include "SendTritium.h" +#include "Tasks.h" +#include "bsp.h" +#include "common.h" +#include "os.h" // global variables extern fault_bitmap_t FaultBitmap; extern os_error_loc_t OSErrLocBitmap; -static const char *MINIONPIN_STRING[] = { - FOREACH_PIN(GENERATE_STRING) -}; +static const char *MINIONPIN_STRING[] = {FOREACH_PIN(GENERATE_STRING)}; -static const char *CONTACTOR_STRING[] = { - FOREACH_contactor(GENERATE_STRING) -}; +static const char *CONTACTOR_STRING[] = {FOREACH_contactor(GENERATE_STRING)}; -static const char *GEAR_STRING[] = { - FOREACH_Gear(GENERATE_STRING) -}; +static const char *GEAR_STRING[] = {FOREACH_Gear(GENERATE_STRING)}; // Need to keep this in sync with Task.h /*----------------------------------------------*/ #define FAULT_BITMAP_NUM 6 #define OS_LOC_NUM 14 -static const char *OS_LOC_STRING[] = { - "OS_NONE_LOC", - "OS_ARRAY_LOC", - "OS_READ_CAN_LOC", - "OS_READ_TRITIUM_LOC", - "OS_SEND_CAN_LOC", - "OS_SEND_TRITIUM_LOC", - "OS_UPDATE_VEL_LOC", - "OS_CONTACTOR_LOC", - "OS_MINIONS_LOC", - "OS_MAIN_LOC", - "OS_CANDRIVER_LOC", - "OS_MOTOR_CONNECTION_LOC", - "OS_DISPLAY_LOC" -}; - -static const char *FAULT_BITMAP_STRING[] = { - "FAULT_NONE", - "FAULT_OS", - "FAULT_UNREACH", - "FAULT_TRITIUM", - "FAULT_READBPS", - "FAULT_DISPLAY", - "FAULT_BPS" -}; +static const char *OS_LOC_STRING[] = { + "OS_NONE_LOC", "OS_ARRAY_LOC", "OS_READ_CAN_LOC", + "OS_READ_TRITIUM_LOC", "OS_SEND_CAN_LOC", "OS_SEND_TRITIUM_LOC", + "OS_UPDATE_VEL_LOC", "OS_CONTACTOR_LOC", "OS_MINIONS_LOC", + "OS_MAIN_LOC", "OS_CANDRIVER_LOC", "OS_MOTOR_CONNECTION_LOC", + "OS_DISPLAY_LOC"}; + +static const char *FAULT_BITMAP_STRING[] = { + "FAULT_NONE", "FAULT_OS", "FAULT_UNREACH", "FAULT_TRITIUM", + "FAULT_READBPS", "FAULT_DISPLAY", "FAULT_BPS"}; /*----------------------------------------------*/ -void Task_DebugDump(void* p_arg) { - OS_ERR err; - - while(1){ +void Task_DebugDump(void *p_arg) { + OS_ERR err; - // Get pedal information - int8_t accelPedal = Pedals_Read(ACCELERATOR); - printf("ACCELERATOR: %d\n\r", accelPedal); + while (1) { + // Get pedal information + int8_t accelPedal = Pedals_Read(ACCELERATOR); + printf("ACCELERATOR: %d\n\r", accelPedal); - int8_t brakePedal = Pedals_Read(BRAKE); - printf("BRAKE: %d\n\r", brakePedal); + int8_t brakePedal = Pedals_Read(BRAKE); + printf("BRAKE: %d\n\r", brakePedal); - // Get minion information - for(pin_t pin = 0; pin < NUM_PINS; pin++){ - bool pinState = Minions_Read(pin); - // Ignition pins are negative logic, special-case them - printf("%s: %s\n\r", MINIONPIN_STRING[pin], pinState ^ (pin == IGN_1 || pin == IGN_2) ? "on" : "off"); - } + // Get minion information + for (pin_t pin = 0; pin < NUM_PINS; pin++) { + bool pinState = Minions_Read(pin); + // Ignition pins are negative logic, special-case them + printf("%s: %s\n\r", MINIONPIN_STRING[pin], + pinState ^ (pin == IGN_1 || pin == IGN_2) ? "on" : "off"); + } - // Get contactor info - for(contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++){ - bool contactorState = Contactors_Get(contactor) == ON ? true : false; - printf("%s: %s\n\r", CONTACTOR_STRING[contactor], contactorState ? "on" : "off"); - } - - // Send Tritium variables - printf("Cruise Enable: %s\n\r", get_cruiseEnable() ? "true" : "false"); - printf("Cruise Set: %s\n\r", get_cruiseSet() ? "true" : "false"); - printf("One Pedal Enable: %s\n\r", get_onePedalEnable() ? "true" : "false"); - printf("Regen Enable: %s\n\r", get_regenEnable() ? "true" : "false"); - printf("Pedal Brake Percent: %d\n\r", get_brakePedalPercent()); - printf("Pedal Accel Percent: %d\n\r", get_accelPedalPercent()); - printf("Current Gear: %s\n\r", GEAR_STRING[get_gear()]); - print_float("Current Setpoint: ", get_currentSetpoint()); - - // fault bitmap - printf("Fault Bitmap: "); - if(FaultBitmap == FAULT_NONE){ - printf("%s", FAULT_BITMAP_STRING[0]); - }else{ - for(int i = 0; i < FAULT_BITMAP_NUM; i++){ - if(FaultBitmap & (1 << i)){ - printf("%s ", FAULT_BITMAP_STRING[i]); - } - } - } - printf("\n\r"); + // Get contactor info + for (contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++) { + bool contactorState = Contactors_Get(contactor) == ON ? true : false; + printf("%s: %s\n\r", CONTACTOR_STRING[contactor], + contactorState ? "on" : "off"); + } - // os loc bitmap - printf("OS Location Bitmap: "); - if(OSErrLocBitmap == OS_NONE_LOC){ - printf("%s", OS_LOC_STRING[0]); + // Send Tritium variables + printf("Cruise Enable: %s\n\r", get_cruiseEnable() ? "true" : "false"); + printf("Cruise Set: %s\n\r", get_cruiseSet() ? "true" : "false"); + printf("One Pedal Enable: %s\n\r", get_onePedalEnable() ? "true" : "false"); + printf("Regen Enable: %s\n\r", get_regenEnable() ? "true" : "false"); + printf("Pedal Brake Percent: %d\n\r", get_brakePedalPercent()); + printf("Pedal Accel Percent: %d\n\r", get_accelPedalPercent()); + printf("Current Gear: %s\n\r", GEAR_STRING[get_gear()]); + print_float("Current Setpoint: ", get_currentSetpoint()); + + // fault bitmap + printf("Fault Bitmap: "); + if (FaultBitmap == FAULT_NONE) { + printf("%s", FAULT_BITMAP_STRING[0]); + } else { + for (int i = 0; i < FAULT_BITMAP_NUM; i++) { + if (FaultBitmap & (1 << i)) { + printf("%s ", FAULT_BITMAP_STRING[i]); } - else{ - for(int i = 0; i < OS_LOC_NUM; i++){ - if(OSErrLocBitmap & (1 << i)){ - printf("%s ", OS_LOC_STRING[i]); - } - } + } + } + printf("\n\r"); + + // os loc bitmap + printf("OS Location Bitmap: "); + if (OSErrLocBitmap == OS_NONE_LOC) { + printf("%s", OS_LOC_STRING[0]); + } else { + for (int i = 0; i < OS_LOC_NUM; i++) { + if (OSErrLocBitmap & (1 << i)) { + printf("%s ", OS_LOC_STRING[i]); } - printf("\n\r"); + } + } + printf("\n\r"); - // Delay of 5 seconds - OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); - if (err != OS_ERR_NONE){ - assertOSError(OS_NONE_LOC, err); - } + // Delay of 5 seconds + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + if (err != OS_ERR_NONE) { + assertOSError(OS_NONE_LOC, err); } + } } \ No newline at end of file diff --git a/Apps/Src/FaultState.c b/Apps/Src/FaultState.c index 2c56136df..8a9e2b0ad 100644 --- a/Apps/Src/FaultState.c +++ b/Apps/Src/FaultState.c @@ -1,151 +1,161 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file FaultState.c - * @brief - * + * @brief + * */ #include "FaultState.h" -#include "Display.h" -#include "Contactors.h" -#include "os.h" -#include "Tasks.h" + +#include "CANbus.h" #include "Contactors.h" +#include "Display.h" #include "Minions.h" -#include "UpdateDisplay.h" #include "ReadTritium.h" -#include "CANbus.h" +#include "Tasks.h" +#include "UpdateDisplay.h" +#include "os.h" #define RESTART_THRESHOLD 3 -static bool fromThread = false; //whether fault was tripped from thread -extern const pinInfo_t PININFO_LUT[]; // For GPIO writes. Externed from Minions Driver C file. - +static bool fromThread = false; // whether fault was tripped from thread +extern const pinInfo_t + PININFO_LUT[]; // For GPIO writes. Externed from Minions Driver C file. static void ArrayMotorKill(void) { - BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN, OFF); - BSP_GPIO_Write_Pin(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN, OFF); - while(1){;} //nonrecoverable + BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN, OFF); + BSP_GPIO_Write_Pin(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN, OFF); + while (1) { + ; + } // nonrecoverable } -static void nonrecoverableFaultHandler(){ - //turn additional brakelight on to indicate critical error - BSP_GPIO_Write_Pin(PININFO_LUT[BRAKELIGHT].port, PININFO_LUT[BRAKELIGHT].pinMask, true); - ArrayMotorKill(); +static void nonrecoverableFaultHandler() { + // turn additional brakelight on to indicate critical error + BSP_GPIO_Write_Pin(PININFO_LUT[BRAKELIGHT].port, + PININFO_LUT[BRAKELIGHT].pinMask, true); + ArrayMotorKill(); } -static void readBPS_ContactorHandler(void){ - //kill contactors - Contactors_Set(ARRAY_CONTACTOR, OFF, true); - Contactors_Set(ARRAY_PRECHARGE, OFF, true); +static void readBPS_ContactorHandler(void) { + // kill contactors + Contactors_Set(ARRAY_CONTACTOR, OFF, true); + Contactors_Set(ARRAY_PRECHARGE, OFF, true); - // turn off the array contactor display light - UpdateDisplay_SetArray(false); + // turn off the array contactor display light + UpdateDisplay_SetArray(false); } void EnterFaultState(void) { - if(FaultBitmap & FAULT_OS){ - nonrecoverableFaultHandler(); + if (FaultBitmap & FAULT_OS) { + nonrecoverableFaultHandler(); + } else if (FaultBitmap & + FAULT_TRITIUM) { // This gets tripped by the ReadTritium thread + tritium_error_code_t TritiumError = + MotorController_getTritiumError(); // get error code to segregate based + // on fault type + + if (TritiumError & + T_HARDWARE_OVER_CURRENT_ERR) { // Tritium signaled too much current + nonrecoverableFaultHandler(); } - else if(FaultBitmap & FAULT_TRITIUM){ //This gets tripped by the ReadTritium thread - tritium_error_code_t TritiumError = MotorController_getTritiumError(); //get error code to segregate based on fault type - - if(TritiumError & T_HARDWARE_OVER_CURRENT_ERR){ //Tritium signaled too much current - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_SOFTWARE_OVER_CURRENT_ERR){ - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_DC_BUS_OVERVOLT_ERR){ //DC bus overvoltage - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_HALL_SENSOR_ERR){ //hall effect error - // Note: separate tripcnt from T_INIT_FAIL - static uint8_t hall_fault_cnt = 0; //trip counter - if(hall_fault_cnt > RESTART_THRESHOLD){ //try to restart the motor a few times and then fail out - nonrecoverableFaultHandler(); - } else { - hall_fault_cnt++; - MotorController_Restart(); //re-initialize motor - FaultBitmap &= ~FAULT_TRITIUM; // Clearing the FAULT_TRITIUM error bit on FaultBitmap - return; - } - } - - if(TritiumError & T_CONFIG_READ_ERR){ //Config read error - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_DESAT_FAULT_ERR){ //Desaturation fault error - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_MOTOR_OVER_SPEED_ERR){ //Motor over speed error - nonrecoverableFaultHandler(); - } - - if(TritiumError & T_INIT_FAIL){ //motorcontroller fails to restart or initialize - nonrecoverableFaultHandler(); - } - return; - - /** - * FAULTS NOT HANDLED : - * Watchdog Last Reset Error - Not using any hardware watchdogs. - * - * Under Voltage Lockout Error - Not really much we can do if we're not giving the controller enough voltage, - * and if we miss drive messages as a result, it will shut itself off. + if (TritiumError & T_SOFTWARE_OVER_CURRENT_ERR) { + nonrecoverableFaultHandler(); + } - */ + if (TritiumError & T_DC_BUS_OVERVOLT_ERR) { // DC bus overvoltage + nonrecoverableFaultHandler(); } - else if(FaultBitmap & FAULT_BPS){ // BPS has been tripped + + if (TritiumError & T_HALL_SENSOR_ERR) { // hall effect error + // Note: separate tripcnt from T_INIT_FAIL + static uint8_t hall_fault_cnt = 0; // trip counter + if (hall_fault_cnt > RESTART_THRESHOLD) { // try to restart the motor a + // few times and then fail out nonrecoverableFaultHandler(); + } else { + hall_fault_cnt++; + MotorController_Restart(); // re-initialize motor + FaultBitmap &= ~FAULT_TRITIUM; // Clearing the FAULT_TRITIUM error bit + // on FaultBitmap return; + } } - else if(FaultBitmap & FAULT_READBPS){ // Missed BPS Charge enable message - readBPS_ContactorHandler(); - - // Reset FaultBitmap since this is a recoverable fault - FaultBitmap &= ~FAULT_READBPS; // Clear the Read BPS fault from FaultBitmap - return; + + if (TritiumError & T_CONFIG_READ_ERR) { // Config read error + nonrecoverableFaultHandler(); } - else if(FaultBitmap & FAULT_UNREACH){ //unreachable code - nonrecoverableFaultHandler(); + + if (TritiumError & T_DESAT_FAULT_ERR) { // Desaturation fault error + nonrecoverableFaultHandler(); } - else if(FaultBitmap & FAULT_DISPLAY){ - static uint8_t disp_fault_cnt = 0; - if(disp_fault_cnt>3){ - Display_Fault(OSErrLocBitmap, FaultBitmap); - } else { - disp_fault_cnt++; - Display_Reset(); - FaultBitmap &= ~FAULT_DISPLAY; // Clear the display fault bit in FaultBitmap - return; - } + + if (TritiumError & T_MOTOR_OVER_SPEED_ERR) { // Motor over speed error + nonrecoverableFaultHandler(); } - if(fromThread){//no recovering if fault state was entered outside of the fault thread - return; + + if (TritiumError & + T_INIT_FAIL) { // motorcontroller fails to restart or initialize + nonrecoverableFaultHandler(); } - while(1){;} + + return; + + /** + * FAULTS NOT HANDLED : + * Watchdog Last Reset Error - Not using any hardware watchdogs. + * + * Under Voltage Lockout Error - Not really much we can do if we're not + giving the controller enough voltage, + * and if we miss drive messages as a result, it will shut itself off. + + */ + } else if (FaultBitmap & FAULT_BPS) { // BPS has been tripped + nonrecoverableFaultHandler(); + return; + } else if (FaultBitmap & FAULT_READBPS) { // Missed BPS Charge enable message + readBPS_ContactorHandler(); + + // Reset FaultBitmap since this is a recoverable fault + FaultBitmap &= ~FAULT_READBPS; // Clear the Read BPS fault from FaultBitmap + return; + } else if (FaultBitmap & FAULT_UNREACH) { // unreachable code + nonrecoverableFaultHandler(); + } else if (FaultBitmap & FAULT_DISPLAY) { + static uint8_t disp_fault_cnt = 0; + if (disp_fault_cnt > 3) { + Display_Fault(OSErrLocBitmap, FaultBitmap); + } else { + disp_fault_cnt++; + Display_Reset(); + FaultBitmap &= + ~FAULT_DISPLAY; // Clear the display fault bit in FaultBitmap + return; + } + } + if (fromThread) { // no recovering if fault state was entered outside of the + // fault thread + return; + } + while (1) { + ; + } } void Task_FaultState(void *p_arg) { - OS_ERR err; - CPU_TS ts; - - FaultBitmap = FAULT_NONE; - OSErrLocBitmap = OS_NONE_LOC; - - // Block until fault is signaled by an assert - while(1){ - OSSemPend(&FaultState_Sem4, 0, OS_OPT_PEND_BLOCKING, &ts, &err); - fromThread = true; - EnterFaultState(); - fromThread = false; - OSTimeDlyHMSM(0,0,0,5,OS_OPT_TIME_HMSM_STRICT,&err); - } + OS_ERR err; + CPU_TS ts; + + FaultBitmap = FAULT_NONE; + OSErrLocBitmap = OS_NONE_LOC; + + // Block until fault is signaled by an assert + while (1) { + OSSemPend(&FaultState_Sem4, 0, OS_OPT_PEND_BLOCKING, &ts, &err); + fromThread = true; + EnterFaultState(); + fromThread = false; + OSTimeDlyHMSM(0, 0, 0, 5, OS_OPT_TIME_HMSM_STRICT, &err); + } } diff --git a/Apps/Src/PedalToPercent.c b/Apps/Src/PedalToPercent.c index 87646432a..026b67841 100644 --- a/Apps/Src/PedalToPercent.c +++ b/Apps/Src/PedalToPercent.c @@ -1,111 +1,20 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file PedalToPercent.c - * @brief - * + * @brief + * */ /* Converts accelerator pedal percentage to a decimal */ const float pedalToPercent[] = { - 0.00f, - 0.01f, - 0.02f, - 0.03f, - 0.04f, - 0.05f, - 0.06f, - 0.07f, - 0.08f, - 0.09f, - 0.1f, - 0.11f, - 0.12f, - 0.13f, - 0.14f, - 0.15f, - 0.16f, - 0.17f, - 0.18f, - 0.19f, - 0.2f, - 0.21f, - 0.22f, - 0.23f, - 0.24f, - 0.25f, - 0.26f, - 0.27f, - 0.28f, - 0.29f, - 0.3f, - 0.31f, - 0.32f, - 0.33f, - 0.34f, - 0.35f, - 0.36f, - 0.37f, - 0.38f, - 0.39f, - 0.4f, - 0.41f, - 0.42f, - 0.43f, - 0.44f, - 0.45f, - 0.46f, - 0.47f, - 0.48f, - 0.49f, - 0.5f, - 0.51f, - 0.52f, - 0.53f, - 0.54f, - 0.55f, - 0.56f, - 0.57f, - 0.58f, - 0.59f, - 0.6f, - 0.61f, - 0.62f, - 0.63f, - 0.64f, - 0.65f, - 0.66f, - 0.67f, - 0.68f, - 0.69f, - 0.7f, - 0.71f, - 0.72f, - 0.73f, - 0.74f, - 0.75f, - 0.76f, - 0.77f, - 0.78f, - 0.79f, - 0.8f, - 0.81f, - 0.82f, - 0.83f, - 0.84f, - 0.85f, - 0.86f, - 0.87f, - 0.88f, - 0.89f, - 0.9f, - 0.91f, - 0.92f, - 0.93f, - 0.94f, - 0.95f, - 0.96f, - 0.97f, - 0.98f, - 0.99f, - 1.0f, + 0.00f, 0.01f, 0.02f, 0.03f, 0.04f, 0.05f, 0.06f, 0.07f, 0.08f, 0.09f, 0.1f, + 0.11f, 0.12f, 0.13f, 0.14f, 0.15f, 0.16f, 0.17f, 0.18f, 0.19f, 0.2f, 0.21f, + 0.22f, 0.23f, 0.24f, 0.25f, 0.26f, 0.27f, 0.28f, 0.29f, 0.3f, 0.31f, 0.32f, + 0.33f, 0.34f, 0.35f, 0.36f, 0.37f, 0.38f, 0.39f, 0.4f, 0.41f, 0.42f, 0.43f, + 0.44f, 0.45f, 0.46f, 0.47f, 0.48f, 0.49f, 0.5f, 0.51f, 0.52f, 0.53f, 0.54f, + 0.55f, 0.56f, 0.57f, 0.58f, 0.59f, 0.6f, 0.61f, 0.62f, 0.63f, 0.64f, 0.65f, + 0.66f, 0.67f, 0.68f, 0.69f, 0.7f, 0.71f, 0.72f, 0.73f, 0.74f, 0.75f, 0.76f, + 0.77f, 0.78f, 0.79f, 0.8f, 0.81f, 0.82f, 0.83f, 0.84f, 0.85f, 0.86f, 0.87f, + 0.88f, 0.89f, 0.9f, 0.91f, 0.92f, 0.93f, 0.94f, 0.95f, 0.96f, 0.97f, 0.98f, + 0.99f, 1.0f, }; diff --git a/Apps/Src/ReadCarCAN.c b/Apps/Src/ReadCarCAN.c index a054307d9..5970f1ca3 100644 --- a/Apps/Src/ReadCarCAN.c +++ b/Apps/Src/ReadCarCAN.c @@ -1,24 +1,27 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file ReadCarCAN.c - * @brief - * + * @brief + * */ - #include "ReadCarCAN.h" -#include "UpdateDisplay.h" + #include "Contactors.h" #include "FaultState.h" #include "Minions.h" +#include "UpdateDisplay.h" #include "os_cfg_app.h" -// Saturation threshold is halfway between 0 and max saturation value (half of summation from one to the number of positions) -#define SATURATION_THRESHOLD (((SAT_BUF_LENGTH + 1) * SAT_BUF_LENGTH) / 4) +// Saturation threshold is halfway between 0 and max saturation value (half of +// summation from one to the number of positions) +#define SATURATION_THRESHOLD (((SAT_BUF_LENGTH + 1) * SAT_BUF_LENGTH) / 4) // timer delay constants #define CAN_WATCH_TMR_DLY_MS 500u -#define CAN_WATCH_TMR_DLY_TMR_TS ((CAN_WATCH_TMR_DLY_MS * OS_CFG_TMR_TASK_RATE_HZ) / (1000u)) //1000 for ms -> s conversion +#define CAN_WATCH_TMR_DLY_TMR_TS \ + ((CAN_WATCH_TMR_DLY_MS * OS_CFG_TMR_TASK_RATE_HZ) / \ + (1000u)) // 1000 for ms -> s conversion #define PRECHARGE_DLY_TMR_TS (PRECHARGE_ARRAY_DELAY * OS_CFG_TMR_TASK_RATE_HZ) // CAN watchdog timer variable @@ -31,8 +34,9 @@ static OS_TMR prechargeDlyTimer; static OS_MUTEX arrayRestartMutex; static bool restartingArray = false; -// NOTE: This should not be written to anywhere other than ReadCarCAN. If the need arises, a mutex to protect it must be added. -// Indicates whether or not regenerative braking / charging is enabled. +// NOTE: This should not be written to anywhere other than ReadCarCAN. If the +// need arises, a mutex to protect it must be added. Indicates whether or not +// regenerative braking / charging is enabled. static bool chargeEnable = false; // Saturation buffer variables @@ -44,220 +48,196 @@ static uint8_t oldestMsgIdx = 0; static uint8_t SOC = 0; static uint32_t SBPV = 0; - // Handler to turn array back on -static void arrayRestart(void *p_tmr, void *p_arg); - +static void arrayRestart(void *p_tmr, void *p_arg); // Getter function for chargeEnable -bool ChargeEnable_Get() -{ - return chargeEnable; -} - +bool ChargeEnable_Get() { return chargeEnable; } /** - * @brief adds new messages by overwriting old messages in the saturation buffer and then updates saturation - * @param chargeMessage whether bps message was charge enable (1) or disable (-1) -*/ -static void updateSaturation(int8_t chargeMessage){ - // Replace oldest message with new charge message and update index for oldest message - chargeMsgBuffer[oldestMsgIdx] = chargeMessage; - oldestMsgIdx = (oldestMsgIdx + 1) % SAT_BUF_LENGTH; - - // Calculate the new saturation value by assigning weightings from 1 to buffer length - // in order of oldest to newest - int newSaturation = 0; - for (uint8_t i = 0; i < SAT_BUF_LENGTH; i++){ - newSaturation += chargeMsgBuffer[(oldestMsgIdx + i) % SAT_BUF_LENGTH] * (i + 1); - } - chargeMsgSaturation = newSaturation; + * @brief adds new messages by overwriting old messages in the saturation buffer + * and then updates saturation + * @param chargeMessage whether bps message was charge enable (1) or disable + * (-1) + */ +static void updateSaturation(int8_t chargeMessage) { + // Replace oldest message with new charge message and update index for oldest + // message + chargeMsgBuffer[oldestMsgIdx] = chargeMessage; + oldestMsgIdx = (oldestMsgIdx + 1) % SAT_BUF_LENGTH; + + // Calculate the new saturation value by assigning weightings from 1 to buffer + // length in order of oldest to newest + int newSaturation = 0; + for (uint8_t i = 0; i < SAT_BUF_LENGTH; i++) { + newSaturation += + chargeMsgBuffer[(oldestMsgIdx + i) % SAT_BUF_LENGTH] * (i + 1); + } + chargeMsgSaturation = newSaturation; } // helper function to disable charging // Turns off contactors by setting fault bitmap and signaling fault state static inline void chargingDisable(void) { - OS_ERR err; - // mark regen as disabled - chargeEnable = false; + OS_ERR err; + // mark regen as disabled + chargeEnable = false; - //kill contactors - Contactors_Set(ARRAY_CONTACTOR, false, true); - Contactors_Set(ARRAY_PRECHARGE, false, true); - - // mark regen as disabled - chargeEnable = false; + // kill contactors + Contactors_Set(ARRAY_CONTACTOR, false, true); + Contactors_Set(ARRAY_PRECHARGE, false, true); - // Set fault bitmap - FaultBitmap |= FAULT_READBPS; + // mark regen as disabled + chargeEnable = false; - // Signal fault state to kill contactors at its earliest convenience - OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); - assertOSError(OS_READ_CAN_LOC,err); + // Set fault bitmap + FaultBitmap |= FAULT_READBPS; + // Signal fault state to kill contactors at its earliest convenience + OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); + assertOSError(OS_READ_CAN_LOC, err); } // helper function to call if charging should be enabled static inline void chargingEnable(void) { - OS_ERR err; - CPU_TS ts; - - // mark regen as enabled - chargeEnable = true; + OS_ERR err; + CPU_TS ts; - // check if we need to run the precharge sequence to turn on the array - bool shouldRestartArray = false; + // mark regen as enabled + chargeEnable = true; - OSMutexPend(&arrayRestartMutex, 0, OS_OPT_PEND_BLOCKING, &ts, &err); - assertOSError(OS_READ_CAN_LOC,err); + // check if we need to run the precharge sequence to turn on the array + bool shouldRestartArray = false; - // if the array is off and we're not already turning it on, start turning it on - shouldRestartArray = !restartingArray && (Contactors_Get(ARRAY_CONTACTOR)==OFF); + OSMutexPend(&arrayRestartMutex, 0, OS_OPT_PEND_BLOCKING, &ts, &err); + assertOSError(OS_READ_CAN_LOC, err); - // wait for precharge for array - if(shouldRestartArray){ + // if the array is off and we're not already turning it on, start turning it + // on + shouldRestartArray = + !restartingArray && (Contactors_Get(ARRAY_CONTACTOR) == OFF); - restartingArray = true; + // wait for precharge for array + if (shouldRestartArray) { + restartingArray = true; - // Wait to make sure precharge is finished and then restart array - OSTmrStart(&prechargeDlyTimer, &err); - assertOSError(OS_READ_CAN_LOC, err); - - } - + // Wait to make sure precharge is finished and then restart array + OSTmrStart(&prechargeDlyTimer, &err); + assertOSError(OS_READ_CAN_LOC, err); + } - OSMutexPost(&arrayRestartMutex, OS_OPT_NONE, &err); - assertOSError(OS_READ_CAN_LOC,err); + OSMutexPost(&arrayRestartMutex, OS_OPT_NONE, &err); + assertOSError(OS_READ_CAN_LOC, err); } /** - * @brief Callback function for the precharge delay timer. Waits for precharge and then restarts the array. + * @brief Callback function for the precharge delay timer. Waits for precharge + * and then restarts the array. * @param p_tmr pointer to the timer that calls this function, passed by timer * @param p_arg pointer to the argument passed by timer - * -*/ -static void arrayRestart(void *p_tmr, void *p_arg){ - if(chargeEnable){ // If regen has been disabled during precharge, we don't want to turn on the main contactor immediately after - Contactors_Set(ARRAY_CONTACTOR, (Minions_Read(IGN_1)), false); //turn on array contactor if the ign switch lets us - UpdateDisplay_SetArray(true); - } - // done restarting the array - restartingArray = false; - + * + */ +static void arrayRestart(void *p_tmr, void *p_arg) { + if (chargeEnable) { // If regen has been disabled during precharge, we don't + // want to turn on the main contactor immediately after + Contactors_Set(ARRAY_CONTACTOR, (Minions_Read(IGN_1)), + false); // turn on array contactor if the ign switch lets us + UpdateDisplay_SetArray(true); + } + // done restarting the array + restartingArray = false; }; /** - * @brief Disables charging when canWatchTimer hits 0 (when charge enable messages are missed) + * @brief Disables charging when canWatchTimer hits 0 (when charge enable + * messages are missed) * @param p_tmr pointer to the timer that calls this function, passed by timer * @param p_arg pointer to the argument passed by timer -*/ -void canWatchTimerCallback (void *p_tmr, void *p_arg){ + */ +void canWatchTimerCallback(void *p_tmr, void *p_arg) { chargingDisable(); } - chargingDisable(); -} +void Task_ReadCarCAN(void *p_arg) { + OS_ERR err; + // data struct for CAN message + CANDATA_t dataBuf; + OSMutexCreate(&arrayRestartMutex, "array restart mutex", &err); + assertOSError(OS_READ_CAN_LOC, err); -void Task_ReadCarCAN(void *p_arg) -{ - OS_ERR err; + // Create the CAN Watchdog (periodic) timer, which disconnects the array and + // disables regenerative braking if we do not get a CAN message with the ID + // Charge_Enable within the desired interval. + OSTmrCreate(&canWatchTimer, "CAN Watch Timer", 0, CAN_WATCH_TMR_DLY_TMR_TS, + OS_OPT_TMR_PERIODIC, canWatchTimerCallback, NULL, &err); + assertOSError(OS_READ_CAN_LOC, err); - //data struct for CAN message - CANDATA_t dataBuf; + OSTmrCreate(&prechargeDlyTimer, "Precharge Delay Timer", PRECHARGE_DLY_TMR_TS, + 0, OS_OPT_TMR_ONE_SHOT, arrayRestart, NULL, &err); + assertOSError(OS_READ_CAN_LOC, err); - OSMutexCreate(&arrayRestartMutex, "array restart mutex", &err); - assertOSError(OS_READ_CAN_LOC,err); + // Start CAN Watchdog timer + OSTmrStart(&canWatchTimer, &err); + assertOSError(OS_READ_CAN_LOC, err); + while (1) { + // Get any message that BPS Sent us + ErrorStatus status = CANbus_Read(&dataBuf, true, CARCAN); + if (status != SUCCESS) { + continue; + } - // Create the CAN Watchdog (periodic) timer, which disconnects the array and disables regenerative braking - // if we do not get a CAN message with the ID Charge_Enable within the desired interval. - OSTmrCreate( - &canWatchTimer, - "CAN Watch Timer", - 0, - CAN_WATCH_TMR_DLY_TMR_TS, - OS_OPT_TMR_PERIODIC, - canWatchTimerCallback, - NULL, - &err - ); - assertOSError(OS_READ_CAN_LOC, err); + switch (dataBuf.ID) { // we got a message + case BPS_TRIP: { + // BPS has a fault and we need to enter fault state (probably) + if (dataBuf.data[0] == 1) { // If buffer contains 1 for a BPS trip, we + // should enter a nonrecoverable fault + OS_ERR err; - OSTmrCreate( - &prechargeDlyTimer, - "Precharge Delay Timer", - PRECHARGE_DLY_TMR_TS, - 0, - OS_OPT_TMR_ONE_SHOT, - arrayRestart, - NULL, - &err - ); - assertOSError(OS_READ_CAN_LOC, err); + Display_Evac(SOC, SBPV); // Display evacuation message - //Start CAN Watchdog timer - OSTmrStart(&canWatchTimer, &err); - assertOSError(OS_READ_CAN_LOC, err); - - - while (1) - { - - //Get any message that BPS Sent us - ErrorStatus status = CANbus_Read(&dataBuf,true,CARCAN); - if(status != SUCCESS) { - continue; + // Set fault bitmap and assert the error + FaultBitmap |= FAULT_BPS; + OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); + assertOSError(OS_READ_CAN_LOC, err); } - - switch(dataBuf.ID){ //we got a message - case BPS_TRIP: { - // BPS has a fault and we need to enter fault state (probably) - if(dataBuf.data[0] == 1){ // If buffer contains 1 for a BPS trip, we should enter a nonrecoverable fault - OS_ERR err; - - Display_Evac(SOC, SBPV); // Display evacuation message - - // Set fault bitmap and assert the error - FaultBitmap |= FAULT_BPS; - OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); - assertOSError(OS_READ_CAN_LOC, err); - } - } - case CHARGE_ENABLE: { - - // Restart CAN Watchdog timer - OSTmrStart(&canWatchTimer, &err); - assertOSError(OS_READ_CAN_LOC, err); - - - if(dataBuf.data[0] == 0){ // If the buffer doesn't contain 1 for enable, turn off chargeEnable and turn array off - chargingDisable(); - updateSaturation(-1); // Update saturation and buffer - - } else { //We got a message of enable with a nonzero value - updateSaturation(1); - - // If the charge message saturation is above the threshold, wait for restart precharge sequence then enable charging. - // If we are already precharging/array is on, nothing will be done - if (chargeMsgSaturation >= SATURATION_THRESHOLD){ - chargingEnable(); - } - - } - break; - } - case SUPPLEMENTAL_VOLTAGE: { - SBPV = (*(uint16_t *) &dataBuf.data); - UpdateDisplay_SetSBPV(SBPV); // Receive value in mV - break; - } - case STATE_OF_CHARGE:{ - SOC = (*(uint32_t*) &dataBuf.data)/(100000); // Convert to integer percent - UpdateDisplay_SetSOC(SOC); - break; - } - default: - break; + } + case CHARGE_ENABLE: { + // Restart CAN Watchdog timer + OSTmrStart(&canWatchTimer, &err); + assertOSError(OS_READ_CAN_LOC, err); + + if (dataBuf.data[0] == + 0) { // If the buffer doesn't contain 1 for enable, turn off + // chargeEnable and turn array off + chargingDisable(); + updateSaturation(-1); // Update saturation and buffer + + } else { // We got a message of enable with a nonzero value + updateSaturation(1); + + // If the charge message saturation is above the threshold, wait for + // restart precharge sequence then enable charging. If we are already + // precharging/array is on, nothing will be done + if (chargeMsgSaturation >= SATURATION_THRESHOLD) { + chargingEnable(); + } } + break; + } + case SUPPLEMENTAL_VOLTAGE: { + SBPV = (*(uint16_t *)&dataBuf.data); + UpdateDisplay_SetSBPV(SBPV); // Receive value in mV + break; + } + case STATE_OF_CHARGE: { + SOC = (*(uint32_t *)&dataBuf.data) / + (100000); // Convert to integer percent + UpdateDisplay_SetSOC(SOC); + break; + } + default: + break; } + } } diff --git a/Apps/Src/ReadTritium.c b/Apps/Src/ReadTritium.c old mode 100755 new mode 100644 index a9799bcbc..adcf1bf86 --- a/Apps/Src/ReadTritium.c +++ b/Apps/Src/ReadTritium.c @@ -1,37 +1,39 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file ReadTritium.c - * @brief - * + * @brief + * */ #include "ReadTritium.h" + +#include + #include "CAN_Queue.h" #include "CANbus.h" #include "UpdateDisplay.h" -#include -//status limit flag masks -#define MASK_MOTOR_TEMP_LIMIT (1<<6) //check if motor temperature is limiting the motor 6 +// status limit flag masks +#define MASK_MOTOR_TEMP_LIMIT \ + (1 << 6) // check if motor temperature is limiting the motor 6 #define MAX_CAN_LEN 8 - uint16_t Motor_FaultBitmap = T_NONE; -static float Motor_RPM = CAR_STOPPED; //Car is stopped until velocity is read -static float Motor_Velocity = CAR_STOPPED; //^^^^ +static float Motor_RPM = CAR_STOPPED; // Car is stopped until velocity is read +static float Motor_Velocity = CAR_STOPPED; //^^^^ /** * @brief Returns highest priority tritium error code - * + * */ -tritium_error_code_t MotorController_getTritiumError(void){ - //TODO: implement for loop to parse this - for(int i = 0; i < NUM_TRITIUM_ERRORS; ++i){ - if(Motor_FaultBitmap & (1< 100){ - return 1.0f; - } - return pedalToPercent[percent]; +static float percentToFloat(uint8_t percent) { + if (percent > 100) { + return 1.0f; + } + return pedalToPercent[percent]; } #ifdef SENDTRITIUM_PRINT_MES /** * @brief Dumps info to UART during testing -*/ -static void getName(char* nameStr, uint8_t stateNameNum){ - switch(stateNameNum){ - case FORWARD_DRIVE: - strcpy(nameStr, "FORWARD_DRIVE"); - break; - case NEUTRAL_DRIVE: - strcpy(nameStr, "NEUTRAL_DRIVE"); - break; - case REVERSE_DRIVE: - strcpy(nameStr, "REVERSE_DRIVE"); - break; - case RECORD_VELOCITY: - strcpy(nameStr, "RECORD_VELOCITY"); - break; - case POWERED_CRUISE: - strcpy(nameStr, "POWERED_CRUISE"); - break; - case COASTING_CRUISE: - strcpy(nameStr, "COASTING_CRUISE"); - break; - case BRAKE_STATE: - strcpy(nameStr, "BRAKE_STATE"); - break; - case ONEPEDAL: - strcpy(nameStr, "ONEPEDAL"); - break; - case ACCELERATE_CRUISE: - strcpy(nameStr, "ACCELERATE_CRUISE"); - break; - default: - strcpy(nameStr, "UNKNOWN"); - break; - } - return; + */ +static void getName(char* nameStr, uint8_t stateNameNum) { + switch (stateNameNum) { + case FORWARD_DRIVE: + strcpy(nameStr, "FORWARD_DRIVE"); + break; + case NEUTRAL_DRIVE: + strcpy(nameStr, "NEUTRAL_DRIVE"); + break; + case REVERSE_DRIVE: + strcpy(nameStr, "REVERSE_DRIVE"); + break; + case RECORD_VELOCITY: + strcpy(nameStr, "RECORD_VELOCITY"); + break; + case POWERED_CRUISE: + strcpy(nameStr, "POWERED_CRUISE"); + break; + case COASTING_CRUISE: + strcpy(nameStr, "COASTING_CRUISE"); + break; + case BRAKE_STATE: + strcpy(nameStr, "BRAKE_STATE"); + break; + case ONEPEDAL: + strcpy(nameStr, "ONEPEDAL"); + break; + case ACCELERATE_CRUISE: + strcpy(nameStr, "ACCELERATE_CRUISE"); + break; + default: + strcpy(nameStr, "UNKNOWN"); + break; + } + return; } -static void dumpInfo(){ - printf("-------------------\n\r"); - char stateName[20]; - getName(stateName, state.name); - printf("State: %s\n\r", stateName); - printf("cruiseEnable: %d\n\r", cruiseEnable); - printf("cruiseSet: %d\n\r", cruiseSet); - printf("onePedalEnable: %d\n\r", onePedalEnable); - printf("brakePedalPercent: %d\n\r", brakePedalPercent); - printf("accelPedalPercent: %d\n\r", accelPedalPercent); - printf("gear: %d\n\r", (uint8_t)gear); - print_float("currentSetpoint: ", currentSetpoint); - print_float("velocitySetpoint: ", velocitySetpoint); - print_float("velocityObserved: ", velocityObserved); - printf("-------------------\n\r"); +static void dumpInfo() { + printf("-------------------\n\r"); + char stateName[20]; + getName(stateName, state.name); + printf("State: %s\n\r", stateName); + printf("cruiseEnable: %d\n\r", cruiseEnable); + printf("cruiseSet: %d\n\r", cruiseSet); + printf("onePedalEnable: %d\n\r", onePedalEnable); + printf("brakePedalPercent: %d\n\r", brakePedalPercent); + printf("accelPedalPercent: %d\n\r", accelPedalPercent); + printf("gear: %d\n\r", (uint8_t)gear); + print_float("currentSetpoint: ", currentSetpoint); + print_float("velocitySetpoint: ", velocitySetpoint); + print_float("velocityObserved: ", velocityObserved); + printf("-------------------\n\r"); } #endif #ifndef SENDTRITIUM_EXPOSE_VARS /** * @brief Reads inputs from the system -*/ -static void readInputs(){ - - // Update pedals - brakePedalPercent = Pedals_Read(BRAKE); - accelPedalPercent = Pedals_Read(ACCELERATOR); - - // Update regen enable - regenEnable = ChargeEnable_Get(); - - // Update buttons - if(Minions_Read(REGEN_SW) && onePedalCounter < DEBOUNCE_PERIOD){onePedalCounter++;} - else if(onePedalCounter > 0){onePedalCounter--;} - - if(Minions_Read(CRUZ_EN) && cruiseEnableCounter < DEBOUNCE_PERIOD){cruiseEnableCounter++;} - else if(cruiseEnableCounter > 0){cruiseEnableCounter--;} - - if(Minions_Read(CRUZ_ST) && cruiseSetCounter < DEBOUNCE_PERIOD){cruiseSetCounter++;} - else if(cruiseSetCounter > 0){cruiseSetCounter--;} - - // Update gears - bool forwardSwitch = Minions_Read(FOR_SW); - bool reverseSwitch = Minions_Read(REV_SW); - bool forwardGear = (forwardSwitch && !reverseSwitch); - bool reverseGear = (!forwardSwitch && reverseSwitch); - bool neutralGear = (!forwardSwitch && !reverseSwitch); - - uint8_t gearFault = (uint8_t) forwardGear + (uint8_t) reverseGear + (uint8_t) neutralGear; - static uint8_t gearFaultCnt = 0; - - if(gearFault != 1){ - // Fault behavior - if(gearFaultCnt > GEAR_FAULT_THRESHOLD) state = FSM[NEUTRAL_DRIVE]; - else gearFaultCnt++; - } - else{ - gearFaultCnt = 0; - } - - if(neutralGear) gear = NEUTRAL_GEAR; - else if(forwardGear) gear = FORWARD_GEAR; - else if(reverseGear) gear = REVERSE_GEAR; - else gear = NEUTRAL_GEAR; - - // Debouncing - if(onePedalCounter == DEBOUNCE_PERIOD){onePedalButton = true;} - else if(onePedalCounter == 0){onePedalButton = false;} - - if(cruiseEnableCounter == DEBOUNCE_PERIOD){cruiseEnableButton = true;} - else if(cruiseEnableCounter == 0){cruiseEnableButton = false;} - - if(cruiseSetCounter == DEBOUNCE_PERIOD){cruiseSet = true;} - else if(cruiseSetCounter == 0){cruiseSet = false;} - - // Toggle - if(onePedalButton != onePedalPrevious && onePedalPrevious){onePedalEnable = !onePedalEnable;} - if(!regenEnable) onePedalEnable = false; - onePedalPrevious = onePedalButton; - - if(cruiseEnableButton != cruiseEnablePrevious && cruiseEnablePrevious){cruiseEnable = !cruiseEnable;} - cruiseEnablePrevious = cruiseEnableButton; - - // Get observed velocity - velocityObserved = Motor_RPM_Get(); + */ +static void readInputs() { + // Update pedals + brakePedalPercent = Pedals_Read(BRAKE); + accelPedalPercent = Pedals_Read(ACCELERATOR); + + // Update regen enable + regenEnable = ChargeEnable_Get(); + + // Update buttons + if (Minions_Read(REGEN_SW) && onePedalCounter < DEBOUNCE_PERIOD) { + onePedalCounter++; + } else if (onePedalCounter > 0) { + onePedalCounter--; + } + + if (Minions_Read(CRUZ_EN) && cruiseEnableCounter < DEBOUNCE_PERIOD) { + cruiseEnableCounter++; + } else if (cruiseEnableCounter > 0) { + cruiseEnableCounter--; + } + + if (Minions_Read(CRUZ_ST) && cruiseSetCounter < DEBOUNCE_PERIOD) { + cruiseSetCounter++; + } else if (cruiseSetCounter > 0) { + cruiseSetCounter--; + } + + // Update gears + bool forwardSwitch = Minions_Read(FOR_SW); + bool reverseSwitch = Minions_Read(REV_SW); + bool forwardGear = (forwardSwitch && !reverseSwitch); + bool reverseGear = (!forwardSwitch && reverseSwitch); + bool neutralGear = (!forwardSwitch && !reverseSwitch); + + uint8_t gearFault = + (uint8_t)forwardGear + (uint8_t)reverseGear + (uint8_t)neutralGear; + static uint8_t gearFaultCnt = 0; + + if (gearFault != 1) { + // Fault behavior + if (gearFaultCnt > GEAR_FAULT_THRESHOLD) + state = FSM[NEUTRAL_DRIVE]; + else + gearFaultCnt++; + } else { + gearFaultCnt = 0; + } + + if (neutralGear) + gear = NEUTRAL_GEAR; + else if (forwardGear) + gear = FORWARD_GEAR; + else if (reverseGear) + gear = REVERSE_GEAR; + else + gear = NEUTRAL_GEAR; + + // Debouncing + if (onePedalCounter == DEBOUNCE_PERIOD) { + onePedalButton = true; + } else if (onePedalCounter == 0) { + onePedalButton = false; + } + + if (cruiseEnableCounter == DEBOUNCE_PERIOD) { + cruiseEnableButton = true; + } else if (cruiseEnableCounter == 0) { + cruiseEnableButton = false; + } + + if (cruiseSetCounter == DEBOUNCE_PERIOD) { + cruiseSet = true; + } else if (cruiseSetCounter == 0) { + cruiseSet = false; + } + + // Toggle + if (onePedalButton != onePedalPrevious && onePedalPrevious) { + onePedalEnable = !onePedalEnable; + } + if (!regenEnable) onePedalEnable = false; + onePedalPrevious = onePedalButton; + + if (cruiseEnableButton != cruiseEnablePrevious && cruiseEnablePrevious) { + cruiseEnable = !cruiseEnable; + } + cruiseEnablePrevious = cruiseEnableButton; + + // Get observed velocity + velocityObserved = Motor_RPM_Get(); } #endif /** - * @brief Linearly map range of integers to another range of integers. + * @brief Linearly map range of integers to another range of integers. * in_min to in_max is mapped to out_min to out_max. * @param input input integer value * @param in_min minimum value of input range * @param in_max maximum value of input range * @param out_min minimum value of output range - * @param out_max maximum value of output range + * @param out_max maximum value of output range * @returns integer value from out_min to out_max -*/ -static uint8_t map(uint8_t input, uint8_t in_min, uint8_t in_max, uint8_t out_min, uint8_t out_max){ - if(in_min >= in_max) in_max = in_min; // The minimum of the input range should never be greater than the maximum of the input range - - if(input <= in_min){ - // Lower bound the input to the minimum possible output - return out_min; - } else if(input >= in_max){ - // Upper bound the input to the maximum output - return out_max; - } else{ - // Linear mapping between ranges - uint8_t offset_in = input - in_min; // If input went from A -> B, it now goes from 0 -> B-A - uint8_t in_range = in_max - in_min; // Input range - uint8_t out_range = out_max - out_min; // Output range - uint8_t offset_out = out_min; - return (offset_in * out_range)/in_range + offset_out; // slope = out_range/in_range. y=mx+b so output=slope*offset_in+offset_out - } + */ +static uint8_t map(uint8_t input, uint8_t in_min, uint8_t in_max, + uint8_t out_min, uint8_t out_max) { + if (in_min >= in_max) + in_max = in_min; // The minimum of the input range should never be greater + // than the maximum of the input range + + if (input <= in_min) { + // Lower bound the input to the minimum possible output + return out_min; + } else if (input >= in_max) { + // Upper bound the input to the maximum output + return out_max; + } else { + // Linear mapping between ranges + uint8_t offset_in = + input - in_min; // If input went from A -> B, it now goes from 0 -> B-A + uint8_t in_range = in_max - in_min; // Input range + uint8_t out_range = out_max - out_min; // Output range + uint8_t offset_out = out_min; + return (offset_in * out_range) / in_range + + offset_out; // slope = out_range/in_range. y=mx+b so + // output=slope*offset_in+offset_out + } } // State Handlers & Deciders /** - * @brief Forward Drive State Handler. Accelerator is mapped directly + * @brief Forward Drive State Handler. Accelerator is mapped directly * to current setpoint at positive velocity. -*/ -static void ForwardDriveHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_DISABLED); - UpdateDisplay_SetRegenState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_FORWARD); - } - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX)); + */ +static void ForwardDriveHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_DISABLED); + UpdateDisplay_SetRegenState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_FORWARD); + } + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = + percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, + CURRENT_SP_MIN, CURRENT_SP_MAX)); } /** * @brief Forward Drive State Decider. Determines transitions out of * forward drive state (brake, record velocity, one pedal, neutral drive). -*/ -static void ForwardDriveDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(cruiseSet && cruiseEnable && velocityObserved >= MIN_CRUISE_VELOCITY){ - state = FSM[RECORD_VELOCITY]; - }else if(onePedalEnable){ - state = FSM[ONEPEDAL]; - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - } + */ +static void ForwardDriveDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (cruiseSet && cruiseEnable && + velocityObserved >= MIN_CRUISE_VELOCITY) { + state = FSM[RECORD_VELOCITY]; + } else if (onePedalEnable) { + state = FSM[ONEPEDAL]; + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } } /** * @brief Neutral Drive State Handler. No current is sent to the motor. -*/ -static void NeutralDriveHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_DISABLED); - UpdateDisplay_SetRegenState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_NEUTRAL); - } - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = 0.0f; - - cruiseEnable = false; - onePedalEnable = false; + */ +static void NeutralDriveHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_DISABLED); + UpdateDisplay_SetRegenState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_NEUTRAL); + } + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = 0.0f; + + cruiseEnable = false; + onePedalEnable = false; } /** * @brief Neutral Drive State Decider. Determines transitions out of * neutral drive state (brake, forward drive, reverse drive). -*/ -static void NeutralDriveDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(gear == FORWARD_GEAR && velocityObserved >= -MAX_GEARSWITCH_VELOCITY){ - state = FSM[FORWARD_DRIVE]; - }else if(gear == REVERSE_GEAR && velocityObserved <= MAX_GEARSWITCH_VELOCITY){ - state = FSM[REVERSE_DRIVE]; - } + */ +static void NeutralDriveDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == FORWARD_GEAR && + velocityObserved >= -MAX_GEARSWITCH_VELOCITY) { + state = FSM[FORWARD_DRIVE]; + } else if (gear == REVERSE_GEAR && + velocityObserved <= MAX_GEARSWITCH_VELOCITY) { + state = FSM[REVERSE_DRIVE]; + } } /** - * @brief Reverse Drive State Handler. Accelerator is mapped directly to + * @brief Reverse Drive State Handler. Accelerator is mapped directly to * current setpoint (at negative velocity). -*/ -static void ReverseDriveHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_DISABLED); - UpdateDisplay_SetRegenState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_REVERSE); - } - velocitySetpoint = -MAX_VELOCITY; - currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX)); - cruiseEnable = false; - onePedalEnable = false; + */ +static void ReverseDriveHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_DISABLED); + UpdateDisplay_SetRegenState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_REVERSE); + } + velocitySetpoint = -MAX_VELOCITY; + currentSetpoint = + percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, + CURRENT_SP_MIN, CURRENT_SP_MAX)); + cruiseEnable = false; + onePedalEnable = false; } /** * @brief Reverse Drive State Decider. Determines transitions out of * reverse drive state (brake, neutral drive). -*/ -static void ReverseDriveDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - } - else if(gear == NEUTRAL_GEAR || gear == FORWARD_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - } + */ +static void ReverseDriveDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == NEUTRAL_GEAR || gear == FORWARD_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } } /** - * @brief Record Velocity State. While pressing the cruise set button, + * @brief Record Velocity State. While pressing the cruise set button, * the car will record the observed velocity into velocitySetpoint. -*/ -static void RecordVelocityHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_ACTIVE); - UpdateDisplay_SetRegenState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_FORWARD); - } - // put car in neutral while recording velocity (while button is held) - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = 0; - cruiseVelSetpoint = velocityObserved; + */ +static void RecordVelocityHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_ACTIVE); + UpdateDisplay_SetRegenState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_FORWARD); + } + // put car in neutral while recording velocity (while button is held) + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = 0; + cruiseVelSetpoint = velocityObserved; } /** - * @brief Record Velocity State Decider. Determines transitions out of record velocity - * state (brake, neutral drive, one pedal, forward drive, powered cruise). -*/ -static void RecordVelocityDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - }else if(onePedalEnable){ - cruiseEnable = false; - state = FSM[ONEPEDAL]; - }else if(!cruiseEnable){ - state = FSM[FORWARD_DRIVE]; - }else if(cruiseEnable && !cruiseSet){ - state = FSM[POWERED_CRUISE]; - } + * @brief Record Velocity State Decider. Determines transitions out of record + * velocity state (brake, neutral drive, one pedal, forward drive, powered + * cruise). + */ +static void RecordVelocityDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } else if (onePedalEnable) { + cruiseEnable = false; + state = FSM[ONEPEDAL]; + } else if (!cruiseEnable) { + state = FSM[FORWARD_DRIVE]; + } else if (cruiseEnable && !cruiseSet) { + state = FSM[POWERED_CRUISE]; + } } /** - * @brief Powered Cruise State. Continue to travel at the recorded velocity as long as - * Observed Velocity <= Velocity Setpoint -*/ -static void PoweredCruiseHandler(){ - velocitySetpoint = cruiseVelSetpoint; - currentSetpoint = 1.0f; + * @brief Powered Cruise State. Continue to travel at the recorded velocity as + * long as Observed Velocity <= Velocity Setpoint + */ +static void PoweredCruiseHandler() { + velocitySetpoint = cruiseVelSetpoint; + currentSetpoint = 1.0f; } /** - * @brief Powered Cruise State Decider. Determines transitions out of powered - * cruise state (brake, neutral drive, one pedal, forward drive, record velocity, - * accelerate cruise, coasting cruise). -*/ -static void PoweredCruiseDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - }else if(onePedalEnable){ - cruiseEnable = false; - state = FSM[ONEPEDAL]; - }else if(!cruiseEnable){ - state = FSM[FORWARD_DRIVE]; - }else if(cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY){ - state = FSM[RECORD_VELOCITY]; - }else if(accelPedalPercent >= ACCEL_PEDAL_THRESHOLD){ - state = FSM[ACCELERATE_CRUISE]; - }else if(velocityObserved > cruiseVelSetpoint){ - state = FSM[COASTING_CRUISE]; - } + * @brief Powered Cruise State Decider. Determines transitions out of powered + * cruise state (brake, neutral drive, one pedal, forward drive, record + * velocity, accelerate cruise, coasting cruise). + */ +static void PoweredCruiseDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } else if (onePedalEnable) { + cruiseEnable = false; + state = FSM[ONEPEDAL]; + } else if (!cruiseEnable) { + state = FSM[FORWARD_DRIVE]; + } else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY) { + state = FSM[RECORD_VELOCITY]; + } else if (accelPedalPercent >= ACCEL_PEDAL_THRESHOLD) { + state = FSM[ACCELERATE_CRUISE]; + } else if (velocityObserved > cruiseVelSetpoint) { + state = FSM[COASTING_CRUISE]; + } } /** - * @brief Coasting Cruise State. We do not want to utilize motor braking - * in cruise control mode due to safety issues. Coast the motor (go into neutral) - * if we want to slow down. -*/ -static void CoastingCruiseHandler(){ - velocitySetpoint = cruiseVelSetpoint; - currentSetpoint = 0; + * @brief Coasting Cruise State. We do not want to utilize motor braking + * in cruise control mode due to safety issues. Coast the motor (go into + * neutral) if we want to slow down. + */ +static void CoastingCruiseHandler() { + velocitySetpoint = cruiseVelSetpoint; + currentSetpoint = 0; } /** - * @brief Coasting Cruise State Decider. Determines transitions out of coasting - * cruise state (brake, neutral drive, one pedal, forward drive, record velocity, - * accelerate cruise, powered cruise). -*/ -static void CoastingCruiseDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - }else if(onePedalEnable){ - cruiseEnable = false; - state = FSM[ONEPEDAL]; - }else if(!cruiseEnable){ - state = FSM[FORWARD_DRIVE]; - }else if(cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY){ - state = FSM[RECORD_VELOCITY]; - }else if(accelPedalPercent >= ACCEL_PEDAL_THRESHOLD){ - state = FSM[ACCELERATE_CRUISE]; - }else if(velocityObserved <= cruiseVelSetpoint){ - state = FSM[POWERED_CRUISE]; - } + * @brief Coasting Cruise State Decider. Determines transitions out of coasting + * cruise state (brake, neutral drive, one pedal, forward drive, record + * velocity, accelerate cruise, powered cruise). + */ +static void CoastingCruiseDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } else if (onePedalEnable) { + cruiseEnable = false; + state = FSM[ONEPEDAL]; + } else if (!cruiseEnable) { + state = FSM[FORWARD_DRIVE]; + } else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY) { + state = FSM[RECORD_VELOCITY]; + } else if (accelPedalPercent >= ACCEL_PEDAL_THRESHOLD) { + state = FSM[ACCELERATE_CRUISE]; + } else if (velocityObserved <= cruiseVelSetpoint) { + state = FSM[POWERED_CRUISE]; + } } /** - * @brief Accelerate Cruise State. In the event that the driver needs to accelerate in cruise - * mode, we will accelerate to the pedal percentage. Upon release of the accelerator - * pedal, we will return to cruise mode at the previously recorded velocity. -*/ -static void AccelerateCruiseHandler(){ - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX)); + * @brief Accelerate Cruise State. In the event that the driver needs to + * accelerate in cruise mode, we will accelerate to the pedal percentage. Upon + * release of the accelerator pedal, we will return to cruise mode at the + * previously recorded velocity. + */ +static void AccelerateCruiseHandler() { + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = + percentToFloat(map(accelPedalPercent, ACCEL_PEDAL_THRESHOLD, PEDAL_MAX, + CURRENT_SP_MIN, CURRENT_SP_MAX)); } /** - * @brief Accelerate Cruise State Decider. Determines transitions out of accelerate - * cruise state (brake, neutral drive, one pedal, forward drive, record velocity, - * coasting cruise). -*/ -static void AccelerateCruiseDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - }else if(onePedalEnable){ - cruiseEnable = false; - state = FSM[ONEPEDAL]; - }else if(!cruiseEnable){ - state = FSM[FORWARD_DRIVE]; - }else if(cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY){ - state = FSM[RECORD_VELOCITY]; - }else if(accelPedalPercent < ACCEL_PEDAL_THRESHOLD){ - state = FSM[COASTING_CRUISE]; - } + * @brief Accelerate Cruise State Decider. Determines transitions out of + * accelerate cruise state (brake, neutral drive, one pedal, forward drive, + * record velocity, coasting cruise). + */ +static void AccelerateCruiseDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + } else if (onePedalEnable) { + cruiseEnable = false; + state = FSM[ONEPEDAL]; + } else if (!cruiseEnable) { + state = FSM[FORWARD_DRIVE]; + } else if (cruiseSet && velocityObserved >= MIN_CRUISE_VELOCITY) { + state = FSM[RECORD_VELOCITY]; + } else if (accelPedalPercent < ACCEL_PEDAL_THRESHOLD) { + state = FSM[COASTING_CRUISE]; + } } /** - * @brief One Pedal Drive State. When in one pedal drive, if the accelerator percentage is lower - * than ONEPEDAL_BRAKE_THRESHOLD, the car will utilize motor braking to slow down. If accelerator - * percentage is in the neutral zone, the car will coast. If accelerator percentage is above - * the NEUTRAL_THRESHOLD, the car will accelerate as normal. -*/ -static void OnePedalDriveHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_FORWARD); - } - if(accelPedalPercent <= ONEPEDAL_BRAKE_THRESHOLD){ - // Regen brake: Map 0 -> brake to 100 -> 0 - velocitySetpoint = 0; - currentSetpoint = percentToFloat(map(accelPedalPercent, PEDAL_MIN, ONEPEDAL_BRAKE_THRESHOLD, CURRENT_SP_MAX, CURRENT_SP_MIN)); - Minions_Write(BRAKELIGHT, true); - UpdateDisplay_SetRegenState(DISP_ACTIVE); - }else if(ONEPEDAL_BRAKE_THRESHOLD < accelPedalPercent && accelPedalPercent <= ONEPEDAL_NEUTRAL_THRESHOLD){ - // Neutral: coast - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = 0; - Minions_Write(BRAKELIGHT, false); - UpdateDisplay_SetRegenState(DISP_ENABLED); - }else if(ONEPEDAL_NEUTRAL_THRESHOLD < accelPedalPercent){ - // Accelerate: Map neutral -> 100 to 0 -> 100 - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = percentToFloat(map(accelPedalPercent, ONEPEDAL_NEUTRAL_THRESHOLD, PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX)); - Minions_Write(BRAKELIGHT, false); - UpdateDisplay_SetRegenState(DISP_ENABLED); - } + * @brief One Pedal Drive State. When in one pedal drive, if the accelerator + * percentage is lower than ONEPEDAL_BRAKE_THRESHOLD, the car will utilize motor + * braking to slow down. If accelerator percentage is in the neutral zone, the + * car will coast. If accelerator percentage is above the NEUTRAL_THRESHOLD, the + * car will accelerate as normal. + */ +static void OnePedalDriveHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_FORWARD); + } + if (accelPedalPercent <= ONEPEDAL_BRAKE_THRESHOLD) { + // Regen brake: Map 0 -> brake to 100 -> 0 + velocitySetpoint = 0; + currentSetpoint = percentToFloat(map(accelPedalPercent, PEDAL_MIN, + ONEPEDAL_BRAKE_THRESHOLD, + CURRENT_SP_MAX, CURRENT_SP_MIN)); + Minions_Write(BRAKELIGHT, true); + UpdateDisplay_SetRegenState(DISP_ACTIVE); + } else if (ONEPEDAL_BRAKE_THRESHOLD < accelPedalPercent && + accelPedalPercent <= ONEPEDAL_NEUTRAL_THRESHOLD) { + // Neutral: coast + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = 0; + Minions_Write(BRAKELIGHT, false); + UpdateDisplay_SetRegenState(DISP_ENABLED); + } else if (ONEPEDAL_NEUTRAL_THRESHOLD < accelPedalPercent) { + // Accelerate: Map neutral -> 100 to 0 -> 100 + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = + percentToFloat(map(accelPedalPercent, ONEPEDAL_NEUTRAL_THRESHOLD, + PEDAL_MAX, CURRENT_SP_MIN, CURRENT_SP_MAX)); + Minions_Write(BRAKELIGHT, false); + UpdateDisplay_SetRegenState(DISP_ENABLED); + } } /** - * @brief One Pedal Drive State Decider. Determines transitions out of one pedal + * @brief One Pedal Drive State Decider. Determines transitions out of one pedal * drive state (brake, record velocity, neutral drive). -*/ -static void OnePedalDriveDecider(){ - if(brakePedalPercent >= BRAKE_PEDAL_THRESHOLD){ - state = FSM[BRAKE_STATE]; - }else if(cruiseSet && cruiseEnable && velocityObserved >= MIN_CRUISE_VELOCITY){ - state = FSM[RECORD_VELOCITY]; - Minions_Write(BRAKELIGHT, false); - }else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR){ - state = FSM[NEUTRAL_DRIVE]; - Minions_Write(BRAKELIGHT, false); - } + */ +static void OnePedalDriveDecider() { + if (brakePedalPercent >= BRAKE_PEDAL_THRESHOLD) { + state = FSM[BRAKE_STATE]; + } else if (cruiseSet && cruiseEnable && + velocityObserved >= MIN_CRUISE_VELOCITY) { + state = FSM[RECORD_VELOCITY]; + Minions_Write(BRAKELIGHT, false); + } else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) { + state = FSM[NEUTRAL_DRIVE]; + Minions_Write(BRAKELIGHT, false); + } } /** - * @brief Brake State. When brake pedal is pressed, physical brakes will be active. - * Put motor in neutral to prevent motor braking while physical brakes are engaged. - * Additionally, disable all cruise control and one pedal functionality. -*/ -static void BrakeHandler(){ - if(prevState.name != state.name){ - UpdateDisplay_SetCruiseState(DISP_DISABLED); - UpdateDisplay_SetRegenState(DISP_DISABLED); - UpdateDisplay_SetGear(DISP_FORWARD); - } - velocitySetpoint = MAX_VELOCITY; - currentSetpoint = 0; - cruiseEnable = false; - onePedalEnable = false; - Minions_Write(BRAKELIGHT, true); + * @brief Brake State. When brake pedal is pressed, physical brakes will be + * active. Put motor in neutral to prevent motor braking while physical brakes + * are engaged. Additionally, disable all cruise control and one pedal + * functionality. + */ +static void BrakeHandler() { + if (prevState.name != state.name) { + UpdateDisplay_SetCruiseState(DISP_DISABLED); + UpdateDisplay_SetRegenState(DISP_DISABLED); + UpdateDisplay_SetGear(DISP_FORWARD); + } + velocitySetpoint = MAX_VELOCITY; + currentSetpoint = 0; + cruiseEnable = false; + onePedalEnable = false; + Minions_Write(BRAKELIGHT, true); } /** - * @brief Brake State Decider. Determines transitions out of brake state (forward drive, - * neutral drive). -*/ -static void BrakeDecider(){ - if(brakePedalPercent < BRAKE_PEDAL_THRESHOLD){ - if(gear == FORWARD_GEAR) state = FSM[FORWARD_DRIVE]; - else if(gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) state = FSM[NEUTRAL_DRIVE]; - Minions_Write(BRAKELIGHT, false); - } + * @brief Brake State Decider. Determines transitions out of brake state + * (forward drive, neutral drive). + */ +static void BrakeDecider() { + if (brakePedalPercent < BRAKE_PEDAL_THRESHOLD) { + if (gear == FORWARD_GEAR) + state = FSM[FORWARD_DRIVE]; + else if (gear == NEUTRAL_GEAR || gear == REVERSE_GEAR) + state = FSM[NEUTRAL_DRIVE]; + Minions_Write(BRAKELIGHT, false); + } } // Task (main loop) /** * @brief Follows the FSM to update the velocity of the car -*/ -void Task_SendTritium(void *p_arg){ - OS_ERR err; + */ +void Task_SendTritium(void* p_arg) { + OS_ERR err; - // Initialize current state to FORWARD_DRIVE - state = FSM[NEUTRAL_DRIVE]; - prevState = FSM[NEUTRAL_DRIVE]; - - #ifndef SENDTRITIUM_PRINT_MES - CANDATA_t driveCmd = { - .ID=MOTOR_DRIVE, - .idx=0, - .data={0.0f, 0.0f}, - }; - #endif - - while(1){ - prevState = state; - - state.stateHandler(); // do what the current state does - #ifndef SENDTRITIUM_EXPOSE_VARS - readInputs(); // read inputs from the system - UpdateDisplay_SetAccel(accelPedalPercent); - #endif - state.stateDecider(); // decide what the next state is - - // Drive - #ifdef SENDTRITIUM_PRINT_MES - dumpInfo(); - #else - if(MOTOR_MSG_COUNTER_THRESHOLD == motorMsgCounter){ - memcpy(&driveCmd.data[4], ¤tSetpoint, sizeof(float)); - memcpy(&driveCmd.data[0], &velocitySetpoint, sizeof(float)); - CANbus_Send(driveCmd, CAN_NON_BLOCKING, MOTORCAN); - motorMsgCounter = 0; - }else{ - motorMsgCounter++; - } - #endif - - // Delay of MOTOR_MSG_PERIOD ms - OSTimeDlyHMSM(0, 0, 0, MOTOR_MSG_PERIOD, OS_OPT_TIME_HMSM_STRICT, &err); - if (err != OS_ERR_NONE){ - assertOSError(OS_UPDATE_VEL_LOC, err); - } + // Initialize current state to FORWARD_DRIVE + state = FSM[NEUTRAL_DRIVE]; + prevState = FSM[NEUTRAL_DRIVE]; + +#ifndef SENDTRITIUM_PRINT_MES + CANDATA_t driveCmd = { + .ID = MOTOR_DRIVE, + .idx = 0, + .data = {0.0f, 0.0f}, + }; +#endif + + while (1) { + prevState = state; + + state.stateHandler(); // do what the current state does +#ifndef SENDTRITIUM_EXPOSE_VARS + readInputs(); // read inputs from the system + UpdateDisplay_SetAccel(accelPedalPercent); +#endif + state.stateDecider(); // decide what the next state is + +// Drive +#ifdef SENDTRITIUM_PRINT_MES + dumpInfo(); +#else + if (MOTOR_MSG_COUNTER_THRESHOLD == motorMsgCounter) { + memcpy(&driveCmd.data[4], ¤tSetpoint, sizeof(float)); + memcpy(&driveCmd.data[0], &velocitySetpoint, sizeof(float)); + CANbus_Send(driveCmd, CAN_NON_BLOCKING, MOTORCAN); + motorMsgCounter = 0; + } else { + motorMsgCounter++; + } +#endif + + // Delay of MOTOR_MSG_PERIOD ms + OSTimeDlyHMSM(0, 0, 0, MOTOR_MSG_PERIOD, OS_OPT_TIME_HMSM_STRICT, &err); + if (err != OS_ERR_NONE) { + assertOSError(OS_UPDATE_VEL_LOC, err); } + } } diff --git a/Apps/Src/Tasks.c b/Apps/Src/Tasks.c index 8b6923e30..91299d535 100644 --- a/Apps/Src/Tasks.c +++ b/Apps/Src/Tasks.c @@ -1,18 +1,19 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Tasks.c - * @brief - * + * @brief + * */ #include "Tasks.h" -#include "FaultState.h" -#include "os.h" + #include "CANbus.h" #include "Contactors.h" #include "Display.h" +#include "FaultState.h" #include "Minions.h" #include "Pedals.h" +#include "os.h" /** * TCBs @@ -55,42 +56,41 @@ OS_SEM FaultState_Sem4; fault_bitmap_t FaultBitmap = FAULT_NONE; os_error_loc_t OSErrLocBitmap = OS_NONE_LOC; -void _assertOSError(uint16_t OS_err_loc, OS_ERR err) -{ - if (err != OS_ERR_NONE) - { - FaultBitmap |= FAULT_OS; - OSErrLocBitmap |= OS_err_loc; +void _assertOSError(uint16_t OS_err_loc, OS_ERR err) { + if (err != OS_ERR_NONE) { + FaultBitmap |= FAULT_OS; + OSErrLocBitmap |= OS_err_loc; - OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); - EnterFaultState(); - } + OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &err); + EnterFaultState(); + } } /** * @brief Hook that's called every context switch - * - * This function will append the task being switched out to the task trace if and only if: + * + * This function will append the task being switched out to the task trace if + * and only if: * 1. It's not a task created automatically by the RTOS - * 2. It's not the previously recorded task (a long running task interrupted by the - * tick task will only show up once) - * This function will overwrite tasks that have been in the trace for a while, keeping only - * the 8 most recent tasks + * 2. It's not the previously recorded task (a long running task + * interrupted by the tick task will only show up once) This function will + * overwrite tasks that have been in the trace for a while, keeping only the 8 + * most recent tasks */ void App_OS_TaskSwHook(void) { - OS_TCB *cur = OSTCBCurPtr; - uint32_t idx = PrevTasks.index; - if (cur == &OSTickTaskTCB) return; // Ignore the tick task - if (cur == &OSIdleTaskTCB) return; // Ignore the idle task - if (cur == &OSTmrTaskTCB ) return; // Ignore the timer task - if (cur == &OSStatTaskTCB) return; // Ignore the stat task - if (cur == PrevTasks.tasks[idx]) return; // Don't record the same task again - if (++idx == TASK_TRACE_LENGTH) idx = 0; - PrevTasks.tasks[idx] = cur; - PrevTasks.index = idx; + OS_TCB *cur = OSTCBCurPtr; + uint32_t idx = PrevTasks.index; + if (cur == &OSTickTaskTCB) return; // Ignore the tick task + if (cur == &OSIdleTaskTCB) return; // Ignore the idle task + if (cur == &OSTmrTaskTCB) return; // Ignore the timer task + if (cur == &OSStatTaskTCB) return; // Ignore the stat task + if (cur == PrevTasks.tasks[idx]) return; // Don't record the same task again + if (++idx == TASK_TRACE_LENGTH) idx = 0; + PrevTasks.tasks[idx] = cur; + PrevTasks.index = idx; } void TaskSwHook_Init(void) { - PrevTasks.index = TASK_TRACE_LENGTH - 1; // List starts out empty - OS_AppTaskSwHookPtr = App_OS_TaskSwHook; + PrevTasks.index = TASK_TRACE_LENGTH - 1; // List starts out empty + OS_AppTaskSwHookPtr = App_OS_TaskSwHook; } diff --git a/Apps/Src/Telemetry.c b/Apps/Src/Telemetry.c index c3bd72572..53d4dc461 100644 --- a/Apps/Src/Telemetry.c +++ b/Apps/Src/Telemetry.c @@ -2,64 +2,65 @@ * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Telemetry.c * @brief Implements the SendCarCAN Task - * + * * Creates a datatype to house the data to be read by telemetry - * - * Gathers the information of the pedals, lights, switches, and contactors + * + * Gathers the information of the pedals, lights, switches, and contactors * to be read by telemetry - * + * */ -#include "Tasks.h" -#include "CANbus.h" #include "CAN_Queue.h" -#include "Pedals.h" -#include "Minions.h" +#include "CANbus.h" #include "Contactors.h" +#include "Minions.h" +#include "Pedals.h" +#include "Tasks.h" #include "common.h" -// Make sure updated to the CarData_t and carMSGID are reflected in the CAN Bus IDs excel sheet +// Make sure updated to the CarData_t and carMSGID are reflected in the CAN Bus +// IDs excel sheet /** - * @brief Sends pedal, switch, light, and contactor information to be read by telemetry - * - * @param p_arg + * @brief Sends pedal, switch, light, and contactor information to be read by + * telemetry + * + * @param p_arg */ -void Task_Telemetry(void *p_arg){ - CANDATA_t carMsg; - carMsg.ID = CARDATA; // ID is wrong - for(int i = 4; i < 8; i++){ - carMsg.data[i] = 0; - } - OS_ERR err; +void Task_Telemetry(void *p_arg) { + CANDATA_t carMsg; + carMsg.ID = CARDATA; // ID is wrong + for (int i = 4; i < 8; i++) { + carMsg.data[i] = 0; + } + OS_ERR err; + while (1) { + // Get pedal information + carMsg.data[0] = Pedals_Read(ACCELERATOR); + carMsg.data[1] = Pedals_Read(BRAKE); - while (1) { - // Get pedal information - carMsg.data[0] = Pedals_Read(ACCELERATOR); - carMsg.data[1] = Pedals_Read(BRAKE); - - // Get minion information - carMsg.data[2] = 0; - for(pin_t pin = 0; pin < NUM_PINS; pin++){ - bool pinState = Minions_Read(pin); - carMsg.data[2] |= pinState << pin; - } + // Get minion information + carMsg.data[2] = 0; + for (pin_t pin = 0; pin < NUM_PINS; pin++) { + bool pinState = Minions_Read(pin); + carMsg.data[2] |= pinState << pin; + } - // Get contactor info - carMsg.data[3] = 0; - for(contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++){ - bool contactorState = Contactors_Get(contactor) == ON ? true : false; - carMsg.data[3] |= contactorState << contactor; - } + // Get contactor info + carMsg.data[3] = 0; + for (contactor_t contactor = 0; contactor < NUM_CONTACTORS; contactor++) { + bool contactorState = Contactors_Get(contactor) == ON ? true : false; + carMsg.data[3] |= contactorState << contactor; + } - // Send car msg - CANbus_Send(carMsg, true, CARCAN); + // Send car msg + CANbus_Send(carMsg, true, CARCAN); - // Delay of few milliseconds (500) - OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); - if (err != OS_ERR_NONE){ - assertOSError(OS_UPDATE_VEL_LOC, err); - } + // Delay of few milliseconds (500) + OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); + if (err != OS_ERR_NONE) { + assertOSError(OS_UPDATE_VEL_LOC, err); } + } } diff --git a/Apps/Src/UpdateDisplay.c b/Apps/Src/UpdateDisplay.c index 1356491e2..006e2b351 100644 --- a/Apps/Src/UpdateDisplay.c +++ b/Apps/Src/UpdateDisplay.c @@ -2,17 +2,19 @@ * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file UpdateDisplay.c * @brief Function implementations for the display application. - * + * * This contains functions relevant to modifying states of specific - * components on our HMI design. The HMI has the ability to indicate + * components on our HMI design. The HMI has the ability to indicate * relevant information about system status to the driver. - * + * */ #include "UpdateDisplay.h" -#include "Minions.h" + #include +#include "Minions.h" + /** * Creates queue for display commands. */ @@ -25,71 +27,65 @@ disp_fifo_t msg_queue; -static OS_SEM DisplayQ_Sem4; // counting semaphore for queue message availability -static OS_MUTEX DisplayQ_Mutex; // mutex to ensure thread safety when writing/reading to queue +static OS_SEM + DisplayQ_Sem4; // counting semaphore for queue message availability +static OS_MUTEX DisplayQ_Mutex; // mutex to ensure thread safety when + // writing/reading to queue /** * Enum and corresponding array for easy component selection. */ -typedef enum{ - // Boolean components - ARRAY=0, - MOTOR, - // Non-boolean components - VELOCITY, - ACCEL_METER, - SOC, - SUPP_BATT, - CRUISE_ST, - REGEN_ST, - GEAR, - // Fault code components - OS_CODE, - FAULT_CODE +typedef enum { + // Boolean components + ARRAY = 0, + MOTOR, + // Non-boolean components + VELOCITY, + ACCEL_METER, + SOC, + SUPP_BATT, + CRUISE_ST, + REGEN_ST, + GEAR, + // Fault code components + OS_CODE, + FAULT_CODE } Component_t; -const char* compStrings[15]= { - // Boolean components - "arr", - "mot", - // Non-boolean components - "vel", - "accel", - "soc", - "supp", - "cruiseSt", - "rbsSt", - "gear", - // Fault code components - "oserr", - "faulterr" -}; +const char* compStrings[15] = { + // Boolean components + "arr", "mot", + // Non-boolean components + "vel", "accel", "soc", "supp", "cruiseSt", "rbsSt", "gear", + // Fault code components + "oserr", "faulterr"}; /** - * @brief Error handler for any UpdateDisplay errors. Call this after any display application function. + * @brief Error handler for any UpdateDisplay errors. Call this after any + * display application function. */ -static void assertUpdateDisplayError(UpdateDisplayError_t err){ - OS_ERR os_err; +static void assertUpdateDisplayError(UpdateDisplayError_t err) { + OS_ERR os_err; - if(err != UPDATEDISPLAY_ERR_NONE || err != UPDATEDISPLAY_ERR_NO_CHANGE){ - FaultBitmap |= FAULT_DISPLAY; + if (err != UPDATEDISPLAY_ERR_NONE || err != UPDATEDISPLAY_ERR_NO_CHANGE) { + FaultBitmap |= FAULT_DISPLAY; - OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &os_err); - assertOSError(OS_DISPLAY_LOC, os_err); - } + OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &os_err); + assertOSError(OS_DISPLAY_LOC, os_err); + } } -UpdateDisplayError_t UpdateDisplay_Init(){ - OS_ERR err; - disp_fifo_renew(&msg_queue); - OSMutexCreate(&DisplayQ_Mutex, "Display mutex", &err); - assertOSError(OS_DISPLAY_LOC, err); - OSSemCreate(&DisplayQ_Sem4, "Display sem4", 0, &err); - assertOSError(OS_DISPLAY_LOC, err); - - UpdateDisplayError_t ret = UpdateDisplay_SetPage(INFO); - assertUpdateDisplayError(ret); - return ret; +UpdateDisplayError_t UpdateDisplay_Init() { + OS_ERR err; + disp_fifo_renew(&msg_queue); + OSMutexCreate(&DisplayQ_Mutex, "Display mutex", &err); + assertOSError(OS_DISPLAY_LOC, err); + OSSemCreate(&DisplayQ_Sem4, "Display sem4", 0, &err); + assertOSError(OS_DISPLAY_LOC, err); + + UpdateDisplayError_t ret = UpdateDisplay_SetPage(INFO); + assertUpdateDisplayError(ret); + return ret; } /** @@ -99,29 +95,29 @@ UpdateDisplayError_t UpdateDisplay_Init(){ * 2) queue is not currently being written to by a separate thread (mutex) * @returns UpdateDisplayError_t */ -static UpdateDisplayError_t UpdateDisplay_PopNext(){ - DisplayCmd_t cmd; +static UpdateDisplayError_t UpdateDisplay_PopNext() { + DisplayCmd_t cmd; - OS_ERR err; - CPU_TS ticks; + OS_ERR err; + CPU_TS ticks; - OSSemPend(&DisplayQ_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); - assertOSError(OS_DISPLAY_LOC, err); - - OSMutexPend(&DisplayQ_Mutex, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); - assertOSError(OS_DISPLAY_LOC, err); + OSSemPend(&DisplayQ_Sem4, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); + assertOSError(OS_DISPLAY_LOC, err); + + OSMutexPend(&DisplayQ_Mutex, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); + assertOSError(OS_DISPLAY_LOC, err); + + bool result = disp_fifo_get(&msg_queue, &cmd); + OSMutexPost(&DisplayQ_Mutex, OS_OPT_POST_ALL, &err); + assertOSError(OS_SEND_CAN_LOC, err); - bool result = disp_fifo_get(&msg_queue, &cmd); - OSMutexPost(&DisplayQ_Mutex, OS_OPT_POST_ALL, &err); - assertOSError(OS_SEND_CAN_LOC, err); - - if(!result){ - assertUpdateDisplayError(UPDATEDISPLAY_ERR_FIFO_POP); - return UPDATEDISPLAY_ERR_FIFO_POP; - } - - assertDisplayError(Display_Send(cmd)); - return UPDATEDISPLAY_ERR_NONE; + if (!result) { + assertUpdateDisplayError(UPDATEDISPLAY_ERR_FIFO_POP); + return UPDATEDISPLAY_ERR_FIFO_POP; + } + + assertDisplayError(Display_Send(cmd)); + return UPDATEDISPLAY_ERR_NONE; } /** @@ -129,28 +125,27 @@ static UpdateDisplayError_t UpdateDisplay_PopNext(){ * threadsafe memory access and signals semaphore upon successful fifo_put. * @returns UpdateDisplayError_t */ -static UpdateDisplayError_t UpdateDisplay_PutNext(DisplayCmd_t cmd){ - CPU_TS ticks; - OS_ERR err; - - OSMutexPend(&DisplayQ_Mutex, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); - assertOSError(OS_DISPLAY_LOC, err); - - bool success = disp_fifo_put(&msg_queue, cmd); - - OSMutexPost(&DisplayQ_Mutex, OS_OPT_POST_ALL, &err); - assertOSError(OS_DISPLAY_LOC, err); - - if(success){ - OSSemPost(&DisplayQ_Sem4, OS_OPT_POST_ALL, &err); - assertOSError(OS_DISPLAY_LOC, err); - } - else{ - assertUpdateDisplayError(UPDATEDISPLAY_ERR_FIFO_PUT); - return UPDATEDISPLAY_ERR_FIFO_PUT; - } - - return UPDATEDISPLAY_ERR_NONE; +static UpdateDisplayError_t UpdateDisplay_PutNext(DisplayCmd_t cmd) { + CPU_TS ticks; + OS_ERR err; + + OSMutexPend(&DisplayQ_Mutex, 0, OS_OPT_PEND_BLOCKING, &ticks, &err); + assertOSError(OS_DISPLAY_LOC, err); + + bool success = disp_fifo_put(&msg_queue, cmd); + + OSMutexPost(&DisplayQ_Mutex, OS_OPT_POST_ALL, &err); + assertOSError(OS_DISPLAY_LOC, err); + + if (success) { + OSSemPost(&DisplayQ_Sem4, OS_OPT_POST_ALL, &err); + assertOSError(OS_DISPLAY_LOC, err); + } else { + assertUpdateDisplayError(UPDATEDISPLAY_ERR_FIFO_PUT); + return UPDATEDISPLAY_ERR_FIFO_PUT; + } + + return UPDATEDISPLAY_ERR_NONE; } /** @@ -159,22 +154,17 @@ static UpdateDisplayError_t UpdateDisplay_PutNext(DisplayCmd_t cmd){ * blinkers, gear selector, cruise control and regen braking indicator. * @returns UpdateDisplayError_t */ -static UpdateDisplayError_t UpdateDisplay_Refresh(){ - DisplayCmd_t refreshCmd = { - .compOrCmd = "click", - .attr = NULL, - .op = NULL, - .numArgs = 2, - .argTypes = {INT_ARG,INT_ARG}, - { - {.num=0}, - {.num=1} - } - }; - - UpdateDisplayError_t ret = UpdateDisplay_PutNext(refreshCmd); - assertUpdateDisplayError(ret); - return ret; +static UpdateDisplayError_t UpdateDisplay_Refresh() { + DisplayCmd_t refreshCmd = {.compOrCmd = "click", + .attr = NULL, + .op = NULL, + .numArgs = 2, + .argTypes = {INT_ARG, INT_ARG}, + {{.num = 0}, {.num = 1}}}; + + UpdateDisplayError_t ret = UpdateDisplay_PutNext(refreshCmd); + assertUpdateDisplayError(ret); + return ret; } /** @@ -184,210 +174,200 @@ static UpdateDisplayError_t UpdateDisplay_Refresh(){ * @param val value * @return UpdateDisplayError_t */ -static UpdateDisplayError_t UpdateDisplay_SetComponent(Component_t comp, uint32_t val){ - UpdateDisplayError_t ret = UPDATEDISPLAY_ERR_NONE; - - // For components that are on/off - if(comp <= MOTOR && val <= 1){ - DisplayCmd_t visCmd = { - .compOrCmd = "vis", - .attr = NULL, - .op = NULL, - .numArgs = 2, - .argTypes = {STR_ARG,INT_ARG}, - { - {.str=(char*)compStrings[comp]}, - {.num=val} - } - }; - - ret = UpdateDisplay_PutNext(visCmd); - assertUpdateDisplayError(ret); - return ret; - } - // For components that have a non-boolean value - else if(comp > MOTOR){ - DisplayCmd_t setCmd = { - .compOrCmd = (char*)compStrings[comp], - .attr = "val", - .op = "=", - .numArgs = 1, - .argTypes = {INT_ARG}, - { - {.num=val} - } - }; - - ret = UpdateDisplay_PutNext(setCmd); - assertUpdateDisplayError(ret); - return ret; - } - else{ - assertUpdateDisplayError(UPDATEDISPLAY_ERR_PARSE_COMP); - return UPDATEDISPLAY_ERR_PARSE_COMP; - } - return UPDATEDISPLAY_ERR_NONE; +static UpdateDisplayError_t UpdateDisplay_SetComponent(Component_t comp, + uint32_t val) { + UpdateDisplayError_t ret = UPDATEDISPLAY_ERR_NONE; + + // For components that are on/off + if (comp <= MOTOR && val <= 1) { + DisplayCmd_t visCmd = {.compOrCmd = "vis", + .attr = NULL, + .op = NULL, + .numArgs = 2, + .argTypes = {STR_ARG, INT_ARG}, + {{.str = (char*)compStrings[comp]}, {.num = val}}}; + + ret = UpdateDisplay_PutNext(visCmd); + assertUpdateDisplayError(ret); + return ret; + } + // For components that have a non-boolean value + else if (comp > MOTOR) { + DisplayCmd_t setCmd = {.compOrCmd = (char*)compStrings[comp], + .attr = "val", + .op = "=", + .numArgs = 1, + .argTypes = {INT_ARG}, + {{.num = val}}}; + + ret = UpdateDisplay_PutNext(setCmd); + assertUpdateDisplayError(ret); + return ret; + } else { + assertUpdateDisplayError(UPDATEDISPLAY_ERR_PARSE_COMP); + return UPDATEDISPLAY_ERR_PARSE_COMP; + } + return UPDATEDISPLAY_ERR_NONE; } -UpdateDisplayError_t UpdateDisplay_SetPage(Page_t page){ - DisplayCmd_t pgCmd = { - .compOrCmd = "page", - .attr = NULL, - .op = NULL, - .numArgs = 1, - .argTypes = {INT_ARG}, - { - {.num=page} - } - }; - - UpdateDisplayError_t ret = UpdateDisplay_PutNext(pgCmd); - return ret; +UpdateDisplayError_t UpdateDisplay_SetPage(Page_t page) { + DisplayCmd_t pgCmd = {.compOrCmd = "page", + .attr = NULL, + .op = NULL, + .numArgs = 1, + .argTypes = {INT_ARG}, + {{.num = page}}}; + + UpdateDisplayError_t ret = UpdateDisplay_PutNext(pgCmd); + return ret; } /* WRAPPERS */ -UpdateDisplayError_t UpdateDisplay_SetSOC(uint8_t percent){ // Integer percentage from 0-100 - static uint8_t lastPercent = 0; - if(percent == lastPercent){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(SOC, percent); - assertUpdateDisplayError(ret); - if(ret != UPDATEDISPLAY_ERR_NONE) return ret; - - ret = UpdateDisplay_Refresh(); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastPercent = percent; - return ret; +UpdateDisplayError_t UpdateDisplay_SetSOC( + uint8_t percent) { // Integer percentage from 0-100 + static uint8_t lastPercent = 0; + if (percent == lastPercent) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(SOC, percent); + assertUpdateDisplayError(ret); + if (ret != UPDATEDISPLAY_ERR_NONE) return ret; + + ret = UpdateDisplay_Refresh(); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastPercent = percent; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetSBPV(uint32_t mv){ - static uint32_t lastMv = 0; - if(mv == lastMv){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } +UpdateDisplayError_t UpdateDisplay_SetSBPV(uint32_t mv) { + static uint32_t lastMv = 0; + if (mv == lastMv) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(SUPP_BATT, mv/100); - assertUpdateDisplayError(ret); - if(ret != UPDATEDISPLAY_ERR_NONE) return ret; + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(SUPP_BATT, mv / 100); + assertUpdateDisplayError(ret); + if (ret != UPDATEDISPLAY_ERR_NONE) return ret; - ret = UpdateDisplay_Refresh(); - assertUpdateDisplayError(ret); + ret = UpdateDisplay_Refresh(); + assertUpdateDisplayError(ret); - if(ret == UPDATEDISPLAY_ERR_NONE) lastMv = mv; - return ret; + if (ret == UPDATEDISPLAY_ERR_NONE) lastMv = mv; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetVelocity(uint32_t mphTenths){ - static uint32_t lastMphTenths = 0; - if(mphTenths == lastMphTenths){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(VELOCITY, mphTenths); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastMphTenths = mphTenths; - return ret; +UpdateDisplayError_t UpdateDisplay_SetVelocity(uint32_t mphTenths) { + static uint32_t lastMphTenths = 0; + if (mphTenths == lastMphTenths) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(VELOCITY, mphTenths); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastMphTenths = mphTenths; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetAccel(uint8_t percent){ - static uint8_t lastPercentAccel = 0; - if(percent == lastPercentAccel){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(ACCEL_METER, percent); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastPercentAccel = percent; - return ret; +UpdateDisplayError_t UpdateDisplay_SetAccel(uint8_t percent) { + static uint8_t lastPercentAccel = 0; + if (percent == lastPercentAccel) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(ACCEL_METER, percent); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastPercentAccel = percent; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetArray(bool state){ - static bool lastState = false; - if(state == lastState){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(ARRAY, (state)?1:0); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastState = state; - return ret; +UpdateDisplayError_t UpdateDisplay_SetArray(bool state) { + static bool lastState = false; + if (state == lastState) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(ARRAY, (state) ? 1 : 0); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastState = state; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetMotor(bool state){ - static bool lastState = false; - if(state == lastState){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(MOTOR, (state)?1:0); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastState = state; - return ret; +UpdateDisplayError_t UpdateDisplay_SetMotor(bool state) { + static bool lastState = false; + if (state == lastState) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(MOTOR, (state) ? 1 : 0); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastState = state; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetGear(TriState_t gear){ - static TriState_t lastGear = STATE_0; - if(gear == lastGear){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(GEAR, (uint32_t)gear); - assertUpdateDisplayError(ret); - if(ret != UPDATEDISPLAY_ERR_NONE) return ret; - - ret = UpdateDisplay_Refresh(); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastGear = gear; - return ret; +UpdateDisplayError_t UpdateDisplay_SetGear(TriState_t gear) { + static TriState_t lastGear = STATE_0; + if (gear == lastGear) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = UpdateDisplay_SetComponent(GEAR, (uint32_t)gear); + assertUpdateDisplayError(ret); + if (ret != UPDATEDISPLAY_ERR_NONE) return ret; + + ret = UpdateDisplay_Refresh(); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastGear = gear; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetRegenState(TriState_t state){ - static TriState_t lastState = STATE_0; - if(state == lastState){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(REGEN_ST, (uint32_t)state); - assertUpdateDisplayError(ret); - if(ret != UPDATEDISPLAY_ERR_NONE) return ret; - - ret = UpdateDisplay_Refresh(); - assertUpdateDisplayError(ret); - - if(ret == UPDATEDISPLAY_ERR_NONE) lastState = state; - return ret; +UpdateDisplayError_t UpdateDisplay_SetRegenState(TriState_t state) { + static TriState_t lastState = STATE_0; + if (state == lastState) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = + UpdateDisplay_SetComponent(REGEN_ST, (uint32_t)state); + assertUpdateDisplayError(ret); + if (ret != UPDATEDISPLAY_ERR_NONE) return ret; + + ret = UpdateDisplay_Refresh(); + assertUpdateDisplayError(ret); + + if (ret == UPDATEDISPLAY_ERR_NONE) lastState = state; + return ret; } -UpdateDisplayError_t UpdateDisplay_SetCruiseState(TriState_t state){ - static TriState_t lastState = STATE_0; - if(state == lastState){ - return UPDATEDISPLAY_ERR_NO_CHANGE; - } - - UpdateDisplayError_t ret = UpdateDisplay_SetComponent(CRUISE_ST, (uint32_t)state); - if(ret != UPDATEDISPLAY_ERR_NONE) return ret; +UpdateDisplayError_t UpdateDisplay_SetCruiseState(TriState_t state) { + static TriState_t lastState = STATE_0; + if (state == lastState) { + return UPDATEDISPLAY_ERR_NO_CHANGE; + } + + UpdateDisplayError_t ret = + UpdateDisplay_SetComponent(CRUISE_ST, (uint32_t)state); + if (ret != UPDATEDISPLAY_ERR_NONE) return ret; - ret = UpdateDisplay_Refresh(); - assertUpdateDisplayError(ret); + ret = UpdateDisplay_Refresh(); + assertUpdateDisplayError(ret); - if(ret == UPDATEDISPLAY_ERR_NONE) lastState = state; - return ret; + if (ret == UPDATEDISPLAY_ERR_NONE) lastState = state; + return ret; } /** * @brief Loops through the display queue and sends all messages */ -void Task_UpdateDisplay(void *p_arg) { - while (1) { - UpdateDisplayError_t err = UpdateDisplay_PopNext(); - assertUpdateDisplayError(err); - } +void Task_UpdateDisplay(void* p_arg) { + while (1) { + UpdateDisplayError_t err = UpdateDisplay_PopNext(); + assertUpdateDisplayError(err); + } } diff --git a/Apps/Src/common.c b/Apps/Src/common.c index 02a769319..78ccde048 100644 --- a/Apps/Src/common.c +++ b/Apps/Src/common.c @@ -1,11 +1,11 @@ #include "common.h" -void print_float(char * str, float f) { - if(str) printf(str); +void print_float(char* str, float f) { + if (str) printf(str); - int32_t n = (int32_t)f; - f -= n; - f *= 100; - int32_t d = (f<0)?-f:f; - printf("%d.%02d\n\r", (int)n, (int)d); + int32_t n = (int32_t)f; + f -= n; + f *= 100; + int32_t d = (f < 0) ? -f : f; + printf("%d.%02d\n\r", (int)n, (int)d); } diff --git a/Apps/Src/main.c b/Apps/Src/main.c index 56655828c..425a3a20f 100644 --- a/Apps/Src/main.c +++ b/Apps/Src/main.c @@ -1,207 +1,150 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file main.c - * @brief - * + * @brief + * */ -#include "common.h" -#include "config.h" -#include "Tasks.h" -#include "stm32f4xx.h" -#include "CANbus.h" #include "CANConfig.h" +#include "CAN_Queue.h" +#include "CANbus.h" #include "Contactors.h" #include "Display.h" #include "Minions.h" #include "Pedals.h" -#include "CAN_Queue.h" +#include "Tasks.h" #include "UpdateDisplay.h" +#include "common.h" +#include "config.h" +#include "stm32f4xx.h" #define IGN_CONT_PERIOD 100 int main(void) { - // Disable interrupts - __disable_irq(); - - // Initialize some fault bitmaps for error checking purposes - OSErrLocBitmap = OS_NONE_LOC; - FaultBitmap = FAULT_NONE; - - OS_ERR err; - OSInit(&err); - TaskSwHook_Init(); - OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); - - assertOSError(OS_MAIN_LOC, err); - - // Initialize apps - OSTaskCreate( - (OS_TCB*)&Init_TCB, - (CPU_CHAR*)"Init", - (OS_TASK_PTR)Task_Init, - (void*)NULL, - (OS_PRIO)TASK_INIT_PRIO, - (CPU_STK*)Init_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT/10, - (CPU_STK_SIZE)TASK_INIT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Enable interrupts - __enable_irq(); - - // Start OS - OSStart(&err); - assertOSError(OS_MAIN_LOC, err); - - while(1); - - return 0; + // Disable interrupts + __disable_irq(); + + // Initialize some fault bitmaps for error checking purposes + OSErrLocBitmap = OS_NONE_LOC; + FaultBitmap = FAULT_NONE; + + OS_ERR err; + OSInit(&err); + TaskSwHook_Init(); + OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); + + assertOSError(OS_MAIN_LOC, err); + + // Initialize apps + OSTaskCreate((OS_TCB *)&Init_TCB, (CPU_CHAR *)"Init", (OS_TASK_PTR)Task_Init, + (void *)NULL, (OS_PRIO)TASK_INIT_PRIO, (CPU_STK *)Init_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT / 10, + (CPU_STK_SIZE)TASK_INIT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)0, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Enable interrupts + __enable_irq(); + + // Start OS + OSStart(&err); + assertOSError(OS_MAIN_LOC, err); + + while (1) + ; + + return 0; } -void Task_Init(void *p_arg){ - OS_ERR err; - - // Start systick - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - - OSTimeDlyHMSM(0,0,5,0,OS_OPT_TIME_HMSM_STRICT,&err); - - assertOSError(OS_MAIN_LOC, err); - - // Initialize drivers - Pedals_Init(); - OSTimeDlyHMSM(0,0,5,0,OS_OPT_TIME_HMSM_STRICT,&err); - OSTimeDlyHMSM(0,0,10,0,OS_OPT_TIME_HMSM_STRICT,&err); - BSP_UART_Init(UART_2); - CANbus_Init(CARCAN, (CANId_t*)carCANFilterList, NUM_CARCAN_FILTERS); - CANbus_Init(MOTORCAN, NULL, NUM_MOTORCAN_FILTERS); - Contactors_Init(); - Display_Init(); - Minions_Init(); - CAN_Queue_Init(); - - // Initialize applications - UpdateDisplay_Init(); - - // Initialize FaultState - OSTaskCreate( - (OS_TCB*)&FaultState_TCB, - (CPU_CHAR*)"FaultState", - (OS_TASK_PTR)Task_FaultState, - (void*)NULL, - (OS_PRIO)TASK_FAULT_STATE_PRIO, - (CPU_STK*)FaultState_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_FAULT_STATE_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize SendTritium - OSTaskCreate( - (OS_TCB*)&SendTritium_TCB, - (CPU_CHAR*)"SendTritium", - (OS_TASK_PTR)Task_SendTritium, - (void*)NULL, - (OS_PRIO)TASK_SEND_TRITIUM_PRIO, - (CPU_STK*)SendTritium_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize ReadCarCAN - OSTaskCreate( - (OS_TCB*)&ReadCarCAN_TCB, - (CPU_CHAR*)"ReadCarCAN", - (OS_TASK_PTR)Task_ReadCarCAN, - (void*)NULL, - (OS_PRIO)TASK_READ_CAR_CAN_PRIO, - (CPU_STK*)ReadCarCAN_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_READ_CAR_CAN_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize UpdateDisplay - OSTaskCreate( - (OS_TCB*)&UpdateDisplay_TCB, - (CPU_CHAR*)"UpdateDisplay", - (OS_TASK_PTR)Task_UpdateDisplay, - (void*)NULL, - (OS_PRIO)TASK_UPDATE_DISPLAY_PRIO, - (CPU_STK*)UpdateDisplay_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_UPDATE_DISPLAY_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize ReadTritium - OSTaskCreate( - (OS_TCB*)&ReadTritium_TCB, - (CPU_CHAR*)"ReadTritium", - (OS_TASK_PTR)Task_ReadTritium, - (void*)NULL, - (OS_PRIO)TASK_READ_TRITIUM_PRIO, - (CPU_STK*)ReadTritium_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_READ_TRITIUM_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize SendCarCAN - OSTaskCreate( - (OS_TCB*)&SendCarCAN_TCB, - (CPU_CHAR*)"SendCarCAN", - (OS_TASK_PTR)Task_SendCarCAN, - (void*)NULL, - (OS_PRIO)TASK_SEND_CAR_CAN_PRIO, - (CPU_STK*)SendCarCAN_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - - while(1){ - Contactors_Set(MOTOR_CONTACTOR, Minions_Read(IGN_2), true); //turn on the contactor if the ign switch lets us - assertOSError(OS_MINIONS_LOC, err); - OSTimeDlyHMSM(0, 0, 0, IGN_CONT_PERIOD, OS_OPT_TIME_HMSM_NON_STRICT, &err); - } +void Task_Init(void *p_arg) { + OS_ERR err; + + // Start systick + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + + assertOSError(OS_MAIN_LOC, err); + + // Initialize drivers + Pedals_Init(); + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + OSTimeDlyHMSM(0, 0, 10, 0, OS_OPT_TIME_HMSM_STRICT, &err); + BSP_UART_Init(UART_2); + CANbus_Init(CARCAN, (CANId_t *)carCANFilterList, NUM_CARCAN_FILTERS); + CANbus_Init(MOTORCAN, NULL, NUM_MOTORCAN_FILTERS); + Contactors_Init(); + Display_Init(); + Minions_Init(); + CAN_Queue_Init(); + + // Initialize applications + UpdateDisplay_Init(); + + // Initialize FaultState + OSTaskCreate((OS_TCB *)&FaultState_TCB, (CPU_CHAR *)"FaultState", + (OS_TASK_PTR)Task_FaultState, (void *)NULL, + (OS_PRIO)TASK_FAULT_STATE_PRIO, (CPU_STK *)FaultState_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_FAULT_STATE_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize SendTritium + OSTaskCreate((OS_TCB *)&SendTritium_TCB, (CPU_CHAR *)"SendTritium", + (OS_TASK_PTR)Task_SendTritium, (void *)NULL, + (OS_PRIO)TASK_SEND_TRITIUM_PRIO, (CPU_STK *)SendTritium_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize ReadCarCAN + OSTaskCreate((OS_TCB *)&ReadCarCAN_TCB, (CPU_CHAR *)"ReadCarCAN", + (OS_TASK_PTR)Task_ReadCarCAN, (void *)NULL, + (OS_PRIO)TASK_READ_CAR_CAN_PRIO, (CPU_STK *)ReadCarCAN_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_READ_CAR_CAN_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize UpdateDisplay + OSTaskCreate((OS_TCB *)&UpdateDisplay_TCB, (CPU_CHAR *)"UpdateDisplay", + (OS_TASK_PTR)Task_UpdateDisplay, (void *)NULL, + (OS_PRIO)TASK_UPDATE_DISPLAY_PRIO, (CPU_STK *)UpdateDisplay_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_UPDATE_DISPLAY_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize ReadTritium + OSTaskCreate((OS_TCB *)&ReadTritium_TCB, (CPU_CHAR *)"ReadTritium", + (OS_TASK_PTR)Task_ReadTritium, (void *)NULL, + (OS_PRIO)TASK_READ_TRITIUM_PRIO, (CPU_STK *)ReadTritium_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_READ_TRITIUM_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize SendCarCAN + OSTaskCreate((OS_TCB *)&SendCarCAN_TCB, (CPU_CHAR *)"SendCarCAN", + (OS_TASK_PTR)Task_SendCarCAN, (void *)NULL, + (OS_PRIO)TASK_SEND_CAR_CAN_PRIO, (CPU_STK *)SendCarCAN_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + while (1) { + Contactors_Set(MOTOR_CONTACTOR, Minions_Read(IGN_2), + true); // turn on the contactor if the ign switch lets us + assertOSError(OS_MINIONS_LOC, err); + OSTimeDlyHMSM(0, 0, 0, IGN_CONT_PERIOD, OS_OPT_TIME_HMSM_NON_STRICT, &err); + } } diff --git a/BSP/Inc/BSP_ADC.h b/BSP/Inc/BSP_ADC.h index 33ccd47b4..121032b30 100644 --- a/BSP/Inc/BSP_ADC.h +++ b/BSP/Inc/BSP_ADC.h @@ -3,7 +3,7 @@ * @file BSP_ADC.h * @brief Header file for the library to interact * with the Analog to Digital Converter (ADC) - * + * * @defgroup BSP_ADC * @addtogroup BSP_ADC * @{ @@ -20,34 +20,29 @@ #define ADC_PRECISION_BITS 12 #define ADC_RANGE_MILLIVOLTS 3300 -typedef enum -{ - Accelerator_ADC, - Brake_ADC, - NUMBER_OF_CHANNELS -} ADC_t; +typedef enum { Accelerator_ADC, Brake_ADC, NUMBER_OF_CHANNELS } ADC_t; /** * @brief Initialize the ADC module * @return None - */ + */ void BSP_ADC_Init(void); /** * @brief Provides the ADC value of the channel at the specified index * @param hardwareDevice pedal enum that represents the specific device * @return Raw ADC value without conversion - */ + */ int16_t BSP_ADC_Get_Value(ADC_t hardwareDevice); /** - * @brief Provides the ADC value in millivolts of the channel at the specified index + * @brief Provides the ADC value in millivolts of the channel at the specified + * index * @param hardwareDevice pedal enum that represents the specific device * @return ADC value in millivolts - */ + */ int16_t BSP_ADC_Get_Millivoltage(ADC_t hardwareDevice); #endif - /* @} */ diff --git a/BSP/Inc/BSP_CAN.h b/BSP/Inc/BSP_CAN.h index 4163e0c2a..62ff4773b 100644 --- a/BSP/Inc/BSP_CAN.h +++ b/BSP/Inc/BSP_CAN.h @@ -3,7 +3,7 @@ * @file BSP_CAN.h * @brief Header file for the library to interact * with both CAN lines in the car - * + * * @defgroup BSP_CAN * @addtogroup BSP_CAN * @{ @@ -12,22 +12,28 @@ #ifndef __BSP_CAN_H #define __BSP_CAN_H +#include + #include "common.h" #include "config.h" -#include -typedef enum {CAN_1=0, CAN_3, NUM_CAN} CAN_t; +typedef enum { CAN_1 = 0, CAN_3, NUM_CAN } CAN_t; /** - * @brief Initializes the CAN module that communicates with the rest of the electrical system. + * @brief Initializes the CAN module that communicates with the rest of the + * electrical system. * @param bus : The bus to initialize. Should only be either CAN_1 or CAN_3. - * @param rxEvent : the function to execute when recieving a message. NULL for no action. - * @param txEnd : the function to execute after transmitting a message. NULL for no action. - * @param idWhitelist : the idWhitelist to use for message filtering. NULL for no filtering. + * @param rxEvent : the function to execute when recieving a message. NULL for + * no action. + * @param txEnd : the function to execute after transmitting a message. NULL + * for no action. + * @param idWhitelist : the idWhitelist to use for message filtering. NULL for + * no filtering. * @param idWhitelistSize : the size of the idWhitelist, if it is not NULL. * @return None */ -void BSP_CAN_Init(CAN_t bus, callback_t rxEvent, callback_t txEnd, uint16_t* idWhitelist, uint8_t idWhitelistSize); +void BSP_CAN_Init(CAN_t bus, callback_t rxEvent, callback_t txEnd, + uint16_t* idWhitelist, uint8_t idWhitelistSize); /** * @brief Writes a message to the specified CAN line @@ -42,7 +48,7 @@ ErrorStatus BSP_CAN_Write(CAN_t bus, uint32_t id, uint8_t data[8], uint8_t len); /** * @brief Reads the message on the specified CAN line - * @param id pointer to integer to store the + * @param id pointer to integer to store the * message ID that was read * @param data pointer to integer array to store * the message in bytes @@ -52,5 +58,4 @@ ErrorStatus BSP_CAN_Read(CAN_t bus, uint32_t* id, uint8_t* data); #endif - /* @} */ diff --git a/BSP/Inc/BSP_GPIO.h b/BSP/Inc/BSP_GPIO.h index 66db068c4..9441f9504 100644 --- a/BSP/Inc/BSP_GPIO.h +++ b/BSP/Inc/BSP_GPIO.h @@ -3,7 +3,7 @@ * @file BSP_GPIO.h * @brief Header file for the library to interact * with the GPIO ports - * + * * @defgroup BSP_GPIO * @addtogroup BSP_GPIO * @{ @@ -12,37 +12,38 @@ #ifndef __BSP_GPIO_H #define __BSP_GPIO_H +#include +#include + #include "common.h" #include "config.h" #include "stm32f4xx_gpio.h" -#include -#include -typedef enum {PORTA = 0, PORTB, PORTC, PORTD, NUM_PORTS} port_t; -typedef enum {INPUT = 0, OUTPUT} direction_t; +typedef enum { PORTA = 0, PORTB, PORTC, PORTD, NUM_PORTS } port_t; +typedef enum { INPUT = 0, OUTPUT } direction_t; /** * @brief Initializes a GPIO port * @param port - port to initialize * @param mask - pins - * @param direction - input or output + * @param direction - input or output * @return None - */ + */ void BSP_GPIO_Init(port_t port, uint16_t mask, direction_t direction); /** * @brief Reads value of the specified port * @param port to read * @return data of the port - */ + */ uint16_t BSP_GPIO_Read(port_t port); /** - * @brief Writes data to a specified port + * @brief Writes data to a specified port * @param port port to write to - * @param data data to write + * @param data data to write * @return None - */ + */ void BSP_GPIO_Write(port_t port, uint16_t data); /** @@ -50,7 +51,7 @@ void BSP_GPIO_Write(port_t port, uint16_t data); * @param port The port to read from * @param pinmask Mask from stm header file that says which pin to read from * @return State of the pin - */ + */ uint8_t BSP_GPIO_Read_Pin(port_t port, uint16_t pinmask); /** @@ -59,7 +60,7 @@ uint8_t BSP_GPIO_Read_Pin(port_t port, uint16_t pinmask); * @param pinmask Mask from stm header file that says which pin to write too * @param state true=ON or false=OFF * @return None - */ + */ void BSP_GPIO_Write_Pin(port_t port, uint16_t pinmask, bool state); /** @@ -67,10 +68,9 @@ void BSP_GPIO_Write_Pin(port_t port, uint16_t pinmask, bool state); * @param port The port to get state from * @param pin The pin to get state from * @return 1 if pin is high, 0 if low - */ + */ uint8_t BSP_GPIO_Get_State(port_t port, uint16_t pin); #endif - /* @} */ diff --git a/BSP/Inc/BSP_OS.h b/BSP/Inc/BSP_OS.h index ab4bfcd87..cd80f58fc 100644 --- a/BSP/Inc/BSP_OS.h +++ b/BSP/Inc/BSP_OS.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file BSP_OS.h - * @brief - * + * @brief + * * @defgroup BSP_OS * @addtogroup BSP_OS * @{ @@ -12,11 +12,10 @@ #define BSP_OS_H typedef struct { - callback_t pend; - callback_t post; + callback_t pend; + callback_t post; } bsp_os_t; #endif - /* @} */ diff --git a/BSP/Inc/BSP_SPI.h b/BSP/Inc/BSP_SPI.h index 27ad70d96..3c368f043 100644 --- a/BSP/Inc/BSP_SPI.h +++ b/BSP/Inc/BSP_SPI.h @@ -3,7 +3,7 @@ * @file BSP_SPI.h * @brief Header file for the library to interact * over SPI with minion board(s) - * + * * @defgroup BSP_SPI * @addtogroup BSP_SPI * @{ @@ -12,15 +12,13 @@ #ifndef __BSP_SPI_H #define __BSP_SPI_H -#include "common.h" #include "bsp.h" +#include "common.h" #include "os.h" - - /** * @brief Initializes the SPI communication - * to be used to communicate with + * to be used to communicate with * minion board(s) * @param None * @return None @@ -47,5 +45,4 @@ void BSP_SPI_Read(uint8_t* rxBuf, uint8_t rxLen); #endif - /* @} */ diff --git a/BSP/Inc/BSP_UART.h b/BSP/Inc/BSP_UART.h index 15743921d..802373fad 100644 --- a/BSP/Inc/BSP_UART.h +++ b/BSP/Inc/BSP_UART.h @@ -3,7 +3,7 @@ * @file BSP_UART.h * @brief Header file for the library to interact * with the UART line - * + * * @defgroup BSP_UART * @addtogroup BSP_UART * @{ @@ -12,17 +12,18 @@ #ifndef __BSP_UART_H #define __BSP_UART_H -#include "common.h" #include -typedef enum {UART_2, UART_3, NUM_UART} UART_t; +#include "common.h" + +typedef enum { UART_2, UART_3, NUM_UART } UART_t; /** * @brief Initializes the UART peripheral */ void BSP_UART_Init(UART_t); /** - * @brief Gets one line of ASCII text that was received + * @brief Gets one line of ASCII text that was received * from a specified UART device * @pre str should be at least 128bytes long. * @param uart device selected @@ -32,17 +33,16 @@ void BSP_UART_Init(UART_t); uint32_t BSP_UART_Read(UART_t uart, char *str); /** - * @brief Transmits data to through a specific - * UART device (represented as a line of data + * @brief Transmits data to through a specific + * UART device (represented as a line of data * in csv file). * @param uart device selected * @param str pointer to buffer with data to send. * @param len size of buffer * @return number of bytes that were sent */ -uint32_t BSP_UART_Write(UART_t uart ,char *str, uint32_t len); +uint32_t BSP_UART_Write(UART_t uart, char *str, uint32_t len); #endif - /* @} */ diff --git a/BSP/Inc/bsp.h b/BSP/Inc/bsp.h old mode 100755 new mode 100644 index f45456695..df4cf9ee8 --- a/BSP/Inc/bsp.h +++ b/BSP/Inc/bsp.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file bsp.h - * @brief - * + * @brief + * * @defgroup bsp * @addtogroup bsp * @{ @@ -11,14 +11,14 @@ #ifndef __BSP_H #define __BSP_H +#include +#include + #include "BSP_ADC.h" #include "BSP_CAN.h" -#include "BSP_UART.h" -#include "BSP_SPI.h" #include "BSP_GPIO.h" - -#include -#include +#include "BSP_SPI.h" +#include "BSP_UART.h" #ifdef SIMULATOR #define DATA_PATH(f) "BSP/Simulator/Hardware/Data/" f diff --git a/BSP/STM32F413/Src/BSP_ADC.c b/BSP/STM32F413/Src/BSP_ADC.c index f49dac17b..4d2e121f4 100644 --- a/BSP/STM32F413/Src/BSP_ADC.c +++ b/BSP/STM32F413/Src/BSP_ADC.c @@ -1,90 +1,97 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ #include "BSP_ADC.h" + #include "stm32f4xx.h" static volatile uint16_t ADCresults[2]; static void ADC_InitDMA(void) { - // Start the clock for the DMA - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); - - // Create the DMA structure - DMA_InitTypeDef DMA_InitStruct; - - DMA_InitStruct.DMA_Channel = DMA_Channel_0; - DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&(ADC1->DR); - DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t) &ADCresults; - DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMA_InitStruct.DMA_BufferSize = 2; - DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; - DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; - DMA_InitStruct.DMA_Mode = DMA_Mode_Circular; - DMA_InitStruct.DMA_Priority = DMA_Priority_High; - DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStruct.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; - DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(DMA2_Stream0, &DMA_InitStruct); - - // Enable DMA2 stream 0 - DMA_Cmd(DMA2_Stream0, ENABLE); + // Start the clock for the DMA + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); + + // Create the DMA structure + DMA_InitTypeDef DMA_InitStruct; + + DMA_InitStruct.DMA_Channel = DMA_Channel_0; + DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t) & (ADC1->DR); + DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&ADCresults; + DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStruct.DMA_BufferSize = 2; + DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStruct.DMA_Mode = DMA_Mode_Circular; + DMA_InitStruct.DMA_Priority = DMA_Priority_High; + DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStruct.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(DMA2_Stream0, &DMA_InitStruct); + + // Enable DMA2 stream 0 + DMA_Cmd(DMA2_Stream0, ENABLE); } /** - * @brief Initializes the ADC module. This is to measure the hall effect sensors - * on the Current Monitor Board. + * @brief Initializes the ADC module. This is to measure the hall effect + * sensors on the Current Monitor Board. * @param None * @return None */ void BSP_ADC_Init(void) { - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); // Enable the ADC clock - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); // Enable the PC clock for port C - - ADC_InitDMA(); - - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; // Using pin PC0 and PC1 - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN; // Analog Input - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; // High impedence - GPIO_Init(GPIOC,&GPIO_InitStruct); - - // ADC Common Init - ADC_CommonInitTypeDef ADC_CommonStruct; - ADC_CommonStruct.ADC_Mode = ADC_Mode_Independent; - ADC_CommonStruct.ADC_Prescaler = ADC_Prescaler_Div2; - ADC_CommonStruct.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; - ADC_CommonStruct.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; - ADC_CommonInit(&ADC_CommonStruct); - - // Set up to use DMA so that multiple channels can be used - ADC_InitTypeDef ADC_InitStruct; // Initialization structure - ADC_InitStruct.ADC_Resolution = ADC_Resolution_12b; // High resolution - ADC_InitStruct.ADC_ScanConvMode = ENABLE; // So we can go through all the channels - ADC_InitStruct.ADC_ContinuousConvMode = ENABLE; // Cycle the channels - ADC_InitStruct.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; - ADC_InitStruct.ADC_ExternalTrigConv = DISABLE; - ADC_InitStruct.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStruct.ADC_NbrOfConversion = 2; // We have two channels that we need to read - - ADC_Init(ADC1, &ADC_InitStruct); - - // Configure the channels - // Apparently channel 2 has priority, or is at least read first. - // If you change the priorities, be prepared to have the order in the array change. - ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_480Cycles); // Accelerator - ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 2, ADC_SampleTime_480Cycles); // Brake - - ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); - - ADC_DMACmd(ADC1, ENABLE); - - // Enable ADC1 - ADC_Cmd(ADC1, ENABLE); - - ADC_SoftwareStartConv(ADC1); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); // Enable the ADC clock + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, + ENABLE); // Enable the PC clock for port C + + ADC_InitDMA(); + + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; // Using pin PC0 and PC1 + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN; // Analog Input + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; // High impedence + GPIO_Init(GPIOC, &GPIO_InitStruct); + + // ADC Common Init + ADC_CommonInitTypeDef ADC_CommonStruct; + ADC_CommonStruct.ADC_Mode = ADC_Mode_Independent; + ADC_CommonStruct.ADC_Prescaler = ADC_Prescaler_Div2; + ADC_CommonStruct.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; + ADC_CommonStruct.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; + ADC_CommonInit(&ADC_CommonStruct); + + // Set up to use DMA so that multiple channels can be used + ADC_InitTypeDef ADC_InitStruct; // Initialization structure + ADC_InitStruct.ADC_Resolution = ADC_Resolution_12b; // High resolution + ADC_InitStruct.ADC_ScanConvMode = + ENABLE; // So we can go through all the channels + ADC_InitStruct.ADC_ContinuousConvMode = ENABLE; // Cycle the channels + ADC_InitStruct.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStruct.ADC_ExternalTrigConv = DISABLE; + ADC_InitStruct.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStruct.ADC_NbrOfConversion = + 2; // We have two channels that we need to read + + ADC_Init(ADC1, &ADC_InitStruct); + + // Configure the channels + // Apparently channel 2 has priority, or is at least read first. + // If you change the priorities, be prepared to have the order in the array + // change. + ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, + ADC_SampleTime_480Cycles); // Accelerator + ADC_RegularChannelConfig(ADC1, ADC_Channel_11, 2, + ADC_SampleTime_480Cycles); // Brake + + ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE); + + ADC_DMACmd(ADC1, ENABLE); + + // Enable ADC1 + ADC_Cmd(ADC1, ENABLE); + + ADC_SoftwareStartConv(ADC1); } /** @@ -93,11 +100,10 @@ void BSP_ADC_Init(void) { * @return millivoltage value ADC measurement */ int16_t BSP_ADC_Get_Value(ADC_t hardwareDevice) { + // Get ADC raw data + uint16_t data = ADCresults[hardwareDevice]; - // Get ADC raw data - uint16_t data = ADCresults[hardwareDevice]; - - return (int16_t) data; + return (int16_t)data; } /** @@ -106,10 +112,9 @@ int16_t BSP_ADC_Get_Value(ADC_t hardwareDevice) { * @return millivoltage value ADC measurement */ int16_t BSP_ADC_Get_Millivoltage(ADC_t hardwareDevice) { + // Get ADC raw data + int16_t data = (int16_t)ADCresults[hardwareDevice]; - // Get ADC raw data - int16_t data = (int16_t) ADCresults[hardwareDevice]; - - // Convert to millivoltage - return (ADC_RANGE_MILLIVOLTS * data) >> ADC_PRECISION_BITS; + // Convert to millivoltage + return (ADC_RANGE_MILLIVOLTS * data) >> ADC_PRECISION_BITS; } diff --git a/BSP/STM32F413/Src/BSP_CAN.c b/BSP/STM32F413/Src/BSP_CAN.c index a45ef70c3..b4b3f0f68 100644 --- a/BSP/STM32F413/Src/BSP_CAN.c +++ b/BSP/STM32F413/Src/BSP_CAN.c @@ -1,14 +1,14 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ #include "BSP_CAN.h" -#include "stm32f4xx.h" + #include "os.h" +#include "stm32f4xx.h" // The message information that we care to receive -typedef struct _msg -{ - uint32_t id; - uint8_t data[8]; +typedef struct _msg { + uint32_t id; + uint8_t data[8]; } msg_t; // Set up a fifo for receiving @@ -17,9 +17,10 @@ typedef struct _msg #define FIFO_NAME msg_queue #include "fifo.h" -#define NUM_FILTER_REGS 4 // Number of 16 bit registers for ids in one CAN_FilterInit struct +#define NUM_FILTER_REGS \ + 4 // Number of 16 bit registers for ids in one CAN_FilterInit struct -//return error if someone tries to call from motor can +// return error if someone tries to call from motor can static msg_queue_t gRxQueue[2]; @@ -35,316 +36,332 @@ void BSP_CAN1_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize); void BSP_CAN3_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize); /** - * @brief Initializes the CAN module that communicates with the rest of the electrical system. - * @param rxEvent : the function to execute when recieving a message. NULL for no action. - * @param txEnd : the function to execute after transmitting a message. NULL for no action. + * @brief Initializes the CAN module that communicates with the rest of the + * electrical system. + * @param rxEvent : the function to execute when recieving a message. NULL for + * no action. + * @param txEnd : the function to execute after transmitting a message. NULL + * for no action. * @return None */ -void BSP_CAN_Init(CAN_t bus, callback_t rxEvent, callback_t txEnd, uint16_t* idWhitelist, uint8_t idWhitelistSize) { - - // Configure event handles - gRxEvent[bus] = rxEvent; - gTxEnd[bus] = txEnd; - - if (bus == CAN_1) - { - BSP_CAN1_Init(idWhitelist, idWhitelistSize); - } - else - { - BSP_CAN3_Init(idWhitelist, idWhitelistSize); - } +void BSP_CAN_Init(CAN_t bus, callback_t rxEvent, callback_t txEnd, + uint16_t* idWhitelist, uint8_t idWhitelistSize) { + // Configure event handles + gRxEvent[bus] = rxEvent; + gTxEnd[bus] = txEnd; + + if (bus == CAN_1) { + BSP_CAN1_Init(idWhitelist, idWhitelistSize); + } else { + BSP_CAN3_Init(idWhitelist, idWhitelistSize); + } } void BSP_CAN1_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize) { - GPIO_InitTypeDef GPIO_InitStruct; - CAN_InitTypeDef CAN_InitStruct; - NVIC_InitTypeDef NVIC_InitStruct; - CAN_FilterInitTypeDef CAN_FilterInitStruct; - - // Initialize the queue - gRxQueue[0] = msg_queue_new(); - - /* CAN GPIOs configuration **************************************************/ - - /* Enable GPIO clock */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - - // Alternate Function 9 - GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_CAN1); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_CAN1); - - /* Configure CAN RX and TX pins */ - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(GPIOA, &GPIO_InitStruct); - - /* CAN configuration ********************************************************/ - /* Enable CAN clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE); - - /* CAN cell init */ - CAN_InitStruct.CAN_TTCM = DISABLE; - CAN_InitStruct.CAN_ABOM = DISABLE; - CAN_InitStruct.CAN_AWUM = DISABLE; - CAN_InitStruct.CAN_NART = DISABLE; - CAN_InitStruct.CAN_RFLM = DISABLE; - CAN_InitStruct.CAN_TXFP = ENABLE; - CAN_InitStruct.CAN_Mode = CAN_Mode_Normal; - CAN_InitStruct.CAN_SJW = CAN_SJW_1tq; - - /* CAN Baudrate = 125 KBps - * 1/(prescalar + (prescalar*BS1) + (prescalar*BS2)) * Clk = CAN Baudrate - * The CAN clk is currently set to 20MHz (APB1 clock set to 20MHz in BSP_PLL_Init()) - */ - CAN_InitStruct.CAN_BS1 = CAN_BS1_3tq; - CAN_InitStruct.CAN_BS2 = CAN_BS2_4tq; - CAN_InitStruct.CAN_Prescaler = 16; - CAN_Init(CAN1, &CAN_InitStruct); - - /* CAN filter init - * Initializes hardware filter banks to be used for filtering CAN IDs (whitelist) - */ - if(idWhitelist == NULL){ - // No filtering. All IDs can pass through. - CAN_FilterInitStruct.CAN_FilterNumber = 0; - CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdMask; - CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_32bit; - CAN_FilterInitStruct.CAN_FilterIdHigh = 0x0000; - CAN_FilterInitStruct.CAN_FilterIdLow = 0x0000; - CAN_FilterInitStruct.CAN_FilterMaskIdHigh = 0x0000; - CAN_FilterInitStruct.CAN_FilterMaskIdLow = 0x0000; - CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; - CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; + GPIO_InitTypeDef GPIO_InitStruct; + CAN_InitTypeDef CAN_InitStruct; + NVIC_InitTypeDef NVIC_InitStruct; + CAN_FilterInitTypeDef CAN_FilterInitStruct; + + // Initialize the queue + gRxQueue[0] = msg_queue_new(); + + /* CAN GPIOs configuration **************************************************/ + + /* Enable GPIO clock */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + + // Alternate Function 9 + GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_CAN1); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_CAN1); + + /* Configure CAN RX and TX pins */ + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* CAN configuration ********************************************************/ + /* Enable CAN clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE); + + /* CAN cell init */ + CAN_InitStruct.CAN_TTCM = DISABLE; + CAN_InitStruct.CAN_ABOM = DISABLE; + CAN_InitStruct.CAN_AWUM = DISABLE; + CAN_InitStruct.CAN_NART = DISABLE; + CAN_InitStruct.CAN_RFLM = DISABLE; + CAN_InitStruct.CAN_TXFP = ENABLE; + CAN_InitStruct.CAN_Mode = CAN_Mode_Normal; + CAN_InitStruct.CAN_SJW = CAN_SJW_1tq; + + /* CAN Baudrate = 125 KBps + * 1/(prescalar + (prescalar*BS1) + (prescalar*BS2)) * Clk = CAN Baudrate + * The CAN clk is currently set to 20MHz (APB1 clock set to 20MHz in + * BSP_PLL_Init()) + */ + CAN_InitStruct.CAN_BS1 = CAN_BS1_3tq; + CAN_InitStruct.CAN_BS2 = CAN_BS2_4tq; + CAN_InitStruct.CAN_Prescaler = 16; + CAN_Init(CAN1, &CAN_InitStruct); + + /* CAN filter init + * Initializes hardware filter banks to be used for filtering CAN IDs + * (whitelist) + */ + if (idWhitelist == NULL) { + // No filtering. All IDs can pass through. + CAN_FilterInitStruct.CAN_FilterNumber = 0; + CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdMask; + CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_32bit; + CAN_FilterInitStruct.CAN_FilterIdHigh = 0x0000; + CAN_FilterInitStruct.CAN_FilterIdLow = 0x0000; + CAN_FilterInitStruct.CAN_FilterMaskIdHigh = 0x0000; + CAN_FilterInitStruct.CAN_FilterMaskIdLow = 0x0000; + CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; + CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; + CAN_FilterInit(CAN1, &CAN_FilterInitStruct); + } else { + // Filter CAN IDs + // So far, if we shift whatever id we need by 5, it works + // If whitelist is passed but no valid ID exists inside of it, filter + // nothing i.e. no ID gets through MAXIMUM CAN ID ALLOWED IS 2047 + + CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdList; // list mode + CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_16bit; + CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; + CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; + uint16_t validIDCounter = 0; + + uint16_t* FilterStructPtr = + (uint16_t*)&(CAN_FilterInitStruct); // address of CAN Filter Struct + for (uint8_t i = 0; i < idWhitelistSize; i++) { + if (idWhitelist[i] == 0) { // zero ID check + continue; + } + + CAN_FilterInitStruct.CAN_FilterNumber = + i / NUM_FILTER_REGS; // determines filter number based on CAN ID + *(FilterStructPtr + (i % NUM_FILTER_REGS)) = idWhitelist[i] << 5; + validIDCounter++; + + if (i % NUM_FILTER_REGS == + NUM_FILTER_REGS - 1) { // if four elements have been written to a + // filter call CAN_FilterInit() CAN_FilterInit(CAN1, &CAN_FilterInitStruct); - } else{ - // Filter CAN IDs - // So far, if we shift whatever id we need by 5, it works - // If whitelist is passed but no valid ID exists inside of it, filter nothing i.e. no ID gets through - // MAXIMUM CAN ID ALLOWED IS 2047 - - CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdList; //list mode - CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_16bit; - CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; - CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; - uint16_t validIDCounter = 0; - - uint16_t* FilterStructPtr = (uint16_t*)&(CAN_FilterInitStruct); //address of CAN Filter Struct - for(uint8_t i = 0; i < idWhitelistSize; i++){ - if (idWhitelist[i] == 0){ //zero ID check - continue; - } - - CAN_FilterInitStruct.CAN_FilterNumber = i / NUM_FILTER_REGS; //determines filter number based on CAN ID - *(FilterStructPtr + (i%NUM_FILTER_REGS)) = idWhitelist[i] << 5; - validIDCounter++; - - if(i % NUM_FILTER_REGS == NUM_FILTER_REGS - 1){ //if four elements have been written to a filter call CAN_FilterInit() - CAN_FilterInit(CAN1, &CAN_FilterInitStruct); - } - else if(i == idWhitelistSize - 1){ //we are out of elements, call CAN_FilterInit() - for(uint8_t j = i%NUM_FILTER_REGS + 1; j <= NUM_FILTER_REGS - 1; j++) // Set unfilled filter registers to 0 - *(FilterStructPtr + j) = 0x0000; - - CAN_FilterInit(CAN1, &CAN_FilterInitStruct); - } - else if(validIDCounter > 112){ //All filter banks are to be filled and there is no point in filtering - for(uint8_t filter = 0; filter < 28; filter++){//Therefore, let all IDs through (no filtering) - CAN_FilterInitStruct.CAN_FilterNumber = filter; - CAN_FilterInitStruct.CAN_FilterActivation = DISABLE; - CAN_FilterInit(CAN1, &CAN_FilterInitStruct); - } - } + } else if (i == idWhitelistSize - + 1) { // we are out of elements, call CAN_FilterInit() + for (uint8_t j = i % NUM_FILTER_REGS + 1; j <= NUM_FILTER_REGS - 1; + j++) // Set unfilled filter registers to 0 + *(FilterStructPtr + j) = 0x0000; + + CAN_FilterInit(CAN1, &CAN_FilterInitStruct); + } else if (validIDCounter > 112) { // All filter banks are to be filled + // and there is no point in filtering + for (uint8_t filter = 0; filter < 28; + filter++) { // Therefore, let all IDs through (no filtering) + CAN_FilterInitStruct.CAN_FilterNumber = filter; + CAN_FilterInitStruct.CAN_FilterActivation = DISABLE; + CAN_FilterInit(CAN1, &CAN_FilterInitStruct); } + } } - - /* Transmit Structure preparation */ - gTxMessage[0].ExtId = 0x5; - gTxMessage[0].RTR = CAN_RTR_DATA; - gTxMessage[0].IDE = CAN_ID_STD; - gTxMessage[0].DLC = 1; - - /* Receive Structure preparation */ - gRxMessage[0].StdId = 0x00; - gRxMessage[0].ExtId = 0x00; - gRxMessage[0].IDE = CAN_ID_STD; - gRxMessage[0].DLC = 0; - gRxMessage[0].FMI = 0; - - /* Enable FIFO 0 message pending Interrupt */ - CAN_ITConfig(CAN1, CAN_IT_FMP0, ENABLE); - - // Enable Rx interrupts - NVIC_InitStruct.NVIC_IRQChannel = CAN1_RX0_IRQn; // TODO: CHECK IRQ CHANNELS - NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; - NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + } + + /* Transmit Structure preparation */ + gTxMessage[0].ExtId = 0x5; + gTxMessage[0].RTR = CAN_RTR_DATA; + gTxMessage[0].IDE = CAN_ID_STD; + gTxMessage[0].DLC = 1; + + /* Receive Structure preparation */ + gRxMessage[0].StdId = 0x00; + gRxMessage[0].ExtId = 0x00; + gRxMessage[0].IDE = CAN_ID_STD; + gRxMessage[0].DLC = 0; + gRxMessage[0].FMI = 0; + + /* Enable FIFO 0 message pending Interrupt */ + CAN_ITConfig(CAN1, CAN_IT_FMP0, ENABLE); + + // Enable Rx interrupts + NVIC_InitStruct.NVIC_IRQChannel = CAN1_RX0_IRQn; // TODO: CHECK IRQ CHANNELS + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); + + if (NULL != gTxEnd[0]) { + // Enable Tx Interrupts + CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE); + NVIC_InitStruct.NVIC_IRQChannel = CAN1_TX_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = + 0x0; // TODO: assess both of these priority settings + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x0; NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStruct); - - if(NULL != gTxEnd[0]) { - // Enable Tx Interrupts - CAN_ITConfig(CAN1, CAN_IT_TME, ENABLE); - NVIC_InitStruct.NVIC_IRQChannel = CAN1_TX_IRQn; - NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x0; // TODO: assess both of these priority settings - NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x0; - NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStruct); - } + } } -void BSP_CAN3_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize) -{ - GPIO_InitTypeDef GPIO_InitStruct; - CAN_InitTypeDef CAN_InitStruct; - NVIC_InitTypeDef NVIC_InitStruct; - CAN_FilterInitTypeDef CAN_FilterInitStruct; - - // Initialize the queue - gRxQueue[1] = msg_queue_new(); - - /* CAN GPIOs configuration **************************************************/ - - /* Enable GPIO clock */ - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - - // Alternate Function 9 - GPIO_PinAFConfig(GPIOA, GPIO_PinSource15, GPIO_AF11_CAN3); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF11_CAN3); - - /* Configure CAN RX and TX pins */ - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_8; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(GPIOA, &GPIO_InitStruct); - - /* CAN configuration ********************************************************/ - /* Enable CAN clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN3, ENABLE); - - /* CAN cell init */ - CAN_InitStruct.CAN_TTCM = DISABLE; - CAN_InitStruct.CAN_ABOM = DISABLE; - CAN_InitStruct.CAN_AWUM = DISABLE; - CAN_InitStruct.CAN_NART = DISABLE; - CAN_InitStruct.CAN_RFLM = DISABLE; - CAN_InitStruct.CAN_TXFP = ENABLE; - CAN_InitStruct.CAN_Mode = CAN_Mode_Normal; - CAN_InitStruct.CAN_SJW = CAN_SJW_1tq; - - /* CAN Baudrate = 125 KBps - * 1/(prescalar + (prescalar*BS1) + (prescalar*BS2)) * Clk = CAN Baudrate - * The CAN clk is currently set to 20MHz (APB1 clock set to 20MHz in BSP_PLL_Init()) - */ - CAN_InitStruct.CAN_BS1 = CAN_BS1_3tq; - CAN_InitStruct.CAN_BS2 = CAN_BS2_4tq; - CAN_InitStruct.CAN_Prescaler = 16; - CAN_Init(CAN3, &CAN_InitStruct); - - /* CAN filter init - * Initializes hardware filter banks to be used for filtering CAN IDs (whitelist) - */ - if(idWhitelist == NULL){ - // No filtering. All IDs can pass through. - CAN_FilterInitStruct.CAN_FilterNumber = 0; - CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdMask; - CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_32bit; - CAN_FilterInitStruct.CAN_FilterIdHigh = 0x0000; - CAN_FilterInitStruct.CAN_FilterIdLow = 0x0000; - CAN_FilterInitStruct.CAN_FilterMaskIdHigh = 0x0000; - CAN_FilterInitStruct.CAN_FilterMaskIdLow = 0x0000; - CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; - CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; +void BSP_CAN3_Init(uint16_t* idWhitelist, uint8_t idWhitelistSize) { + GPIO_InitTypeDef GPIO_InitStruct; + CAN_InitTypeDef CAN_InitStruct; + NVIC_InitTypeDef NVIC_InitStruct; + CAN_FilterInitTypeDef CAN_FilterInitStruct; + + // Initialize the queue + gRxQueue[1] = msg_queue_new(); + + /* CAN GPIOs configuration **************************************************/ + + /* Enable GPIO clock */ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + + // Alternate Function 9 + GPIO_PinAFConfig(GPIOA, GPIO_PinSource15, GPIO_AF11_CAN3); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF11_CAN3); + + /* Configure CAN RX and TX pins */ + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15 | GPIO_Pin_8; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* CAN configuration ********************************************************/ + /* Enable CAN clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN3, ENABLE); + + /* CAN cell init */ + CAN_InitStruct.CAN_TTCM = DISABLE; + CAN_InitStruct.CAN_ABOM = DISABLE; + CAN_InitStruct.CAN_AWUM = DISABLE; + CAN_InitStruct.CAN_NART = DISABLE; + CAN_InitStruct.CAN_RFLM = DISABLE; + CAN_InitStruct.CAN_TXFP = ENABLE; + CAN_InitStruct.CAN_Mode = CAN_Mode_Normal; + CAN_InitStruct.CAN_SJW = CAN_SJW_1tq; + + /* CAN Baudrate = 125 KBps + * 1/(prescalar + (prescalar*BS1) + (prescalar*BS2)) * Clk = CAN Baudrate + * The CAN clk is currently set to 20MHz (APB1 clock set to 20MHz in + * BSP_PLL_Init()) + */ + CAN_InitStruct.CAN_BS1 = CAN_BS1_3tq; + CAN_InitStruct.CAN_BS2 = CAN_BS2_4tq; + CAN_InitStruct.CAN_Prescaler = 16; + CAN_Init(CAN3, &CAN_InitStruct); + + /* CAN filter init + * Initializes hardware filter banks to be used for filtering CAN IDs + * (whitelist) + */ + if (idWhitelist == NULL) { + // No filtering. All IDs can pass through. + CAN_FilterInitStruct.CAN_FilterNumber = 0; + CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdMask; + CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_32bit; + CAN_FilterInitStruct.CAN_FilterIdHigh = 0x0000; + CAN_FilterInitStruct.CAN_FilterIdLow = 0x0000; + CAN_FilterInitStruct.CAN_FilterMaskIdHigh = 0x0000; + CAN_FilterInitStruct.CAN_FilterMaskIdLow = 0x0000; + CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; + CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; + CAN_FilterInit(CAN3, &CAN_FilterInitStruct); + } else { + // Filter CAN IDs + CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdList; // list mode + CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_16bit; + CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; + CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; + + uint16_t* FilterStructPtr = + (uint16_t*)&(CAN_FilterInitStruct); // address of CAN Filter Struct + for (uint8_t i = 0; i < idWhitelistSize; i++) { + CAN_FilterInitStruct.CAN_FilterNumber = + i / NUM_FILTER_REGS; // determines filter number based on CAN ID + *(FilterStructPtr + (i % NUM_FILTER_REGS)) = idWhitelist[i]; + + if (i % NUM_FILTER_REGS == + NUM_FILTER_REGS - 1) { // if four elements have been written to a + // filter call CAN_FilterInit() CAN_FilterInit(CAN3, &CAN_FilterInitStruct); - } else{ - // Filter CAN IDs - CAN_FilterInitStruct.CAN_FilterMode = CAN_FilterMode_IdList; //list mode - CAN_FilterInitStruct.CAN_FilterScale = CAN_FilterScale_16bit; - CAN_FilterInitStruct.CAN_FilterFIFOAssignment = 0; - CAN_FilterInitStruct.CAN_FilterActivation = ENABLE; - - uint16_t* FilterStructPtr = (uint16_t*)&(CAN_FilterInitStruct); //address of CAN Filter Struct - for(uint8_t i = 0; i < idWhitelistSize; i++){ - CAN_FilterInitStruct.CAN_FilterNumber = i / NUM_FILTER_REGS; //determines filter number based on CAN ID - *(FilterStructPtr + (i%NUM_FILTER_REGS)) = idWhitelist[i]; - - if(i % NUM_FILTER_REGS == NUM_FILTER_REGS - 1){ //if four elements have been written to a filter call CAN_FilterInit() - CAN_FilterInit(CAN3, &CAN_FilterInitStruct); - } - else if(i == idWhitelistSize - 1){ //we are out of elements, call CAN_FilterInit() - for(uint8_t j = i%NUM_FILTER_REGS + 1; j <= NUM_FILTER_REGS - 1; j++) // Set unfilled filter registers to 0 - *(FilterStructPtr + j) = 0x0000; - - CAN_FilterInit(CAN3, &CAN_FilterInitStruct); - } - } - } - - // CAN_SlaveStartBank(CAN1, 0); + } else if (i == idWhitelistSize - + 1) { // we are out of elements, call CAN_FilterInit() + for (uint8_t j = i % NUM_FILTER_REGS + 1; j <= NUM_FILTER_REGS - 1; + j++) // Set unfilled filter registers to 0 + *(FilterStructPtr + j) = 0x0000; - /* Transmit Structure preparation */ - gTxMessage[1].ExtId = 0x5; - gTxMessage[1].RTR = CAN_RTR_DATA; - gTxMessage[1].IDE = CAN_ID_STD; - gTxMessage[1].DLC = 1; - - /* Receive Structure preparation */ - gRxMessage[1].StdId = 0x00; - gRxMessage[1].ExtId = 0x00; - gRxMessage[1].IDE = CAN_ID_STD; - gRxMessage[1].DLC = 0; - gRxMessage[1].FMI = 0; - - /* Enable FIFO 0 message pending Interrupt */ - CAN_ITConfig(CAN3, CAN_IT_FMP0, ENABLE); - - //TODO: Double check preemption priority and subpriority - // Enable Rx interrupts - NVIC_InitStruct.NVIC_IRQChannel = CAN3_RX0_IRQn; + CAN_FilterInit(CAN3, &CAN_FilterInitStruct); + } + } + } + + // CAN_SlaveStartBank(CAN1, 0); + + /* Transmit Structure preparation */ + gTxMessage[1].ExtId = 0x5; + gTxMessage[1].RTR = CAN_RTR_DATA; + gTxMessage[1].IDE = CAN_ID_STD; + gTxMessage[1].DLC = 1; + + /* Receive Structure preparation */ + gRxMessage[1].StdId = 0x00; + gRxMessage[1].ExtId = 0x00; + gRxMessage[1].IDE = CAN_ID_STD; + gRxMessage[1].DLC = 0; + gRxMessage[1].FMI = 0; + + /* Enable FIFO 0 message pending Interrupt */ + CAN_ITConfig(CAN3, CAN_IT_FMP0, ENABLE); + + // TODO: Double check preemption priority and subpriority + // Enable Rx interrupts + NVIC_InitStruct.NVIC_IRQChannel = CAN3_RX0_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); + + // Enable Tx interrupts + if (NULL != gTxEnd[1]) { + CAN_ITConfig(CAN3, CAN_IT_TME, ENABLE); + NVIC_InitStruct.NVIC_IRQChannel = CAN3_TX_IRQn; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStruct); - - // Enable Tx interrupts - if(NULL != gTxEnd[1]){ - CAN_ITConfig(CAN3,CAN_IT_TME,ENABLE); - NVIC_InitStruct.NVIC_IRQChannel = CAN3_TX_IRQn; - NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00; - NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; - NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStruct); - } + } } /** * @brief Transmits the data onto the CAN bus with the specified id - * @param id : Message of ID. Also indicates the priority of message. The lower the value, the higher the priority. + * @param id : Message of ID. Also indicates the priority of message. The + * lower the value, the higher the priority. * @param data : data to be transmitted. The max is 8 bytes. - * @param length : num of bytes of data to be transmitted. This must be <= 8 bytes or else the rest of the message is dropped. - * @return ERROR if module was unable to transmit the data onto the CAN bus. SUCCESS indicates data was transmitted. + * @param length : num of bytes of data to be transmitted. This must be <= 8 + * bytes or else the rest of the message is dropped. + * @return ERROR if module was unable to transmit the data onto the CAN bus. + * SUCCESS indicates data was transmitted. */ -ErrorStatus BSP_CAN_Write(CAN_t bus, uint32_t id, uint8_t data[8], uint8_t length) -{ - - gTxMessage[bus].StdId = id; - gTxMessage[bus].DLC = length; - for (int i = 0; i < length; i++) - { - gTxMessage[bus].Data[i] = data[i]; - } - - uint8_t retVal = (CAN_Transmit(bus == CAN_1 ? CAN1 : CAN3, &gTxMessage[bus]) != 0); - if (retVal == CAN_TxStatus_NoMailBox) - { - return ERROR; - } - return SUCCESS; +ErrorStatus BSP_CAN_Write(CAN_t bus, uint32_t id, uint8_t data[8], + uint8_t length) { + gTxMessage[bus].StdId = id; + gTxMessage[bus].DLC = length; + for (int i = 0; i < length; i++) { + gTxMessage[bus].Data[i] = data[i]; + } + + uint8_t retVal = + (CAN_Transmit(bus == CAN_1 ? CAN1 : CAN3, &gTxMessage[bus]) != 0); + if (retVal == CAN_TxStatus_NoMailBox) { + return ERROR; + } + return SUCCESS; } /** @@ -352,128 +369,113 @@ ErrorStatus BSP_CAN_Write(CAN_t bus, uint32_t id, uint8_t data[8], uint8_t lengt * @note Non-blocking statement * @pre The data parameter must be at least 8 bytes or hardfault may occur. * @param id : pointer to store id of the message that was received. - * @param data : pointer to store data that was received. Must be 8bytes or bigger. - * @return ERROR if nothing was received so ignore id and data that was received. SUCCESS indicates data was received and stored. + * @param data : pointer to store data that was received. Must be 8bytes or + * bigger. + * @return ERROR if nothing was received so ignore id and data that was + * received. SUCCESS indicates data was received and stored. */ -ErrorStatus BSP_CAN_Read(CAN_t bus, uint32_t *id, uint8_t *data) -{ - // If the queue is empty, return err - if (msg_queue_is_empty(&gRxQueue[bus])) - { - return ERROR; - } - - // Get the message - msg_t msg; - msg_queue_get(&gRxQueue[bus], &msg); - - // Transfer the message to the provided pointers - for (int i = 0; i < 8; i++) - { - data[i] = msg.data[i]; - } - *id = msg.id; - - return SUCCESS; +ErrorStatus BSP_CAN_Read(CAN_t bus, uint32_t* id, uint8_t* data) { + // If the queue is empty, return err + if (msg_queue_is_empty(&gRxQueue[bus])) { + return ERROR; + } + + // Get the message + msg_t msg; + msg_queue_get(&gRxQueue[bus], &msg); + + // Transfer the message to the provided pointers + for (int i = 0; i < 8; i++) { + data[i] = msg.data[i]; + } + *id = msg.id; + + return SUCCESS; } -void CAN3_RX0_IRQHandler() -{ - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); - - // Take any pending messages into a queue - while (CAN_MessagePending(CAN3, CAN_FIFO0)) - { - CAN_Receive(CAN3, CAN_FIFO0, &gRxMessage[1]); - - msg_t rxMsg; - rxMsg.id = gRxMessage[1].StdId; - memcpy(&rxMsg.data[0], gRxMessage[1].Data, 8); - - // Place the message in the queue - if (msg_queue_put(&gRxQueue[1], rxMsg)) - { - // If the queue was not already full... - // Call the driver-provided function, if it is not null - if (gRxEvent[1] != NULL) - { - gRxEvent[1](); - } - } - else - { - // If the queue is already full, then we can't really do anything else - break; - } +void CAN3_RX0_IRQHandler() { + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); + + // Take any pending messages into a queue + while (CAN_MessagePending(CAN3, CAN_FIFO0)) { + CAN_Receive(CAN3, CAN_FIFO0, &gRxMessage[1]); + + msg_t rxMsg; + rxMsg.id = gRxMessage[1].StdId; + memcpy(&rxMsg.data[0], gRxMessage[1].Data, 8); + + // Place the message in the queue + if (msg_queue_put(&gRxQueue[1], rxMsg)) { + // If the queue was not already full... + // Call the driver-provided function, if it is not null + if (gRxEvent[1] != NULL) { + gRxEvent[1](); + } + } else { + // If the queue is already full, then we can't really do anything else + break; } + } - OSIntExit(); // Signal to uC/OS + OSIntExit(); // Signal to uC/OS } -void CAN1_RX0_IRQHandler(void) -{ - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); - - // Take any pending messages into a queue - while (CAN_MessagePending(CAN1, CAN_FIFO0)) - { - CAN_Receive(CAN1, CAN_FIFO0, &gRxMessage[0]); - - msg_t rxMsg; - rxMsg.id = gRxMessage[0].StdId; - memcpy(&rxMsg.data[0], gRxMessage[0].Data, 8); - - // Place the message in the queue - if (msg_queue_put(&gRxQueue[0], rxMsg)) - { - // If the queue was not already full... - // Call the driver-provided function, if it is not null - if (gRxEvent[0] != NULL) - { - gRxEvent[0](); - } - } - else - { - // If the queue is already full, then we can't really do anything else - break; - } +void CAN1_RX0_IRQHandler(void) { + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); + + // Take any pending messages into a queue + while (CAN_MessagePending(CAN1, CAN_FIFO0)) { + CAN_Receive(CAN1, CAN_FIFO0, &gRxMessage[0]); + + msg_t rxMsg; + rxMsg.id = gRxMessage[0].StdId; + memcpy(&rxMsg.data[0], gRxMessage[0].Data, 8); + + // Place the message in the queue + if (msg_queue_put(&gRxQueue[0], rxMsg)) { + // If the queue was not already full... + // Call the driver-provided function, if it is not null + if (gRxEvent[0] != NULL) { + gRxEvent[0](); + } + } else { + // If the queue is already full, then we can't really do anything else + break; } + } - OSIntExit(); // Signal to uC/OS + OSIntExit(); // Signal to uC/OS } -void CAN3_TX_IRQHandler(void) -{ - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); +void CAN3_TX_IRQHandler(void) { + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); - // Acknowledge - CAN_ClearFlag(CAN3, CAN_FLAG_RQCP0 | CAN_FLAG_RQCP1 | CAN_FLAG_RQCP2); + // Acknowledge + CAN_ClearFlag(CAN3, CAN_FLAG_RQCP0 | CAN_FLAG_RQCP1 | CAN_FLAG_RQCP2); - // Call the function provided - gTxEnd[1](); + // Call the function provided + gTxEnd[1](); - OSIntExit(); // Signal to uC/OS + OSIntExit(); // Signal to uC/OS } -void CAN1_TX_IRQHandler(void) -{ - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); - // Call the function provided - CAN_ClearFlag(CAN1, CAN_FLAG_RQCP0 | CAN_FLAG_RQCP1 | CAN_FLAG_RQCP2); - gTxEnd[0](); - - OSIntExit(); // Signal to uC/OS +void CAN1_TX_IRQHandler(void) { + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); + // Call the function provided + CAN_ClearFlag(CAN1, CAN_FLAG_RQCP0 | CAN_FLAG_RQCP1 | CAN_FLAG_RQCP2); + gTxEnd[0](); + + OSIntExit(); // Signal to uC/OS } diff --git a/BSP/STM32F413/Src/BSP_GPIO.c b/BSP/STM32F413/Src/BSP_GPIO.c old mode 100755 new mode 100644 index ffd20a800..5b495f466 --- a/BSP/STM32F413/Src/BSP_GPIO.c +++ b/BSP/STM32F413/Src/BSP_GPIO.c @@ -1,110 +1,103 @@ /* Copyright (c) 2021 UT Longhorn Racing Solar */ #include "BSP_GPIO.h" -#include "Tasks.h" -static GPIO_TypeDef* GPIO_GetPort(port_t port){ - const GPIO_TypeDef* gpio_mapping[4] = {GPIOA, GPIOB, GPIOC, GPIOD}; +#include "Tasks.h" - return (GPIO_TypeDef *) gpio_mapping[port]; +static GPIO_TypeDef *GPIO_GetPort(port_t port) { + const GPIO_TypeDef *gpio_mapping[4] = {GPIOA, GPIOB, GPIOC, GPIOD}; + return (GPIO_TypeDef *)gpio_mapping[port]; } - /** * @brief Initializes a GPIO port * @param port - port to initialize * @param mask - pins - * @param direction - input or output + * @param direction - input or output * @return None - */ - -void BSP_GPIO_Init(port_t port, uint16_t mask, direction_t direction){ - GPIO_InitTypeDef GPIO_InitStruct; - - RCC_AHB1PeriphClockCmd(1 << port, ENABLE); - // Configure the pins to be generic GPIO - GPIO_InitStruct.GPIO_Pin = mask; - GPIO_InitStruct.GPIO_Mode = direction ? GPIO_Mode_OUT : GPIO_Mode_IN; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_InitStruct.GPIO_Speed = GPIO_Low_Speed; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; // TODO: verify - - // Compute the offset for the port handle from the port passed in - GPIO_TypeDef *portHandle = GPIO_GetPort(port); - - // Initialize the GPIO - GPIO_Init(portHandle, &GPIO_InitStruct); -} + */ + +void BSP_GPIO_Init(port_t port, uint16_t mask, direction_t direction) { + GPIO_InitTypeDef GPIO_InitStruct; + + RCC_AHB1PeriphClockCmd(1 << port, ENABLE); + // Configure the pins to be generic GPIO + GPIO_InitStruct.GPIO_Pin = mask; + GPIO_InitStruct.GPIO_Mode = direction ? GPIO_Mode_OUT : GPIO_Mode_IN; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_Speed = GPIO_Low_Speed; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; // TODO: verify + + // Compute the offset for the port handle from the port passed in + GPIO_TypeDef *portHandle = GPIO_GetPort(port); + // Initialize the GPIO + GPIO_Init(portHandle, &GPIO_InitStruct); +} /** * @brief Reads value of the specified port * @param port to read * @return data of the port - */ + */ -uint16_t BSP_GPIO_Read(port_t port){ - GPIO_TypeDef *gpio_port = GPIO_GetPort(port); +uint16_t BSP_GPIO_Read(port_t port) { + GPIO_TypeDef *gpio_port = GPIO_GetPort(port); - return GPIO_ReadInputData(gpio_port); + return GPIO_ReadInputData(gpio_port); } - /** - * @brief Writes data to a specified port + * @brief Writes data to a specified port * @param port port to write to - * @param data data to write + * @param data data to write * @return None - */ + */ -void BSP_GPIO_Write(port_t port, uint16_t data){ - GPIO_TypeDef *gpio_port = GPIO_GetPort(port); +void BSP_GPIO_Write(port_t port, uint16_t data) { + GPIO_TypeDef *gpio_port = GPIO_GetPort(port); - GPIO_Write(gpio_port, data); + GPIO_Write(gpio_port, data); } - /** - * @brief Reads data from a specified input pin (not applicable to output pins) + * @brief Reads data from a specified input pin (not applicable to output + * pins) * @param port The port to read from - * @param pin The pin to read from + * @param pin The pin to read from * @return State of the pin - */ + */ -uint8_t BSP_GPIO_Read_Pin(port_t port, uint16_t pinmask){ - GPIO_TypeDef *gpio_port = GPIO_GetPort(port); +uint8_t BSP_GPIO_Read_Pin(port_t port, uint16_t pinmask) { + GPIO_TypeDef *gpio_port = GPIO_GetPort(port); - return GPIO_ReadInputDataBit(gpio_port, pinmask); + return GPIO_ReadInputDataBit(gpio_port, pinmask); } - - /** * @brief Writes data to a specified pin * @param port The port to write to - * @param pin The pin to write to + * @param pin The pin to write to * @param state true=ON or false=OFF * @return None - */ + */ -void BSP_GPIO_Write_Pin(port_t port, uint16_t pinmask, bool state){ - GPIO_TypeDef *gpio_port = GPIO_GetPort(port); - GPIO_WriteBit(gpio_port, pinmask, (state==ON)?Bit_SET:Bit_RESET); +void BSP_GPIO_Write_Pin(port_t port, uint16_t pinmask, bool state) { + GPIO_TypeDef *gpio_port = GPIO_GetPort(port); + GPIO_WriteBit(gpio_port, pinmask, (state == ON) ? Bit_SET : Bit_RESET); } - - /** * @brief Returns state of output pin (not applicable to input pins) * @param port The port to get state from * @param pin The pin to get state from * @return 1 if pin is high, 0 if low - */ + */ -uint8_t BSP_GPIO_Get_State(port_t port, uint16_t pin){ - GPIO_TypeDef *gpio_port = GPIO_GetPort(port); +uint8_t BSP_GPIO_Get_State(port_t port, uint16_t pin) { + GPIO_TypeDef *gpio_port = GPIO_GetPort(port); - return GPIO_ReadOutputDataBit(gpio_port, pin); + return GPIO_ReadOutputDataBit(gpio_port, pin); } diff --git a/BSP/STM32F413/Src/BSP_SPI.c b/BSP/STM32F413/Src/BSP_SPI.c index 32e8fd4c1..dbc20dafa 100644 --- a/BSP/STM32F413/Src/BSP_SPI.c +++ b/BSP/STM32F413/Src/BSP_SPI.c @@ -1,8 +1,9 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ #include "BSP_SPI.h" -#include "stm32f4xx.h" + #include "os.h" +#include "stm32f4xx.h" #define SPI_PORT SPI1 @@ -28,27 +29,29 @@ static rxfifo_t spiRxFifo; static OS_SEM SPI_Update_Sem4; static void spi_post(void) { - OS_ERR err; - OSSemPost(&SPI_Update_Sem4, OS_OPT_POST_1, &err); - // TODO: error handling + OS_ERR err; + OSSemPost(&SPI_Update_Sem4, OS_OPT_POST_1, &err); + // TODO: error handling } static void spi_pend(void) { - OS_ERR err; - CPU_TS ts; - OSSemPend(&SPI_Update_Sem4, 0, OS_OPT_PEND_BLOCKING, &ts, &err); - // TODO: error handling + OS_ERR err; + CPU_TS ts; + OSSemPend(&SPI_Update_Sem4, 0, OS_OPT_PEND_BLOCKING, &ts, &err); + // TODO: error handling } // Use this inline function to wait until SPI communication is complete static inline void SPI_Wait(void) { - while((SPI1->SR & (SPI_SR_TXE | SPI_SR_RXNE)) == 0 || (SPI1->SR & SPI_SR_BSY)); + while ((SPI1->SR & (SPI_SR_TXE | SPI_SR_RXNE)) == 0 || + (SPI1->SR & SPI_SR_BSY)) + ; } // Use this inline function to wait until SPI communication is complete static inline void SPI_WaitTx(void) { - SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_TXE, ENABLE); - spi_pend(); + SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_TXE, ENABLE); + spi_pend(); } /** SPI_WriteRead @@ -56,92 +59,87 @@ static inline void SPI_WaitTx(void) { * @param txData single byte that will be sent to the device. * @return rxData single byte that was read from the device. */ -static uint8_t SPI_WriteRead(uint8_t txData){ - - SPI_TypeDef *bus = SPI_PORT; - - SPI_Wait(); - bus->DR = txData & 0x00FF; - SPI_Wait(); - return bus->DR & 0x00FF; -} +static uint8_t SPI_WriteRead(uint8_t txData) { + SPI_TypeDef *bus = SPI_PORT; + SPI_Wait(); + bus->DR = txData & 0x00FF; + SPI_Wait(); + return bus->DR & 0x00FF; +} /** * @brief Initializes the SPI port. * @return None */ void BSP_SPI_Init(void) { - - OS_ERR err; - OSSemCreate(&SPI_Update_Sem4, "SPI Update", 0, &err); - - spiTxFifo = txfifo_new(); - spiRxFifo = rxfifo_new(); - - GPIO_InitTypeDef GPIO_InitStruct; - SPI_InitTypeDef SPI_InitStruct; - - - - // SPI configuration: - // speed : 125kbps - // CPOL : 1 (polarity of clock during idle is high) - // CPHA : 1 (tx recorded during 2nd edge) - // Pins: - // SPI1: - // PA5 : SCK - // PA6 : MISO - // PA7 : MOSI - // PA4 : CS - - // Initialize clocks - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); - - // Initialize pins - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_SPI1); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_SPI1); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_SPI1); - - // Initialize SPI port - SPI_InitStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex; - SPI_InitStruct.SPI_Mode = SPI_Mode_Master; - SPI_InitStruct.SPI_DataSize = SPI_DataSize_8b; - SPI_InitStruct.SPI_CPOL = SPI_CPOL_High; - SPI_InitStruct.SPI_CPHA = SPI_CPHA_2Edge; - SPI_InitStruct.SPI_NSS = SPI_NSS_Soft; - SPI_InitStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256; - SPI_InitStruct.SPI_FirstBit = SPI_FirstBit_MSB; - SPI_InitStruct.SPI_CRCPolynomial = 0; - SPI_Init(SPI1, &SPI_InitStruct); - SPI_Cmd(SPI1, ENABLE); - - // Initialize CS pin - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_Init(GPIOA, &GPIO_InitStruct); - - // Configure SPI1 interrupt priority - NVIC_InitTypeDef NVIC_InitStruct; - NVIC_InitStruct.NVIC_IRQChannel = SPI1_IRQn; - NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1; - NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStruct); - - // Enable the Rx buffer not empty interrupt - SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, ENABLE); + OS_ERR err; + OSSemCreate(&SPI_Update_Sem4, "SPI Update", 0, &err); + + spiTxFifo = txfifo_new(); + spiRxFifo = rxfifo_new(); + + GPIO_InitTypeDef GPIO_InitStruct; + SPI_InitTypeDef SPI_InitStruct; + + // SPI configuration: + // speed : 125kbps + // CPOL : 1 (polarity of clock during idle is high) + // CPHA : 1 (tx recorded during 2nd edge) + // Pins: + // SPI1: + // PA5 : SCK + // PA6 : MISO + // PA7 : MOSI + // PA4 : CS + + // Initialize clocks + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + + // Initialize pins + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_SPI1); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_SPI1); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_SPI1); + + // Initialize SPI port + SPI_InitStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + SPI_InitStruct.SPI_Mode = SPI_Mode_Master; + SPI_InitStruct.SPI_DataSize = SPI_DataSize_8b; + SPI_InitStruct.SPI_CPOL = SPI_CPOL_High; + SPI_InitStruct.SPI_CPHA = SPI_CPHA_2Edge; + SPI_InitStruct.SPI_NSS = SPI_NSS_Soft; + SPI_InitStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256; + SPI_InitStruct.SPI_FirstBit = SPI_FirstBit_MSB; + SPI_InitStruct.SPI_CRCPolynomial = 0; + SPI_Init(SPI1, &SPI_InitStruct); + SPI_Cmd(SPI1, ENABLE); + + // Initialize CS pin + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + // Configure SPI1 interrupt priority + NVIC_InitTypeDef NVIC_InitStruct; + NVIC_InitStruct.NVIC_IRQChannel = SPI1_IRQn; + NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStruct); + + // Enable the Rx buffer not empty interrupt + SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, ENABLE); } /** @@ -153,27 +151,27 @@ void BSP_SPI_Init(void) { * Do not call from an ISR */ void BSP_SPI_Write(uint8_t *txBuf, uint8_t txLen) { - // If we're below an experimentally-determined value, just use polling - if(txLen < 8) { - for(int i = 0; i < txLen; i++) { - SPI_WriteRead(txBuf[i]); - } - } else { - // If we have a lot of data, we use interrupts to mitigate it - // Fill as much of the fifo as possible - size_t i = 0; - while(i < txLen) { - // Put as much data into the fifo as can fit - while(i < txLen && txfifo_put(&spiTxFifo, txBuf[i])) { - i++; - } - - // Wait for the transmission to complete - SPI_WaitTx(); - } - - SPI_Wait(); - } + // If we're below an experimentally-determined value, just use polling + if (txLen < 8) { + for (int i = 0; i < txLen; i++) { + SPI_WriteRead(txBuf[i]); + } + } else { + // If we have a lot of data, we use interrupts to mitigate it + // Fill as much of the fifo as possible + size_t i = 0; + while (i < txLen) { + // Put as much data into the fifo as can fit + while (i < txLen && txfifo_put(&spiTxFifo, txBuf[i])) { + i++; + } + + // Wait for the transmission to complete + SPI_WaitTx(); + } + + SPI_Wait(); + } } /** @@ -185,67 +183,65 @@ void BSP_SPI_Write(uint8_t *txBuf, uint8_t txLen) { * Do not call from an ISR */ void BSP_SPI_Read(uint8_t *rxBuf, uint8_t rxLen) { - - // If we have only a little amount of data, just use polling - if(rxLen < 8) { - for(int i = 0; i < rxLen; i++) { - rxBuf[i] = SPI_WriteRead(0x00); - } - } else { - SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, ENABLE); - // Fill the fifo with zeros to read - size_t i = 0, r = 0; - // Empty the fifo - rxfifo_renew(&spiRxFifo); - // Read the data - while(i < rxLen) { - // Keep filling the fifo with data until we have read everything - while(i < rxLen && txfifo_put(&spiTxFifo, 0)) { - i++; - } - - // Wait for the transmission to complete - SPI_WaitTx(); - - // Busy wait the last bit, just to ensure all bytes have been received - SPI_Wait(); - - // Copy the data out of the fifo - while(r < i && rxfifo_get(&spiRxFifo, &rxBuf[r])) { - r++; - } - } - SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, DISABLE); - } + // If we have only a little amount of data, just use polling + if (rxLen < 8) { + for (int i = 0; i < rxLen; i++) { + rxBuf[i] = SPI_WriteRead(0x00); + } + } else { + SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, ENABLE); + // Fill the fifo with zeros to read + size_t i = 0, r = 0; + // Empty the fifo + rxfifo_renew(&spiRxFifo); + // Read the data + while (i < rxLen) { + // Keep filling the fifo with data until we have read everything + while (i < rxLen && txfifo_put(&spiTxFifo, 0)) { + i++; + } + + // Wait for the transmission to complete + SPI_WaitTx(); + + // Busy wait the last bit, just to ensure all bytes have been received + SPI_Wait(); + + // Copy the data out of the fifo + while (r < i && rxfifo_get(&spiRxFifo, &rxBuf[r])) { + r++; + } + } + SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_RXNE, DISABLE); + } } - -void SPI1_IRQHandler(){ - // Save the CPU registers - CPU_SR_ALLOC(); - - // Protect a critical section - CPU_CRITICAL_ENTER(); - - // make the kernel aware that the interrupt has started - OSIntEnter(); - CPU_CRITICAL_EXIT(); - - // Handle the interrupts - if (SPI_I2S_GetITStatus(SPI1, SPI_I2S_IT_TXE) == SET){ - // Check to see if there is any data awaiting transmission - if(!txfifo_get(&spiTxFifo, (uint8_t*)&SPI1->DR)) { - // We are out of data, so turn off the interrupt and post the semaphore - SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_TXE, DISABLE); - spi_post(); - } - } - if (SPI_I2S_GetITStatus(SPI1, SPI_I2S_IT_RXNE) == SET){ - // Get the incoming data, put it in the fifo - // If this overflows, it's the user's fault. - rxfifo_put(&spiRxFifo, SPI1->DR); - } - - //make the kernel aware that the interrupt has ended - OSIntExit(); +void SPI1_IRQHandler() { + // Save the CPU registers + CPU_SR_ALLOC(); + + // Protect a critical section + CPU_CRITICAL_ENTER(); + + // make the kernel aware that the interrupt has started + OSIntEnter(); + CPU_CRITICAL_EXIT(); + + // Handle the interrupts + if (SPI_I2S_GetITStatus(SPI1, SPI_I2S_IT_TXE) == SET) { + // Check to see if there is any data awaiting transmission + if (!txfifo_get(&spiTxFifo, (uint8_t *)&SPI1->DR)) { + // We are out of data, so turn off the interrupt and post the semaphore + SPI_I2S_ITConfig(SPI1, SPI_I2S_IT_TXE, DISABLE); + spi_post(); + } + } + if (SPI_I2S_GetITStatus(SPI1, SPI_I2S_IT_RXNE) == SET) { + // Get the incoming data, put it in the fifo + // If this overflows, it's the user's fault. + rxfifo_put(&spiRxFifo, SPI1->DR); + } + + // make the kernel aware that the interrupt has ended + OSIntExit(); } diff --git a/BSP/STM32F413/Src/BSP_UART.c b/BSP/STM32F413/Src/BSP_UART.c index f63cf8f19..9e0d37912 100644 --- a/BSP/STM32F413/Src/BSP_UART.c +++ b/BSP/STM32F413/Src/BSP_UART.c @@ -1,11 +1,12 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ #include "BSP_UART.h" -#include "stm32f4xx.h" + #include "os.h" +#include "stm32f4xx.h" -#define TX_SIZE 128 -#define RX_SIZE 64 +#define TX_SIZE 128 +#define RX_SIZE 64 // Initialize the FIFOs @@ -31,164 +32,168 @@ static callback_t usbTxCallback = NULL; static callback_t displayRxCallback = NULL; static callback_t displayTxCallback = NULL; -static rxfifo_t *rx_fifos[NUM_UART] = {&usbRxFifo, &displayRxFifo}; -static txfifo_t *tx_fifos[NUM_UART] = {&usbTxFifo, &displayTxFifo}; -static bool *lineRecvd[NUM_UART] = {&usbLineReceived, &displayLineReceived}; +static rxfifo_t *rx_fifos[NUM_UART] = {&usbRxFifo, &displayRxFifo}; +static txfifo_t *tx_fifos[NUM_UART] = {&usbTxFifo, &displayTxFifo}; +static bool *lineRecvd[NUM_UART] = {&usbLineReceived, &displayLineReceived}; static USART_TypeDef *handles[NUM_UART] = {USART2, USART3}; static void USART_DISPLAY_Init() { - displayTxFifo = txfifo_new(); - displayRxFifo = rxfifo_new(); - - GPIO_InitTypeDef GPIO_InitStruct = {0}; - USART_InitTypeDef UART_InitStruct = {0}; - - // Initialize clocks - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - - // Initialize pins - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz; - GPIO_Init(GPIOB, &GPIO_InitStruct); - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5; - GPIO_Init(GPIOC, &GPIO_InitStruct); - - GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3); - GPIO_PinAFConfig(GPIOC, GPIO_PinSource5, GPIO_AF_USART3); - - //Initialize UART3 - UART_InitStruct.USART_BaudRate = 115200; - UART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; - UART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; - UART_InitStruct.USART_Parity = USART_Parity_No; - UART_InitStruct.USART_StopBits = USART_StopBits_1; - UART_InitStruct.USART_WordLength = USART_WordLength_8b; - USART_Init(USART3, &UART_InitStruct); - - // Enable interrupts - - USART_Cmd(USART3, ENABLE); - - // Enable NVIC - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + displayTxFifo = txfifo_new(); + displayRxFifo = rxfifo_new(); + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + USART_InitTypeDef UART_InitStruct = {0}; + + // Initialize clocks + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); + + // Initialize pins + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz; + GPIO_Init(GPIOB, &GPIO_InitStruct); + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_5; + GPIO_Init(GPIOC, &GPIO_InitStruct); + + GPIO_PinAFConfig(GPIOB, GPIO_PinSource10, GPIO_AF_USART3); + GPIO_PinAFConfig(GPIOC, GPIO_PinSource5, GPIO_AF_USART3); + + // Initialize UART3 + UART_InitStruct.USART_BaudRate = 115200; + UART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + UART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; + UART_InitStruct.USART_Parity = USART_Parity_No; + UART_InitStruct.USART_StopBits = USART_StopBits_1; + UART_InitStruct.USART_WordLength = USART_WordLength_8b; + USART_Init(USART3, &UART_InitStruct); + + // Enable interrupts + + USART_Cmd(USART3, ENABLE); + + // Enable NVIC + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); } static void USART_USB_Init() { - usbTxFifo = txfifo_new(); - usbRxFifo = rxfifo_new(); - - GPIO_InitTypeDef GPIO_InitStruct = {0}; - USART_InitTypeDef UART_InitStruct = {0}; - - // Initialize clocks - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - - // Initialize pins - GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; - GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz; - GPIO_Init(GPIOA, &GPIO_InitStruct); - - GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_USART2); - GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_USART2); - - //Initialize UART2 - UART_InitStruct.USART_BaudRate = 115200; - UART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; - UART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; - UART_InitStruct.USART_Parity = USART_Parity_No; - UART_InitStruct.USART_StopBits = USART_StopBits_1; - UART_InitStruct.USART_WordLength = USART_WordLength_8b; - USART_Init(USART2, &UART_InitStruct); - - USART_Cmd(USART2, ENABLE); - - // Enable NVIC - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - - setvbuf(stdout, NULL, _IONBF, 0); + usbTxFifo = txfifo_new(); + usbRxFifo = rxfifo_new(); + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + USART_InitTypeDef UART_InitStruct = {0}; + + // Initialize clocks + RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + + // Initialize pins + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_USART2); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_USART2); + + // Initialize UART2 + UART_InitStruct.USART_BaudRate = 115200; + UART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + UART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; + UART_InitStruct.USART_Parity = USART_Parity_No; + UART_InitStruct.USART_StopBits = USART_StopBits_1; + UART_InitStruct.USART_WordLength = USART_WordLength_8b; + USART_Init(USART2, &UART_InitStruct); + + USART_Cmd(USART2, ENABLE); + + // Enable NVIC + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + setvbuf(stdout, NULL, _IONBF, 0); } /** * @brief Initializes the UART peripheral */ -static void BSP_UART_Init_Internal(callback_t rxCallback, callback_t txCallback, UART_t uart) { - switch(uart){ - case UART_2: // their UART_USB - USART_USB_Init(); - usbRxCallback = rxCallback; - usbTxCallback = txCallback; - break; - case UART_3: // their UART_DISPLAY - USART_DISPLAY_Init(); - displayRxCallback = rxCallback; - displayTxCallback = txCallback; - break; +static void BSP_UART_Init_Internal(callback_t rxCallback, callback_t txCallback, + UART_t uart) { + switch (uart) { + case UART_2: // their UART_USB + USART_USB_Init(); + usbRxCallback = rxCallback; + usbTxCallback = txCallback; + break; + case UART_3: // their UART_DISPLAY + USART_DISPLAY_Init(); + displayRxCallback = rxCallback; + displayTxCallback = txCallback; + break; default: - // Error - break; - } + // Error + break; + } } void BSP_UART_Init(UART_t uart) { - // Not using callbacks for now - BSP_UART_Init_Internal(NULL, NULL, uart); + // Not using callbacks for now + BSP_UART_Init_Internal(NULL, NULL, uart); } /** - * @brief Gets one line of ASCII text that was received. The '\n' and '\r' characters will not be stored (tested on Putty on Windows) + * @brief Gets one line of ASCII text that was received. The '\n' and '\r' + * characters will not be stored (tested on Putty on Windows) * @pre str should be at least 128bytes long. - * @param str : pointer to buffer to store the string. This buffer should be initialized - * before hand. + * @param str : pointer to buffer to store the string. This buffer should be + * initialized before hand. * @param usart : which usart to read from (2 or 3) * @return number of bytes that was read */ uint32_t BSP_UART_Read(UART_t usart, char *str) { - char data = 0; - uint32_t recvd = 0; + char data = 0; + uint32_t recvd = 0; - bool *line_recvd = lineRecvd[usart]; - - while(*line_recvd == false){ - BSP_UART_Write(usart, "", 0); // needs to be in. Otherwise, usbLineRecieved will not update - } + bool *line_recvd = lineRecvd[usart]; + + while (*line_recvd == false) { + BSP_UART_Write( + usart, "", + 0); // needs to be in. Otherwise, usbLineRecieved will not update + } + + USART_TypeDef *usart_handle = handles[usart]; + + USART_ITConfig(usart_handle, USART_IT_RXNE, RESET); - USART_TypeDef *usart_handle = handles[usart]; + rxfifo_t *fifo = rx_fifos[usart]; - USART_ITConfig(usart_handle, USART_IT_RXNE, RESET); - - rxfifo_t *fifo = rx_fifos[usart]; - + rxfifo_peek(fifo, &data); + while (!rxfifo_is_empty(fifo) && data != '\r') { + recvd += rxfifo_get(fifo, (char *)str++); rxfifo_peek(fifo, &data); - while(!rxfifo_is_empty(fifo) && data != '\r') { - recvd += rxfifo_get(fifo, (char*)str++); - rxfifo_peek(fifo, &data); - } + } - rxfifo_get(fifo, &data); - *str = 0; - *line_recvd = false; - USART_ITConfig(usart_handle, USART_IT_RXNE, SET); + rxfifo_get(fifo, &data); + *str = 0; + *line_recvd = false; + USART_ITConfig(usart_handle, USART_IT_RXNE, SET); - return recvd; + return recvd; } /** @@ -197,127 +202,127 @@ uint32_t BSP_UART_Read(UART_t usart, char *str) { * @param len : size of buffer * @param usart : which usart to read from (2 or 3) * @return number of bytes that were sent - * + * * @note This function uses a fifo to buffer the write. If that * fifo is full, this function may block while waiting for * space to open up. Do not call from timing-critical * sections of code. */ uint32_t BSP_UART_Write(UART_t usart, char *str, uint32_t len) { - uint32_t sent = 0; + uint32_t sent = 0; - USART_TypeDef *usart_handle = handles[usart]; + USART_TypeDef *usart_handle = handles[usart]; - USART_ITConfig(usart_handle, USART_IT_TC, RESET); + USART_ITConfig(usart_handle, USART_IT_TC, RESET); - txfifo_t *fifo = tx_fifos[usart]; + txfifo_t *fifo = tx_fifos[usart]; - while(sent < len) { - if(!txfifo_put(fifo, str[sent])) { - // Allow the interrupt to fire - USART_ITConfig(usart_handle, USART_IT_TC, SET); - // Wait for space to open up - while(txfifo_is_full(fifo)); - // Disable the interrupt again - USART_ITConfig(usart_handle, USART_IT_TC, RESET); - } else { - sent++; - } + while (sent < len) { + if (!txfifo_put(fifo, str[sent])) { + // Allow the interrupt to fire + USART_ITConfig(usart_handle, USART_IT_TC, SET); + // Wait for space to open up + while (txfifo_is_full(fifo)) + ; + // Disable the interrupt again + USART_ITConfig(usart_handle, USART_IT_TC, RESET); + } else { + sent++; } + } - USART_ITConfig(usart_handle, USART_IT_TC, SET); + USART_ITConfig(usart_handle, USART_IT_TC, SET); - return sent; + return sent; } void USART2_IRQHandler(void) { - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); - - if(USART_GetFlagStatus(USART2, USART_FLAG_RXNE) != RESET) { - uint8_t data = USART2->DR; - bool removeSuccess = 1; - if(data == '\r' || data == '\n'){ - usbLineReceived = true; - if(usbRxCallback != NULL) - usbRxCallback(); - } - // Check if it was a backspace. - // '\b' for minicmom - // '\177' for putty - else if(data != '\b' && data != '\177') { - rxfifo_put(&usbRxFifo, data); - } - // Sweet, just a "regular" key. Put it into the fifo - // Doesn't matter if it fails. If it fails, then the data gets thrown away - // and the easiest solution for this is to increase RX_SIZE - else { - char junk; - // Delete the last entry! - removeSuccess = rxfifo_popback(&usbRxFifo, &junk); - - USART_SendData(UART_2, 0x7F); // TODO: Not sure if the backspace works. Need to test - } - if(removeSuccess) { - USART2->DR = data; - } + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); + + if (USART_GetFlagStatus(USART2, USART_FLAG_RXNE) != RESET) { + uint8_t data = USART2->DR; + bool removeSuccess = 1; + if (data == '\r' || data == '\n') { + usbLineReceived = true; + if (usbRxCallback != NULL) usbRxCallback(); } - if(USART_GetITStatus(USART2, USART_IT_TC) != RESET) { - // If getting data from fifo fails i.e. the tx fifo is empty, then turn off the TX interrupt - if(!txfifo_get(&usbTxFifo, (char*)&(USART2->DR))) { - USART_ITConfig(USART2, USART_IT_TC, RESET); // Turn off the interrupt - if(usbTxCallback != NULL) - usbTxCallback(); // Callback - } + // Check if it was a backspace. + // '\b' for minicmom + // '\177' for putty + else if (data != '\b' && data != '\177') { + rxfifo_put(&usbRxFifo, data); } - if(USART_GetITStatus(USART2, USART_IT_ORE) != RESET); - - - OSIntExit(); + // Sweet, just a "regular" key. Put it into the fifo + // Doesn't matter if it fails. If it fails, then the data gets thrown away + // and the easiest solution for this is to increase RX_SIZE + else { + char junk; + // Delete the last entry! + removeSuccess = rxfifo_popback(&usbRxFifo, &junk); + + USART_SendData( + UART_2, 0x7F); // TODO: Not sure if the backspace works. Need to test + } + if (removeSuccess) { + USART2->DR = data; + } + } + if (USART_GetITStatus(USART2, USART_IT_TC) != RESET) { + // If getting data from fifo fails i.e. the tx fifo is empty, then turn off + // the TX interrupt + if (!txfifo_get(&usbTxFifo, (char *)&(USART2->DR))) { + USART_ITConfig(USART2, USART_IT_TC, RESET); // Turn off the interrupt + if (usbTxCallback != NULL) usbTxCallback(); // Callback + } + } + if (USART_GetITStatus(USART2, USART_IT_ORE) != RESET) + ; + OSIntExit(); } void USART3_IRQHandler(void) { - CPU_SR_ALLOC(); - CPU_CRITICAL_ENTER(); - OSIntEnter(); - CPU_CRITICAL_EXIT(); - - if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) { - uint8_t data = USART3->DR; - bool removeSuccess = 1; - if(data == '\r'){ - displayLineReceived = true; - if(displayRxCallback != NULL) - displayRxCallback(); - } - // Check if it was a backspace. - // '\b' for minicmom - // '\177' for putty - if(data != '\b' && data != '\177') rxfifo_put(&displayRxFifo, data); - // Sweet, just a "regular" key. Put it into the fifo - // Doesn't matter if it fails. If it fails, then the data gets thrown away - // and the easiest solution for this is to increase RX_SIZE - else { - char junk; - // Delete the last entry! - removeSuccess = rxfifo_popback(&displayRxFifo, &junk); - } - if(removeSuccess) { - USART3->DR = data; - } + CPU_SR_ALLOC(); + CPU_CRITICAL_ENTER(); + OSIntEnter(); + CPU_CRITICAL_EXIT(); + + if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) { + uint8_t data = USART3->DR; + bool removeSuccess = 1; + if (data == '\r') { + displayLineReceived = true; + if (displayRxCallback != NULL) displayRxCallback(); + } + // Check if it was a backspace. + // '\b' for minicmom + // '\177' for putty + if (data != '\b' && data != '\177') rxfifo_put(&displayRxFifo, data); + // Sweet, just a "regular" key. Put it into the fifo + // Doesn't matter if it fails. If it fails, then the data gets thrown away + // and the easiest solution for this is to increase RX_SIZE + else { + char junk; + // Delete the last entry! + removeSuccess = rxfifo_popback(&displayRxFifo, &junk); + } + if (removeSuccess) { + USART3->DR = data; } - if(USART_GetITStatus(USART3, USART_IT_TC) != RESET) { - // If getting data from fifo fails i.e. the tx fifo is empty, then turn off the TX interrupt - if(!txfifo_get(&displayTxFifo, (char*)&(USART3->DR))) { - USART_ITConfig(USART3, USART_IT_TC, RESET); - if(displayTxCallback != NULL) - displayTxCallback(); - } + } + if (USART_GetITStatus(USART3, USART_IT_TC) != RESET) { + // If getting data from fifo fails i.e. the tx fifo is empty, then turn off + // the TX interrupt + if (!txfifo_get(&displayTxFifo, (char *)&(USART3->DR))) { + USART_ITConfig(USART3, USART_IT_TC, RESET); + if (displayTxCallback != NULL) displayTxCallback(); } - if(USART_GetITStatus(USART3, USART_IT_ORE) != RESET); + } + if (USART_GetITStatus(USART3, USART_IT_ORE) != RESET) + ; - OSIntExit(); + OSIntExit(); } diff --git a/BSP/STM32F413/Src/retarget.c b/BSP/STM32F413/Src/retarget.c index 305ffb46b..3bea3fe2c 100644 --- a/BSP/STM32F413/Src/retarget.c +++ b/BSP/STM32F413/Src/retarget.c @@ -1,35 +1,25 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ -#include "common.h" #include "BSP_UART.h" +#include "common.h" -#define STDIN_FILENO 0 +#define STDIN_FILENO 0 #define STDOUT_FILENO 1 #define STDERR_FILENO 2 int _write(int fd, char *buffer, unsigned int len) { - if(buffer != NULL) { - BSP_UART_Write(UART_2, buffer, len); - } - return len; + if (buffer != NULL) { + BSP_UART_Write(UART_2, buffer, len); + } + return len; } int _read(int const fd, char *buffer, unsigned const len) { - if(buffer != NULL) { - - } - return 1; -} - -int _close(int file) -{ - return -1; + if (buffer != NULL) { + } + return 1; } -int _lseek(int file, int ptr, int dir) -{ - return 0; -} - - +int _close(int file) { return -1; } +int _lseek(int file, int ptr, int dir) { return 0; } diff --git a/Drivers/Inc/CANConfig.h b/Drivers/Inc/CANConfig.h index ad743993b..38f628995 100644 --- a/Drivers/Inc/CANConfig.h +++ b/Drivers/Inc/CANConfig.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CANConfig.h - * @brief - * + * @brief + * * @defgroup CANConfig * @addtogroup CANConfig * @{ @@ -13,18 +13,17 @@ /** * Filter Lists for CarCAN and MotorCAN -*/ + */ #define NUM_CARCAN_FILTERS 4 #define NUM_MOTORCAN_FILTERS 0 -extern CANId_t carCANFilterList[NUM_CARCAN_FILTERS]; -extern CANId_t motorCANFilterList[NUM_MOTORCAN_FILTERS]; - +extern CANId_t carCANFilterList[NUM_CARCAN_FILTERS]; +extern CANId_t motorCANFilterList[NUM_MOTORCAN_FILTERS]; /** - * The lookup table containing the entries for all of our CAN messages. Located in CANLUT.c + * The lookup table containing the entries for all of our CAN messages. Located + * in CANLUT.c */ extern const CANLUT_T CANLUT[NUM_CAN_IDS]; #endif - /* @} */ diff --git a/Drivers/Inc/CANbus.h b/Drivers/Inc/CANbus.h index 33e8fc0ee..d4224b64c 100644 --- a/Drivers/Inc/CANbus.h +++ b/Drivers/Inc/CANbus.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CANbus.h - * @brief - * + * @brief + * * @defgroup CANbus * @addtogroup CANbus * @{ @@ -13,66 +13,69 @@ #include "BSP_CAN.h" -#define CARCAN CAN_1 //convenience aliases for the CANBuses +#define CARCAN CAN_1 // convenience aliases for the CANBuses #define MOTORCAN CAN_3 /** - * This enum is used to signify the ID of the message you want to send. - * It is used internally to index our lookup table (CANLUT.C) and get message-specific fields. - * For user purposes, it selects the message to send. - * - * If changing the order of this enum, make sure to mirror that change in the lookup table, or - * else the driver will not work properly. - * - * If adding new types of CAN messages, add the identifier wherever it fits in - * (the enum is sorted in ascending order on purpose), and then add an entry to the lookup table. + * This enum is used to signify the ID of the message you want to send. + * It is used internally to index our lookup table (CANLUT.C) and get + * message-specific fields. For user purposes, it selects the message to send. + * + * If changing the order of this enum, make sure to mirror that change in the + * lookup table, or else the driver will not work properly. + * + * If adding new types of CAN messages, add the identifier wherever it fits in + * (the enum is sorted in ascending order on purpose), and then add an entry to + * the lookup table. */ -typedef enum { - BPS_TRIP = 0x002, - BPS_ALL_CLEAR = 0x101, - BPS_CONTACTOR_STATE = 0x102, - STATE_OF_CHARGE = 0x106, - WDOG_TRIGGERED = 0x107, - SUPPLEMENTAL_VOLTAGE = 0x10B, - CHARGE_ENABLE = 0x10C, - MOTOR_DRIVE = 0x221, - MOTOR_POWER = 0x222, - MOTOR_RESET = 0x223, - MOTOR_STATUS = 0x241, - MC_BUS = 0x242, - VELOCITY = 0x243, - MC_PHASE_CURRENT = 0x244, - VOLTAGE_VEC = 0x245, - CURRENT_VEC = 0x246, - BACKEMF = 0x247, - TEMPERATURE = 0x24B, - ODOMETER_AMPHOURS = 0x24E, - ARRAY_CONTACTOR_STATE_CHANGE = 0x24F, - CARDATA = 0x581, - NUM_CAN_IDS +typedef enum { + BPS_TRIP = 0x002, + BPS_ALL_CLEAR = 0x101, + BPS_CONTACTOR_STATE = 0x102, + STATE_OF_CHARGE = 0x106, + WDOG_TRIGGERED = 0x107, + SUPPLEMENTAL_VOLTAGE = 0x10B, + CHARGE_ENABLE = 0x10C, + MOTOR_DRIVE = 0x221, + MOTOR_POWER = 0x222, + MOTOR_RESET = 0x223, + MOTOR_STATUS = 0x241, + MC_BUS = 0x242, + VELOCITY = 0x243, + MC_PHASE_CURRENT = 0x244, + VOLTAGE_VEC = 0x245, + CURRENT_VEC = 0x246, + BACKEMF = 0x247, + TEMPERATURE = 0x24B, + ODOMETER_AMPHOURS = 0x24E, + ARRAY_CONTACTOR_STATE_CHANGE = 0x24F, + CARDATA = 0x581, + NUM_CAN_IDS } CANId_t; /** * @brief Struct to use in CAN MSG LUT * @param idxEn Whether or not this message is part of a sequence of messages. - * @param size Size of message's data. Should be a maximum of eight (in decimal). + * @param size Size of message's data. Should be a maximum of eight (in + * decimal). */ typedef struct { - bool idxEn: 1; - unsigned int size: 7; + bool idxEn : 1; + unsigned int size : 7; } CANLUT_T; /** * Standard CAN packet * @param ID CANId_t value indicating which message we are trying to send - * @param idx If message is part of a sequence of messages (for messages longer than 64 bits), this indicates the index of the message. - * This is not designed to exceed the 8bit unsigned max value. + * @param idx If message is part of a sequence of messages (for messages + * longer than 64 bits), this indicates the index of the message. This is not + * designed to exceed the 8bit unsigned max value. * @param data data of the message -*/ + */ typedef struct { - CANId_t ID; - uint8_t idx; - uint8_t data[8]; + CANId_t ID; + uint8_t idx; + uint8_t data[8]; } CANDATA_t; /** @@ -81,39 +84,46 @@ typedef struct { */ // typedef enum {CAN_BLOCKING=0, CAN_NON_BLOCKING} CAN_blocking_t; -//Compatibility macros for deprecated enum +// Compatibility macros for deprecated enum #define CAN_BLOCKING true #define CAN_NON_BLOCKING false - /** * @brief Initializes the CAN system for a given bus - * @param bus The bus to initialize. You can either use CAN_1, CAN_3, or the convenience macros CARCAN and MOTORCAN. CAN2 will not be supported. - * @param idWhitelist A list of CAN IDs that we want to receive. If NULL, we will receive all messages. + * @param bus The bus to initialize. You can either use CAN_1, CAN_3, or the + * convenience macros CARCAN and MOTORCAN. CAN2 will not be supported. + * @param idWhitelist A list of CAN IDs that we want to receive. If NULL, we + * will receive all messages. * @param idWhitelistSize The size of the whitelist. * @return ERROR if bus != CAN1 or CAN3, SUCCESS otherwise */ -ErrorStatus CANbus_Init(CAN_t bus, CANId_t* idWhitelist, uint8_t idWhitelistSize); +ErrorStatus CANbus_Init(CAN_t bus, CANId_t* idWhitelist, + uint8_t idWhitelistSize); /** - * @brief Transmits data onto the CANbus. Transmits up to 8 bytes at a time. If more is necessary, please use an IDX message. + * @brief Transmits data onto the CANbus. Transmits up to 8 bytes at a time. + * If more is necessary, please use an IDX message. * @param CanData The data to be transmitted - * @param blocking Whether or not this transmission should be a blocking send. - * @param bus The bus to transmit on. This should be either CARCAN or MOTORCAN. + * @param blocking Whether or not this transmission should be a + * blocking send. + * @param bus The bus to transmit on. This should be either + * CARCAN or MOTORCAN. * @return ERROR if data wasn't sent, otherwise it was sent. */ -ErrorStatus CANbus_Send(CANDATA_t CanData,bool blocking, CAN_t bus); +ErrorStatus CANbus_Send(CANDATA_t CanData, bool blocking, CAN_t bus); /** - * @brief Reads a CAN message from the CAN hardware and returns it to the provided pointers. - * @param data pointer to where to store the CAN id of the recieved msg + * @brief Reads a CAN message from the CAN hardware and returns it to the + * provided pointers. + * @param data pointer to where to store the CAN id of the recieved + * msg * @param blocking Whether or not this read should be a blocking read - * @param bus The bus to use. This should either be CARCAN or MOTORCAN. + * @param bus The bus to use. This should either be CARCAN or + * MOTORCAN. * @returns ERROR if read failed, SUCCESS otherwise */ ErrorStatus CANbus_Read(CANDATA_t* data, bool blocking, CAN_t bus); #endif - /* @} */ diff --git a/Drivers/Inc/Contactors.h b/Drivers/Inc/Contactors.h index e460b2d44..1264cbb13 100644 --- a/Drivers/Inc/Contactors.h +++ b/Drivers/Inc/Contactors.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Contactors.h - * @brief - * + * @brief + * * @defgroup Contactors * @addtogroup Contactors * @{ @@ -11,42 +11,38 @@ #ifndef __CONTACTORS_H #define __CONTACTORS_H +#include "BSP_GPIO.h" #include "common.h" #include "config.h" -#include "BSP_GPIO.h" #include "stm32f4xx_gpio.h" -#define CONTACTORS_PORT PORTC +#define CONTACTORS_PORT PORTC #define ARRAY_PRECHARGE_PIN GPIO_Pin_10 #define ARRAY_CONTACTOR_PIN GPIO_Pin_11 #define MOTOR_CONTACTOR_PIN GPIO_Pin_12 -#define FOREACH_contactor(contactor) \ - contactor(ARRAY_CONTACTOR) \ - contactor(ARRAY_PRECHARGE) \ - contactor(MOTOR_CONTACTOR) \ +#define FOREACH_contactor(contactor) \ + contactor(ARRAY_CONTACTOR) contactor(ARRAY_PRECHARGE) \ + contactor(MOTOR_CONTACTOR) typedef enum contactor_ENUM { - FOREACH_contactor(GENERATE_ENUM) - NUM_CONTACTORS, -}contactor_t; - - + FOREACH_contactor(GENERATE_ENUM) NUM_CONTACTORS, +} contactor_t; /** * @brief Initializes contactors to be used * in connection with the Motor and Array * @return None - */ + */ void Contactors_Init(); /** - * @brief Returns the current state of + * @brief Returns the current state of * a specified contactor * @param contactor the contactor * (MOTOR_PRECHARGE/ARRAY_PRECHARGE/ARRAY_CONTACTOR) * @return The contactor's state (ON/OFF) - */ + */ bool Contactors_Get(contactor_t contactor); /** @@ -60,5 +56,4 @@ ErrorStatus Contactors_Set(contactor_t contactor, bool state, bool blocking); #endif - /* @} */ diff --git a/Drivers/Inc/Display.h b/Drivers/Inc/Display.h index 20b016ab8..4e5edabee 100644 --- a/Drivers/Inc/Display.h +++ b/Drivers/Inc/Display.h @@ -2,11 +2,11 @@ * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Display.h * @brief Function prototypes for the display driver. - * + * * This contains function prototypes relevant to sending/receiving messages * to/from our Nextion HMI. Call assertDisplayError after calling any of the * functions in this application. - * + * * @defgroup Display * @addtogroup Display * @{ @@ -15,63 +15,60 @@ #ifndef __DISPLAY_H #define __DISPLAY_H -#include "common.h" // common headers -#include "Tasks.h" // for os and fault error locs +#include "Tasks.h" // for os and fault error locs +#include "common.h" // common headers -#define MAX_ARGS 2 // maximum # of arguments in a command packet +#define MAX_ARGS 2 // maximum # of arguments in a command packet /** * Error types */ -typedef enum{ - DISPLAY_ERR_NONE = 0, - DISPLAY_ERR_PARSE =-1, // Error parsing command struct passed to Display_Send - DISPLAY_ERR_INV_INSTR =-2, // Invalid instruction passed to nextion (0x00) - DISPLAY_ERR_INV_COMP =-3, // Invalid component id passed to nextion (0x02) - DISPLAY_ERR_INV_PGID =-4, // Invalid page id passed to nextion (0x03) - DISPLAY_ERR_INV_VAR =-5, // Invalid variable name passed to nextion (0x1A) - DISPLAY_ERR_INV_VAROP =-6, // Invalid variable operation passed to nextion (0x1B) - DISPLAY_ERR_ASSIGN =-7, // Assignment failure nextion (0x1C) - DISPLAY_ERR_PARAMS =-8, // Invalid number of parameters passed to nextion (0x1E) - DISPLAY_ERR_MAX_ARGS =-9, // Command arg list exceeded MAX_ARGS elements - DISPLAY_ERR_OTHER =-10 // Other nextion display error +typedef enum { + DISPLAY_ERR_NONE = 0, + DISPLAY_ERR_PARSE = + -1, // Error parsing command struct passed to Display_Send + DISPLAY_ERR_INV_INSTR = -2, // Invalid instruction passed to nextion (0x00) + DISPLAY_ERR_INV_COMP = -3, // Invalid component id passed to nextion (0x02) + DISPLAY_ERR_INV_PGID = -4, // Invalid page id passed to nextion (0x03) + DISPLAY_ERR_INV_VAR = -5, // Invalid variable name passed to nextion (0x1A) + DISPLAY_ERR_INV_VAROP = + -6, // Invalid variable operation passed to nextion (0x1B) + DISPLAY_ERR_ASSIGN = -7, // Assignment failure nextion (0x1C) + DISPLAY_ERR_PARAMS = + -8, // Invalid number of parameters passed to nextion (0x1E) + DISPLAY_ERR_MAX_ARGS = -9, // Command arg list exceeded MAX_ARGS elements + DISPLAY_ERR_OTHER = -10 // Other nextion display error } DisplayError_t; /** - * @brief Error handler for any display errors. Call this after any display driver function. + * @brief Error handler for any display errors. Call this after any display + * driver function. */ void assertDisplayError(DisplayError_t err); /** - * All three pages on the HMI + * All three pages on the HMI */ -typedef enum{ - STARTUP =0, - INFO, - FAULT -} Page_t; +typedef enum { STARTUP = 0, INFO, FAULT } Page_t; /** * Argument types */ -typedef enum{ - STR_ARG, - INT_ARG -} Arg_e; +typedef enum { STR_ARG, INT_ARG } Arg_e; /** * Packages relevant display command data */ -typedef struct{ - char* compOrCmd; - char* attr; - char* op; - uint8_t numArgs; - Arg_e argTypes[MAX_ARGS]; // TRUE for integers, FALSE for strings - union{ - char* str; - uint32_t num; - } args[MAX_ARGS]; +typedef struct { + char* compOrCmd; + char* attr; + char* op; + uint8_t numArgs; + Arg_e argTypes[MAX_ARGS]; // TRUE for integers, FALSE for strings + union { + char* str; + uint32_t num; + } args[MAX_ARGS]; } DisplayCmd_t; /** @@ -93,22 +90,23 @@ DisplayError_t Display_Init(void); DisplayError_t Display_Reset(void); /** - * @brief Overwrites any processing commands and triggers the display fault screen + * @brief Overwrites any processing commands and triggers the display fault + * screen * @param osErrCode the os error location (will be displayed in hex) * @param faultCode the generic fault code (will be displayed in hex) * @returns DisplayError_t */ -DisplayError_t Display_Fault(os_error_loc_t osErrCode, fault_bitmap_t faultCode); +DisplayError_t Display_Fault(os_error_loc_t osErrCode, + fault_bitmap_t faultCode); /** * @brief Overwrites any processing commands and triggers the evacuation screen * @param SOC_percent the state of charge of the battery in percent * @param supp_mv the voltage of the battery in millivolts * @returns DisplayError_t -*/ + */ DisplayError_t Display_Evac(uint8_t SOC_percent, uint32_t supp_mv); #endif - /* @} */ diff --git a/Drivers/Inc/GPIOExpander.h b/Drivers/Inc/GPIOExpander.h index 5e8644c85..405a08851 100644 --- a/Drivers/Inc/GPIOExpander.h +++ b/Drivers/Inc/GPIOExpander.h @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file GPIOExpander.h - * @brief - * + * @brief + * * @defgroup GPIOExpander * @addtogroup GPIOExpander * @{ @@ -16,38 +16,37 @@ #include "common.h" typedef struct _spi_message { - int8_t opcode; - int8_t requestedPort; - int8_t data; + int8_t opcode; + int8_t requestedPort; + int8_t data; } spi_message; - // Opcodes -#define SPI_OPCODE_R 0x41 -#define SPI_OPCODE_W 0x40 +#define SPI_OPCODE_R 0x41 +#define SPI_OPCODE_W 0x40 // Register Addresses (BANK = 0) -#define SPI_IODIRA 0x00 -#define SPI_IOPOLA 0x02 -#define SPI_GPINTENA 0x04 -#define SPI_DEFVALA 0x06 -#define SPI_INTCONA 0x08 -#define SPI_IOCONA 0x0A -#define SPI_GPPUA 0x0C -#define SPI_INTFA 0x0E -#define SPI_INTCAPA 0x10 -#define SPI_GPIOA 0x12 - -#define SPI_IODIRB 0x01 -#define SPI_IOPOLB 0x03 -#define SPI_GPINTENB 0x05 -#define SPI_DEFVALB 0x07 -#define SPI_INTCONB 0x09 -#define SPI_GPPUAB 0x0D -#define SPI_INTFAB 0x0F -#define SPI_INTCAPB 0x11 -#define SPI_GPIOB 0x13 -#define SPI_OLATB 0x15 +#define SPI_IODIRA 0x00 +#define SPI_IOPOLA 0x02 +#define SPI_GPINTENA 0x04 +#define SPI_DEFVALA 0x06 +#define SPI_INTCONA 0x08 +#define SPI_IOCONA 0x0A +#define SPI_GPPUA 0x0C +#define SPI_INTFA 0x0E +#define SPI_INTCAPA 0x10 +#define SPI_GPIOA 0x12 + +#define SPI_IODIRB 0x01 +#define SPI_IOPOLB 0x03 +#define SPI_GPINTENB 0x05 +#define SPI_DEFVALB 0x07 +#define SPI_INTCONB 0x09 +#define SPI_GPPUAB 0x0D +#define SPI_INTFAB 0x0F +#define SPI_INTCAPB 0x11 +#define SPI_GPIOB 0x13 +#define SPI_OLATB 0x15 #endif diff --git a/Drivers/Inc/Minions.h b/Drivers/Inc/Minions.h index 52d628f05..534c16889 100644 --- a/Drivers/Inc/Minions.h +++ b/Drivers/Inc/Minions.h @@ -2,10 +2,10 @@ /** * @defgroup Minions - * - * This modules allows us to use GPIO more easily + * + * This modules allows us to use GPIO more easily * for our application's purposes - * + * * @defgroup Minions * @addtogroup Minions * @{ @@ -13,42 +13,42 @@ #ifndef MINIONS_H #define MINIONS_H -#include "common.h" #include + #include "BSP_GPIO.h" +#include "common.h" /* Should keep in line with the LUT in Minions.c */ #define FOREACH_PIN(PIN) \ - PIN(IGN_1) \ - PIN(IGN_2) \ - PIN(REGEN_SW) \ - PIN(FOR_SW) \ - PIN(REV_SW) \ - PIN(CRUZ_EN) \ - PIN(CRUZ_ST) \ - PIN(BRAKELIGHT) \ + PIN(IGN_1) \ + PIN(IGN_2) \ + PIN(REGEN_SW) \ + PIN(FOR_SW) \ + PIN(REV_SW) \ + PIN(CRUZ_EN) \ + PIN(CRUZ_ST) \ + PIN(BRAKELIGHT) typedef enum MINIONPIN_ENUM { - FOREACH_PIN(GENERATE_ENUM) - NUM_PINS, + FOREACH_PIN(GENERATE_ENUM) NUM_PINS, } pin_t; typedef struct { - uint16_t pinMask; - port_t port; - direction_t direction; + uint16_t pinMask; + port_t port; + direction_t direction; } pinInfo_t; /** * @brief Initializes digital I/O - * + * */ void Minions_Init(void); /** * @brief Reads the status of a pin - * - * @param pin + * + * @param pin * @return true is high * @return false is low */ @@ -56,14 +56,14 @@ bool Minions_Read(pin_t pin); /** * @brief Updates the status of a pin - * - * @param pin - * @param status + * + * @param pin + * @param status * @return true is fail (wrote to an input) * @return false is success (wrote to an output) */ bool Minions_Write(pin_t pin, bool status); -#endif +#endif /* @} */ diff --git a/Drivers/Inc/Pedals.h b/Drivers/Inc/Pedals.h index 365180e10..8dc3dbc0b 100644 --- a/Drivers/Inc/Pedals.h +++ b/Drivers/Inc/Pedals.h @@ -2,7 +2,7 @@ * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Pedals.h * @brief Header file for the Pedals driver - * + * * @defgroup Pedals * @addtogroup Pedals * @{ @@ -15,31 +15,25 @@ /** * @brief Stuff - * + * */ -typedef enum -{ - ACCELERATOR, - BRAKE, - NUMBER_OF_PEDALS -} pedal_t; +typedef enum { ACCELERATOR, BRAKE, NUMBER_OF_PEDALS } pedal_t; /** * @brief Initialize the pedals * @param None * @return None - */ + */ void Pedals_Init(void); /** - * @brief Provides the pedal distance pressed in percetage (accelerator or brake) + * @brief Provides the pedal distance pressed in percetage (accelerator or + * brake) * @param pedal_t pedal, ACCELERATOR or BRAKE as defined in enum * @return distance the pedal has been pressed in percentage - */ + */ int8_t Pedals_Read(pedal_t pedal); - #endif - /* @} */ diff --git a/Drivers/Src/CANConfig.c b/Drivers/Src/CANConfig.c index 97bde91fe..fa5f4eab8 100644 --- a/Drivers/Src/CANConfig.c +++ b/Drivers/Src/CANConfig.c @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CANConfig.c - * @brief - * + * @brief + * */ #include "CANConfig.h" @@ -14,40 +14,44 @@ #define IDX true /** - * @brief Lookup table to simplify user-defined packet structs. Contains metadata fields that are always the same for every message of a given ID. - * Indexed by CANId_t values. Any changes or additions must be made in parallel with changes made to the CANID_t enum in CANbus.h + * @brief Lookup table to simplify user-defined packet structs. Contains + * metadata fields that are always the same for every message of a given ID. + * Indexed by CANId_t values. Any changes or additions must be made in + * parallel with changes made to the CANID_t enum in CANbus.h */ const CANLUT_T CANLUT[NUM_CAN_IDS] = { - [BPS_TRIP] = {NOIDX, DOUBLE}, /** BPS_TRIP **/ - [BPS_ALL_CLEAR] = {NOIDX, DOUBLE}, /** BPS_ALL_CLEAR **/ - [BPS_CONTACTOR_STATE] = {NOIDX, DOUBLE}, /** BPS_CONTACTOR_STATE **/ - [CHARGE_ENABLE] = {NOIDX, DOUBLE}, /** CHARGE_ENABLE **/ - [STATE_OF_CHARGE] = {NOIDX, DOUBLE}, /** STATE_OF_CHARGE **/ - [SUPPLEMENTAL_VOLTAGE] = {NOIDX, DOUBLE}, /** SUPPLEMENTAL_VOLTAGE **/ - [MC_BUS] = {NOIDX, DOUBLE}, /** MC_BUS **/ - [VELOCITY] = {NOIDX, DOUBLE}, /** VELOCITY **/ - [MC_PHASE_CURRENT] = {NOIDX, DOUBLE}, /** MC_PHASE_CURRENT **/ - [VOLTAGE_VEC] = {NOIDX, DOUBLE}, /** VOLTAGE_VEC **/ - [CURRENT_VEC] = {NOIDX, DOUBLE}, /** CURRENT_VEC **/ - [BACKEMF] = {NOIDX, DOUBLE}, /** BACKEMF **/ - [TEMPERATURE] = {NOIDX, DOUBLE}, /** TEMPERATURE **/ - [ODOMETER_AMPHOURS] = {NOIDX, DOUBLE}, /** ODOMETER_AMPHOURS **/ - [ARRAY_CONTACTOR_STATE_CHANGE] = {NOIDX, BYTE }, /** ARRAY_CONTACTOR_STATE_CHANGE **/ - [MOTOR_DRIVE] = {NOIDX, DOUBLE}, /** MOTOR_DRIVE **/ - [MOTOR_POWER] = {NOIDX, DOUBLE}, /** MOTOR_POWER **/ - [MOTOR_RESET] = {NOIDX, DOUBLE}, /** MOTOR_RESET **/ - [MOTOR_STATUS] = {NOIDX, DOUBLE}, /** MOTOR_STATUS **/ - [CARDATA] = {NOIDX, DOUBLE}, /** CARDAT **/ + [BPS_TRIP] = + {NOIDX, + DOUBLE}, /** BPS_TRIP **/ + [BPS_ALL_CLEAR] = {NOIDX, + DOUBLE}, /** BPS_ALL_CLEAR **/ + [BPS_CONTACTOR_STATE] = + {NOIDX, DOUBLE}, /** BPS_CONTACTOR_STATE **/ + [CHARGE_ENABLE] = {NOIDX, DOUBLE}, /** CHARGE_ENABLE **/ + [STATE_OF_CHARGE] = {NOIDX, DOUBLE}, /** STATE_OF_CHARGE **/ + [SUPPLEMENTAL_VOLTAGE] = {NOIDX, DOUBLE}, /** SUPPLEMENTAL_VOLTAGE **/ + [MC_BUS] = {NOIDX, DOUBLE}, /** MC_BUS **/ + [VELOCITY] = {NOIDX, DOUBLE}, /** VELOCITY **/ + [MC_PHASE_CURRENT] = {NOIDX, DOUBLE}, /** MC_PHASE_CURRENT **/ + [VOLTAGE_VEC] = {NOIDX, DOUBLE}, /** VOLTAGE_VEC **/ + [CURRENT_VEC] = {NOIDX, DOUBLE}, /** CURRENT_VEC **/ + [BACKEMF] = {NOIDX, DOUBLE}, /** BACKEMF **/ + [TEMPERATURE] = {NOIDX, DOUBLE}, /** TEMPERATURE **/ + [ODOMETER_AMPHOURS] = {NOIDX, DOUBLE}, /** ODOMETER_AMPHOURS **/ + [ARRAY_CONTACTOR_STATE_CHANGE] = + {NOIDX, BYTE}, /** ARRAY_CONTACTOR_STATE_CHANGE **/ + [MOTOR_DRIVE] = {NOIDX, DOUBLE}, /** MOTOR_DRIVE **/ + [MOTOR_POWER] = {NOIDX, DOUBLE}, /** MOTOR_POWER **/ + [MOTOR_RESET] = {NOIDX, DOUBLE}, /** MOTOR_RESET **/ + [MOTOR_STATUS] = {NOIDX, DOUBLE}, /** MOTOR_STATUS **/ + [CARDATA] = {NOIDX, DOUBLE}, /** CARDAT **/ }; /** - * @brief Lists of CAN IDs that we want to receive. Used to initialize the CAN filters for CarCAN and MotorCAN. -*/ + * @brief Lists of CAN IDs that we want to receive. Used to initialize the CAN + * filters for CarCAN and MotorCAN. + */ CANId_t carCANFilterList[NUM_CARCAN_FILTERS] = { - BPS_TRIP, - STATE_OF_CHARGE, - SUPPLEMENTAL_VOLTAGE, - CHARGE_ENABLE -}; + BPS_TRIP, STATE_OF_CHARGE, SUPPLEMENTAL_VOLTAGE, CHARGE_ENABLE}; CANId_t motorCANFilterList[NUM_MOTORCAN_FILTERS] = {}; diff --git a/Drivers/Src/CANbus.c b/Drivers/Src/CANbus.c old mode 100755 new mode 100644 index c199747c0..178f4f30a --- a/Drivers/Src/CANbus.c +++ b/Drivers/Src/CANbus.c @@ -1,260 +1,236 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file CANbus.c - * @brief - * + * @brief + * */ #include "CANbus.h" + +#include "CANConfig.h" +#include "Tasks.h" #include "config.h" #include "os.h" -#include "Tasks.h" -#include "CANConfig.h" - -static OS_SEM CANMail_Sem4[NUM_CAN]; // sem4 to count how many sending hardware mailboxes we have left (start at 3) -static OS_SEM CANBus_RecieveSem4[NUM_CAN]; // sem4 to count how many msgs in our recieving queue -static OS_MUTEX CANbus_TxMutex[NUM_CAN]; // mutex to lock tx line -static OS_MUTEX CANbus_RxMutex[NUM_CAN]; // mutex to lock Rx line - +static OS_SEM CANMail_Sem4[NUM_CAN]; // sem4 to count how many sending hardware + // mailboxes we have left (start at 3) +static OS_SEM CANBus_RecieveSem4[NUM_CAN]; // sem4 to count how many msgs in + // our recieving queue +static OS_MUTEX CANbus_TxMutex[NUM_CAN]; // mutex to lock tx line +static OS_MUTEX CANbus_RxMutex[NUM_CAN]; // mutex to lock Rx line /** - * @brief this function will be passed down to the BSP layer to trigger on RX events. Increments the recieve semaphore to signal message in hardware mailbox. Do not access directly outside this driver. + * @brief this function will be passed down to the BSP layer to trigger on RX + * events. Increments the recieve semaphore to signal message in hardware + * mailbox. Do not access directly outside this driver. * @param bus The CAN bus to operate on. Should be CARCAN or MOTORCAN. */ -void CANbus_RxHandler(CAN_t bus) -{ - OS_ERR err; - OSSemPost(&(CANBus_RecieveSem4[bus]), OS_OPT_POST_1, &err); // increment our queue counter - assertOSError(OS_CANDRIVER_LOC,err); +void CANbus_RxHandler(CAN_t bus) { + OS_ERR err; + OSSemPost(&(CANBus_RecieveSem4[bus]), OS_OPT_POST_1, + &err); // increment our queue counter + assertOSError(OS_CANDRIVER_LOC, err); } /** - * @brief this function will be passed down to the BSP layer to trigger on TXend. Releases hold of the mailbox semaphore (Increments it to show mailbox available). Do not access directly outside this driver. + * @brief this function will be passed down to the BSP layer to trigger on + * TXend. Releases hold of the mailbox semaphore (Increments it to show mailbox + * available). Do not access directly outside this driver. * @param bus The CAN bus to operate on. Should be CARCAN or MOTORCAN. */ -void CANbus_TxHandler(CAN_t bus) -{ - OS_ERR err; - OSSemPost(&(CANMail_Sem4[bus]), OS_OPT_POST_1, &err); - assertOSError(OS_CANDRIVER_LOC,err); +void CANbus_TxHandler(CAN_t bus) { + OS_ERR err; + OSSemPost(&(CANMail_Sem4[bus]), OS_OPT_POST_1, &err); + assertOSError(OS_CANDRIVER_LOC, err); } -//wrapper functions for the interrupt customized for each bus -void CANbus_TxHandler_1(){ - CANbus_TxHandler(CAN_1); -} +// wrapper functions for the interrupt customized for each bus +void CANbus_TxHandler_1() { CANbus_TxHandler(CAN_1); } -void CANbus_RxHandler_1(){ - CANbus_RxHandler(CAN_1); -} -void CANbus_TxHandler_3(){ - CANbus_TxHandler(CAN_3); -} -void CANbus_RxHandler_3(){ - CANbus_RxHandler(CAN_3); -} +void CANbus_RxHandler_1() { CANbus_RxHandler(CAN_1); } +void CANbus_TxHandler_3() { CANbus_TxHandler(CAN_3); } +void CANbus_RxHandler_3() { CANbus_RxHandler(CAN_3); } /** * @brief Checks each CAN ID. If an ID is not in CANLUT, set that ID to NULL * @param wlist The whitelist containing the IDs to be checked * @param size The size of the whitelist of IDs - * @return Returns the whitelist to be -*/ -static CANId_t* whitelist_validator(CANId_t* wlist, uint8_t size){ - for(int i = 0; i < size; i++){ - CANId_t curr = wlist[i]; - if(curr >= NUM_CAN_IDS) { - wlist[i] = 0; - } else if (CANLUT[curr].size == 0 ) { - wlist[i] = 0; - } + * @return Returns the whitelist to be + */ +static CANId_t* whitelist_validator(CANId_t* wlist, uint8_t size) { + for (int i = 0; i < size; i++) { + CANId_t curr = wlist[i]; + if (curr >= NUM_CAN_IDS) { + wlist[i] = 0; + } else if (CANLUT[curr].size == 0) { + wlist[i] = 0; } - return wlist; + } + return wlist; } -ErrorStatus CANbus_Init(CAN_t bus, CANId_t* idWhitelist, uint8_t idWhitelistSize) -{ - // initialize CAN mailbox semaphore to 3 for the 3 CAN mailboxes that we have - // initialize tx - OS_ERR err; - idWhitelist = whitelist_validator(idWhitelist, idWhitelistSize); - if(bus==CAN_1){ - BSP_CAN_Init(bus,&CANbus_RxHandler_1,&CANbus_TxHandler_1, (uint16_t*)idWhitelist, idWhitelistSize); - } else if (bus==CAN_3){ - BSP_CAN_Init(bus,&CANbus_RxHandler_3,&CANbus_TxHandler_3, (uint16_t*)idWhitelist, idWhitelistSize); - } else { - return ERROR; - } - - OSMutexCreate(&(CANbus_TxMutex[bus]), (bus == CAN_1 ? "CAN TX Lock 1":"CAN TX Lock 3"), &err); - assertOSError(OS_CANDRIVER_LOC,err); - - OSMutexCreate(&(CANbus_RxMutex[bus]), (bus == CAN_1 ? "CAN RX Lock 1":"CAN RX Lock 3"), &err); - assertOSError(OS_CANDRIVER_LOC,err); - - OSSemCreate(&(CANMail_Sem4[bus]), (bus == CAN_1 ? "CAN Mailbox Semaphore 1":"CAN Mailbox Semaphore 3"), 3, &err); // there's 3 hardware mailboxes on the board, so 3 software mailboxes - assertOSError(OS_CANDRIVER_LOC,err); - - OSSemCreate(&(CANBus_RecieveSem4[bus]), (bus == CAN_1 ? "CAN Recieved Msg Queue Ctr 1":"CAN Recieved Msg Queue Ctr 3"), 0, &err); // create a mailbox counter to hold the messages in as they come in - assertOSError(OS_CANDRIVER_LOC,err); - - return SUCCESS; +ErrorStatus CANbus_Init(CAN_t bus, CANId_t* idWhitelist, + uint8_t idWhitelistSize) { + // initialize CAN mailbox semaphore to 3 for the 3 CAN mailboxes that we have + // initialize tx + OS_ERR err; + idWhitelist = whitelist_validator(idWhitelist, idWhitelistSize); + if (bus == CAN_1) { + BSP_CAN_Init(bus, &CANbus_RxHandler_1, &CANbus_TxHandler_1, + (uint16_t*)idWhitelist, idWhitelistSize); + } else if (bus == CAN_3) { + BSP_CAN_Init(bus, &CANbus_RxHandler_3, &CANbus_TxHandler_3, + (uint16_t*)idWhitelist, idWhitelistSize); + } else { + return ERROR; + } + + OSMutexCreate(&(CANbus_TxMutex[bus]), + (bus == CAN_1 ? "CAN TX Lock 1" : "CAN TX Lock 3"), &err); + assertOSError(OS_CANDRIVER_LOC, err); + + OSMutexCreate(&(CANbus_RxMutex[bus]), + (bus == CAN_1 ? "CAN RX Lock 1" : "CAN RX Lock 3"), &err); + assertOSError(OS_CANDRIVER_LOC, err); + + OSSemCreate( + &(CANMail_Sem4[bus]), + (bus == CAN_1 ? "CAN Mailbox Semaphore 1" : "CAN Mailbox Semaphore 3"), 3, + &err); // there's 3 hardware mailboxes on the board, so 3 software + // mailboxes + assertOSError(OS_CANDRIVER_LOC, err); + + OSSemCreate(&(CANBus_RecieveSem4[bus]), + (bus == CAN_1 ? "CAN Recieved Msg Queue Ctr 1" + : "CAN Recieved Msg Queue Ctr 3"), + 0, &err); // create a mailbox counter to hold the messages in as + // they come in + assertOSError(OS_CANDRIVER_LOC, err); + + return SUCCESS; } -ErrorStatus CANbus_Send(CANDATA_t CanData,bool blocking, CAN_t bus) -{ - CPU_TS timestamp; - OS_ERR err; +ErrorStatus CANbus_Send(CANDATA_t CanData, bool blocking, CAN_t bus) { + CPU_TS timestamp; + OS_ERR err; - //error check the id - if(CanData.ID >= NUM_CAN_IDS){return ERROR;} + // error check the id + if (CanData.ID >= NUM_CAN_IDS) { + return ERROR; + } - CANLUT_T msginfo = CANLUT[CanData.ID]; //lookup msg information in table - - if(msginfo.size == 0){return ERROR;} //if they passed in an invalid id, it will be zero + CANLUT_T msginfo = CANLUT[CanData.ID]; // lookup msg information in table + if (msginfo.size == 0) { + return ERROR; + } // if they passed in an invalid id, it will be zero - // make sure that Can mailbox is available - if (blocking == CAN_BLOCKING) - { - OSSemPend( - &(CANMail_Sem4[bus]), - 0, - OS_OPT_PEND_BLOCKING, - ×tamp, - &err); - } - else - { - OSSemPend( - &(CANMail_Sem4[bus]), - 0, - OS_OPT_PEND_NON_BLOCKING, - ×tamp, - &err); - - // don't crash if we are just using this in non-blocking mode and don't block - if(err == OS_ERR_PEND_WOULD_BLOCK){ - return ERROR; - } - } - if (err != OS_ERR_NONE) - { - assertOSError(OS_CANDRIVER_LOC,err); - return ERROR; - } + // make sure that Can mailbox is available + if (blocking == CAN_BLOCKING) { + OSSemPend(&(CANMail_Sem4[bus]), 0, OS_OPT_PEND_BLOCKING, ×tamp, &err); + } else { + OSSemPend(&(CANMail_Sem4[bus]), 0, OS_OPT_PEND_NON_BLOCKING, ×tamp, + &err); - uint8_t txdata[8]; - if(msginfo.idxEn){ //first byte of txData should be the idx value - memcpy(txdata, &CanData.idx, 1); - memcpy(&(txdata[sizeof(CanData.idx)]), &CanData.data, msginfo.size); - } else { //non-idx case - memcpy(txdata, &CanData.data, msginfo.size); + // don't crash if we are just using this in non-blocking mode and don't + // block + if (err == OS_ERR_PEND_WOULD_BLOCK) { + return ERROR; } - - OSMutexPend( // ensure that tx line is available - &(CANbus_TxMutex[bus]), - 0, - OS_OPT_PEND_BLOCKING, - ×tamp, - &err); - assertOSError(OS_CANDRIVER_LOC,err); // couldn't lock tx line - - // tx line locked - ErrorStatus retval = BSP_CAN_Write( - bus, //bus to transmit onto - CanData.ID, //ID from Data struct - txdata, //data we memcpy'd earlier - (msginfo.idxEn ? msginfo.size+sizeof(CanData.idx) : msginfo.size) //if IDX then add one to the msg size, else the msg size - ); - - OSMutexPost( // unlock the TX line - &(CANbus_TxMutex[bus]), - OS_OPT_POST_NONE, - &err); - assertOSError(OS_CANDRIVER_LOC,err); - if(retval == ERROR){ - CANbus_TxHandler(bus); //release the mailbox by posting back to the counter semaphore - } - - return retval; + } + if (err != OS_ERR_NONE) { + assertOSError(OS_CANDRIVER_LOC, err); + return ERROR; + } + + uint8_t txdata[8]; + if (msginfo.idxEn) { // first byte of txData should be the idx value + memcpy(txdata, &CanData.idx, 1); + memcpy(&(txdata[sizeof(CanData.idx)]), &CanData.data, msginfo.size); + } else { // non-idx case + memcpy(txdata, &CanData.data, msginfo.size); + } + + OSMutexPend( // ensure that tx line is available + &(CANbus_TxMutex[bus]), 0, OS_OPT_PEND_BLOCKING, ×tamp, &err); + assertOSError(OS_CANDRIVER_LOC, err); // couldn't lock tx line + + // tx line locked + ErrorStatus retval = BSP_CAN_Write( + bus, // bus to transmit onto + CanData.ID, // ID from Data struct + txdata, // data we memcpy'd earlier + (msginfo.idxEn ? msginfo.size + sizeof(CanData.idx) + : msginfo.size) // if IDX then add one to the msg size, + // else the msg size + ); + + OSMutexPost( // unlock the TX line + &(CANbus_TxMutex[bus]), OS_OPT_POST_NONE, &err); + assertOSError(OS_CANDRIVER_LOC, err); + if (retval == ERROR) { + CANbus_TxHandler( + bus); // release the mailbox by posting back to the counter semaphore + } + + return retval; } -ErrorStatus CANbus_Read(CANDATA_t* MsgContainer, bool blocking, CAN_t bus) -{ - CPU_TS timestamp; - OS_ERR err; - - if (blocking == CAN_BLOCKING) - { - OSSemPend( // check if the queue actually has anything - &(CANBus_RecieveSem4[bus]), - 0, - OS_OPT_PEND_BLOCKING, - ×tamp, - &err); +ErrorStatus CANbus_Read(CANDATA_t* MsgContainer, bool blocking, CAN_t bus) { + CPU_TS timestamp; + OS_ERR err; + + if (blocking == CAN_BLOCKING) { + OSSemPend( // check if the queue actually has anything + &(CANBus_RecieveSem4[bus]), 0, OS_OPT_PEND_BLOCKING, ×tamp, &err); + } else { + OSSemPend(&(CANBus_RecieveSem4[bus]), 0, OS_OPT_PEND_NON_BLOCKING, + ×tamp, &err); + + // don't crash if we are just using this in non-blocking mode and don't + // block + if (err == OS_ERR_PEND_WOULD_BLOCK) { + return ERROR; } - else - { - OSSemPend( - &(CANBus_RecieveSem4[bus]), - 0, - OS_OPT_PEND_NON_BLOCKING, - ×tamp, - &err); - - // don't crash if we are just using this in non-blocking mode and don't block - if(err == OS_ERR_PEND_WOULD_BLOCK){ - return ERROR; - } - } - if (err != OS_ERR_NONE) - { - assertOSError(OS_CANDRIVER_LOC,err); - return ERROR; - } - - OSMutexPend( // ensure that RX line is available - &(CANbus_RxMutex[bus]), - 0, - OS_OPT_PEND_BLOCKING, - ×tamp, - &err); - assertOSError(OS_CANDRIVER_LOC,err); - - // Actually get the message - uint32_t id; - ErrorStatus status = BSP_CAN_Read(bus, &id, MsgContainer->data); - - OSMutexPost( // unlock RX line - &(CANbus_RxMutex[bus]), - OS_OPT_POST_NONE, - &err); - assertOSError(OS_CANDRIVER_LOC,err); - if(status == ERROR){ - return ERROR; - } - - //error check the id - MsgContainer->ID = (CANId_t) id; - if(MsgContainer->ID >= NUM_CAN_IDS){ - MsgContainer = NULL; - return ERROR; - } - CANLUT_T entry = CANLUT[MsgContainer->ID]; //lookup msg information in table - if(entry.size == 0){ - MsgContainer = NULL; - return ERROR; - } //if they passed in an invalid id, it will be zero - - //search LUT for id to populate idx and trim data - if(entry.idxEn==true){ - MsgContainer->idx = MsgContainer->data[0]; - memmove( // Can't use memcpy, as memory regions overlap - MsgContainer->data, - &(MsgContainer->data[1]), - 7 // max size of data (8) - size of idx byte (1) - ); - } - return status; + } + if (err != OS_ERR_NONE) { + assertOSError(OS_CANDRIVER_LOC, err); + return ERROR; + } + + OSMutexPend( // ensure that RX line is available + &(CANbus_RxMutex[bus]), 0, OS_OPT_PEND_BLOCKING, ×tamp, &err); + assertOSError(OS_CANDRIVER_LOC, err); + + // Actually get the message + uint32_t id; + ErrorStatus status = BSP_CAN_Read(bus, &id, MsgContainer->data); + + OSMutexPost( // unlock RX line + &(CANbus_RxMutex[bus]), OS_OPT_POST_NONE, &err); + assertOSError(OS_CANDRIVER_LOC, err); + if (status == ERROR) { + return ERROR; + } + + // error check the id + MsgContainer->ID = (CANId_t)id; + if (MsgContainer->ID >= NUM_CAN_IDS) { + MsgContainer = NULL; + return ERROR; + } + CANLUT_T entry = CANLUT[MsgContainer->ID]; // lookup msg information in table + if (entry.size == 0) { + MsgContainer = NULL; + return ERROR; + } // if they passed in an invalid id, it will be zero + + // search LUT for id to populate idx and trim data + if (entry.idxEn == true) { + MsgContainer->idx = MsgContainer->data[0]; + memmove( // Can't use memcpy, as memory regions overlap + MsgContainer->data, &(MsgContainer->data[1]), + 7 // max size of data (8) - size of idx byte (1) + ); + } + return status; } diff --git a/Drivers/Src/Contactors.c b/Drivers/Src/Contactors.c index 919dbaaa5..590f17659 100644 --- a/Drivers/Src/Contactors.c +++ b/Drivers/Src/Contactors.c @@ -1,86 +1,86 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Contactors.h - * @brief - * + * @brief + * */ #include "Contactors.h" -#include "stm32f4xx_gpio.h" + #include "Tasks.h" +#include "stm32f4xx_gpio.h" static OS_MUTEX contactorsMutex; /** * @brief Helper function for setting contactors without mutex. - * Should only be called if mutex is held and struct contactor has been checked + * Should only be called if mutex is held and struct contactor has been + * checked * @param contactor the contactor * (MOTOR_PRECHARGE/ARRAY_PRECHARGE/ARRAY_CONTACTOR) * @param state the state to set (ON/OFF) * @return None - */ + */ static void setContactor(contactor_t contactor, bool state) { - switch (contactor) { - case ARRAY_CONTACTOR : - BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN, state); - break; - case ARRAY_PRECHARGE : - BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_PRECHARGE_PIN, state); - break; - case MOTOR_CONTACTOR : - BSP_GPIO_Write_Pin(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN, state); - break; - default: - break; - } + switch (contactor) { + case ARRAY_CONTACTOR: + BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN, state); + break; + case ARRAY_PRECHARGE: + BSP_GPIO_Write_Pin(CONTACTORS_PORT, ARRAY_PRECHARGE_PIN, state); + break; + case MOTOR_CONTACTOR: + BSP_GPIO_Write_Pin(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN, state); + break; + default: + break; + } } /** * @brief Initializes contactors to be used * in connection with the Motor and Array * @return None - */ + */ void Contactors_Init() { - BSP_GPIO_Init(CONTACTORS_PORT, - (ARRAY_CONTACTOR_PIN) | - (ARRAY_PRECHARGE_PIN) | - (MOTOR_CONTACTOR_PIN), - 1); + BSP_GPIO_Init( + CONTACTORS_PORT, + (ARRAY_CONTACTOR_PIN) | (ARRAY_PRECHARGE_PIN) | (MOTOR_CONTACTOR_PIN), 1); - // start disabled - for (int contactor = 0; contactor < NUM_CONTACTORS; ++contactor) { - setContactor(contactor, OFF); - } + // start disabled + for (int contactor = 0; contactor < NUM_CONTACTORS; ++contactor) { + setContactor(contactor, OFF); + } - // initialize mutex - OS_ERR err; - OSMutexCreate(&contactorsMutex, "Contactors lock", &err); - assertOSError(OS_CONTACTOR_LOC, err); + // initialize mutex + OS_ERR err; + OSMutexCreate(&contactorsMutex, "Contactors lock", &err); + assertOSError(OS_CONTACTOR_LOC, err); } /** - * @brief Returns the current state of + * @brief Returns the current state of * a specified contactor * @param contactor the contactor * (MOTOR_PRECHARGE/ARRAY_PRECHARGE/ARRAY_CONTACTOR) * @return The contactor's state (ON/OFF) - */ + */ bool Contactors_Get(contactor_t contactor) { - State state = OFF; - switch (contactor) { - case ARRAY_CONTACTOR : - state = BSP_GPIO_Get_State(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN); - break; - case ARRAY_PRECHARGE : - state = BSP_GPIO_Get_State(CONTACTORS_PORT, ARRAY_PRECHARGE_PIN); - break; - case MOTOR_CONTACTOR : - state = BSP_GPIO_Get_State(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN); - break; - default: - break; - } - return (state == ON) ? ON : OFF; + State state = OFF; + switch (contactor) { + case ARRAY_CONTACTOR: + state = BSP_GPIO_Get_State(CONTACTORS_PORT, ARRAY_CONTACTOR_PIN); + break; + case ARRAY_PRECHARGE: + state = BSP_GPIO_Get_State(CONTACTORS_PORT, ARRAY_PRECHARGE_PIN); + break; + case MOTOR_CONTACTOR: + state = BSP_GPIO_Get_State(CONTACTORS_PORT, MOTOR_CONTACTOR_PIN); + break; + default: + break; + } + return (state == ON) ? ON : OFF; } /** @@ -92,26 +92,28 @@ bool Contactors_Get(contactor_t contactor) { * @return Whether or not the contactor was successfully set */ ErrorStatus Contactors_Set(contactor_t contactor, bool state, bool blocking) { - CPU_TS timestamp; - OS_ERR err; - ErrorStatus result = ERROR; + CPU_TS timestamp; + OS_ERR err; + ErrorStatus result = ERROR; + + // acquire lock if its available + OSMutexPend(&contactorsMutex, 0, + blocking ? OS_OPT_PEND_BLOCKING : OS_OPT_PEND_NON_BLOCKING, + ×tamp, &err); - // acquire lock if its available - OSMutexPend(&contactorsMutex, 0, blocking ? OS_OPT_PEND_BLOCKING : OS_OPT_PEND_NON_BLOCKING, ×tamp, &err); - - if(err == OS_ERR_PEND_WOULD_BLOCK){ - return ERROR; - } - assertOSError(OS_CONTACTOR_LOC, err); + if (err == OS_ERR_PEND_WOULD_BLOCK) { + return ERROR; + } + assertOSError(OS_CONTACTOR_LOC, err); - // change contactor to match state and make sure it worked - setContactor(contactor, state); - bool ret = (bool)Contactors_Get(contactor); - result = (ret == state) ? SUCCESS: ERROR; + // change contactor to match state and make sure it worked + setContactor(contactor, state); + bool ret = (bool)Contactors_Get(contactor); + result = (ret == state) ? SUCCESS : ERROR; - // release lock - OSMutexPost(&contactorsMutex, OS_OPT_POST_NONE, &err); - assertOSError(OS_CONTACTOR_LOC, err); + // release lock + OSMutexPost(&contactorsMutex, OS_OPT_POST_NONE, &err); + assertOSError(OS_CONTACTOR_LOC, err); - return result; + return result; } diff --git a/Drivers/Src/Display.c b/Drivers/Src/Display.c index c033940d6..cf209d03d 100644 --- a/Drivers/Src/Display.c +++ b/Drivers/Src/Display.c @@ -5,139 +5,144 @@ * * This contains functions relevant to sending/receiving messages * to/from our Nextion display. - * + * */ #include "Display.h" -#include "bsp.h" // for writing to UART -#include "Tasks.h" // for os and fault error codes + +#include "Tasks.h" // for os and fault error codes +#include "bsp.h" // for writing to UART #define DISP_OUT UART_3 #define MAX_MSG_LEN 32 #define MAX_ARG_LEN 16 // Assignment commands have only 1 arg, an operator, and an attribute -#define isAssignCmd(cmd) (cmd.compOrCmd != NULL && cmd.op != NULL && cmd.attr != NULL && cmd.numArgs == 1) -// Operational commands have no attribute and no operator, just a command and >= 0 arguments +#define isAssignCmd(cmd) \ + (cmd.compOrCmd != NULL && cmd.op != NULL && cmd.attr != NULL && \ + cmd.numArgs == 1) +// Operational commands have no attribute and no operator, just a command and >= +// 0 arguments #define isOpCmd(cmd) (cmd.op == NULL && cmd.attr == NULL) static const char *TERMINATOR = "\xff\xff\xff"; -DisplayError_t Display_Init(){ - BSP_UART_Init(DISP_OUT); - return Display_Reset(); +DisplayError_t Display_Init() { + BSP_UART_Init(DISP_OUT); + return Display_Reset(); } -DisplayError_t Display_Send(DisplayCmd_t cmd){ - char msgArgs[MAX_MSG_LEN]; - if (isAssignCmd(cmd)){ - if (cmd.argTypes[0] == INT_ARG){ - sprintf(msgArgs, "%d", (int)cmd.args[0].num); - } - else{ - if (cmd.args[0].str == NULL){return DISPLAY_ERR_PARSE;} - sprintf(msgArgs, "%s", cmd.args[0].str); - } - - BSP_UART_Write(DISP_OUT, cmd.compOrCmd, strlen(cmd.compOrCmd)); - BSP_UART_Write(DISP_OUT, ".", 1); - BSP_UART_Write(DISP_OUT, cmd.attr, strlen(cmd.attr)); - BSP_UART_Write(DISP_OUT, cmd.op, strlen(cmd.op)); - } - else if (isOpCmd(cmd)){ - msgArgs[0] = ' '; // No args - msgArgs[1] = '\0'; - if (cmd.numArgs > MAX_ARGS){return DISPLAY_ERR_OTHER;} - if (cmd.numArgs >= 1){ // If there are arguments - for (int i = 0; i < cmd.numArgs; i++){ - char arg[MAX_ARG_LEN]; - if (cmd.argTypes[i] == INT_ARG){ - sprintf(arg, "%d", (int)cmd.args[i].num); - } - else{ - sprintf(arg, "%s", cmd.args[i].str); - } - - strcat(msgArgs, arg); - - if (i < cmd.numArgs - 1){ // delimiter - strcat(msgArgs, ","); - } - } - } - BSP_UART_Write(DISP_OUT, cmd.compOrCmd, strlen(cmd.compOrCmd)); - } - else{ // Error parsing command struct - return DISPLAY_ERR_PARSE; - } - - if (cmd.numArgs >= 1){ // If there are arguments - BSP_UART_Write(DISP_OUT, msgArgs, strlen(msgArgs)); - } - - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - - return DISPLAY_ERR_NONE; +DisplayError_t Display_Send(DisplayCmd_t cmd) { + char msgArgs[MAX_MSG_LEN]; + if (isAssignCmd(cmd)) { + if (cmd.argTypes[0] == INT_ARG) { + sprintf(msgArgs, "%d", (int)cmd.args[0].num); + } else { + if (cmd.args[0].str == NULL) { + return DISPLAY_ERR_PARSE; + } + sprintf(msgArgs, "%s", cmd.args[0].str); + } + + BSP_UART_Write(DISP_OUT, cmd.compOrCmd, strlen(cmd.compOrCmd)); + BSP_UART_Write(DISP_OUT, ".", 1); + BSP_UART_Write(DISP_OUT, cmd.attr, strlen(cmd.attr)); + BSP_UART_Write(DISP_OUT, cmd.op, strlen(cmd.op)); + } else if (isOpCmd(cmd)) { + msgArgs[0] = ' '; // No args + msgArgs[1] = '\0'; + if (cmd.numArgs > MAX_ARGS) { + return DISPLAY_ERR_OTHER; + } + if (cmd.numArgs >= 1) { // If there are arguments + for (int i = 0; i < cmd.numArgs; i++) { + char arg[MAX_ARG_LEN]; + if (cmd.argTypes[i] == INT_ARG) { + sprintf(arg, "%d", (int)cmd.args[i].num); + } else { + sprintf(arg, "%s", cmd.args[i].str); + } + + strcat(msgArgs, arg); + + if (i < cmd.numArgs - 1) { // delimiter + strcat(msgArgs, ","); + } + } + } + BSP_UART_Write(DISP_OUT, cmd.compOrCmd, strlen(cmd.compOrCmd)); + } else { // Error parsing command struct + return DISPLAY_ERR_PARSE; + } + + if (cmd.numArgs >= 1) { // If there are arguments + BSP_UART_Write(DISP_OUT, msgArgs, strlen(msgArgs)); + } + + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + + return DISPLAY_ERR_NONE; } -DisplayError_t Display_Reset(){ - DisplayCmd_t restCmd = { - .compOrCmd = "rest", - .attr = NULL, - .op = NULL, - .numArgs = 0}; +DisplayError_t Display_Reset() { + DisplayCmd_t restCmd = { + .compOrCmd = "rest", .attr = NULL, .op = NULL, .numArgs = 0}; - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); // Terminates any in progress command + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, + strlen(TERMINATOR)); // Terminates any in progress command - return Display_Send(restCmd); + return Display_Send(restCmd); } -DisplayError_t Display_Fault(os_error_loc_t osErrCode, fault_bitmap_t faultCode){ - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); // Terminates any in progress command +DisplayError_t Display_Fault(os_error_loc_t osErrCode, + fault_bitmap_t faultCode) { + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, + strlen(TERMINATOR)); // Terminates any in progress command - char faultPage[7] = "page 2"; - BSP_UART_Write(DISP_OUT, faultPage, strlen(faultPage)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char faultPage[7] = "page 2"; + BSP_UART_Write(DISP_OUT, faultPage, strlen(faultPage)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - char setOSCode[17]; - sprintf(setOSCode, "%s\"%04x\"", "oserr.txt=", (uint16_t)osErrCode); - BSP_UART_Write(DISP_OUT, setOSCode, strlen(setOSCode)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char setOSCode[17]; + sprintf(setOSCode, "%s\"%04x\"", "oserr.txt=", (uint16_t)osErrCode); + BSP_UART_Write(DISP_OUT, setOSCode, strlen(setOSCode)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - char setFaultCode[18]; - sprintf(setFaultCode, "%s\"%02x\"", "faulterr.txt=", (uint8_t)faultCode); - BSP_UART_Write(DISP_OUT, setFaultCode, strlen(setFaultCode)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char setFaultCode[18]; + sprintf(setFaultCode, "%s\"%02x\"", "faulterr.txt=", (uint8_t)faultCode); + BSP_UART_Write(DISP_OUT, setFaultCode, strlen(setFaultCode)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - return DISPLAY_ERR_NONE; + return DISPLAY_ERR_NONE; } -DisplayError_t Display_Evac(uint8_t SOC_percent, uint32_t supp_mv){ - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); // Terminates any in progress command +DisplayError_t Display_Evac(uint8_t SOC_percent, uint32_t supp_mv) { + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, + strlen(TERMINATOR)); // Terminates any in progress command - char evacPage[7] = "page 3"; - BSP_UART_Write(DISP_OUT, evacPage, strlen(evacPage)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char evacPage[7] = "page 3"; + BSP_UART_Write(DISP_OUT, evacPage, strlen(evacPage)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - char soc[13]; - sprintf(soc, "%s%d", "soc.val=", (int)SOC_percent); - BSP_UART_Write(DISP_OUT, soc, strlen(soc)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char soc[13]; + sprintf(soc, "%s%d", "soc.val=", (int)SOC_percent); + BSP_UART_Write(DISP_OUT, soc, strlen(soc)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - char supp[18]; - sprintf(supp, "%s%d", "supp.val=", (int)supp_mv); - BSP_UART_Write(DISP_OUT, supp, strlen(supp)); - BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); + char supp[18]; + sprintf(supp, "%s%d", "supp.val=", (int)supp_mv); + BSP_UART_Write(DISP_OUT, supp, strlen(supp)); + BSP_UART_Write(DISP_OUT, (char *)TERMINATOR, strlen(TERMINATOR)); - return DISPLAY_ERR_NONE; + return DISPLAY_ERR_NONE; } -void assertDisplayError(DisplayError_t err){ - OS_ERR os_err; +void assertDisplayError(DisplayError_t err) { + OS_ERR os_err; - if (err != DISPLAY_ERR_NONE){ - FaultBitmap |= FAULT_DISPLAY; + if (err != DISPLAY_ERR_NONE) { + FaultBitmap |= FAULT_DISPLAY; - OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &os_err); - assertOSError(OS_DISPLAY_LOC, os_err); - } + OSSemPost(&FaultState_Sem4, OS_OPT_POST_1, &os_err); + assertOSError(OS_DISPLAY_LOC, os_err); + } } diff --git a/Drivers/Src/Minions.c b/Drivers/Src/Minions.c index b42857275..1af7f3d53 100644 --- a/Drivers/Src/Minions.c +++ b/Drivers/Src/Minions.c @@ -1,41 +1,39 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Minions.c - * @brief - * + * @brief + * */ #include "Minions.h" /* Should be in sync with enum in Minions.h */ const pinInfo_t PININFO_LUT[NUM_PINS] = { - {GPIO_Pin_1, PORTA, INPUT}, - {GPIO_Pin_0, PORTA, INPUT}, - {GPIO_Pin_4, PORTA, INPUT}, - {GPIO_Pin_5, PORTA, INPUT}, - {GPIO_Pin_6, PORTA, INPUT}, - {GPIO_Pin_7, PORTA, INPUT}, - {GPIO_Pin_4, PORTB, INPUT}, - {GPIO_Pin_5, PORTB, OUTPUT} -}; + {GPIO_Pin_1, PORTA, INPUT}, {GPIO_Pin_0, PORTA, INPUT}, + {GPIO_Pin_4, PORTA, INPUT}, {GPIO_Pin_5, PORTA, INPUT}, + {GPIO_Pin_6, PORTA, INPUT}, {GPIO_Pin_7, PORTA, INPUT}, + {GPIO_Pin_4, PORTB, INPUT}, {GPIO_Pin_5, PORTB, OUTPUT}}; -void Minions_Init(void){ - for(uint8_t i = 0; i < NUM_PINS; i++){ - BSP_GPIO_Init(PININFO_LUT[i].port, PININFO_LUT[i].pinMask, PININFO_LUT[i].direction); - } +void Minions_Init(void) { + for (uint8_t i = 0; i < NUM_PINS; i++) { + BSP_GPIO_Init(PININFO_LUT[i].port, PININFO_LUT[i].pinMask, + PININFO_LUT[i].direction); + } } -bool Minions_Read(pin_t pin){ - if((PININFO_LUT[pin].direction == INPUT)){ - return (bool) BSP_GPIO_Read_Pin(PININFO_LUT[pin].port, PININFO_LUT[pin].pinMask); - } else{ - return (bool)BSP_GPIO_Get_State(PININFO_LUT[pin].port, PININFO_LUT[pin].pinMask); - } +bool Minions_Read(pin_t pin) { + if ((PININFO_LUT[pin].direction == INPUT)) { + return (bool)BSP_GPIO_Read_Pin(PININFO_LUT[pin].port, + PININFO_LUT[pin].pinMask); + } else { + return (bool)BSP_GPIO_Get_State(PININFO_LUT[pin].port, + PININFO_LUT[pin].pinMask); + } } -bool Minions_Write(pin_t pin, bool status){ - if(PININFO_LUT[pin].direction == OUTPUT){ - BSP_GPIO_Write_Pin(PININFO_LUT[pin].port, PININFO_LUT[pin].pinMask, status); - return true; - } - return false; +bool Minions_Write(pin_t pin, bool status) { + if (PININFO_LUT[pin].direction == OUTPUT) { + BSP_GPIO_Write_Pin(PININFO_LUT[pin].port, PININFO_LUT[pin].pinMask, status); + return true; + } + return false; } diff --git a/Drivers/Src/Pedals.c b/Drivers/Src/Pedals.c old mode 100755 new mode 100644 index e28397fdf..d62498f4d --- a/Drivers/Src/Pedals.c +++ b/Drivers/Src/Pedals.c @@ -1,8 +1,8 @@ /** * @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar * @file Pedals.c - * @brief - * + * @brief + * */ #include "Pedals.h" @@ -11,47 +11,45 @@ // Indexed using pedal_t // Refine in testing static const int16_t LowerBound[NUMBER_OF_PEDALS] = { - 400, // Accelerator lower bound - 2100, // Brake lower bound + 400, // Accelerator lower bound + 2100, // Brake lower bound }; static const int16_t UpperBound[NUMBER_OF_PEDALS] = { - 900, // Accelerator upper bound - 3300, // Brake upper bound + 900, // Accelerator upper bound + 3300, // Brake upper bound }; /** - * @brief Initializes the brake and accelerator by using the + * @brief Initializes the brake and accelerator by using the * BSP_ADC_Init function with parameters ACCELERATOR - * and BRAKE + * and BRAKE * @param None * @return None */ -void Pedals_Init(){ - BSP_ADC_Init(); -} +void Pedals_Init() { BSP_ADC_Init(); } /** - * @brief Fetches the millivoltage value of the potentiomenter as provided + * @brief Fetches the millivoltage value of the potentiomenter as provided * by the ADC channel of the requested pedal (Accelerator or Brake), - * converts it to a percentage of the total distance pressed using + * converts it to a percentage of the total distance pressed using * data from calibration testing, and returns it * @param pedal_t, ACCELERATOR or BRAKE as defined in enum * @return percent amount the pedal has been pressed in percentage */ -int8_t Pedals_Read(pedal_t pedal){ - if (pedal >= NUMBER_OF_PEDALS) return 0; - int16_t millivoltsPedal = (int16_t) BSP_ADC_Get_Millivoltage(pedal); +int8_t Pedals_Read(pedal_t pedal) { + if (pedal >= NUMBER_OF_PEDALS) return 0; + int16_t millivoltsPedal = (int16_t)BSP_ADC_Get_Millivoltage(pedal); + + int8_t percentage = 0; - int8_t percentage = 0; - - if (millivoltsPedal >= LowerBound[pedal]) { - percentage = (int8_t) ( (int32_t) (millivoltsPedal - LowerBound[pedal]) * 100 / - (UpperBound[pedal] - LowerBound[pedal])); - } + if (millivoltsPedal >= LowerBound[pedal]) { + percentage = (int8_t)((int32_t)(millivoltsPedal - LowerBound[pedal]) * 100 / + (UpperBound[pedal] - LowerBound[pedal])); + } - if (percentage > 100) return 100; - if (percentage < 0) return 0; + if (percentage > 100) return 100; + if (percentage < 0) return 0; - return percentage; + return percentage; } diff --git a/Tests/Test_App_CommandLine.c b/Tests/Test_App_CommandLine.c index 606ec3c8a..4da69d5c0 100644 --- a/Tests/Test_App_CommandLine.c +++ b/Tests/Test_App_CommandLine.c @@ -1,18 +1,18 @@ -#include "os.h" -#include "Tasks.h" #include "BSP_UART.h" #include "CANbus.h" #include "Contactors.h" #include "Minions.h" #include "Pedals.h" +#include "Tasks.h" +#include "os.h" /* Test does not use OS. This is just to test the logic */ -int main(void){ - BSP_UART_Init(UART_2); - CANbus_Init(CAN_1, NULL, 0); - Contactors_Init(); - Minions_Init(); - Pedals_Init(); - Task_CommandLine(NULL); +int main(void) { + BSP_UART_Init(UART_2); + CANbus_Init(CAN_1, NULL, 0); + Contactors_Init(); + Minions_Init(); + Pedals_Init(); + Task_CommandLine(NULL); } \ No newline at end of file diff --git a/Tests/Test_App_DebugDump.c b/Tests/Test_App_DebugDump.c index ab65a207b..e6eeb94e8 100644 --- a/Tests/Test_App_DebugDump.c +++ b/Tests/Test_App_DebugDump.c @@ -1,219 +1,154 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ -#include "common.h" -#include "config.h" -#include "Tasks.h" -#include "stm32f4xx.h" -#include "CANbus.h" #include "CANConfig.h" +#include "CAN_Queue.h" +#include "CANbus.h" #include "Contactors.h" #include "Display.h" #include "Minions.h" #include "Pedals.h" -#include "CAN_Queue.h" +#include "Tasks.h" #include "UpdateDisplay.h" +#include "common.h" +#include "config.h" +#include "stm32f4xx.h" #define IGN_CONT_PERIOD 100 int main(void) { - // Disable interrupts - __disable_irq(); - - // Initialize some fault bitmaps for error checking purposes - OSErrLocBitmap = OS_NONE_LOC; - FaultBitmap = FAULT_NONE; - - OS_ERR err; - OSInit(&err); - OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); - - assertOSError(OS_MAIN_LOC, err); - - // Initialize apps - OSTaskCreate( - (OS_TCB*)&Init_TCB, - (CPU_CHAR*)"Init", - (OS_TASK_PTR)Task_Init, - (void*)NULL, - (OS_PRIO)TASK_INIT_PRIO, - (CPU_STK*)Init_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT/10, - (CPU_STK_SIZE)TASK_INIT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Enable interrupts - __enable_irq(); - - // Start OS - OSStart(&err); - assertOSError(OS_MAIN_LOC, err); - - while(1); - - return 0; + // Disable interrupts + __disable_irq(); + + // Initialize some fault bitmaps for error checking purposes + OSErrLocBitmap = OS_NONE_LOC; + FaultBitmap = FAULT_NONE; + + OS_ERR err; + OSInit(&err); + OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); + + assertOSError(OS_MAIN_LOC, err); + + // Initialize apps + OSTaskCreate((OS_TCB *)&Init_TCB, (CPU_CHAR *)"Init", (OS_TASK_PTR)Task_Init, + (void *)NULL, (OS_PRIO)TASK_INIT_PRIO, (CPU_STK *)Init_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT / 10, + (CPU_STK_SIZE)TASK_INIT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)0, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Enable interrupts + __enable_irq(); + + // Start OS + OSStart(&err); + assertOSError(OS_MAIN_LOC, err); + + while (1) + ; + + return 0; } -void Task_Init(void *p_arg){ - OS_ERR err; - - // Start systick - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - - OSTimeDlyHMSM(0,0,5,0,OS_OPT_TIME_HMSM_STRICT,&err); - - assertOSError(OS_MAIN_LOC, err); - - // Initialize drivers - Pedals_Init(); - OSTimeDlyHMSM(0,0,5,0,OS_OPT_TIME_HMSM_STRICT,&err); - OSTimeDlyHMSM(0,0,10,0,OS_OPT_TIME_HMSM_STRICT,&err); - BSP_UART_Init(UART_2); - CANbus_Init(CARCAN, (CANId_t*)carCANFilterList, NUM_CARCAN_FILTERS); - CANbus_Init(MOTORCAN, NULL, NUM_MOTORCAN_FILTERS); - Contactors_Init(); - Display_Init(); - Minions_Init(); - CAN_Queue_Init(); - - // Initialize applications - UpdateDisplay_Init(); - - /* Commented due to it messing up the test */ - // Initialize FaultState - OSTaskCreate( - (OS_TCB*)&FaultState_TCB, - (CPU_CHAR*)"FaultState", - (OS_TASK_PTR)Task_FaultState, - (void*)NULL, - (OS_PRIO)TASK_FAULT_STATE_PRIO, - (CPU_STK*)FaultState_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_FAULT_STATE_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize SendTritium - OSTaskCreate( - (OS_TCB*)&SendTritium_TCB, - (CPU_CHAR*)"SendTritium", - (OS_TASK_PTR)Task_SendTritium, - (void*)NULL, - (OS_PRIO)TASK_SEND_TRITIUM_PRIO, - (CPU_STK*)SendTritium_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize ReadCarCAN - OSTaskCreate( - (OS_TCB*)&ReadCarCAN_TCB, - (CPU_CHAR*)"ReadCarCAN", - (OS_TASK_PTR)Task_ReadCarCAN, - (void*)NULL, - (OS_PRIO)TASK_READ_CAR_CAN_PRIO, - (CPU_STK*)ReadCarCAN_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_READ_CAR_CAN_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize UpdateDisplay - OSTaskCreate( - (OS_TCB*)&UpdateDisplay_TCB, - (CPU_CHAR*)"UpdateDisplay", - (OS_TASK_PTR)Task_UpdateDisplay, - (void*)NULL, - (OS_PRIO)TASK_UPDATE_DISPLAY_PRIO, - (CPU_STK*)UpdateDisplay_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_UPDATE_DISPLAY_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize ReadTritium - OSTaskCreate( - (OS_TCB*)&ReadTritium_TCB, - (CPU_CHAR*)"ReadTritium", - (OS_TASK_PTR)Task_ReadTritium, - (void*)NULL, - (OS_PRIO)TASK_READ_TRITIUM_PRIO, - (CPU_STK*)ReadTritium_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_READ_TRITIUM_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize SendCarCAN - OSTaskCreate( - (OS_TCB*)&SendCarCAN_TCB, - (CPU_CHAR*)"SendCarCAN", - (OS_TASK_PTR)Task_SendCarCAN, - (void*)NULL, - (OS_PRIO)TASK_SEND_CAR_CAN_PRIO, - (CPU_STK*)SendCarCAN_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - // Initialize DebugDump - OSTaskCreate( - (OS_TCB*)&DebugDump_TCB, - (CPU_CHAR*)"DebugDump", - (OS_TASK_PTR)Task_DebugDump, - (void*)NULL, - (OS_PRIO)TASK_DEBUG_DUMP_PRIO, - (CPU_STK*)DebugDump_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_DEBUG_DUMP_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - while(1){ - Contactors_Set(MOTOR_CONTACTOR, Minions_Read(IGN_2), true); //turn on the contactor if the ign switch lets us - assertOSError(OS_MINIONS_LOC, err); - OSTimeDlyHMSM(0, 0, 0, IGN_CONT_PERIOD, OS_OPT_TIME_HMSM_NON_STRICT, &err); - } +void Task_Init(void *p_arg) { + OS_ERR err; + + // Start systick + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + + assertOSError(OS_MAIN_LOC, err); + + // Initialize drivers + Pedals_Init(); + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + OSTimeDlyHMSM(0, 0, 10, 0, OS_OPT_TIME_HMSM_STRICT, &err); + BSP_UART_Init(UART_2); + CANbus_Init(CARCAN, (CANId_t *)carCANFilterList, NUM_CARCAN_FILTERS); + CANbus_Init(MOTORCAN, NULL, NUM_MOTORCAN_FILTERS); + Contactors_Init(); + Display_Init(); + Minions_Init(); + CAN_Queue_Init(); + + // Initialize applications + UpdateDisplay_Init(); + + /* Commented due to it messing up the test */ + // Initialize FaultState + OSTaskCreate((OS_TCB *)&FaultState_TCB, (CPU_CHAR *)"FaultState", + (OS_TASK_PTR)Task_FaultState, (void *)NULL, + (OS_PRIO)TASK_FAULT_STATE_PRIO, (CPU_STK *)FaultState_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_FAULT_STATE_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize SendTritium + OSTaskCreate((OS_TCB *)&SendTritium_TCB, (CPU_CHAR *)"SendTritium", + (OS_TASK_PTR)Task_SendTritium, (void *)NULL, + (OS_PRIO)TASK_SEND_TRITIUM_PRIO, (CPU_STK *)SendTritium_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize ReadCarCAN + OSTaskCreate((OS_TCB *)&ReadCarCAN_TCB, (CPU_CHAR *)"ReadCarCAN", + (OS_TASK_PTR)Task_ReadCarCAN, (void *)NULL, + (OS_PRIO)TASK_READ_CAR_CAN_PRIO, (CPU_STK *)ReadCarCAN_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_READ_CAR_CAN_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize UpdateDisplay + OSTaskCreate((OS_TCB *)&UpdateDisplay_TCB, (CPU_CHAR *)"UpdateDisplay", + (OS_TASK_PTR)Task_UpdateDisplay, (void *)NULL, + (OS_PRIO)TASK_UPDATE_DISPLAY_PRIO, (CPU_STK *)UpdateDisplay_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_UPDATE_DISPLAY_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize ReadTritium + OSTaskCreate((OS_TCB *)&ReadTritium_TCB, (CPU_CHAR *)"ReadTritium", + (OS_TASK_PTR)Task_ReadTritium, (void *)NULL, + (OS_PRIO)TASK_READ_TRITIUM_PRIO, (CPU_STK *)ReadTritium_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_READ_TRITIUM_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize SendCarCAN + OSTaskCreate((OS_TCB *)&SendCarCAN_TCB, (CPU_CHAR *)"SendCarCAN", + (OS_TASK_PTR)Task_SendCarCAN, (void *)NULL, + (OS_PRIO)TASK_SEND_CAR_CAN_PRIO, (CPU_STK *)SendCarCAN_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_SEND_CAR_CAN_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)0, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // Initialize DebugDump + OSTaskCreate( + (OS_TCB *)&DebugDump_TCB, (CPU_CHAR *)"DebugDump", + (OS_TASK_PTR)Task_DebugDump, (void *)NULL, (OS_PRIO)TASK_DEBUG_DUMP_PRIO, + (CPU_STK *)DebugDump_Stk, (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_DEBUG_DUMP_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)0, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + while (1) { + Contactors_Set(MOTOR_CONTACTOR, Minions_Read(IGN_2), + true); // turn on the contactor if the ign switch lets us + assertOSError(OS_MINIONS_LOC, err); + OSTimeDlyHMSM(0, 0, 0, IGN_CONT_PERIOD, OS_OPT_TIME_HMSM_NON_STRICT, &err); + } } diff --git a/Tests/Test_App_FaultState.c b/Tests/Test_App_FaultState.c index f1372af08..6d85d4968 100644 --- a/Tests/Test_App_FaultState.c +++ b/Tests/Test_App_FaultState.c @@ -1,69 +1,44 @@ +#include "Tasks.h" +#include "bsp.h" #include "common.h" #include "config.h" #include "os.h" -#include "Tasks.h" -#include "bsp.h" static OS_TCB Task1TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE]; - -void Task1(void* arg){ - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - OS_ERR err; - OS_MUTEX testMut; - OS_TICK tick = (OS_TICK)0; - CPU_TS ts; - OSMutexPend(&testMut,tick, OS_OPT_POST_NONE, &ts,&err); - assertOSError(OS_MAIN_LOC,err); - +void Task1(void* arg) { + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + OS_ERR err; + OS_MUTEX testMut; + OS_TICK tick = (OS_TICK)0; + CPU_TS ts; + OSMutexPend(&testMut, tick, OS_OPT_POST_NONE, &ts, &err); + assertOSError(OS_MAIN_LOC, err); }; -int main(){ - OS_ERR err; - OSInit(&err); - OSSemCreate( //create fault sem4 - &FaultState_Sem4, - "Fault State Semaphore", - 0, - &err - ); - assertOSError(OS_MAIN_LOC,err); +int main() { + OS_ERR err; + OSInit(&err); + OSSemCreate( // create fault sem4 + &FaultState_Sem4, "Fault State Semaphore", 0, &err); + assertOSError(OS_MAIN_LOC, err); - OSTaskCreate( //create fault task - (OS_TCB*)&FaultState_TCB, - (CPU_CHAR*)"Fault State", - (OS_TASK_PTR)&Task_FaultState, - (void*)NULL, - (OS_PRIO)1, - (CPU_STK*)FaultState_Stk, - (CPU_STK_SIZE)128/10, - (CPU_STK_SIZE)128, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC,err); + OSTaskCreate( // create fault task + (OS_TCB*)&FaultState_TCB, (CPU_CHAR*)"Fault State", + (OS_TASK_PTR)&Task_FaultState, (void*)NULL, (OS_PRIO)1, + (CPU_STK*)FaultState_Stk, (CPU_STK_SIZE)128 / 10, (CPU_STK_SIZE)128, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); - //create tester thread - OSTaskCreate( - (OS_TCB*)&Task1TCB, - (CPU_CHAR*)"Task 1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)13, - (CPU_STK*)Task1Stk, - (CPU_STK_SIZE)128/10, - (CPU_STK_SIZE)128, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC,err); + // create tester thread + OSTaskCreate((OS_TCB*)&Task1TCB, (CPU_CHAR*)"Task 1", (OS_TASK_PTR)Task1, + (void*)NULL, (OS_PRIO)13, (CPU_STK*)Task1Stk, + (CPU_STK_SIZE)128 / 10, (CPU_STK_SIZE)128, (OS_MSG_QTY)0, + (OS_TICK)NULL, (void*)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); - OSStart(&err); -} \ No newline at end of file + OSStart(&err); +} \ No newline at end of file diff --git a/Tests/Test_App_ReadCarCAN.c b/Tests/Test_App_ReadCarCAN.c index bab090bc4..de4ba7481 100644 --- a/Tests/Test_App_ReadCarCAN.c +++ b/Tests/Test_App_ReadCarCAN.c @@ -1,11 +1,11 @@ -#include "Tasks.h" -#include "CANbus.h" +#include "CANConfig.h" #include "CAN_Queue.h" -#include "ReadCarCAN.h" +#include "CANbus.h" #include "Contactors.h" #include "Display.h" +#include "ReadCarCAN.h" +#include "Tasks.h" #include "UpdateDisplay.h" -#include "CANConfig.h" static OS_TCB Task1_TCB; #define STACK_SIZE 128 @@ -13,69 +13,50 @@ static CPU_STK Task1_Stk[STACK_SIZE]; #define CARCAN_FILTER_SIZE (sizeof carCANFilterList / sizeof(CANId_t)) -void Task1(){ - OS_ERR err; - - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - Contactors_Init(); - Contactors_Enable(ARRAY_CONTACTOR); - Contactors_Enable(MOTOR_CONTACTOR); - Contactors_Enable(ARRAY_PRECHARGE); - CANbus_Init(CARCAN, &carCANFilterList, CARCAN_FILTER_SIZE); - Display_Init(); - UpdateDisplay_Init(); - BSP_UART_Init(UART_2); - - OSTaskCreate( - (OS_TCB*)&ReadCarCAN_TCB, - (CPU_CHAR*)"ReadCarCAN", - (OS_TASK_PTR)Task_ReadCarCAN, - (void*)NULL, - (OS_PRIO)3, - (CPU_STK*)ReadCarCAN_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); +void Task1() { + OS_ERR err; + + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + Contactors_Init(); + Contactors_Enable(ARRAY_CONTACTOR); + Contactors_Enable(MOTOR_CONTACTOR); + Contactors_Enable(ARRAY_PRECHARGE); + CANbus_Init(CARCAN, &carCANFilterList, CARCAN_FILTER_SIZE); + Display_Init(); + UpdateDisplay_Init(); + BSP_UART_Init(UART_2); + + OSTaskCreate((OS_TCB*)&ReadCarCAN_TCB, (CPU_CHAR*)"ReadCarCAN", + (OS_TASK_PTR)Task_ReadCarCAN, (void*)NULL, (OS_PRIO)3, + (CPU_STK*)ReadCarCAN_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void*)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), + (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); + + while (1) { + printf("\nchargeEnable: %d", chargeEnable_Get()); + OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err); assertOSError(OS_MAIN_LOC, err); - - while(1){ - printf("\nchargeEnable: %d", chargeEnable_Get()); - OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err); - assertOSError(OS_MAIN_LOC, err); - } + } } -int main(){ - OS_ERR err; - OSInit(&err); - - if(err != OS_ERR_NONE){ - printf("OS error code %d\n",err); - } - OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)4, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); - assertOSError(OS_MAIN_LOC, err); +int main() { + OS_ERR err; + OSInit(&err); + + if (err != OS_ERR_NONE) { + printf("OS error code %d\n", err); + } + OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); + OSTaskCreate( + (OS_TCB*)&Task1_TCB, (CPU_CHAR*)"Task1", (OS_TASK_PTR)Task1, (void*)NULL, + (OS_PRIO)4, (CPU_STK*)Task1_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); + assertOSError(OS_MAIN_LOC, err); } \ No newline at end of file diff --git a/Tests/Test_App_SendCarCAN.c b/Tests/Test_App_SendCarCAN.c index 78ef52c9f..632fda90b 100644 --- a/Tests/Test_App_SendCarCAN.c +++ b/Tests/Test_App_SendCarCAN.c @@ -1,89 +1,70 @@ -#include "Tasks.h" +#include "CAN_Queue.h" #include "CANbus.h" +#include "Tasks.h" #include "stm32f4xx.h" -#include "CAN_Queue.h" static OS_TCB Task1_TCB; static CPU_STK Task1_Stk[128]; #define STACK_SIZE 128 -void Task1(void *p_arg){ - CPU_Init(); - // OS_CPU_SysTickInit(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - OS_ERR err; - // CPU_TS ts; - CAN_Queue_Init(); - CANbus_Init(CARCAN); - - //spawn can send task - OSTaskCreate( - (OS_TCB*)&SendCarCAN_TCB, - (CPU_CHAR*)"SendCarCan", - (OS_TASK_PTR)Task_SendCarCAN, - (void*)NULL, - (OS_PRIO)3, - (CPU_STK*)SendCarCAN_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); +void Task1(void *p_arg) { + CPU_Init(); + // OS_CPU_SysTickInit(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + OS_ERR err; + // CPU_TS ts; + CAN_Queue_Init(); + CANbus_Init(CARCAN); - CANDATA_t msg; - msg.ID = MOTOR_STATUS; - msg.idx = 0; - msg.data[0] = 0x12; - msg.data[1] = 0x34; - msg.data[2] = 0x56; - msg.data[3] = 0x78; - msg.data[4] = 0x9A; - msg.data[5] = 0xBC; - msg.data[6] = 0xDE; - msg.data[7] = 0xF0; - while(1){ - (msg.ID)++; - if(msg.ID > 0x24F){ - msg.ID = MOTOR_STATUS; - } - CAN_Queue_Post(msg); - OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err); - } + // spawn can send task + OSTaskCreate( + (OS_TCB *)&SendCarCAN_TCB, (CPU_CHAR *)"SendCarCan", + (OS_TASK_PTR)Task_SendCarCAN, (void *)NULL, (OS_PRIO)3, + (CPU_STK *)SendCarCAN_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), (OS_ERR *)&err); -} -int main(void){ //startup OS stuff, spawn test task - - OS_ERR err; - OSInit(&err); - if(err != OS_ERR_NONE){ - printf("OS error code %d\n",err); + CANDATA_t msg; + msg.ID = MOTOR_STATUS; + msg.idx = 0; + msg.data[0] = 0x12; + msg.data[1] = 0x34; + msg.data[2] = 0x56; + msg.data[3] = 0x78; + msg.data[4] = 0x9A; + msg.data[5] = 0xBC; + msg.data[6] = 0xDE; + msg.data[7] = 0xF0; + while (1) { + (msg.ID)++; + if (msg.ID > 0x24F) { + msg.ID = MOTOR_STATUS; } - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)4, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); + CAN_Queue_Post(msg); + OSTimeDlyHMSM(0, 0, 0, 100, OS_OPT_TIME_HMSM_STRICT, &err); + } +} +int main(void) { // startup OS stuff, spawn test task - if (err != OS_ERR_NONE) { - printf("Task1 error code %d\n", err); - } - OSStart(&err); - if (err != OS_ERR_NONE) { - printf("OS error code %d\n", err); - } - return 0; + OS_ERR err; + OSInit(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n", err); + } + OSTaskCreate((OS_TCB *)&Task1_TCB, (CPU_CHAR *)"Task1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)4, (CPU_STK *)Task1_Stk, + (CPU_STK_SIZE)STACK_SIZE / 10, (CPU_STK_SIZE)STACK_SIZE, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), + (OS_ERR *)&err); + + if (err != OS_ERR_NONE) { + printf("Task1 error code %d\n", err); + } + OSStart(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n", err); + } + return 0; } \ No newline at end of file diff --git a/Tests/Test_App_SendTritium.c b/Tests/Test_App_SendTritium.c index 085f007ae..c127f95b8 100644 --- a/Tests/Test_App_SendTritium.c +++ b/Tests/Test_App_SendTritium.c @@ -1,650 +1,649 @@ +#include "BSP_UART.h" +#include "CANbus.h" +#include "FaultState.h" #include "Minions.h" #include "Pedals.h" -#include "FaultState.h" -#include "CANbus.h" -#include "UpdateDisplay.h" #include "ReadCarCAN.h" -#include "BSP_UART.h" -#include "Tasks.h" - #include "SendTritium.h" +#include "Tasks.h" +#include "UpdateDisplay.h" static OS_TCB Task1TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE]; -void stateBuffer(){ - OS_ERR err; - OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); - assertOSError(OS_UPDATE_VEL_LOC, err); +void stateBuffer() { + OS_ERR err; + OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); + assertOSError(OS_UPDATE_VEL_LOC, err); } -void goToBrakeState(){ - // Brake State - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} +void goToBrakeState() { + // Brake State + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } } -void goToForwardDrive(){ - goToBrakeState(); +void goToForwardDrive() { + goToBrakeState(); - //Forward Drive - set_brakePedalPercent(0); - set_velocityObserved(mpsToRpm(2)); - set_gear(FORWARD_GEAR); + // Forward Drive + set_brakePedalPercent(0); + set_velocityObserved(mpsToRpm(2)); + set_gear(FORWARD_GEAR); - stateBuffer(); - while(get_state().name != FORWARD_DRIVE){} + stateBuffer(); + while (get_state().name != FORWARD_DRIVE) { + } } -void goToNeutralDrive(){ - goToForwardDrive(); +void goToNeutralDrive() { + goToForwardDrive(); - // Neutral Drive - set_gear(NEUTRAL_GEAR); + // Neutral Drive + set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } } -void goToReverseDrive(){ - goToForwardDrive(); - - // Reverse Drive - set_gear(REVERSE_GEAR); +void goToReverseDrive() { + goToForwardDrive(); - stateBuffer(); // Transition to neutral // DOESN"T GO TO NEUTRAL??? - // while(get_state().name != NEUTRAL_DRIVE){} + // Reverse Drive + set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); // Transition to reverse - while(get_state().name != REVERSE_DRIVE){} -} - - -void goToOnePedalDrive(){ - goToForwardDrive(); + stateBuffer(); // Transition to neutral // DOESN"T GO TO NEUTRAL??? + // while(get_state().name != NEUTRAL_DRIVE){} - // One Pedal Drive - set_cruiseEnable(false); - set_onePedalEnable(true); - set_regenEnable(true); - stateBuffer(); - while(get_state().name != ONEPEDAL){} + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); // Transition to reverse + while (get_state().name != REVERSE_DRIVE) { + } } -void goToRecordVelocity(){ - goToForwardDrive(); +void goToOnePedalDrive() { + goToForwardDrive(); - // Record Velocity - set_cruiseEnable(true); - set_cruiseSet(true); - set_velocityObserved(mpsToRpm(30)); - stateBuffer(); - while(get_state().name != RECORD_VELOCITY){} + // One Pedal Drive + set_cruiseEnable(false); + set_onePedalEnable(true); + set_regenEnable(true); + stateBuffer(); + while (get_state().name != ONEPEDAL) { + } } -void goToPoweredCruise(){ - goToRecordVelocity(); +void goToRecordVelocity() { + goToForwardDrive(); - // Powered Cruise - set_cruiseEnable(true); - set_cruiseSet(false); - set_velocityObserved(mpsToRpm(30)); - set_cruiseVelSetpoint(mpsToRpm(40)); - stateBuffer(); - while(get_state().name != POWERED_CRUISE){} + // Record Velocity + set_cruiseEnable(true); + set_cruiseSet(true); + set_velocityObserved(mpsToRpm(30)); + stateBuffer(); + while (get_state().name != RECORD_VELOCITY) { + } } -void goToCoastingCruise(){ - goToPoweredCruise(); +void goToPoweredCruise() { + goToRecordVelocity(); + + // Powered Cruise + set_cruiseEnable(true); + set_cruiseSet(false); + set_velocityObserved(mpsToRpm(30)); + set_cruiseVelSetpoint(mpsToRpm(40)); + stateBuffer(); + while (get_state().name != POWERED_CRUISE) { + } +} - // Coasting Cruise - set_cruiseEnable(true); - set_cruiseSet(false); - set_velocityObserved(mpsToRpm(40)); - set_cruiseVelSetpoint(mpsToRpm(30)); - stateBuffer(); - while(get_state().name != COASTING_CRUISE){} +void goToCoastingCruise() { + goToPoweredCruise(); + + // Coasting Cruise + set_cruiseEnable(true); + set_cruiseSet(false); + set_velocityObserved(mpsToRpm(40)); + set_cruiseVelSetpoint(mpsToRpm(30)); + stateBuffer(); + while (get_state().name != COASTING_CRUISE) { + } } -void goToAccelerateCruise(){ - goToPoweredCruise(); - - // Coasting Cruise - set_cruiseEnable(true); - set_cruiseSet(false); - set_velocityObserved(mpsToRpm(30)); - set_cruiseVelSetpoint(mpsToRpm(30)); - set_accelPedalPercent(10); - stateBuffer(); - while(get_state().name != ACCELERATE_CRUISE){} +void goToAccelerateCruise() { + goToPoweredCruise(); + + // Coasting Cruise + set_cruiseEnable(true); + set_cruiseSet(false); + set_velocityObserved(mpsToRpm(30)); + set_cruiseVelSetpoint(mpsToRpm(30)); + set_accelPedalPercent(10); + stateBuffer(); + while (get_state().name != ACCELERATE_CRUISE) { + } } -void Task1(void *arg) -{ - OS_ERR err; - - CPU_Init(); - BSP_UART_Init(UART_2); - Pedals_Init(); - CANbus_Init(MOTORCAN, NULL, 0); - Minions_Init(); - UpdateDisplay_Init(); - - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - set_regenEnable(ON); - - OSTaskCreate( - (OS_TCB*)&SendTritium_TCB, - (CPU_CHAR*)"SendTritium", - (OS_TASK_PTR)Task_SendTritium, - (void*) NULL, - (OS_PRIO)TASK_SEND_TRITIUM_PRIO, - (CPU_STK*)SendTritium_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT/10, - (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, - (OS_MSG_QTY) 0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - //assertOSError(err); - /** - * ======= Forward Drive ========== - * State Transitions: - * Brake, Record Velocity, One Pedal, Neutral, Reverse - */ - printf("\n\r============ Testing Forward Drive State ============\n\r"); - - // Forward Drive to Brake - printf("\n\rTesting: Forward Drive -> Brake\n\r"); - goToForwardDrive(); - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} - - // Forward Drive to Record Velocity - printf("\n\rTesting: Forward Drive -> Record Velocity\n\r"); - goToForwardDrive(); - set_cruiseEnable(true); - set_cruiseSet(true); - set_velocityObserved(mpsToRpm(20.0)); - stateBuffer(); - set_velocityObserved(mpsToRpm(25.0)); - stateBuffer(); - while(get_state().name != RECORD_VELOCITY){} - - // Forward Drive to One Pedal - printf("\n\rTesting: Forward Drive -> One Pedal\n\r"); - goToForwardDrive(); - set_onePedalEnable(true); - stateBuffer(); - while(get_state().name != ONEPEDAL){} - - // Forward Drive to Neutral Drive - printf("\n\rTesting: Forward Drive -> Neutral Drive\n\r"); - goToForwardDrive(); - set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} - - // Forward Drive to Reverse Drive - printf("\n\rTesting: Forward Drive -> Reverse Drive\n\r"); - goToForwardDrive(); - set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != REVERSE_DRIVE){} - - /** - * ======= Neutral Drive ========== - * State Transitions: - * Brake, Forward Drive, Reverse Drive - */ - - printf("\n\r============ Testing Neutral Drive State ============\n\r"); - - // Neutral Drive to Brake - printf("\n\rTesting: Neutral Drive -> Brake\n\r"); - goToNeutralDrive(); - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} - - // Neutral Drive to Forward Drive - printf("\n\rTesting: Neutral Drive -> Forward Drive\n\r"); - goToNeutralDrive(); - set_velocityObserved(mpsToRpm(5)); - set_gear(FORWARD_GEAR); - stateBuffer(); - while(get_state().name != FORWARD_DRIVE){} - - // Neutral Drive to Reverse Drive - printf("\n\rTesting: Neutral Drive -> Reverse Drive\n\r"); - goToNeutralDrive(); - set_velocityObserved(mpsToRpm(5)); - set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != REVERSE_DRIVE){} - - /** - * ======= Reverse Drive ========== - * State Transitions: - * Brake, Neutral Drive - */ - - printf("\n\r============ Testing Reverse Drive State ============\n\r"); - - // Reverse Drive to Brake - printf("\n\rTesting: Reverse Drive -> Brake\n\r"); - goToReverseDrive(); - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} - - // Reverse Drive to Neutral Drive - printf("\n\rTesting: Reverse Drive -> Neutral Drive\n\r"); - goToReverseDrive(); - set_velocityObserved(mpsToRpm(35)); - set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} - - // Reverse Drive to Forward Drive - printf("\n\rTesting: Reverse Drive -> Forward Drive\n\r"); - goToReverseDrive(); - set_gear(FORWARD_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != FORWARD_DRIVE){} - - /** - * ======= Record Velocity ========== - * State Transitions: - * Brake State, Neutral, One Pedal, Forward Drive, Powered Cruise - */ - printf("\n\r============ Testing Record Velocity State ============\n\r"); - - // Record Velocity to Brake State - printf("\n\rTesting: Record Velocity -> Brake\n\r"); - goToRecordVelocity(); - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} - - // Record Velocity to Neutral Drive - printf("\n\rTesting: Record Velocity -> Neutral Drive\n\r"); - goToRecordVelocity(); - set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} - - // Record Velocity to Reverse Drive - printf("\n\rTesting: Record Velocity -> Reverse Drive\n\r"); - goToRecordVelocity(); - set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != REVERSE_DRIVE){} - - // Record Velocity to One Pedal Drive - printf("\n\rTesting: Record Velocity -> One Pedal Drive\n\r"); - goToRecordVelocity(); - set_onePedalEnable(true); - stateBuffer(); - while(get_state().name != ONEPEDAL){} - - // Record Velocity to Forward Normal Drive - printf("\n\rTesting: Record Velocity -> Forward Drive\n\r"); - goToRecordVelocity(); - set_cruiseEnable(false); - stateBuffer(); - while(get_state().name != FORWARD_DRIVE){} - - // Record Velocity to Powered Cruise - // printf("\n\rTesting: Record Velocity -> Powered Cruise\n\r"); - // goToRecordVelocity(); - // set_cruiseEnable(true); - // set_cruiseSet(false); - // stateBuffer(); - // while(get_state().name != POWERED_CRUISE){} - - /** - * ======= Powered Cruise ========== - * State Transitions: - * Brake, Neutral, One Pedal, Forward Drive, Record Velocity, Accelerate Cruise, Coasting Cruise - */ - printf("\n\r============ Testing Powered Cruise State ============\n\r"); - - // // Powered Cruise to Brake State - // printf("\n\rTesting: Powered Cruise -> Brake\n\r"); - // goToPoweredCruise(); - // set_brakePedalPercent(15); - // stateBuffer(); - // while(get_state().name != BRAKE_STATE){} - - // // Powered Cruise to Neutral Drive - // printf("\n\rTesting: Powered Cruise -> Neutral Drive\n\r"); - // goToPoweredCruise(); - // set_gear(NEUTRAL_GEAR); - // stateBuffer(); - // while(get_state().name != NEUTRAL_DRIVE){} - - // // Powered Cruise to Reverse Drive - // printf("\n\rTesting: Powered Cruise -> Reverse Drive\n\r"); - // goToPoweredCruise(); - // set_gear(REVERSE_GEAR); - // set_velocityObserved(mpsToRpm(35)); - // stateBuffer(); - // set_velocityObserved(mpsToRpm(5)); - // stateBuffer(); - // while(get_state().name != REVERSE_DRIVE){} - - // // Powered Cruise to One Pedal Drive - // printf("\n\rTesting: Powered Cruise -> One Pedal Drive\n\r"); - // goToPoweredCruise(); - // set_onePedalEnable(true); - // stateBuffer(); - // while(get_state().name != ONEPEDAL){} - - // // Powered Cruise to Forward Drive - // printf("\n\rTesting: Powered Cruise -> Forward Drive\n\r"); - // goToPoweredCruise(); - // set_cruiseEnable(false); - // stateBuffer(); - // while(get_state().name != FORWARD_DRIVE){} - - // // Powered Cruise to Record Velocity - // printf("\n\rTesting: Powered Cruise -> Record Velocity\n\r"); - // goToPoweredCruise(); - // set_cruiseSet(true); - // stateBuffer(); - // while(get_state().name != RECORD_VELOCITY){} - - // // Powered Cruise to Accelerate Cruise - // printf("\n\rTesting: Powered Cruise -> Accelerate Cruise\n\r"); - // goToPoweredCruise(); - // set_accelPedalPercent(10); - // stateBuffer(); - // while(get_state().name != ACCELERATE_CRUISE){} - - // // Powered Cruise to Coasting Cruise - // printf("\n\rTesting: Powered Cruise -> Coasting Cruise\n\r"); - // goToPoweredCruise(); - // set_accelPedalPercent(0); - // set_velocityObserved(mpsToRpm(40)); - // set_cruiseVelSetpoint(mpsToRpm(30)); - // stateBuffer(); - // while(get_state().name != COASTING_CRUISE){} - - /** - * ======= Coasting Cruise ========== - * State Transitions: - * Brake, Neutral Drive, One Pedal, Forward Drive, Record Velocity, - * Accelerate Cruise, Powered Cruise - */ - printf("\n\r============ Testing Coasting Cruise State ============\n\r"); - - // // Coasting Cruise to Brake State - // printf("\n\rTesting: Coasting Cruise -> Brake\n\r"); - // goToCoastingCruise(); - // set_brakePedalPercent(15); - // stateBuffer(); - // while(get_state().name != BRAKE_STATE){} - - // // Coasting Cruise to Neutral Drive - // printf("\n\rTesting: Coasting Cruise -> Neutral Drive\n\r"); - // goToCoastingCruise(); - // set_gear(NEUTRAL_GEAR); - // stateBuffer(); - // while(get_state().name != NEUTRAL_DRIVE){} - - // // Coasting Cruise to Reverse Drive - // printf("\n\rTesting: Coasting Cruise -> Reverse Drive\n\r"); - // goToCoastingCruise(); - // set_gear(REVERSE_GEAR); - // set_velocityObserved(mpsToRpm(35)); - // stateBuffer(); - // set_velocityObserved(mpsToRpm(5)); - // stateBuffer(); - // while(get_state().name != REVERSE_DRIVE){} - - // // Coasting Cruise to One Pedal Drive - // printf("\n\rTesting: Coasting Cruise -> One Pedal Drive\n\r"); - // goToCoastingCruise(); - // set_onePedalEnable(true); - // stateBuffer(); - // while(get_state().name != ONEPEDAL){} - - // // Coasting Cruise to Forward Drive - // printf("\n\rTesting: Coasting Cruise -> Forward Drive\n\r"); - // goToCoastingCruise(); - // set_cruiseEnable(false); - // stateBuffer(); - // while(get_state().name != FORWARD_DRIVE){} - - // // Coasting Cruise to Record Velocity - // printf("\n\rTesting: Coasting Cruise -> Record Velocity\n\r"); - // goToCoastingCruise(); - // set_cruiseSet(true); - // set_velocityObserved(mpsToRpm(25.0)); - // stateBuffer(); - // while(get_state().name != RECORD_VELOCITY){} - - // // Coasting Cruise to Accelerate Cruise - // printf("\n\rTesting: Coasting Cruise -> Accelerate Cruise\n\r"); - // goToCoastingCruise(); - // set_accelPedalPercent(10); - // stateBuffer(); - // while(get_state().name != ACCELERATE_CRUISE){} - - // // Coasting Cruise to Powered Cruise - // printf("\n\rTesting: Coasting Cruise -> Powered Cruise\n\r"); - // goToCoastingCruise(); - // set_accelPedalPercent(0); - // set_velocityObserved(mpsToRpm(29)); - // set_cruiseVelSetpoint(mpsToRpm(30)); - // stateBuffer(); - // while(get_state().name != POWERED_CRUISE){} - - /** - * ======= Accelerate Cruise State ========== - * State Transitions: - * Coasting Cruise, Brake State - */ - printf("\n\r============ Testing Accelerate Cruise State ============\n\r"); - - // // Accelerate Cruise to Brake State - // printf("\n\rTesting: Accelerate Cruise -> Brake\n\r"); - // goToAccelerateCruise(); - // set_brakePedalPercent(15); - // stateBuffer(); - // while(get_state().name != BRAKE_STATE){} - - // // Accelerate Cruise to Neutral Drive - // printf("\n\rTesting: Accelerate Cruise -> Neutral Drive\n\r"); - // goToAccelerateCruise(); - // set_gear(NEUTRAL_GEAR); - // stateBuffer(); - // while(get_state().name != NEUTRAL_DRIVE){} - - // // Accelerate Cruise to Reverse Drive - // printf("\n\rTesting: Accelerate Cruise -> Reverse Drive\n\r"); - // goToAccelerateCruise(); - // set_gear(REVERSE_GEAR); - // set_velocityObserved(mpsToRpm(35)); - // stateBuffer(); - // set_velocityObserved(mpsToRpm(5)); - // stateBuffer(); - // while(get_state().name != REVERSE_DRIVE){} - - // // Accelerate Cruise to One Pedal Drive - // printf("\n\rTesting: Accelerate Cruise -> One Pedal Drive\n\r"); - // goToAccelerateCruise(); - // set_onePedalEnable(true); - // stateBuffer(); - // while(get_state().name != ONEPEDAL){} - - // // Accelerate Cruise to Forward Drive - // printf("\n\rTesting: Accelerate Cruise -> Forward Drive\n\r"); - // goToAccelerateCruise(); - // set_cruiseEnable(false); - // stateBuffer(); - // while(get_state().name != FORWARD_DRIVE){} - - // // Accelerate Cruise to Record Velocity - // printf("\n\rTesting: Accelerate Cruise -> Record Velocity\n\r"); - // goToAccelerateCruise(); - // set_cruiseSet(true); - // set_velocityObserved(mpsToRpm(25.0)); - // stateBuffer(); - // while(get_state().name != RECORD_VELOCITY){} - - // // Accelerate Cruise to Coasting Cruise - // printf("\n\rTesting: Accelerate Cruise -> Coasting Cruise\n\r"); - // goToAccelerateCruise(); - // set_accelPedalPercent(0); - // stateBuffer(); - // while(get_state().name != COASTING_CRUISE){} - - /** - * ======= One Pedal Drive ========== - * State Transitions: - * Brake, Neutral Drive, Forward Drive - */ - - printf("\n\r============ Testing One Pedal Drive State ============\n\r"); - - // One Pedal Drive to Brake - printf("\n\rTesting: One Pedal Drive -> Brake\n\r"); - goToOnePedalDrive(); - set_brakePedalPercent(15); - stateBuffer(); - while(get_state().name != BRAKE_STATE){} - - // One Pedal Drive to Neutral Drive - printf("\n\rTesting: One Pedal Drive -> Neutral Drive\n\r"); - goToOnePedalDrive(); - set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} - - // One Pedal Drive to Reverse Drive - printf("\n\rTesting: One Pedal Drive -> Reverse Drive\n\r"); - goToOnePedalDrive(); - set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != REVERSE_DRIVE){} - - // One Pedal Drive to Record Velocity - // printf("\n\rTesting: One Pedal Drive -> Record Velocity\n\r"); - // goToOnePedalDrive(); - // set_cruiseSet(true); // DOES WORK BUT NEEDS TO ENFORCE 1 TRANSITION - // set_cruiseEnable(true); - // set_velocityObserved(mpsToRpm(25.0)); - // stateBuffer(); - // while(get_state().name != RECORD_VELOCITY){} - - /** - * ======= Brake State ========== - * State Transitions: - * Brake Disable State - */ - printf("\n\r============ Testing Brake State ============\n\r"); - - // Brake State to Forward Drive - printf("\n\rTesting: Brake -> Forward Drive\n\r"); - goToBrakeState(); - set_brakePedalPercent(1); - set_gear(FORWARD_GEAR); - stateBuffer(); - while(get_state().name != FORWARD_DRIVE){} - - // Brake State to Neutral Drive - printf("\n\rTesting: Brake -> Neutral Drive\n\r"); - goToBrakeState(); - set_brakePedalPercent(1); - set_gear(NEUTRAL_GEAR); - stateBuffer(); - while(get_state().name != NEUTRAL_DRIVE){} - - // Brake State to Reverse Drive - printf("\n\rTesting: Brake -> Reverse Drive\n\r"); - goToBrakeState(); - set_brakePedalPercent(1); - set_gear(REVERSE_GEAR); - set_velocityObserved(mpsToRpm(35)); - stateBuffer(); - set_velocityObserved(mpsToRpm(5)); - stateBuffer(); - while(get_state().name != REVERSE_DRIVE){} - - OS_TaskSuspend(&SendTritium_TCB, &err); +void Task1(void *arg) { + OS_ERR err; + + CPU_Init(); + BSP_UART_Init(UART_2); + Pedals_Init(); + CANbus_Init(MOTORCAN, NULL, 0); + Minions_Init(); + UpdateDisplay_Init(); + + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + set_regenEnable(ON); + + OSTaskCreate((OS_TCB *)&SendTritium_TCB, (CPU_CHAR *)"SendTritium", + (OS_TASK_PTR)Task_SendTritium, (void *)NULL, + (OS_PRIO)TASK_SEND_TRITIUM_PRIO, (CPU_STK *)SendTritium_Stk, + (CPU_STK_SIZE)WATERMARK_STACK_LIMIT / 10, + (CPU_STK_SIZE)TASK_SEND_TRITIUM_STACK_SIZE, (OS_MSG_QTY)0, + (OS_TICK)NULL, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + // assertOSError(err); + /** + * ======= Forward Drive ========== + * State Transitions: + * Brake, Record Velocity, One Pedal, Neutral, Reverse + */ + printf("\n\r============ Testing Forward Drive State ============\n\r"); + + // Forward Drive to Brake + printf("\n\rTesting: Forward Drive -> Brake\n\r"); + goToForwardDrive(); + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } + + // Forward Drive to Record Velocity + printf("\n\rTesting: Forward Drive -> Record Velocity\n\r"); + goToForwardDrive(); + set_cruiseEnable(true); + set_cruiseSet(true); + set_velocityObserved(mpsToRpm(20.0)); + stateBuffer(); + set_velocityObserved(mpsToRpm(25.0)); + stateBuffer(); + while (get_state().name != RECORD_VELOCITY) { + } + + // Forward Drive to One Pedal + printf("\n\rTesting: Forward Drive -> One Pedal\n\r"); + goToForwardDrive(); + set_onePedalEnable(true); + stateBuffer(); + while (get_state().name != ONEPEDAL) { + } + + // Forward Drive to Neutral Drive + printf("\n\rTesting: Forward Drive -> Neutral Drive\n\r"); + goToForwardDrive(); + set_gear(NEUTRAL_GEAR); + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } + + // Forward Drive to Reverse Drive + printf("\n\rTesting: Forward Drive -> Reverse Drive\n\r"); + goToForwardDrive(); + set_gear(REVERSE_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != REVERSE_DRIVE) { + } + + /** + * ======= Neutral Drive ========== + * State Transitions: + * Brake, Forward Drive, Reverse Drive + */ + + printf("\n\r============ Testing Neutral Drive State ============\n\r"); + + // Neutral Drive to Brake + printf("\n\rTesting: Neutral Drive -> Brake\n\r"); + goToNeutralDrive(); + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } + + // Neutral Drive to Forward Drive + printf("\n\rTesting: Neutral Drive -> Forward Drive\n\r"); + goToNeutralDrive(); + set_velocityObserved(mpsToRpm(5)); + set_gear(FORWARD_GEAR); + stateBuffer(); + while (get_state().name != FORWARD_DRIVE) { + } + + // Neutral Drive to Reverse Drive + printf("\n\rTesting: Neutral Drive -> Reverse Drive\n\r"); + goToNeutralDrive(); + set_velocityObserved(mpsToRpm(5)); + set_gear(REVERSE_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != REVERSE_DRIVE) { + } + + /** + * ======= Reverse Drive ========== + * State Transitions: + * Brake, Neutral Drive + */ + + printf("\n\r============ Testing Reverse Drive State ============\n\r"); + + // Reverse Drive to Brake + printf("\n\rTesting: Reverse Drive -> Brake\n\r"); + goToReverseDrive(); + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } + + // Reverse Drive to Neutral Drive + printf("\n\rTesting: Reverse Drive -> Neutral Drive\n\r"); + goToReverseDrive(); + set_velocityObserved(mpsToRpm(35)); + set_gear(NEUTRAL_GEAR); + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } + + // Reverse Drive to Forward Drive + printf("\n\rTesting: Reverse Drive -> Forward Drive\n\r"); + goToReverseDrive(); + set_gear(FORWARD_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != FORWARD_DRIVE) { + } + + /** + * ======= Record Velocity ========== + * State Transitions: + * Brake State, Neutral, One Pedal, Forward Drive, Powered Cruise + */ + printf("\n\r============ Testing Record Velocity State ============\n\r"); + + // Record Velocity to Brake State + printf("\n\rTesting: Record Velocity -> Brake\n\r"); + goToRecordVelocity(); + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } + + // Record Velocity to Neutral Drive + printf("\n\rTesting: Record Velocity -> Neutral Drive\n\r"); + goToRecordVelocity(); + set_gear(NEUTRAL_GEAR); + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } + + // Record Velocity to Reverse Drive + printf("\n\rTesting: Record Velocity -> Reverse Drive\n\r"); + goToRecordVelocity(); + set_gear(REVERSE_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != REVERSE_DRIVE) { + } + + // Record Velocity to One Pedal Drive + printf("\n\rTesting: Record Velocity -> One Pedal Drive\n\r"); + goToRecordVelocity(); + set_onePedalEnable(true); + stateBuffer(); + while (get_state().name != ONEPEDAL) { + } + + // Record Velocity to Forward Normal Drive + printf("\n\rTesting: Record Velocity -> Forward Drive\n\r"); + goToRecordVelocity(); + set_cruiseEnable(false); + stateBuffer(); + while (get_state().name != FORWARD_DRIVE) { + } + + // Record Velocity to Powered Cruise + // printf("\n\rTesting: Record Velocity -> Powered Cruise\n\r"); + // goToRecordVelocity(); + // set_cruiseEnable(true); + // set_cruiseSet(false); + // stateBuffer(); + // while(get_state().name != POWERED_CRUISE){} + + /** + * ======= Powered Cruise ========== + * State Transitions: + * Brake, Neutral, One Pedal, Forward Drive, Record Velocity, Accelerate + * Cruise, Coasting Cruise + */ + printf("\n\r============ Testing Powered Cruise State ============\n\r"); + + // // Powered Cruise to Brake State + // printf("\n\rTesting: Powered Cruise -> Brake\n\r"); + // goToPoweredCruise(); + // set_brakePedalPercent(15); + // stateBuffer(); + // while(get_state().name != BRAKE_STATE){} + + // // Powered Cruise to Neutral Drive + // printf("\n\rTesting: Powered Cruise -> Neutral Drive\n\r"); + // goToPoweredCruise(); + // set_gear(NEUTRAL_GEAR); + // stateBuffer(); + // while(get_state().name != NEUTRAL_DRIVE){} + + // // Powered Cruise to Reverse Drive + // printf("\n\rTesting: Powered Cruise -> Reverse Drive\n\r"); + // goToPoweredCruise(); + // set_gear(REVERSE_GEAR); + // set_velocityObserved(mpsToRpm(35)); + // stateBuffer(); + // set_velocityObserved(mpsToRpm(5)); + // stateBuffer(); + // while(get_state().name != REVERSE_DRIVE){} + + // // Powered Cruise to One Pedal Drive + // printf("\n\rTesting: Powered Cruise -> One Pedal Drive\n\r"); + // goToPoweredCruise(); + // set_onePedalEnable(true); + // stateBuffer(); + // while(get_state().name != ONEPEDAL){} + + // // Powered Cruise to Forward Drive + // printf("\n\rTesting: Powered Cruise -> Forward Drive\n\r"); + // goToPoweredCruise(); + // set_cruiseEnable(false); + // stateBuffer(); + // while(get_state().name != FORWARD_DRIVE){} + + // // Powered Cruise to Record Velocity + // printf("\n\rTesting: Powered Cruise -> Record Velocity\n\r"); + // goToPoweredCruise(); + // set_cruiseSet(true); + // stateBuffer(); + // while(get_state().name != RECORD_VELOCITY){} + + // // Powered Cruise to Accelerate Cruise + // printf("\n\rTesting: Powered Cruise -> Accelerate Cruise\n\r"); + // goToPoweredCruise(); + // set_accelPedalPercent(10); + // stateBuffer(); + // while(get_state().name != ACCELERATE_CRUISE){} + + // // Powered Cruise to Coasting Cruise + // printf("\n\rTesting: Powered Cruise -> Coasting Cruise\n\r"); + // goToPoweredCruise(); + // set_accelPedalPercent(0); + // set_velocityObserved(mpsToRpm(40)); + // set_cruiseVelSetpoint(mpsToRpm(30)); + // stateBuffer(); + // while(get_state().name != COASTING_CRUISE){} + + /** + * ======= Coasting Cruise ========== + * State Transitions: + * Brake, Neutral Drive, One Pedal, Forward Drive, Record Velocity, + * Accelerate Cruise, Powered Cruise + */ + printf("\n\r============ Testing Coasting Cruise State ============\n\r"); + + // // Coasting Cruise to Brake State + // printf("\n\rTesting: Coasting Cruise -> Brake\n\r"); + // goToCoastingCruise(); + // set_brakePedalPercent(15); + // stateBuffer(); + // while(get_state().name != BRAKE_STATE){} + + // // Coasting Cruise to Neutral Drive + // printf("\n\rTesting: Coasting Cruise -> Neutral Drive\n\r"); + // goToCoastingCruise(); + // set_gear(NEUTRAL_GEAR); + // stateBuffer(); + // while(get_state().name != NEUTRAL_DRIVE){} + + // // Coasting Cruise to Reverse Drive + // printf("\n\rTesting: Coasting Cruise -> Reverse Drive\n\r"); + // goToCoastingCruise(); + // set_gear(REVERSE_GEAR); + // set_velocityObserved(mpsToRpm(35)); + // stateBuffer(); + // set_velocityObserved(mpsToRpm(5)); + // stateBuffer(); + // while(get_state().name != REVERSE_DRIVE){} + + // // Coasting Cruise to One Pedal Drive + // printf("\n\rTesting: Coasting Cruise -> One Pedal Drive\n\r"); + // goToCoastingCruise(); + // set_onePedalEnable(true); + // stateBuffer(); + // while(get_state().name != ONEPEDAL){} + + // // Coasting Cruise to Forward Drive + // printf("\n\rTesting: Coasting Cruise -> Forward Drive\n\r"); + // goToCoastingCruise(); + // set_cruiseEnable(false); + // stateBuffer(); + // while(get_state().name != FORWARD_DRIVE){} + + // // Coasting Cruise to Record Velocity + // printf("\n\rTesting: Coasting Cruise -> Record Velocity\n\r"); + // goToCoastingCruise(); + // set_cruiseSet(true); + // set_velocityObserved(mpsToRpm(25.0)); + // stateBuffer(); + // while(get_state().name != RECORD_VELOCITY){} + + // // Coasting Cruise to Accelerate Cruise + // printf("\n\rTesting: Coasting Cruise -> Accelerate Cruise\n\r"); + // goToCoastingCruise(); + // set_accelPedalPercent(10); + // stateBuffer(); + // while(get_state().name != ACCELERATE_CRUISE){} + + // // Coasting Cruise to Powered Cruise + // printf("\n\rTesting: Coasting Cruise -> Powered Cruise\n\r"); + // goToCoastingCruise(); + // set_accelPedalPercent(0); + // set_velocityObserved(mpsToRpm(29)); + // set_cruiseVelSetpoint(mpsToRpm(30)); + // stateBuffer(); + // while(get_state().name != POWERED_CRUISE){} + + /** + * ======= Accelerate Cruise State ========== + * State Transitions: + * Coasting Cruise, Brake State + */ + printf("\n\r============ Testing Accelerate Cruise State ============\n\r"); + + // // Accelerate Cruise to Brake State + // printf("\n\rTesting: Accelerate Cruise -> Brake\n\r"); + // goToAccelerateCruise(); + // set_brakePedalPercent(15); + // stateBuffer(); + // while(get_state().name != BRAKE_STATE){} + + // // Accelerate Cruise to Neutral Drive + // printf("\n\rTesting: Accelerate Cruise -> Neutral Drive\n\r"); + // goToAccelerateCruise(); + // set_gear(NEUTRAL_GEAR); + // stateBuffer(); + // while(get_state().name != NEUTRAL_DRIVE){} + + // // Accelerate Cruise to Reverse Drive + // printf("\n\rTesting: Accelerate Cruise -> Reverse Drive\n\r"); + // goToAccelerateCruise(); + // set_gear(REVERSE_GEAR); + // set_velocityObserved(mpsToRpm(35)); + // stateBuffer(); + // set_velocityObserved(mpsToRpm(5)); + // stateBuffer(); + // while(get_state().name != REVERSE_DRIVE){} + + // // Accelerate Cruise to One Pedal Drive + // printf("\n\rTesting: Accelerate Cruise -> One Pedal Drive\n\r"); + // goToAccelerateCruise(); + // set_onePedalEnable(true); + // stateBuffer(); + // while(get_state().name != ONEPEDAL){} + + // // Accelerate Cruise to Forward Drive + // printf("\n\rTesting: Accelerate Cruise -> Forward Drive\n\r"); + // goToAccelerateCruise(); + // set_cruiseEnable(false); + // stateBuffer(); + // while(get_state().name != FORWARD_DRIVE){} + + // // Accelerate Cruise to Record Velocity + // printf("\n\rTesting: Accelerate Cruise -> Record Velocity\n\r"); + // goToAccelerateCruise(); + // set_cruiseSet(true); + // set_velocityObserved(mpsToRpm(25.0)); + // stateBuffer(); + // while(get_state().name != RECORD_VELOCITY){} + + // // Accelerate Cruise to Coasting Cruise + // printf("\n\rTesting: Accelerate Cruise -> Coasting Cruise\n\r"); + // goToAccelerateCruise(); + // set_accelPedalPercent(0); + // stateBuffer(); + // while(get_state().name != COASTING_CRUISE){} + + /** + * ======= One Pedal Drive ========== + * State Transitions: + * Brake, Neutral Drive, Forward Drive + */ + + printf("\n\r============ Testing One Pedal Drive State ============\n\r"); + + // One Pedal Drive to Brake + printf("\n\rTesting: One Pedal Drive -> Brake\n\r"); + goToOnePedalDrive(); + set_brakePedalPercent(15); + stateBuffer(); + while (get_state().name != BRAKE_STATE) { + } + + // One Pedal Drive to Neutral Drive + printf("\n\rTesting: One Pedal Drive -> Neutral Drive\n\r"); + goToOnePedalDrive(); + set_gear(NEUTRAL_GEAR); + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } + + // One Pedal Drive to Reverse Drive + printf("\n\rTesting: One Pedal Drive -> Reverse Drive\n\r"); + goToOnePedalDrive(); + set_gear(REVERSE_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != REVERSE_DRIVE) { + } + + // One Pedal Drive to Record Velocity + // printf("\n\rTesting: One Pedal Drive -> Record Velocity\n\r"); + // goToOnePedalDrive(); + // set_cruiseSet(true); // DOES WORK BUT NEEDS TO ENFORCE 1 + // TRANSITION set_cruiseEnable(true); set_velocityObserved(mpsToRpm(25.0)); + // stateBuffer(); + // while(get_state().name != RECORD_VELOCITY){} + + /** + * ======= Brake State ========== + * State Transitions: + * Brake Disable State + */ + printf("\n\r============ Testing Brake State ============\n\r"); + + // Brake State to Forward Drive + printf("\n\rTesting: Brake -> Forward Drive\n\r"); + goToBrakeState(); + set_brakePedalPercent(1); + set_gear(FORWARD_GEAR); + stateBuffer(); + while (get_state().name != FORWARD_DRIVE) { + } + + // Brake State to Neutral Drive + printf("\n\rTesting: Brake -> Neutral Drive\n\r"); + goToBrakeState(); + set_brakePedalPercent(1); + set_gear(NEUTRAL_GEAR); + stateBuffer(); + while (get_state().name != NEUTRAL_DRIVE) { + } + + // Brake State to Reverse Drive + printf("\n\rTesting: Brake -> Reverse Drive\n\r"); + goToBrakeState(); + set_brakePedalPercent(1); + set_gear(REVERSE_GEAR); + set_velocityObserved(mpsToRpm(35)); + stateBuffer(); + set_velocityObserved(mpsToRpm(5)); + stateBuffer(); + while (get_state().name != REVERSE_DRIVE) { + } + + OS_TaskSuspend(&SendTritium_TCB, &err); + assertOSError(OS_MAIN_LOC, err); + while (1) { + printf("\n\r\n\rSUCCESS! ALL TESTS PASSED\n\r\n\r"); + OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); assertOSError(OS_MAIN_LOC, err); - while (1){ - printf("\n\r\n\rSUCCESS! ALL TESTS PASSED\n\r\n\r"); - OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); - assertOSError(OS_MAIN_LOC, err); - } + } }; -int main() -{ - OS_ERR err; - OSInit(&err); - OSSemCreate( // create fault sem4 - &FaultState_Sem4, - "Fault State Semaphore", - 0, - &err); - assertOSError(OS_MAIN_LOC, err); - - OSTaskCreate( // create fault task - (OS_TCB *)&FaultState_TCB, - (CPU_CHAR *)"Fault State", - (OS_TASK_PTR)&Task_FaultState, - (void *)NULL, - (OS_PRIO)1, - (CPU_STK *)FaultState_Stk, - (CPU_STK_SIZE)128 / 10, - (CPU_STK_SIZE)128, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - // create tester thread - OSTaskCreate( - (OS_TCB *)&Task1TCB, - (CPU_CHAR *)"Task 1", - (OS_TASK_PTR)Task1, - (void *)NULL, - (OS_PRIO)13, - (CPU_STK *)Task1Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); +int main() { + OS_ERR err; + OSInit(&err); + OSSemCreate( // create fault sem4 + &FaultState_Sem4, "Fault State Semaphore", 0, &err); + assertOSError(OS_MAIN_LOC, err); + + OSTaskCreate( // create fault task + (OS_TCB *)&FaultState_TCB, (CPU_CHAR *)"Fault State", + (OS_TASK_PTR)&Task_FaultState, (void *)NULL, (OS_PRIO)1, + (CPU_STK *)FaultState_Stk, (CPU_STK_SIZE)128 / 10, (CPU_STK_SIZE)128, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // create tester thread + OSTaskCreate((OS_TCB *)&Task1TCB, (CPU_CHAR *)"Task 1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)13, (CPU_STK *)Task1Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); } \ No newline at end of file diff --git a/Tests/Test_App_Telemetry.c b/Tests/Test_App_Telemetry.c index 6c08a237d..3e9ae20ba 100644 --- a/Tests/Test_App_Telemetry.c +++ b/Tests/Test_App_Telemetry.c @@ -1,95 +1,77 @@ -#include "os.h" -#include "Tasks.h" #include -#include "config.h" -#include "common.h" -#include "Pedals.h" -#include "Minions.h" -#include "Contactors.h" -#include "CANbus.h" + #include "CAN_Queue.h" +#include "CANbus.h" +#include "Contactors.h" +#include "Minions.h" +#include "Pedals.h" +#include "Tasks.h" +#include "common.h" +#include "config.h" +#include "os.h" static OS_TCB Task1TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE]; -void Task1(void *arg) -{ - CPU_Init(); - - CANbus_Init(CARCAN); - CAN_Queue_Init(); - // BSP_UART_Init(UART_2); - Pedals_Init(); - Minions_Init(); - Contactors_Init(); - - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - - OS_ERR err; - - // Initialize Telemetry - OSTaskCreate( - (OS_TCB*)&Telemetry_TCB, - (CPU_CHAR*)"Telemetry", - (OS_TASK_PTR)Task_Telemetry, - (void*)NULL, - (OS_PRIO)TASK_TELEMETRY_PRIO, - (CPU_STK*)Telemetry_Stk, - (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, - (CPU_STK_SIZE)TASK_TELEMETRY_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)0, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - bool lightState = false; - State contactorState = OFF; - - CANDATA_t msg; - - while (1){ - // Switches can be modified through hardware - - // Check light - lightState = !lightState; - Minions_Write(BRAKELIGHT, lightState); - - // Check contactors (HAVE NOTHING HOOKED UP TO CONTACTORS) - contactorState = contactorState == OFF ? ON : OFF; - Contactors_Set(MOTOR_CONTACTOR, contactorState, true); - Contactors_Set(ARRAY_CONTACTOR, contactorState, true); - - CANbus_Read(&msg, CAN_BLOCKING, CARCAN); - - OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); - // Use a logic analyzer to read the CAN line and see if the data shows up correctly - } +void Task1(void *arg) { + CPU_Init(); + + CANbus_Init(CARCAN); + CAN_Queue_Init(); + // BSP_UART_Init(UART_2); + Pedals_Init(); + Minions_Init(); + Contactors_Init(); + + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + + OS_ERR err; + + // Initialize Telemetry + OSTaskCreate( + (OS_TCB *)&Telemetry_TCB, (CPU_CHAR *)"Telemetry", + (OS_TASK_PTR)Task_Telemetry, (void *)NULL, (OS_PRIO)TASK_TELEMETRY_PRIO, + (CPU_STK *)Telemetry_Stk, (CPU_STK_SIZE)WATERMARK_STACK_LIMIT, + (CPU_STK_SIZE)TASK_TELEMETRY_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)0, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + bool lightState = false; + State contactorState = OFF; + + CANDATA_t msg; + + while (1) { + // Switches can be modified through hardware + + // Check light + lightState = !lightState; + Minions_Write(BRAKELIGHT, lightState); + + // Check contactors (HAVE NOTHING HOOKED UP TO CONTACTORS) + contactorState = contactorState == OFF ? ON : OFF; + Contactors_Set(MOTOR_CONTACTOR, contactorState, true); + Contactors_Set(ARRAY_CONTACTOR, contactorState, true); + + CANbus_Read(&msg, CAN_BLOCKING, CARCAN); + + OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); + // Use a logic analyzer to read the CAN line and see if the data shows up + // correctly + } } -int main() -{ - OS_ERR err; - OSInit(&err); - - // create tester thread - OSTaskCreate( - (OS_TCB *)&Task1TCB, - (CPU_CHAR *)"Task 1", - (OS_TASK_PTR)Task1, - (void *)NULL, - (OS_PRIO)5, - (CPU_STK *)Task1Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); +int main() { + OS_ERR err; + OSInit(&err); + + // create tester thread + OSTaskCreate((OS_TCB *)&Task1TCB, (CPU_CHAR *)"Task 1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)5, (CPU_STK *)Task1Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); } \ No newline at end of file diff --git a/Tests/Test_App_UpdateDisplay.c b/Tests/Test_App_UpdateDisplay.c index 072805e2b..71e44ea1a 100644 --- a/Tests/Test_App_UpdateDisplay.c +++ b/Tests/Test_App_UpdateDisplay.c @@ -1,187 +1,154 @@ +#include "Display.h" +#include "Tasks.h" #include "common.h" #include "config.h" #include "os.h" -#include "Tasks.h" -#include "Display.h" // #include "bsp.h" // #include "Contactors.h" -#include "UpdateDisplay.h" #include "FaultState.h" - - +#include "UpdateDisplay.h" static OS_TCB Task1TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE]; -void testBoolComp(UpdateDisplayError_t(*function)(bool)){ - OS_ERR e; +void testBoolComp(UpdateDisplayError_t (*function)(bool)) { + OS_ERR e; - function(false); - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(true); - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(false); - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(false); + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(true); + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(false); + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); } -void testPercentageComp(UpdateDisplayError_t(*function)(uint8_t)){ - OS_ERR e; - - function(0); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(25); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(50); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(75); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(100); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(0); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); +void testPercentageComp(UpdateDisplayError_t (*function)(uint8_t)) { + OS_ERR e; + + function(0); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(25); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(50); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(75); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(100); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(0); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); } -void testTriStateComp(UpdateDisplayError_t(*function)(TriState_t)){ - OS_ERR e; - - function(DISABLED); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(ENABLED); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(ACTIVE); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - function(DISABLED); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); +void testTriStateComp(UpdateDisplayError_t (*function)(TriState_t)) { + OS_ERR e; + + function(DISABLED); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(ENABLED); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(ACTIVE); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + function(DISABLED); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); } -void Task1(void *arg) -{ - DisplayError_t error; - - CPU_Init(); - error = Display_Init(); - assertDisplayError(error); - UpdateDisplay_Init(); - - - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - - OS_ERR e; - - OSTaskCreate( - (OS_TCB *)&UpdateDisplay_TCB, - (CPU_CHAR *)"UpdateDisplay_TCB", - (OS_TASK_PTR)Task_UpdateDisplay, - (void *)NULL, - (OS_PRIO)13, - (CPU_STK *)UpdateDisplay_Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&e); - assertOSError(OS_MAIN_LOC, e); - - OSTimeDlyHMSM(0, 0, 7, 0, OS_OPT_TIME_HMSM_STRICT, &e); - - testTriStateComp(&UpdateDisplay_SetGear); - - UpdateDisplay_SetVelocity(12); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - UpdateDisplay_SetVelocity(345); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - UpdateDisplay_SetVelocity(6789); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - - testTriStateComp(&UpdateDisplay_SetCruiseState); - testTriStateComp(&UpdateDisplay_SetRegenState); - testBoolComp(&UpdateDisplay_SetMotor); - testBoolComp(&UpdateDisplay_SetArray); - testPercentageComp(&UpdateDisplay_SetSOC); - - UpdateDisplay_SetSBPV(12); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - UpdateDisplay_SetSBPV(345); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - UpdateDisplay_SetSBPV(6789); - - OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); - - testPercentageComp(&UpdateDisplay_SetAccel); - - error = Display_Fault(OSErrLocBitmap, FaultBitmap); - assertDisplayError(error); - OSTimeDlyHMSM(0, 0, 3, 0, OS_OPT_TIME_HMSM_STRICT, &e); - - error = Display_Reset(); - assertDisplayError(error); - OSTimeDlyHMSM(0, 0, 3, 0, OS_OPT_TIME_HMSM_STRICT, &e); - - while (1) { - OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &e); - } +void Task1(void *arg) { + DisplayError_t error; + + CPU_Init(); + error = Display_Init(); + assertDisplayError(error); + UpdateDisplay_Init(); + + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + + OS_ERR e; + + OSTaskCreate((OS_TCB *)&UpdateDisplay_TCB, (CPU_CHAR *)"UpdateDisplay_TCB", + (OS_TASK_PTR)Task_UpdateDisplay, (void *)NULL, (OS_PRIO)13, + (CPU_STK *)UpdateDisplay_Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&e); + assertOSError(OS_MAIN_LOC, e); + + OSTimeDlyHMSM(0, 0, 7, 0, OS_OPT_TIME_HMSM_STRICT, &e); + + testTriStateComp(&UpdateDisplay_SetGear); + + UpdateDisplay_SetVelocity(12); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + UpdateDisplay_SetVelocity(345); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + UpdateDisplay_SetVelocity(6789); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + + testTriStateComp(&UpdateDisplay_SetCruiseState); + testTriStateComp(&UpdateDisplay_SetRegenState); + testBoolComp(&UpdateDisplay_SetMotor); + testBoolComp(&UpdateDisplay_SetArray); + testPercentageComp(&UpdateDisplay_SetSOC); + + UpdateDisplay_SetSBPV(12); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + UpdateDisplay_SetSBPV(345); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + UpdateDisplay_SetSBPV(6789); + + OSTimeDlyHMSM(0, 0, 0, 200, OS_OPT_TIME_HMSM_STRICT, &e); + + testPercentageComp(&UpdateDisplay_SetAccel); + + error = Display_Fault(OSErrLocBitmap, FaultBitmap); + assertDisplayError(error); + OSTimeDlyHMSM(0, 0, 3, 0, OS_OPT_TIME_HMSM_STRICT, &e); + + error = Display_Reset(); + assertDisplayError(error); + OSTimeDlyHMSM(0, 0, 3, 0, OS_OPT_TIME_HMSM_STRICT, &e); + + while (1) { + OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &e); + } }; -int main() -{ - OS_ERR err; - OSInit(&err); - OSSemCreate( // create fault sem4 - &FaultState_Sem4, - "Fault State Semaphore", - 0, - &err); - assertOSError(OS_MAIN_LOC, err); - - OSTaskCreate( // create fault task - (OS_TCB *)&FaultState_TCB, - (CPU_CHAR *)"Fault State", - (OS_TASK_PTR)&Task_FaultState, - (void *)NULL, - (OS_PRIO)1, - (CPU_STK *)FaultState_Stk, - (CPU_STK_SIZE)128 / 10, - (CPU_STK_SIZE)128, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - // create tester thread - OSTaskCreate( - (OS_TCB *)&Task1TCB, - (CPU_CHAR *)"Task 1", - (OS_TASK_PTR)Task1, - (void *)NULL, - (OS_PRIO)12, - (CPU_STK *)Task1Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); +int main() { + OS_ERR err; + OSInit(&err); + OSSemCreate( // create fault sem4 + &FaultState_Sem4, "Fault State Semaphore", 0, &err); + assertOSError(OS_MAIN_LOC, err); + + OSTaskCreate( // create fault task + (OS_TCB *)&FaultState_TCB, (CPU_CHAR *)"Fault State", + (OS_TASK_PTR)&Task_FaultState, (void *)NULL, (OS_PRIO)1, + (CPU_STK *)FaultState_Stk, (CPU_STK_SIZE)128 / 10, (CPU_STK_SIZE)128, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + // create tester thread + OSTaskCreate((OS_TCB *)&Task1TCB, (CPU_CHAR *)"Task 1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)12, (CPU_STK *)Task1Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); } \ No newline at end of file diff --git a/Tests/Test_BPSFault.c b/Tests/Test_BPSFault.c index 00e9c7b43..8dab1dce6 100644 --- a/Tests/Test_BPSFault.c +++ b/Tests/Test_BPSFault.c @@ -1,17 +1,17 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ -#include "Tasks.h" -#include "CANbus.h" +#include "CANConfig.h" #include "CAN_Queue.h" -#include "ReadCarCAN.h" -#include "Display.h" +#include "CANbus.h" #include "Contactors.h" -#include "CANConfig.h" +#include "Display.h" +#include "ReadCarCAN.h" +#include "Tasks.h" /* * NOTE: This test must be run with car CAN in loopback mode * TODO: automate this, either with arguments to BSP or #define -*/ + */ static OS_TCB Task1_TCB; #define STACK_SIZE 256 @@ -19,78 +19,51 @@ static CPU_STK Task1_Stk[STACK_SIZE]; #define CARCAN_FILTER_SIZE (sizeof carCANFilterList / sizeof(CANId_t)) -void Task1(){ - OS_ERR err; +void Task1() { + OS_ERR err; - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - CANbus_Init(CARCAN, carCANFilterList, CARCAN_FILTER_SIZE); - Contactors_Init(); + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + CANbus_Init(CARCAN, carCANFilterList, CARCAN_FILTER_SIZE); + Contactors_Init(); - // Send a BPS trip - CANDATA_t data = {.ID = BPS_TRIP, .idx = 0, .data = {1}}; - CANbus_Send(data, true, CARCAN); - OSTaskDel(NULL, &err); + // Send a BPS trip + CANDATA_t data = {.ID = BPS_TRIP, .idx = 0, .data = {1}}; + CANbus_Send(data, true, CARCAN); + OSTaskDel(NULL, &err); } -int main(){ - OS_ERR err; - OSInit(&err); - assertOSError(OS_MAIN_LOC, err); - TaskSwHook_Init(); +int main() { + OS_ERR err; + OSInit(&err); + assertOSError(OS_MAIN_LOC, err); + TaskSwHook_Init(); - OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)4, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); + OSSemCreate(&FaultState_Sem4, "Fault State Semaphore", 0, &err); + OSTaskCreate( + (OS_TCB*)&Task1_TCB, (CPU_CHAR*)"Task1", (OS_TASK_PTR)Task1, (void*)NULL, + (OS_PRIO)4, (CPU_STK*)Task1_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); - OSTaskCreate( - (OS_TCB*)&FaultState_TCB, - (CPU_CHAR*)"Fault State", - (OS_TASK_PTR)Task_FaultState, - (void*)NULL, - (OS_PRIO)TASK_FAULT_STATE_PRIO, - (CPU_STK*)FaultState_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); + OSTaskCreate( + (OS_TCB*)&FaultState_TCB, (CPU_CHAR*)"Fault State", + (OS_TASK_PTR)Task_FaultState, (void*)NULL, (OS_PRIO)TASK_FAULT_STATE_PRIO, + (CPU_STK*)FaultState_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); - OSTaskCreate( - (OS_TCB*)&ReadCarCAN_TCB, - (CPU_CHAR*)"ReadCarCAN", - (OS_TASK_PTR)Task_ReadCarCAN, - (void*)NULL, - (OS_PRIO)TASK_READ_CAR_CAN_PRIO, - (CPU_STK*)ReadCarCAN_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); + OSTaskCreate((OS_TCB*)&ReadCarCAN_TCB, (CPU_CHAR*)"ReadCarCAN", + (OS_TASK_PTR)Task_ReadCarCAN, (void*)NULL, + (OS_PRIO)TASK_READ_CAR_CAN_PRIO, (CPU_STK*)ReadCarCAN_Stk, + (CPU_STK_SIZE)STACK_SIZE / 10, (CPU_STK_SIZE)STACK_SIZE, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), + (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); - OSStart(&err); - assertOSError(OS_MAIN_LOC, err); + OSStart(&err); + assertOSError(OS_MAIN_LOC, err); } diff --git a/Tests/Test_BSP.c b/Tests/Test_BSP.c index 0a425afe3..00c4c7af7 100644 --- a/Tests/Test_BSP.c +++ b/Tests/Test_BSP.c @@ -1,17 +1,17 @@ /** * Test file that unifies all individual test files of the BSP module - * - * Run this test in conjunction with the simulator - * GUI. - * - */ + * + * Run this test in conjunction with the simulator + * GUI. + * + */ -#include "common.h" -#include "config.h" +#include #include #include "BSP_ADC.h" -#include +#include "common.h" +#include "config.h" #define TX_SIZE 128 #define LEN 4 @@ -19,208 +19,209 @@ * Moves the cursor n lines */ static void moveCursorUp(int n) { - if (n < 1) n = 1; - if (n > 99) n = 99; - printf("%c[%d;%dH", 0x1B, 1, 1); + if (n < 1) n = 1; + if (n > 99) n = 99; + printf("%c[%d;%dH", 0x1B, 1, 1); } int main() { + BSP_ADC_Init(Accelerator_ADC); + BSP_ADC_Init(Brake_ADC); + + BSP_CAN_Init(CAN_1); + // BSP_CAN_Init(CAN_2); + + BSP_Contactors_Init(MOTOR); + BSP_Contactors_Init(ARRAY); + + BSP_UART_Init(UART_3); + BSP_UART_Init(UART_2); + + BSP_Switches_Init(); + + uint8_t txData[LEN] = {0x00, 0x12, 0x42, 0x5A}; + + uint8_t txData2[LEN] = {0}; + uint8_t rxData[LEN] = {0}; + uint8_t rxData2[LEN] = {0}; + + int id2 = 0x321; + + bool negativeAcc = false; - BSP_ADC_Init(Accelerator_ADC); - BSP_ADC_Init(Brake_ADC); - - BSP_CAN_Init(CAN_1); - //BSP_CAN_Init(CAN_2); - - BSP_Contactors_Init(MOTOR); - BSP_Contactors_Init(ARRAY); - - BSP_UART_Init(UART_3); - BSP_UART_Init(UART_2); - - BSP_Switches_Init(); - - uint8_t txData[LEN] = {0x00, 0x12, 0x42, 0x5A}; - - uint8_t txData2[LEN] = {0}; - uint8_t rxData[LEN] = {0}; - uint8_t rxData2[LEN] = {0}; - - int id2 = 0x321; - - bool negativeAcc = false; - - int threshold = 1000 * 10; - int writevsread = threshold; - - while(1) { - printf("%c[%d;%dH", 0x1B, 1, 1); - printf("-----PLEASE RUN THIS TEST FILE IN CONJUCTION WITH THE GUI-----\n"); - //BSP_ADC TEST ----------------------------------------------------------- - printf("-------------------------ADC TEST-----------------------------\n"); - printf("--------------------------------------------------------------\n"); - printf("As you move the pedals in the GUI their voltage values should change\n"); - printf("Accelerator: %5.1d mV\tBrake: %5.1d mV\n", - BSP_ADC_Get_Millivoltage(Accelerator_ADC), BSP_ADC_Get_Millivoltage(Brake_ADC)); - - //BSP_CAN TEST ----------------------------------------------------------- - //NOTE: The can test file can test both CAN1 ot CAN2 and - // it has them in separate functions - printf("-------------------------CANs TEST----------------------------\n"); - printf("--------------------------------------------------------------\n"); - printf("CAN bus 1 sends an ID and Message, which can be seen in the GUI\n"); - // CAN 1 TEST - BASICALLY JUST COPIED IT FROM BSP_CAN.c - if(txData[3] > 0x70){ - negativeAcc = true; - }else if(txData[3] < 0x40){ - negativeAcc = false; - } - - if(!negativeAcc){ - txData[3] = txData[3]+1; - }else{ - txData[3] = txData[3]-1; - } - - uint32_t id; - BSP_CAN_Write(CAN_1, 0x221, txData, LEN); - - uint8_t len = BSP_CAN_Read(CAN_1, &id, rxData); - - // printf("ID: 0x%x\nData: ", id); - // for (uint8_t i = 0; i < len; i++) { - // printf("0x%x ", rxData[i]); - // } - // printf("\n"); - - // CAN 2 test by random values - - if(writevsread == threshold){ - writevsread = 0; - id2 = rand() % 0xFFF; - for(int i = 0; i < LEN; i++){ - txData2[i] = rand() % 0xFF; - } - } else { - writevsread++; - } - - - uint8_t can2len = BSP_CAN_Read(CAN_2, &id2, rxData2); - printf("Values read from CAN_2\n"); - printf("ID: 0x%x length: %d\n", id2, can2len); - for(int i = 0; i < can2len; i++){ - printf("0x%x ", rxData2[i]); - fflush(stdout); - } - printf("\n"); - - //BSP_SWITCHES TEST ------------------------------------------------------ - printf("---------------------- SWITCHES TEST--------------------------\n"); - printf("--------------------------------------------------------------\n"); - printf("Press switches on the GUI, their state will be displayed:\n"); - printf("LT\tRT HDLT FWD_REV HZD CRS_SET CRS_EN REGEN IGN_1 IGN_2\n"); - printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", - BSP_Switches_Read(LT), - BSP_Switches_Read(RT), - BSP_Switches_Read(HDLT), - BSP_Switches_Read(FWD_REV), - BSP_Switches_Read(HZD), - BSP_Switches_Read(CRS_SET), - BSP_Switches_Read(CRS_EN), - BSP_Switches_Read(REGEN), - BSP_Switches_Read(IGN_1), - BSP_Switches_Read(IGN_2)); - - //BSP_UART TEST ---------------------------------------------------------- - //printf("--------------------------------------------------------------\n"); - printf("-------------------------UART TEST----------------------------\n"); - printf("--------------------------------------------------------------\n"); - printf("The UARTs are showing random values, UART 1 communicates with\n"); - printf("the gecko display while UART2 with the user direclty \n"); - - uint8_t byte1 = rand() % 256; - uint8_t byte2 = rand() % 256; - uint8_t byte3 = rand() % 256; - uint8_t byte4 = rand() % 256; - uint8_t byte5 = rand() % 256; - char str[TX_SIZE]; - sprintf(str, "%x%x%x%x%x", byte1, byte2, byte3, byte4, byte5); - - float speed1 = (rand() % 500) / 10.0; - int cruiseEn1 = rand() % 2; - int cruiseSet1 = rand() % 2; - int regenEn1 = rand() % 2; - int CANerr1 = rand() % 10; - char str1[TX_SIZE]; - sprintf(str1, "%f, %d, %d, %d, %d", speed1, cruiseEn1, cruiseSet1, regenEn1, CANerr1); - - BSP_UART_Write(UART_2, str , TX_SIZE); - BSP_UART_Write(UART_3, str1, TX_SIZE); - - char out[2][TX_SIZE]; - BSP_UART_Read(UART_3, out[UART_3]); - BSP_UART_Read(UART_2, out[UART_2]); - out[UART_3][strlen(out[UART_3])-1] = 0; // remove the newline, so we can print both in one line for now - out[UART_2][strlen(out[UART_2])-1] = 0; - - printf(" SPEED CRS_EN CRS_SET REGEN CAN_ERROR\n"); - printf("UART 1: %s\n", out[UART_3]); - printf("UART 2: %s\n", out[UART_2]); - - //BSP_Contactors ----------------------------------------------------- - //NOTE: The contactors test file requires input from the user in the original test - printf("----------------------CONTACTORS TEST-------------------------\n"); - printf("--------------------------------------------------------------\n"); - printf("Contactors are constantly updated randomly, changes also visible in GUI\n"); - int motorOrArray = rand() % 2; - int onOrOff = rand() % 2; - if (motorOrArray == 1){ - if (onOrOff == 1){ - BSP_Contactors_Set(MOTOR, ON); - } - else { - BSP_Contactors_Set(MOTOR, OFF); - } - }else{ - if (onOrOff == 1){ - BSP_Contactors_Set(ARRAY, ON); - } - else { - BSP_Contactors_Set(ARRAY, OFF); - } - } - printf("MOTOR CONTACTOR STATE: %d\n", BSP_Contactors_Get(MOTOR)); - printf("ARRAY CONTACTOR STATE: %d\n", BSP_Contactors_Get(ARRAY)); - - //BSP_Precharge ----------------------------------------------------- - //NOTE: The precharge test file requires input from the user in the original test - printf("----------------------Precharge TEST--------------------------\n"); - printf("--------------------------------------------------------------\n"); - int precCase = rand() % 4; - - switch (precCase) - { - case 0: - BSP_Precharge_Write(MOTOR_PRECHARGE, OFF); - printf("MOTOR PRECHARGE STATE SET TO: OFF\n"); - break; - case 1: - BSP_Precharge_Write(MOTOR_PRECHARGE, ON); - printf("MOTOR PRECHARGE STATE SET TO: ON \n"); - break; - case 2: - BSP_Precharge_Write(ARRAY_PRECHARGE, OFF); - printf("ARRAY PRECHARGE STATE SET TO: OFF\n"); - break; - case 3: - BSP_Precharge_Write(ARRAY_PRECHARGE, ON); - printf("ARRAY PRECHARGE STATE SET TO: ON \n"); - break; - default: - break; - } + int threshold = 1000 * 10; + int writevsread = threshold; + + while (1) { + printf("%c[%d;%dH", 0x1B, 1, 1); + printf("-----PLEASE RUN THIS TEST FILE IN CONJUCTION WITH THE GUI-----\n"); + // BSP_ADC TEST ----------------------------------------------------------- + printf("-------------------------ADC TEST-----------------------------\n"); + printf("--------------------------------------------------------------\n"); + printf( + "As you move the pedals in the GUI their voltage values should " + "change\n"); + printf("Accelerator: %5.1d mV\tBrake: %5.1d mV\n", + BSP_ADC_Get_Millivoltage(Accelerator_ADC), + BSP_ADC_Get_Millivoltage(Brake_ADC)); + + // BSP_CAN TEST ----------------------------------------------------------- + // NOTE: The can test file can test both CAN1 ot CAN2 and + // it has them in separate functions + printf("-------------------------CANs TEST----------------------------\n"); + printf("--------------------------------------------------------------\n"); + printf("CAN bus 1 sends an ID and Message, which can be seen in the GUI\n"); + // CAN 1 TEST - BASICALLY JUST COPIED IT FROM BSP_CAN.c + if (txData[3] > 0x70) { + negativeAcc = true; + } else if (txData[3] < 0x40) { + negativeAcc = false; + } + + if (!negativeAcc) { + txData[3] = txData[3] + 1; + } else { + txData[3] = txData[3] - 1; + } + + uint32_t id; + BSP_CAN_Write(CAN_1, 0x221, txData, LEN); + + uint8_t len = BSP_CAN_Read(CAN_1, &id, rxData); + + // printf("ID: 0x%x\nData: ", id); + // for (uint8_t i = 0; i < len; i++) { + // printf("0x%x ", rxData[i]); + // } + // printf("\n"); + + // CAN 2 test by random values + + if (writevsread == threshold) { + writevsread = 0; + id2 = rand() % 0xFFF; + for (int i = 0; i < LEN; i++) { + txData2[i] = rand() % 0xFF; + } + } else { + writevsread++; + } + + uint8_t can2len = BSP_CAN_Read(CAN_2, &id2, rxData2); + printf("Values read from CAN_2\n"); + printf("ID: 0x%x length: %d\n", id2, can2len); + for (int i = 0; i < can2len; i++) { + printf("0x%x ", rxData2[i]); + fflush(stdout); } printf("\n"); - return 0; + + // BSP_SWITCHES TEST ------------------------------------------------------ + printf("---------------------- SWITCHES TEST--------------------------\n"); + printf("--------------------------------------------------------------\n"); + printf("Press switches on the GUI, their state will be displayed:\n"); + printf( + "LT\tRT HDLT FWD_REV HZD CRS_SET CRS_EN REGEN IGN_1 " + "IGN_2\n"); + printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", BSP_Switches_Read(LT), + BSP_Switches_Read(RT), BSP_Switches_Read(HDLT), + BSP_Switches_Read(FWD_REV), BSP_Switches_Read(HZD), + BSP_Switches_Read(CRS_SET), BSP_Switches_Read(CRS_EN), + BSP_Switches_Read(REGEN), BSP_Switches_Read(IGN_1), + BSP_Switches_Read(IGN_2)); + + // BSP_UART TEST ---------------------------------------------------------- + // printf("--------------------------------------------------------------\n"); + printf("-------------------------UART TEST----------------------------\n"); + printf("--------------------------------------------------------------\n"); + printf("The UARTs are showing random values, UART 1 communicates with\n"); + printf("the gecko display while UART2 with the user direclty \n"); + + uint8_t byte1 = rand() % 256; + uint8_t byte2 = rand() % 256; + uint8_t byte3 = rand() % 256; + uint8_t byte4 = rand() % 256; + uint8_t byte5 = rand() % 256; + char str[TX_SIZE]; + sprintf(str, "%x%x%x%x%x", byte1, byte2, byte3, byte4, byte5); + + float speed1 = (rand() % 500) / 10.0; + int cruiseEn1 = rand() % 2; + int cruiseSet1 = rand() % 2; + int regenEn1 = rand() % 2; + int CANerr1 = rand() % 10; + char str1[TX_SIZE]; + sprintf(str1, "%f, %d, %d, %d, %d", speed1, cruiseEn1, cruiseSet1, regenEn1, + CANerr1); + + BSP_UART_Write(UART_2, str, TX_SIZE); + BSP_UART_Write(UART_3, str1, TX_SIZE); + + char out[2][TX_SIZE]; + BSP_UART_Read(UART_3, out[UART_3]); + BSP_UART_Read(UART_2, out[UART_2]); + out[UART_3][strlen(out[UART_3]) - 1] = + 0; // remove the newline, so we can print both in one line for now + out[UART_2][strlen(out[UART_2]) - 1] = 0; + + printf(" SPEED CRS_EN CRS_SET REGEN CAN_ERROR\n"); + printf("UART 1: %s\n", out[UART_3]); + printf("UART 2: %s\n", out[UART_2]); + + // BSP_Contactors ----------------------------------------------------- + // NOTE: The contactors test file requires input from the user in the + // original test + printf("----------------------CONTACTORS TEST-------------------------\n"); + printf("--------------------------------------------------------------\n"); + printf( + "Contactors are constantly updated randomly, changes also visible in " + "GUI\n"); + int motorOrArray = rand() % 2; + int onOrOff = rand() % 2; + if (motorOrArray == 1) { + if (onOrOff == 1) { + BSP_Contactors_Set(MOTOR, ON); + } else { + BSP_Contactors_Set(MOTOR, OFF); + } + } else { + if (onOrOff == 1) { + BSP_Contactors_Set(ARRAY, ON); + } else { + BSP_Contactors_Set(ARRAY, OFF); + } + } + printf("MOTOR CONTACTOR STATE: %d\n", BSP_Contactors_Get(MOTOR)); + printf("ARRAY CONTACTOR STATE: %d\n", BSP_Contactors_Get(ARRAY)); + + // BSP_Precharge ----------------------------------------------------- + // NOTE: The precharge test file requires input from the user in the + // original test + printf("----------------------Precharge TEST--------------------------\n"); + printf("--------------------------------------------------------------\n"); + int precCase = rand() % 4; + + switch (precCase) { + case 0: + BSP_Precharge_Write(MOTOR_PRECHARGE, OFF); + printf("MOTOR PRECHARGE STATE SET TO: OFF\n"); + break; + case 1: + BSP_Precharge_Write(MOTOR_PRECHARGE, ON); + printf("MOTOR PRECHARGE STATE SET TO: ON \n"); + break; + case 2: + BSP_Precharge_Write(ARRAY_PRECHARGE, OFF); + printf("ARRAY PRECHARGE STATE SET TO: OFF\n"); + break; + case 3: + BSP_Precharge_Write(ARRAY_PRECHARGE, ON); + printf("ARRAY PRECHARGE STATE SET TO: ON \n"); + break; + default: + break; + } + } + printf("\n"); + return 0; } diff --git a/Tests/Test_BSP_ADC.c b/Tests/Test_BSP_ADC.c index a2515fb1d..21bd53e05 100644 --- a/Tests/Test_BSP_ADC.c +++ b/Tests/Test_BSP_ADC.c @@ -1,38 +1,42 @@ /** * Test file for library to interact with hardware connected to ADC - * - * Run this test in conjunction with the simulator + * + * Run this test in conjunction with the simulator * GUI. As you move the accelerator and brake on the GUI, the respective - * pressed/slided percentage will change from '0' to '100' on the terminal and display - * to show that sliding the pedals is read by the BSP - * - * Uncomment the "individual tests for each function" to determine the output of individual functions - * of the ADC module - */ + * pressed/slided percentage will change from '0' to '100' on the terminal and + * display to show that sliding the pedals is read by the BSP + * + * Uncomment the "individual tests for each function" to determine the output of + * individual functions of the ADC module + */ + +#include -#include "common.h" -#include "config.h" #include "BSP_ADC.h" #include "BSP_UART.h" -#include +#include "common.h" +#include "config.h" int main() { - BSP_ADC_Init(); - BSP_UART_Init(UART_2); - - /* INDIVIDUAL TESTS FOR EACH FUNCTION - printf("TESTS\n"); + BSP_ADC_Init(); + BSP_UART_Init(UART_2); + + /* INDIVIDUAL TESTS FOR EACH FUNCTION + printf("TESTS\n"); - printf("TESTING VALUE AND MILLIVOLTAGE\n"); - printf("ADC at channel of Accelerator is %d\n", BSP_ADC_Get_Value(Accelerator)); - printf("ADC at channel of Brake is %d\n", BSP_ADC_Get_Value(Brake)); + printf("TESTING VALUE AND MILLIVOLTAGE\n"); + printf("ADC at channel of Accelerator is %d\n", + BSP_ADC_Get_Value(Accelerator)); printf("ADC at channel of Brake is %d\n", + BSP_ADC_Get_Value(Brake)); - printf("ADC at channel of Accelerator in mV is %d\n", BSP_ADC_Get_Millivoltage(Accelerator)); - printf("ADC at channel of Brake in mV is %d\n", BSP_ADC_Get_Millivoltage(Brake)); - */ + printf("ADC at channel of Accelerator in mV is %d\n", + BSP_ADC_Get_Millivoltage(Accelerator)); printf("ADC at channel of Brake in mV + is %d\n", BSP_ADC_Get_Millivoltage(Brake)); + */ - while(1) { - printf("Accelerator: %5.1d\tBrake: %5.1d\r", - BSP_ADC_Get_Millivoltage(Accelerator_ADC), BSP_ADC_Get_Millivoltage(Brake_ADC)); - } + while (1) { + printf("Accelerator: %5.1d\tBrake: %5.1d\r", + BSP_ADC_Get_Millivoltage(Accelerator_ADC), + BSP_ADC_Get_Millivoltage(Brake_ADC)); + } } diff --git a/Tests/Test_BSP_CAN.c b/Tests/Test_BSP_CAN.c old mode 100755 new mode 100644 index 1939d07f9..92d65ae61 --- a/Tests/Test_BSP_CAN.c +++ b/Tests/Test_BSP_CAN.c @@ -1,45 +1,43 @@ -#include "common.h" #include "CANbus.h" +#include "common.h" #include "config.h" -int generalTest(void){ - // Tests sending and receiving messages - uint32_t ids[10] = {0x242, 0x243, 0x244, 0x245, 0x246, 0x247, 0x24B, 0x24E, 0x580, CHARGE_ENABLE}; - uint8_t buffer[8]; +int generalTest(void) { + // Tests sending and receiving messages + uint32_t ids[10] = {0x242, 0x243, 0x244, 0x245, 0x246, + 0x247, 0x24B, 0x24E, 0x580, CHARGE_ENABLE}; + uint8_t buffer[8]; - CANData_t data; - data.d = 0x87654321; + CANData_t data; + data.d = 0x87654321; - CANPayload_t payload; - payload.data = data; - payload.bytes = 4; + CANPayload_t payload; + payload.data = data; + payload.bytes = 4; - for(int i=0; i + #include "common.h" #include "config.h" -#include int main(void) { + BSP_GPIO_Init(PORTA, 0xFFFF, 0); + BSP_GPIO_Init(PORTB, 0xFFFF, 1); - BSP_GPIO_Init(PORTA, 0xFFFF, 0); - BSP_GPIO_Init(PORTB, 0xFFFF, 1); - - char port; - char rw; - uint16_t data = 0; + char port; + char rw; + uint16_t data = 0; - while (1) { - /*printf("Choose Port (A, B): "); - port = getchar(); - getchar(); + while (1) { + /*printf("Choose Port (A, B): "); + port = getchar(); + getchar(); - if (port - 'A' >= NUM_PORTS) { - printf("Invalid input\n\r"); - continue; - }*/ + if (port - 'A' >= NUM_PORTS) { + printf("Invalid input\n\r"); + continue; + }*/ - printf("All pins in Port A are only read, all pins in Port B are only write\n"); - printf("Choose read (from Port A) or write (to Port B)(r/w): "); - rw = getchar(); - getchar(); + printf( + "All pins in Port A are only read, all pins in Port B are only " + "write\n"); + printf("Choose read (from Port A) or write (to Port B)(r/w): "); + rw = getchar(); + getchar(); - if (rw == 'r' || rw == 'R') { - port = 'A'; - data = BSP_GPIO_Read(PORTA); - printf("Data from Port A: %d\n\r\n\r", data); - } else if (rw == 'w' || rw == 'W') { - printf("Enter the data: "); - char buffer[10] = {0}; - fgets(buffer, 10, stdin); - data = atoi(buffer); - printf("\n\r"); - BSP_GPIO_Write(PORTB, data); - } else { - printf("Invalid input\n\r"); - } + if (rw == 'r' || rw == 'R') { + port = 'A'; + data = BSP_GPIO_Read(PORTA); + printf("Data from Port A: %d\n\r\n\r", data); + } else if (rw == 'w' || rw == 'W') { + printf("Enter the data: "); + char buffer[10] = {0}; + fgets(buffer, 10, stdin); + data = atoi(buffer); + printf("\n\r"); + BSP_GPIO_Write(PORTB, data); + } else { + printf("Invalid input\n\r"); } + } } \ No newline at end of file diff --git a/Tests/Test_BSP_UART.c b/Tests/Test_BSP_UART.c index 2e9e2fcdd..2a472c223 100644 --- a/Tests/Test_BSP_UART.c +++ b/Tests/Test_BSP_UART.c @@ -1,34 +1,40 @@ /** * Test file for library to interact with UART - * + * * Run this test in conjunction with the simulator * GUI. As this generates randomized values, the display * will update the values accordingly to show that the * display is reading the UART properly */ +#include + #include "common.h" #include "config.h" -#include #define TX_SIZE 128 int main(void) { - // BSP_UART_Init(UART_2); - BSP_UART_Init(UART_3); - const char* testStr = "\xff\xff\xffpage 2\xff\xff\xff"; - BSP_UART_Write(UART_3, (char*) testStr , strlen(testStr)); - while (1) {volatile int x=0; x++;} + // BSP_UART_Init(UART_2); + BSP_UART_Init(UART_3); + const char* testStr = "\xff\xff\xffpage 2\xff\xff\xff"; + BSP_UART_Write(UART_3, (char*)testStr, strlen(testStr)); + while (1) { + volatile int x = 0; + x++; + } - // char out[2][TX_SIZE]; - // BSP_UART_Read(UART_2, out[UART_2]); - // BSP_UART_Read(UART_3, out[UART_3]); - // out[UART_2][strlen(out[UART_2])-1] = 0; // remove the newline, so we can print both in one line for now - // out[UART_3][strlen(out[UART_3])-1] = 0; - // printf("UART 2: %s\tUART 3: %s\r", out[UART_2], out[UART_3]); - // /* - // * If a long message is sent before a short message, the messages will overlap - // * on the display. This is not an issue with UART_2, but just a consequence of - // * how these tests must be structured and outputted. - // */ + // char out[2][TX_SIZE]; + // BSP_UART_Read(UART_2, out[UART_2]); + // BSP_UART_Read(UART_3, out[UART_3]); + // out[UART_2][strlen(out[UART_2])-1] = 0; // remove the newline, so we can + // print both in one line for now out[UART_3][strlen(out[UART_3])-1] = 0; + // printf("UART 2: %s\tUART 3: %s\r", out[UART_2], out[UART_3]); + // /* + // * If a long message is sent before a short message, the messages will + // overlap + // * on the display. This is not an issue with UART_2, but just a consequence + // of + // * how these tests must be structured and outputted. + // */ } diff --git a/Tests/Test_CANFilter_ReadBPS.c b/Tests/Test_CANFilter_ReadBPS.c index f24522946..87d1449f8 100644 --- a/Tests/Test_CANFilter_ReadBPS.c +++ b/Tests/Test_CANFilter_ReadBPS.c @@ -1,7 +1,7 @@ -#include "Tasks.h" -#include "CANbus.h" #include "BSP_UART.h" #include "CANConfig.h" +#include "CANbus.h" +#include "Tasks.h" static OS_TCB Task1_TCB; static CPU_STK Task1_Stk[128]; @@ -10,62 +10,46 @@ static CPU_STK Task1_Stk[128]; #define CARCAN_FILTER_SIZE (sizeof carCANFilterList / sizeof(CANId_t)) +void Task1(void *p_arg) { + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + CANbus_Init(CARCAN, carCANFilterList, NUM_CARCAN_FILTERS); -void Task1(void *p_arg){ - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - CANbus_Init(CARCAN, carCANFilterList, NUM_CARCAN_FILTERS); - - CANDATA_t dataBuf; // A buffer in which we can store the messages we read - - while(1){ - - ErrorStatus status = CANbus_Read(&dataBuf, true, CARCAN); - if (status != SUCCESS) { - printf("CANbus Read error status: %d\n\r", status); - } - - // Print the message ID we receive out - printf("%d\n\r", dataBuf.ID); + CANDATA_t dataBuf; // A buffer in which we can store the messages we read - + while (1) { + ErrorStatus status = CANbus_Read(&dataBuf, true, CARCAN); + if (status != SUCCESS) { + printf("CANbus Read error status: %d\n\r", status); } + // Print the message ID we receive out + printf("%d\n\r", dataBuf.ID); + } } -int main(void){ //initialize things and spawn task - OS_ERR err; - OSInit(&err); - if(err != OS_ERR_NONE){ - printf("OS error code %d\n\r",err); - } - - BSP_UART_Init(UART_2); - - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)4, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - - - if (err != OS_ERR_NONE) { - printf("Task1 error code %d\n\r", err); - } - OSStart(&err); - if (err != OS_ERR_NONE) { - printf("OS error code %d\n\r", err); - } - return 0; - +int main(void) { // initialize things and spawn task + OS_ERR err; + OSInit(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n\r", err); + } + + BSP_UART_Init(UART_2); + + OSTaskCreate((OS_TCB *)&Task1_TCB, (CPU_CHAR *)"Task1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)4, (CPU_STK *)Task1_Stk, + (CPU_STK_SIZE)STACK_SIZE / 10, (CPU_STK_SIZE)STACK_SIZE, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), + (OS_ERR *)&err); + + if (err != OS_ERR_NONE) { + printf("Task1 error code %d\n\r", err); + } + OSStart(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n\r", err); + } + return 0; } \ No newline at end of file diff --git a/Tests/Test_Driver_CANbus.c b/Tests/Test_Driver_CANbus.c index c410484d9..ec71da5ce 100644 --- a/Tests/Test_Driver_CANbus.c +++ b/Tests/Test_Driver_CANbus.c @@ -1,7 +1,7 @@ -#include "Tasks.h" -#include "CANbus.h" #include "BSP_UART.h" #include "CANConfig.h" +#include "CANbus.h" +#include "Tasks.h" static OS_TCB Task1_TCB; static CPU_STK Task1_Stk[128]; @@ -21,86 +21,80 @@ uint64_t data = 0xdeadbeef12345678; */ void Task1(void *p_arg) { - (void) p_arg; + (void)p_arg; - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - CANbus_Init(CARCAN, carCANFilterList, NUM_CARCAN_FILTERS); + CANbus_Init(CARCAN, carCANFilterList, NUM_CARCAN_FILTERS); - dataBuf.ID = CHARGE_ENABLE; // First, send a value that we want to be able to receive - memcpy(&dataBuf.data, &data, sizeof data); + dataBuf.ID = + CHARGE_ENABLE; // First, send a value that we want to be able to receive + memcpy(&dataBuf.data, &data, sizeof data); - assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); - assert(memcmp(&dataBuf, &resultBuf, sizeof dataBuf) == 0); + assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); + assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); + assert(memcmp(&dataBuf, &resultBuf, sizeof dataBuf) == 0); - dataBuf.ID = ODOMETER_AMPHOURS; // Now, send a message that we shouldn't receive - assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - assert(CANbus_Read(&resultBuf, false, CARCAN) == ERROR); // check that no can message is read + dataBuf.ID = + ODOMETER_AMPHOURS; // Now, send a message that we shouldn't receive + assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); + assert(CANbus_Read(&resultBuf, false, CARCAN) == + ERROR); // check that no can message is read - // Now send a bunch of messages that should be ignored, followed by those that shouldn't - for (int i=0; i<10; i++) { - assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - } - - dataBuf.ID = BPS_TRIP; - assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - dataBuf.ID = STATE_OF_CHARGE; - assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - dataBuf.ID = SUPPLEMENTAL_VOLTAGE; + // Now send a bunch of messages that should be ignored, followed by those that + // shouldn't + for (int i = 0; i < 10; i++) { assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); - - // Make sure that only three messages make it through - assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); - assert(resultBuf.ID == BPS_TRIP); - assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); - assert(resultBuf.ID == STATE_OF_CHARGE); - assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); - assert(resultBuf.ID == SUPPLEMENTAL_VOLTAGE); - assert(CANbus_Read(&resultBuf, false, CARCAN) == ERROR); // No more messages - - printf("Success!\r\n"); - for (;;); + } + + dataBuf.ID = BPS_TRIP; + assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); + dataBuf.ID = STATE_OF_CHARGE; + assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); + dataBuf.ID = SUPPLEMENTAL_VOLTAGE; + assert(CANbus_Send(dataBuf, true, CARCAN) == SUCCESS); + + // Make sure that only three messages make it through + assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); + assert(resultBuf.ID == BPS_TRIP); + assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); + assert(resultBuf.ID == STATE_OF_CHARGE); + assert(CANbus_Read(&resultBuf, true, CARCAN) == SUCCESS); + assert(resultBuf.ID == SUPPLEMENTAL_VOLTAGE); + assert(CANbus_Read(&resultBuf, false, CARCAN) == ERROR); // No more messages + + printf("Success!\r\n"); + for (;;) + ; } -int main(void){ //initialize things and spawn task - OS_ERR err; - OSInit(&err); - if(err != OS_ERR_NONE){ - printf("OS error code %d\n\r",err); - } - - BSP_UART_Init(UART_2); - - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)4, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - - - if (err != OS_ERR_NONE) { - printf("Task1 error code %d\n\r", err); - } - OSStart(&err); - if (err != OS_ERR_NONE) { - printf("OS error code %d\n\r", err); - } - return 0; - +int main(void) { // initialize things and spawn task + OS_ERR err; + OSInit(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n\r", err); + } + + BSP_UART_Init(UART_2); + + OSTaskCreate((OS_TCB *)&Task1_TCB, (CPU_CHAR *)"Task1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)4, (CPU_STK *)Task1_Stk, + (CPU_STK_SIZE)STACK_SIZE / 10, (CPU_STK_SIZE)STACK_SIZE, + (OS_MSG_QTY)0, (OS_TICK)NULL, (void *)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), + (OS_ERR *)&err); + + if (err != OS_ERR_NONE) { + printf("Task1 error code %d\n\r", err); + } + OSStart(&err); + if (err != OS_ERR_NONE) { + printf("OS error code %d\n\r", err); + } + return 0; } static void assert(bool cond) { - if (!cond) __asm("bkpt"); + if (!cond) __asm("bkpt"); } diff --git a/Tests/Test_Driver_Contactors.c b/Tests/Test_Driver_Contactors.c index 5b1977452..945524543 100644 --- a/Tests/Test_Driver_Contactors.c +++ b/Tests/Test_Driver_Contactors.c @@ -1,55 +1,44 @@ -#include "Tasks.h" #include "Contactors.h" +#include "Tasks.h" static OS_TCB Task1_TCB; #define STACK_SIZE 128 static CPU_STK Task1_Stk[STACK_SIZE]; - /* * When running this test on the motor testbench, hardcode the SendTritium task * to always send an unobtainable velocity. This ensures that no regen braking * takes place */ -void Task1(){ - OS_ERR err; +void Task1() { + OS_ERR err; - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U) OSCfg_TickRate_Hz); - BSP_UART_Init(UART_2); - Contactors_Init(); + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + BSP_UART_Init(UART_2); + Contactors_Init(); - for (;;) { - bool set = Contactors_Get(ARRAY_CONTACTOR); - printf("Turning contactor %s\r\n", set ? "off" : "on"); - Contactors_Set(ARRAY_CONTACTOR, !set, true); - OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); - } + for (;;) { + bool set = Contactors_Get(ARRAY_CONTACTOR); + printf("Turning contactor %s\r\n", set ? "off" : "on"); + Contactors_Set(ARRAY_CONTACTOR, !set, true); + OSTimeDlyHMSM(0, 0, 5, 0, OS_OPT_TIME_HMSM_STRICT, &err); + } } -int main(){ - OS_ERR err; - OSInit(&err); - assertOSError(OS_MAIN_LOC, err); - - OSTaskCreate( - (OS_TCB*)&Task1_TCB, - (CPU_CHAR*)"Task1", - (OS_TASK_PTR)Task1, - (void*)NULL, - (OS_PRIO)2, - (CPU_STK*)Task1_Stk, - (CPU_STK_SIZE)STACK_SIZE/10, - (CPU_STK_SIZE)STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void*)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR|OS_OPT_TASK_STK_CHK), - (OS_ERR*)&err - ); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); - assertOSError(OS_MAIN_LOC, err); +int main() { + OS_ERR err; + OSInit(&err); + assertOSError(OS_MAIN_LOC, err); + + OSTaskCreate( + (OS_TCB*)&Task1_TCB, (CPU_CHAR*)"Task1", (OS_TASK_PTR)Task1, (void*)NULL, + (OS_PRIO)2, (CPU_STK*)Task1_Stk, (CPU_STK_SIZE)STACK_SIZE / 10, + (CPU_STK_SIZE)STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, (void*)NULL, + (OS_OPT)(OS_OPT_TASK_STK_CLR | OS_OPT_TASK_STK_CHK), (OS_ERR*)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); + assertOSError(OS_MAIN_LOC, err); } \ No newline at end of file diff --git a/Tests/Test_Driver_Display.c b/Tests/Test_Driver_Display.c index 2eaca7059..02a5684c9 100644 --- a/Tests/Test_Driver_Display.c +++ b/Tests/Test_Driver_Display.c @@ -22,121 +22,100 @@ /** * Enum and corresponding array for easy component selection. */ -typedef enum -{ - // Boolean components - LEFT = 0, - HEAD, - RIGHT, - HZD, - ARRAY, - MOTOR, - // Non-boolean components - VELOCITY, - ACCEL_METER, - SOC, - SUPP_BATT, - CRUISE_ST, - REGEN_ST, - GEAR, - // Fault code components - OS_CODE, - FAULT_CODE +typedef enum { + // Boolean components + LEFT = 0, + HEAD, + RIGHT, + HZD, + ARRAY, + MOTOR, + // Non-boolean components + VELOCITY, + ACCEL_METER, + SOC, + SUPP_BATT, + CRUISE_ST, + REGEN_ST, + GEAR, + // Fault code components + OS_CODE, + FAULT_CODE } Component_t; static char *compStrings[15] = { - // Boolean components - "ltime", - "head", - "rtime", - "hzd", - "arr", - "mot", - // Non-boolean components - "vel", - "accel", - "soc", - "supp", - "cruiseSt", - "rbsSt", - "gear", - // Fault code components - "oserr", - "faulterr"}; + // Boolean components + "ltime", "head", "rtime", "hzd", "arr", "mot", + // Non-boolean components + "vel", "accel", "soc", "supp", "cruiseSt", "rbsSt", "gear", + // Fault code components + "oserr", "faulterr"}; // Delay; Don't know how long -void delay(void) -{ - volatile int j; - for (j = 0; j < 9999999; j++) - { - continue; - } +void delay(void) { + volatile int j; + for (j = 0; j < 9999999; j++) { + continue; + } } -int main() -{ - DisplayError_t err; +int main() { + DisplayError_t err; - err = Display_Init(); - assertDisplayError(err); - delay(); + err = Display_Init(); + assertDisplayError(err); + delay(); - // Display the fault page - DisplayCmd_t pgCmd = { - .compOrCmd = "page", - .attr = NULL, - .op = NULL, - .numArgs = 1, - .argTypes = {true}, - {{.num = FAULT}}}; - err = Display_Send(pgCmd); - //assertDisplayError(err); - delay(); + // Display the fault page + DisplayCmd_t pgCmd = {.compOrCmd = "page", + .attr = NULL, + .op = NULL, + .numArgs = 1, + .argTypes = {true}, + {{.num = FAULT}}}; + err = Display_Send(pgCmd); + // assertDisplayError(err); + delay(); - // Display the info page - pgCmd = (DisplayCmd_t){ - .compOrCmd = "page", - .attr = NULL, - .op = NULL, - .numArgs = 1, - .argTypes = {true}, - {{.num = INFO}}}; - err = Display_Send(pgCmd); - //assertDisplayError(err); - delay(); + // Display the info page + pgCmd = (DisplayCmd_t){.compOrCmd = "page", + .attr = NULL, + .op = NULL, + .numArgs = 1, + .argTypes = {true}, + {{.num = INFO}}}; + err = Display_Send(pgCmd); + // assertDisplayError(err); + delay(); - // Show the array icon - DisplayCmd_t toggleCmd = { - .compOrCmd = "vis", - .attr = NULL, - .op = NULL, - .numArgs = 2, - .argTypes = {STR_ARG, INT_ARG}, - {{.str = compStrings[ARRAY]}, {.num = 1}}}; - err = Display_Send(toggleCmd); - //assertDisplayError(err); - delay(); + // Show the array icon + DisplayCmd_t toggleCmd = {.compOrCmd = "vis", + .attr = NULL, + .op = NULL, + .numArgs = 2, + .argTypes = {STR_ARG, INT_ARG}, + {{.str = compStrings[ARRAY]}, {.num = 1}}}; + err = Display_Send(toggleCmd); + // assertDisplayError(err); + delay(); - // Don't show the array icon - toggleCmd = (DisplayCmd_t){ - .compOrCmd = "vis", - .attr = NULL, - .op = NULL, - .numArgs = 2, - .argTypes = {STR_ARG, INT_ARG}, - {{.str = compStrings[ARRAY]}, {.num = 0}}}; - err = Display_Send(toggleCmd); - //assertDisplayError(err); - delay(); + // Don't show the array icon + toggleCmd = (DisplayCmd_t){.compOrCmd = "vis", + .attr = NULL, + .op = NULL, + .numArgs = 2, + .argTypes = {STR_ARG, INT_ARG}, + {{.str = compStrings[ARRAY]}, {.num = 0}}}; + err = Display_Send(toggleCmd); + // assertDisplayError(err); + delay(); - // Test the fault screen - os_error_loc_t osErrCode = 0x0420; - fault_bitmap_t faultCode = 0x69; - err = Display_Fault(osErrCode, faultCode); - //assertDisplayError(err); + // Test the fault screen + os_error_loc_t osErrCode = 0x0420; + fault_bitmap_t faultCode = 0x69; + err = Display_Fault(osErrCode, faultCode); + // assertDisplayError(err); - while (1) - { - } + while (1) { + } } \ No newline at end of file diff --git a/Tests/Test_Driver_Minions.c b/Tests/Test_Driver_Minions.c index fd3b3cd90..947f8eceb 100644 --- a/Tests/Test_Driver_Minions.c +++ b/Tests/Test_Driver_Minions.c @@ -1,72 +1,62 @@ /* Copyright (c) 2020 UT Longhorn Racing Solar */ -#include "os.h" -#include "Tasks.h" -#include "Minions.h" #include -#include "config.h" + +#include "Minions.h" +#include "Tasks.h" #include "common.h" +#include "config.h" +#include "os.h" static OS_TCB Task1TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE]; #define tof(b) (b ? "on" : "off") -void Task1(void *arg) { - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - - BSP_UART_Init(UART_2); - Minion_Init(); - - OS_ERR err; - Minion_Error_t mErr; - bool brake_on = false; - bool ign1, ign2, regen_sw, for_sw, rev_sw, cruz_en, cruz_st; - - for (;;) { - Minion_Write_Output(BRAKELIGHT, brake_on, &mErr); - ign1 = !Minion_Read_Pin(IGN_1, &mErr); - ign2 = !Minion_Read_Pin(IGN_2, &mErr); - regen_sw = Minion_Read_Pin(REGEN_SW, &mErr); - for_sw = Minion_Read_Pin(FOR_SW, &mErr); - rev_sw = Minion_Read_Pin(REV_SW, &mErr); - cruz_en = Minion_Read_Pin(CRUZ_EN, &mErr); - cruz_st = Minion_Read_Pin(CRUZ_ST, &mErr); - printf( - "--------------------\r\nign1: %s\r\nign2: %s\r\n"\ - "regen_sw: %s\r\nfor_sw: %s\r\nrev_sw: %s\r\n"\ - "cruz_en: %s\r\ncruz_st: %s\r\n--------------------\r\n\r\n", - tof(ign1), tof(ign2), tof(regen_sw), tof(for_sw), - tof(rev_sw), tof(cruz_en), tof(cruz_st) - ); - - brake_on = !brake_on; - OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); - } +void Task1(void *arg) { + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + + BSP_UART_Init(UART_2); + Minion_Init(); + + OS_ERR err; + Minion_Error_t mErr; + bool brake_on = false; + bool ign1, ign2, regen_sw, for_sw, rev_sw, cruz_en, cruz_st; + + for (;;) { + Minion_Write_Output(BRAKELIGHT, brake_on, &mErr); + ign1 = !Minion_Read_Pin(IGN_1, &mErr); + ign2 = !Minion_Read_Pin(IGN_2, &mErr); + regen_sw = Minion_Read_Pin(REGEN_SW, &mErr); + for_sw = Minion_Read_Pin(FOR_SW, &mErr); + rev_sw = Minion_Read_Pin(REV_SW, &mErr); + cruz_en = Minion_Read_Pin(CRUZ_EN, &mErr); + cruz_st = Minion_Read_Pin(CRUZ_ST, &mErr); + printf( + "--------------------\r\nign1: %s\r\nign2: %s\r\n" + "regen_sw: %s\r\nfor_sw: %s\r\nrev_sw: %s\r\n" + "cruz_en: %s\r\ncruz_st: %s\r\n--------------------\r\n\r\n", + tof(ign1), tof(ign2), tof(regen_sw), tof(for_sw), tof(rev_sw), + tof(cruz_en), tof(cruz_st)); + + brake_on = !brake_on; + OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); + } }; int main(void) { - OS_ERR err; - OSInit(&err); - - // create tester thread - OSTaskCreate( - (OS_TCB *)&Task1TCB, - (CPU_CHAR *)"Task 1", - (OS_TASK_PTR)Task1, - (void *)NULL, - (OS_PRIO)13, - (CPU_STK *)Task1Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); - assertOSError(OS_MAIN_LOC, err); - - OSStart(&err); + OS_ERR err; + OSInit(&err); + + // create tester thread + OSTaskCreate((OS_TCB *)&Task1TCB, (CPU_CHAR *)"Task 1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)13, (CPU_STK *)Task1Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); + assertOSError(OS_MAIN_LOC, err); + + OSStart(&err); } - diff --git a/Tests/Test_Driver_Pedal_.c b/Tests/Test_Driver_Pedal_.c index 1bf737e02..6f71fe6d2 100644 --- a/Tests/Test_Driver_Pedal_.c +++ b/Tests/Test_Driver_Pedal_.c @@ -1,25 +1,24 @@ /** * Test file for library to interact with the pedal driver file - * - * Run this test in conjunction with the simulator + * + * Run this test in conjunction with the simulator * GUI. As you move the accelerator and brake on the GUI, the respective - * pressed/slided percentage will change from '0' to '100' on the terminal and display - * to show that sliding the pedals is read by the BSP - * - */ + * pressed/slided percentage will change from '0' to '100' on the terminal and + * display to show that sliding the pedals is read by the BSP + * + */ -#include "common.h" -#include "config.h" -#include "Pedals.h" #include +#include "Pedals.h" +#include "common.h" +#include "config.h" int main() { - Pedals_Init(); + Pedals_Init(); - while(1) { - printf("Accelerator: %5.1d%%\tBrake: %5.1d%%\r", - Pedals_Read(ACCELERATOR),Pedals_Read(BRAKE)); - } + while (1) { + printf("Accelerator: %5.1d%%\tBrake: %5.1d%%\r", Pedals_Read(ACCELERATOR), + Pedals_Read(BRAKE)); + } } - diff --git a/Tests/Test_Drivers.c b/Tests/Test_Drivers.c index 23334cb19..7fdae6d16 100644 --- a/Tests/Test_Drivers.c +++ b/Tests/Test_Drivers.c @@ -1,60 +1,67 @@ +#include "CANbus.h" +#include "Display.h" +#include "Pedals.h" #include "common.h" #include "config.h" -#include "Pedals.h" -#include "Display.h" -#include "CANbus.h" - int main() { - //CAN Test - printf("Testing CAN module:"); - CANbus_Init(); - uint32_t ids[10] = {0x242, 0x243, 0x244, 0x245, 0x246, 0x247, 0x24B, 0x24E, 0x580, CHARGE_ENABLE}; - uint8_t buffer[8]; - CANData_t data; - data.w = 0x87654321; - CANPayload_t payload; - payload.data = data; - payload.bytes = 4; + // CAN Test + printf("Testing CAN module:"); + CANbus_Init(); + uint32_t ids[10] = {0x242, 0x243, 0x244, 0x245, 0x246, + 0x247, 0x24B, 0x24E, 0x580, CHARGE_ENABLE}; + uint8_t buffer[8]; + CANData_t data; + data.w = 0x87654321; + CANPayload_t payload; + payload.data = data; + payload.bytes = 4; - for(int i=0; iNamePtr : "No Task"; -} +static char *getTCBName(OS_TCB *tcb) { return tcb ? tcb->NamePtr : "No Task"; } void dumpTrace(void) { - int idx = PrevTasks.index; - printf("Task trace: ["); - for (int i=0; i<8; i++) { - if (!PrevTasks.tasks[idx]) break; - printf("%s, ", getTCBName(PrevTasks.tasks[idx])); - idx = (idx + TASK_TRACE_LENGTH - 1) % TASK_TRACE_LENGTH; // decrement idx, wrapping around at 0 - } - printf("]\n\r"); + int idx = PrevTasks.index; + printf("Task trace: ["); + for (int i = 0; i < 8; i++) { + if (!PrevTasks.tasks[idx]) break; + printf("%s, ", getTCBName(PrevTasks.tasks[idx])); + idx = (idx + TASK_TRACE_LENGTH - 1) % + TASK_TRACE_LENGTH; // decrement idx, wrapping around at 0 + } + printf("]\n\r"); } -#define expect(str) printf("Previous expected: %s, previous actual: %s\n\r", (str), getTCBName(PrevTasks.tasks[PrevTasks.index])) +#define expect(str) \ + printf("Previous expected: %s, previous actual: %s\n\r", (str), \ + getTCBName(PrevTasks.tasks[PrevTasks.index])) static OS_TCB Task1TCB, Task2TCB; static CPU_STK Task1Stk[DEFAULT_STACK_SIZE], Task2Stk[DEFAULT_STACK_SIZE]; void Task1(void *p_arg) { - (void) p_arg; - OS_ERR err; - - CPU_Init(); - OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); - BSP_UART_Init(UART_2); + (void)p_arg; + OS_ERR err; - expect("No Task"); - dumpTrace(); - OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); - expect("Task 2"); - dumpTrace(); - OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); - expect("Task 2"); - dumpTrace(); - while (1); + CPU_Init(); + OS_CPU_SysTickInit(SystemCoreClock / (CPU_INT32U)OSCfg_TickRate_Hz); + BSP_UART_Init(UART_2); + expect("No Task"); + dumpTrace(); + OSTimeDlyHMSM(0, 0, 0, 500, OS_OPT_TIME_HMSM_STRICT, &err); + expect("Task 2"); + dumpTrace(); + OSTimeDlyHMSM(0, 0, 1, 0, OS_OPT_TIME_HMSM_STRICT, &err); + expect("Task 2"); + dumpTrace(); + while (1) + ; } void Task2(void *p_arg) { - (void) p_arg; - OS_ERR err; + (void)p_arg; + OS_ERR err; - expect("Task 1"); - dumpTrace(); - OSTimeDlyHMSM(0, 0, 0, 600, OS_OPT_TIME_HMSM_STRICT, &err); - expect("Task 1"); - dumpTrace(); - while (1); + expect("Task 1"); + dumpTrace(); + OSTimeDlyHMSM(0, 0, 0, 600, OS_OPT_TIME_HMSM_STRICT, &err); + expect("Task 1"); + dumpTrace(); + while (1) + ; } int main(void) { - OS_ERR err; - OSInit(&err); - TaskSwHook_Init(); + OS_ERR err; + OSInit(&err); + TaskSwHook_Init(); - OSTaskCreate( - (OS_TCB *)&Task1TCB, - (CPU_CHAR *)"Task 1", - (OS_TASK_PTR)Task1, - (void *)NULL, - (OS_PRIO)5, - (CPU_STK *)Task1Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); + OSTaskCreate((OS_TCB *)&Task1TCB, (CPU_CHAR *)"Task 1", (OS_TASK_PTR)Task1, + (void *)NULL, (OS_PRIO)5, (CPU_STK *)Task1Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); - OSTaskCreate( - (OS_TCB *)&Task2TCB, - (CPU_CHAR *)"Task 2", - (OS_TASK_PTR)Task2, - (void *)NULL, - (OS_PRIO)6, - (CPU_STK *)Task2Stk, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, - (CPU_STK_SIZE)DEFAULT_STACK_SIZE, - (OS_MSG_QTY)0, - (OS_TICK)NULL, - (void *)NULL, - (OS_OPT)(OS_OPT_TASK_STK_CLR), - (OS_ERR *)&err); + OSTaskCreate((OS_TCB *)&Task2TCB, (CPU_CHAR *)"Task 2", (OS_TASK_PTR)Task2, + (void *)NULL, (OS_PRIO)6, (CPU_STK *)Task2Stk, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE / 10, + (CPU_STK_SIZE)DEFAULT_STACK_SIZE, (OS_MSG_QTY)0, (OS_TICK)NULL, + (void *)NULL, (OS_OPT)(OS_OPT_TASK_STK_CLR), (OS_ERR *)&err); - OSStart(&err); + OSStart(&err); } \ No newline at end of file