Skip to content

Commit

Permalink
Changed contactor pinouts, added pwr cmd canmsg, increased deadzone. …
Browse files Browse the repository at this point in the history 
…Hey guysd :3
  • Loading branch information
@Mikail-Sadic-UT
Mikail-Sadic-UT committed Nov 17, 2024
1 parent 0a07044 commit c0ba96a
Show file tree
Hide file tree
Showing 3 changed files with 475 additions and 324 deletions.
195 changes: 104 additions & 91 deletions Apps/Src/ReadTritium.c
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/**
* @copyright Copyright (c) 2018-2023 UT Longhorn Racing Solar
* @file ReadTritium.c
* @brief
*
* @brief
*
*/

#include "ReadTritium.h"
Expand All @@ -12,15 +12,14 @@
#include "os_cfg_app.h"
#include <string.h>

//status limit flag masks
#define MASK_MOTOR_TEMP_LIMIT (1<<6) //check if motor temperature is limiting the motor
// status limit flag masks
#define MASK_MOTOR_TEMP_LIMIT (1 << 6) // check if motor temperature is limiting the motor
#define MAX_CAN_LEN 8
#define RESTART_THRESHOLD 3 // Number of times to restart before asserting a nonrecoverable error
#define RESTART_THRESHOLD 3 // Number of times to restart before asserting a nonrecoverable error
#define MOTOR_TIMEOUT_SECS 1 // Timeout for several missed motor messages
#define MOTOR_TIMEOUT_TICKS (MOTOR_TIMEOUT_SECS * OS_CFG_TMR_TASK_RATE_HZ)


tritium_error_code_t Motor_FaultBitmap = T_NONE; //initialized to no error, changed when the motor asserts an error
tritium_error_code_t Motor_FaultBitmap = T_NONE; // initialized to no error, changed when the motor asserts an error
static float Motor_RPM = 0;
static float Motor_Velocity = 0;
static float Motor_BusVoltage = 0;
Expand All @@ -34,152 +33,166 @@ static OS_TMR MotorWatchdog;
static void assertTritiumError(tritium_error_code_t motor_err);

// Callback for motor watchdog
static void motorWatchdog(void *tmr, void *p_arg) {
// Attempt to restart 3 times, then fail
assertTritiumError(T_MOTOR_WATCHDOG_TRIP);
static void motorWatchdog(void *tmr, void *p_arg)
{
// Attempt to restart 3 times, then fail
assertTritiumError(T_MOTOR_WATCHDOG_TRIP);
}


void Task_ReadTritium(void *p_arg){
void Task_ReadTritium(void *p_arg)
{
OS_ERR err;
CANDATA_t dataBuf = {0};

static bool watchdogCreated = false;

while (1){
while (1)
{
ErrorStatus status = CANbus_Read(&dataBuf, true, MOTORCAN);

if (status == SUCCESS){
if(!watchdogCreated){ // Timer doesn't seem to trigger without initial delay? Might be an RTOS bug
OSTmrCreate(&MotorWatchdog, "Motor watchdog", MOTOR_TIMEOUT_TICKS, MOTOR_TIMEOUT_TICKS, OS_OPT_TMR_PERIODIC, motorWatchdog, NULL, &err);
assertOSError(err);

OSTmrStart(&MotorWatchdog, &err);
assertOSError(err);

watchdogCreated = true;
}

switch(dataBuf.ID){
case MC_BUS:{
Motor_BusVoltage = *((float*)&dataBuf.data[0]);
Motor_BusCurrent = *((float*)&dataBuf.data[4]);

UpdateDisplay_SetMCVoltage(Motor_BusVoltage*10);
UpdateDisplay_SetMCCurrent(Motor_BusCurrent*10);
}
case MOTOR_STATUS:{
// motor status error flags is in bytes 4-5
Motor_FaultBitmap = (*((uint16_t*)(&dataBuf.data[4])) & 0x1FE); //Storing error flags into Motor_FaultBitmap
motorstatusmsg = dataBuf;

assertTritiumError(Motor_FaultBitmap);
break;
}

case VELOCITY:{
OSTmrStart(&MotorWatchdog, &err); // Reset the watchdog
assertOSError(err);
memcpy(&Motor_RPM, &dataBuf.data[0], sizeof(float));
memcpy(&Motor_Velocity, &dataBuf.data[4], sizeof(float));

//Motor RPM is in bytes 0-3
Motor_RPM = *((float*)(&dataBuf.data[0]));

//Car Velocity (in m/s) is in bytes 4-7
Motor_Velocity = *((float*)(&dataBuf.data[4]));
float Car_Velocity = Motor_Velocity * 1000;

Car_Velocity = (Car_Velocity * 223694) / 10000000;

UpdateDisplay_SetVelocity(Car_Velocity);

}

case TEMPERATURE:{
UpdateDisplay_SetHeatSinkTemp(*(float *)(&dataBuf.data[4]));
}

default:{
break; //for cases not handled currently
}
if (status == SUCCESS)
{
if (!watchdogCreated)
{ // Timer doesn't seem to trigger without initial delay? Might be an RTOS bug
OSTmrCreate(&MotorWatchdog, "Motor watchdog", MOTOR_TIMEOUT_TICKS, MOTOR_TIMEOUT_TICKS, OS_OPT_TMR_PERIODIC, motorWatchdog, NULL, &err);
assertOSError(err);

OSTmrStart(&MotorWatchdog, &err);
assertOSError(err);

watchdogCreated = true;
}

switch (dataBuf.ID)
{
case MC_BUS:
{
Motor_BusVoltage = *((float *)&dataBuf.data[0]);
Motor_BusCurrent = *((float *)&dataBuf.data[4]);

UpdateDisplay_SetMCVoltage(Motor_BusVoltage * 10);
UpdateDisplay_SetMCCurrent(Motor_BusCurrent * 10);
}
case MOTOR_STATUS:
{
// motor status error flags is in bytes 4-5
Motor_FaultBitmap = (*((uint16_t *)(&dataBuf.data[4])) & 0x1FE); // Storing error flags into Motor_FaultBitmap
motorstatusmsg = dataBuf;

// assertTritiumError(Motor_FaultBitmap);
break;
}

case VELOCITY:
{
OSTmrStart(&MotorWatchdog, &err); // Reset the watchdog
assertOSError(err);
memcpy(&Motor_RPM, &dataBuf.data[0], sizeof(float));
memcpy(&Motor_Velocity, &dataBuf.data[4], sizeof(float));

// Motor RPM is in bytes 0-3
Motor_RPM = *((float *)(&dataBuf.data[0]));

// Car Velocity (in m/s) is in bytes 4-7
Motor_Velocity = *((float *)(&dataBuf.data[4]));
float Car_Velocity = Motor_Velocity * 1000;

Car_Velocity = (Car_Velocity * 223694) / 10000000;

UpdateDisplay_SetVelocity(Car_Velocity);
}

SendCarCAN_Put(dataBuf); // Forward message on CarCAN for telemetry

case TEMPERATURE:
{
UpdateDisplay_SetHeatSinkTemp(*(float *)(&dataBuf.data[4]));
}

default:
{
break; // for cases not handled currently
}
}

SendCarCAN_Put(dataBuf); // Forward message on CarCAN for telemetry
}
}
}

static void restartMotorController(void){
static void restartMotorController(void)
{
CANDATA_t resetmsg = {0};
resetmsg.ID = MOTOR_RESET;
CANbus_Send(resetmsg, true, MOTORCAN);
}


float Motor_RPM_Get(){ //getter function for motor RPM
float Motor_RPM_Get()
{ // getter function for motor RPM
return Motor_RPM;
}

float Motor_Velocity_Get(){ //getter function for motor velocity
float Motor_Velocity_Get()
{ // getter function for motor velocity
return Motor_Velocity;
}


/**
* Error handler functions
* Passed as callback functions to the main throwTaskError function by assertTritiumError
*/
*/

/**
* @brief A callback function to be run by the main throwTaskError function for hall sensor errors
* restart the motor if the number of hall errors is still less than the MOTOR_RESTART_THRESHOLD.
*/
static inline void handler_ReadTritium_HallError(void) {
restartMotorController();
*/
static inline void handler_ReadTritium_HallError(void)
{
restartMotorController();
}


/**
* @brief Assert a Tritium error by checking Motor_FaultBitmap
* and asserting the error with its handler callback if one exists.
* Can result in restarting the motor (for hall sensor errors while less than MOTOR_RESTART_THRESHOLD)
* or locking the scheduler and entering a nonrecoverable fault (all other cases)
* @param motor_err Bitmap with motor error codes to check
*/
static void assertTritiumError(tritium_error_code_t motor_err){
static uint8_t hall_fault_cnt = 0; //trip counter, doesn't ever reset
static void assertTritiumError(tritium_error_code_t motor_err)
{
static uint8_t hall_fault_cnt = 0; // trip counter, doesn't ever reset
static uint8_t motor_fault_cnt = 0;

Error_ReadTritium = (error_code_t)motor_err; // Store error codes for inspection info
if(motor_err == T_NONE || motor_err == T_WATCHDOG_LAST_RESET_ERR) return; // No error, return
// NOTE: If we had >1 recoverable errors,
// Hall sensor error is the only recoverable error, so any other error // make sure a combination of them wouldn't
// or combination of errors includes at least one that is nonrecoverable // accidentally fall into this nonrecoverable bucket
if(motor_err != T_HALL_SENSOR_ERR && motor_err != T_MOTOR_WATCHDOG_TRIP){
if (motor_err == T_NONE || motor_err == T_WATCHDOG_LAST_RESET_ERR)
return; // No error, return
// NOTE: If we had >1 recoverable errors,
// Hall sensor error is the only recoverable error, so any other error // make sure a combination of them wouldn't
// or combination of errors includes at least one that is nonrecoverable // accidentally fall into this nonrecoverable bucket
if (motor_err != T_HALL_SENSOR_ERR && motor_err != T_MOTOR_WATCHDOG_TRIP)
{
// Assert a nonrecoverable error with no callback function- nonrecoverable will kill the motor and infinite loop
throwTaskError(Error_ReadTritium, NULL, OPT_LOCK_SCHED, OPT_NONRECOV);
return;
}

// If it's purely a hall sensor error, try to restart the motor a few times and then fail out

if(motor_err == T_HALL_SENSOR_ERR && ++hall_fault_cnt > RESTART_THRESHOLD){ // Threshold has been exceeded
if (motor_err == T_HALL_SENSOR_ERR && ++hall_fault_cnt > RESTART_THRESHOLD)
{ // Threshold has been exceeded
// Assert a nonrecoverable error that will kill the motor, display a fault screen, and infinite loop
throwTaskError(Error_ReadTritium, NULL, OPT_LOCK_SCHED, OPT_NONRECOV);
return;
}

//try to restart the motor a few times and then fail out
if(motor_err == T_MOTOR_WATCHDOG_TRIP && ++motor_fault_cnt > RESTART_THRESHOLD){
// try to restart the motor a few times and then fail out
if (motor_err == T_MOTOR_WATCHDOG_TRIP && ++motor_fault_cnt > RESTART_THRESHOLD)
{
// Assert a nonrecoverable error that will kill the motor, display a fault screen, and infinite loop
throwTaskError(Error_ReadTritium, NULL, OPT_LOCK_SCHED, OPT_NONRECOV);
return;
}

// Threshold hasn't been exceeded, so assert a recoverable error with the motor restart callback function
throwTaskError(Error_ReadTritium, handler_ReadTritium_HallError, OPT_NO_LOCK_SCHED, OPT_RECOV);
throwTaskError(Error_ReadTritium, handler_ReadTritium_HallError, OPT_NO_LOCK_SCHED, OPT_RECOV);

Error_ReadTritium = T_NONE; // Clear the error after handling it
}
Loading

0 comments on commit c0ba96a

Please sign in to comment.