File content as of revision 27:52260996ce32:

#define Track   1.240f      //Average wheel track
#define Base    1.560f      //wheel base
#define Rwhl    0.230f      //Wheel radius
#define HCG     0.204f      //Height of CG
#define Mtot    320.0f      //Total weight with driver
#define g0      9.81f       //gravity
#define EG      0.000f      //EG<0: over steer, EG>0:under steer
#define K_yaw   1.000f      //Gain for yaw regulator
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

// CAN ID define
#define FL_HSB_ID 0xA0      // Rx, 100Hz
#define FR_HSB_ID 0xA1      // Rx, 100Hz
#define RL_HSB_ID 0xA2      // Rx, 100Hz
#define RR_HSB_ID 0xA3      // Rx, 100Hz
#define FL_LSB_ID 0xB0      // Rx, 10Hz
#define FR_LSB_ID 0xB1      // Rx, 10Hz
#define RL_LSB_ID 0xB2      // Rx, 10Hz
#define RR_LSB_ID 0xB3      // Rx, 10Hz
#define HMI_cmd_ID 0xC4     // Rx, 100Hz
#define FL_CMD_ID 0xC0      // Tx, 100Hz
#define FR_CMD_ID 0xC1      // Tx, 100Hz
#define RL_CMD_ID 0xC2      // Tx, 100Hz
#define RR_CMD_ID 0xC3      // Tx, 100Hz
#define AUX_sense_ID 0xE0   // Tx, 10Hz
#define Qdrv_stat_ID 0xE1   // Tx, 10Hz
#define IMU_sense_ID 0xE2   // Tx, 100Hz
#define RegularVar_ID 0xE3  // Tx, 100Hz
#define PedalStat_ID 0xE4   // Tx, 10Hz


#define MODOFL_VDUFLTCode       0x0001U     //Drive module timeout
#define PSUOFL_VDUFLTCode       0x0002U     //Pedal unit timeout
#define IMUSTA_VDUFLTCode       0x0004U     //IMU module abnormal
#define ShDVol_VDUFLTCode       0x0008U     //Shutdown voltage abnormal
#define DSM_VDUFLTCode          0x0010U     //One or more slave report DSM_Fault

//CAN msg bank
char    temp_msg[8] = {0,0,0,0,0,0,0,0};

//IMU readings
float YR_imu = 0;                   // deg/s yawrate estimated threw IMU sensor fusion
float Roll_imu = 0;                 // deg
float Pitch_imu = 0;                // deg
float Yaw_imu = 0;                  // deg
float Ax_imu = 0;                   // g
float Ay_imu = 0;                   // g

//AWD variable
float YR_ref = 0;                   // rad/s yawrate reference generate from ideal single track model
float Vx_wss = 0;                   // m/s estimated body Velocity X (wheelspeed only)
float Vx_fil = 0;                   // m/s estimated body Velocity X
float Mz_reg = 0;                   // Nm addon yaw torque regulating yawrate, gain given by "K_yaw"
float sig = 0;                      // Front rear distribution factor

//CAN to motor drive module, 100Hz
//msg ID: 0xC0~0xC3
float FL_Tcmd = 0;                  // *100 before sent in int16_t
float FR_Tcmd = 0;
float RL_Tcmd = 0;
float RR_Tcmd = 0;
uint8_t RTD_cmd = 0;                // start inverter switching cmd
uint8_t RST_cmd = 0;                // send out once to reset module fault

//Module online flag
uint8_t FL_online = 0;              // 0 indicate detection timeout
uint8_t FR_online = 0;              // reset value is 3 to hold for 0.03sec
uint8_t RL_online = 0;              // -1 per 100Hz task
uint8_t RR_online = 0;
uint8_t PSU_online = 0;

//Feedback data decoded out storage
float FL_Tmotor = 0;                // motor temperature degC, 10Hz recieving
float FR_Tmotor = 0;
float RL_Tmotor = 0;
float RR_Tmotor = 0;
float Max_Tmotor = 0;               // motor temperature degC, 10Hz recieving
float FL_Tmodule = 0;               // inverter temperature degC, 10Hz recieving
float FR_Tmodule = 0;
float RL_Tmodule = 0;
float RR_Tmodule = 0;
float Max_Tmodule = 0;              // motor temperature degC, 10Hz recieving
uint16_t FL_FLT_Run = 0;            // RUN fault code, 10Hz recieving
uint16_t FR_FLT_Run = 0;
uint16_t RL_FLT_Run = 0;
uint16_t RR_FLT_Run = 0;
uint16_t FLT_Run = 0;
uint16_t FLT_Run_ind = 0;           // for indication
uint16_t FL_FLT_Post = 0;           // POST fault code, 10Hz recieving
uint16_t FR_FLT_Post = 0;
uint16_t RL_FLT_Post = 0;
uint16_t RR_FLT_Post = 0;
uint16_t FLT_Post = 0;
uint16_t FLT_Post_ind = 0;           // for indication
float FL_Trq_fil3 = 0;              // Internal Tcmd, 100Hz recieving
float FR_Trq_fil3 = 0;
float RL_Trq_fil3 = 0;
float RR_Trq_fil3 = 0;
float FL_Trq_est = 0;               // Estimated Torque, 100Hz recieving
float FR_Trq_est = 0;
float RL_Trq_est = 0;
float RR_Trq_est = 0;
float FL_W_ele = 0;                 // Estimated W_ele, 100Hz recieving
float FR_W_ele = 0;
float RL_W_ele = 0;
float RR_W_ele = 0;
uint8_t FL_DSM = 0;                 // DSM state, 100Hz recieving
uint8_t FR_DSM = 0;
uint8_t RL_DSM = 0;
uint8_t RR_DSM = 0;

//From Pedal Box msg
uint8_t RTD_HMI = 0;                // 1 = HMI requesting
uint8_t RST_HMI = 0;                // 1 = HMI request once
uint8_t AWD_HMI = 0;                // 1 = HMI requesting
float Trq_HMI = 0.0f;               // Nm user request total torque
float Steer_HMI = 0.0f;             // Steering wheel signal, *st2r to get rad
uint8_t RTD_SW = 0;                 // 1 = RTD switch on

//10/100Hz tasking
uint8_t HSTick = 5;                 // High speed tick
uint8_t LSTick = 0;
uint8_t HST_EXFL = 0;               // High speed execution flag
uint8_t LST_EXFL = 0;
uint8_t FLT_print = 0;              // Send repeative error message
uint8_t FLT_print_cnt = 0;
uint16_t AUX_1_u16 = 0x0;           // acquired data
uint16_t AUX_2_u16 = 0x0;
uint16_t AUX_3_u16 = 0x0;
uint16_t AUX_4_u16 = 0x0;
float SDn_voltage = 0.0f;
float Brk_voltage = 0.0f;

//VDU states
typedef enum {
    VDU_PowerOn               = 0U,
    VDU_Idle                  = 1U,
    VDU_Run                   = 2U,
    VDU_Fault                 = 3U,
    VDU_Reset                 = 4U
} VDU_STATE_TYPE;
VDU_STATE_TYPE VDU_STAT = VDU_PowerOn;  // VDU current state
uint16_t VDU_FLT = 0;                   // VDU internal fault code
uint16_t VDU_FLT_ind = 0;               // A copy for indication
uint8_t Reset_to = 0;                   // Timer for reset pending

//Indicator pattern generation
uint8_t Ind_refresh = 0;                // Flag to copy error bits for indicator
uint8_t Pattern = 0U;
uint16_t Repeat = 0U;                   // Bitwise repeative variable
uint8_t Phase = 0U;                     // 0:Type ind, 1:Num ind, 2:Blank
uint8_t Blk_n = 0U;
//Category repetive code
#define Post_rep 0b00000010
#define Run_rep  0b00000001
#define VDU_rep  0b00000100
//Blink Pattern
#define F_Blink 0b01010101              // blink from LSB to MSB
#define L_Pulse 0b01111110
#define S_Pulse 0b01000000
#define N_Pulse 0b00000000

//Prototype
void CAN_init(void);            //Initial CAN frequency filter...
void Module_WD(void);           //Software watchdog indicate module state
void IMU_isr(void);             //Use if test mode
void IMU_read(void);            //Update IMU readings by once
void Aux_read(void);            //Update AUX reafings by once
void Idle2Run(void);            //Initializing before running
void Run2Idle(void);            //Initializing before idling
void POST(void);                //Check IMU error
void RUNT(void);                //Check POST, timeout, ShutDrv voltage error
void AWD(void);                 //AWD main program, all wheel drive
void ASL(void);                 //ASL main program, anti slip
//void REG_mask(void);          //For low speed non regen rule, implement in PSU
void Cooler(void);              //Function for active cooler working
void Rx_CAN1(void);             //CAN RX handler
void Tx_CLRerr_CAN1(void);      //Send reset cmd to modules
void Tx_Estop_CAN1(void);       //Send out heart beat but force RTD off
void Tx_Tcmd_CAN1(void);        //Send out heart beat command
void Tx_Qdrv_CAN1(void);        //Send out low speed heart beat for logging
void CANpendTX(void);           //Helper function for CAN Tx
int16_t max_uval(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
float max_fval(float i1, float i2, float i3, float i4);
uint8_t Indication(             //Blink indicator in pattern * repeat
    uint8_t pattern,
    uint16_t*repeat,
    uint8_t*blk_n);
uint8_t IndicationKernel(       //Generate blink pattern, return 1 if all done
    uint8_t*pattern,
    uint16_t*repeat,
    uint8_t*phase,
    uint16_t*Post_ind,
    uint16_t*Run_ind,
    uint16_t*VDU_ind);