File content as of revision 7:f674ef860c9c:

#include "mbed.h"
#define dt  0.01f
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
#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, 10Hz

#define MODOFL_VDUFLTCode       0x0001U     //Drive module timeout after once online
#define PSUOFL_VDUFLTCode       0x0002U     //Pedal unit timeout after once online
#define IMUSTA_VDUFLTCode       0x0004U     //IMU module abnormal


DigitalOut  Aux_Rly(PC_10,0);                       //Control aux relay, 1 active
DigitalOut  Fault_Ind(PC_10,0);                     //Indicate fault bt flashing, 1 active
DigitalOut  LED(D13, 0);                            //Internal LED output, general purpose
AnalogIn    AUX_1(PC_0);                            //Auxilaru analog sensor
AnalogIn    AUX_2(PC_4);
AnalogIn    AUX_3(PC_2);
AnalogIn    AUX_4(PC_1);
AnalogIn    SDn_sense(PB_0);                        //Shutdown circuit driving end detection
CAN     can1(PB_8, PB_9, 1000000);                  //1Mbps, contain critical torque command message
Serial  pc(USBTX, USBRX, 115200);
Ticker  ticker1;                                    //100Hz task

CANMessage  can_msg_Rx;
CANMessage  can_msg_Tx;

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

//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 FL_Tmodule = 0;               // inverter temperature degC, 10Hz recieving
float FR_Tmodule = 0;
float RL_Tmodule = 0;
float RR_Tmodule = 0;
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 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;
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;             // deg steering wheel angel

//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;

//VDU states
typedef enum {
    VDU_PowerOn               = 0U,
    VDU_Idle                  = 1U,
    VDU_Run                   = 2U,
    VDU_Fault                 = 3U
} VDU_STATE_TYPE;
VDU_STATE_TYPE VDU_STAT = VDU_PowerOn;  //VDU current state
uint16_t VDU_FLT = 0;               //VDU internal fault code

//Prototype
void CAN_init(void);            //Initial CAN frequency filter...
void Module_WD(void);           //Software watchdog indicate module state
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
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_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4);

void timer1_interrupt(void)
{
    HSTick += 1;
    LSTick += 1;
    if (HSTick > 9) {           // 100Hz
        HST_EXFL = 1;
        HSTick = 0;
    }
    if (LSTick > 99) {          // 10Hz
        LST_EXFL = 1;
        LSTick = 0;
    }
}

int main()
{
    //Init CAN network
    CAN_init();

    //Start House keeping task
    printf("VDU start up, pend for module online\n");
    ticker1.attach_us(&timer1_interrupt, 1000);     //1 ms Systick
    while(1) {

        // Do high speed loop
        if (HST_EXFL == 1) {
            HST_EXFL = 0;

            // Get IMU, Auxs
            IMU_read();
            Aux_read();
            Module_WD();                            // Module online watch dog

            // Run state machine
            switch (VDU_STAT) {

                case VDU_PowerOn:
                    /* Power on state
                     * Description:
                     *  Simple start up sequence will be done here
                     * Do:
                     *  VDU internal POST
                     *  Wait till modules + PSU online
                     * To VDU_Idle (RTD off):
                     *  Prepare for 4WD main program
                     * To VDU_Fault:
                     *  Run the error handling service
                     */

                    //Basic IMU test
                    POST();

                    //Check if state transition only when all module online
//                    if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) {
                    if(1) {                           //Force online check pass only for debug 2019/11/14

                        if (                            //Check if any error
                            (FL_DSM == 3U)||
                            (FR_DSM == 3U)||
                            (RL_DSM == 3U)||
                            (RR_DSM == 3U)||
                            (VDU_FLT != 0)) {
                            VDU_STAT = VDU_Fault;
                            FLT_print = 1;
                        } else {
                            //All module online & POST pass
                            VDU_STAT = VDU_Idle;
                            printf("All module online, VDU now Idle\n");
                        }
                    }                                   //Else keep in state VDU_PowerOn
                    break;

                case VDU_Idle:
                    /* Controller Idle state
                     * Description:
                     *  Normal latched state, wait for RTD_HMI set from PSU
                     *  4WD in running but output mask to 0
                     * Do:
                     *  4WD controller
                     * Check:
                     *  RUN faults if any
                     * To VDU_Run:
                     *  Initialize parameters for start up, set RTD_cmd
                     * To VDU_Fault:
                     *  Run the error handling service
                     */

                    //Forced RTD_HMI for debug purpose 2019/11/14
                    RTD_HMI = 1;                        //Should be set if can bus received data
                    //Forced RTD_HMI for debug purpose 2019/11/14

                    RUNT();                             //Run test
                    AWD();                              //AWD main program

                    if (                                //Check if any error
                        (FL_DSM == 3U)||
                        (FR_DSM == 3U)||
                        (RL_DSM == 3U)||
                        (RR_DSM == 3U)||
                        (VDU_FLT != 0)) {
                        VDU_STAT = VDU_Fault;
                        printf("Idle 2 Fault\n");
                        FLT_print = 1;
                    } else if (RTD_HMI != 0) {          //Or command to run threw PSU
                        //Prepare to send out RTD and start motor
                        Idle2Run();
                        VDU_STAT = VDU_Run;
                        printf("Idle 2 Run\n");
                    }                                   //Else keep in state
                    break;

                case VDU_Run:
                    /* Controller Run state
                     * Description:
                     *  Normal latched state, after RTD_HMI is set from PSU
                     *  Same to Idle state except RTD_cmd is set
                     * Do:
                     *  4WD controller
                     * Check:
                     *  RUN faults if any
                     * To VDU_Idle:
                     *  Initialize parameters for idling, reset RTD_cmd
                     * To VDU_Fault:
                     *  Run the error handling service
                     */

                    RUNT();                             //Run test
                    AWD();                              //AWD main program

                    //Temporary debug posting area 2019/11/14
                    //printf("%d,%d\n", Encoder_cnt, Encoder_del);
                    //printf("%d\n\r", (int16_t)Tmodule);//
                    //printf("%d\n\r", (int16_t)Vbus);

                    if (                                //Check if any error
                        (FL_DSM == 3U)||
                        (FR_DSM == 3U)||
                        (RL_DSM == 3U)||
                        (RR_DSM == 3U)||
                        (VDU_FLT != 0)) {
                        VDU_STAT = VDU_Fault;
                        printf("Run 2 Fault\n");
                        FLT_print = 1;
                    } else if (RTD_HMI != 1) {          //Or command to stop threw can bus
                        Run2Idle();
                        VDU_STAT = VDU_Idle;
                        printf("Run 2 Idle\n");
                    }                                   //Else keep in state
                    break;

                case VDU_Fault:
                    /* Controller Fault state
                     * Description:
                     *  Fault latched state if any faults is detected
                     *  Same to Idle state except keep till RTD_HMI reset
                     * Do:
                     *  Nothing, like a piece of shit
                     * Check:
                     *  RUN faults if any
                     * To VDU_PowerOn:
                     *  Restart VDU
                     */

                    RUNT();                             //Run test

                    if (RST_HMI == 1) {                 //PSU reset to clear error
                        RST_cmd = 1;
                        FLT_print = 0;
                        VDU_STAT = VDU_PowerOn;
                        printf("Module reset\nVDU restarting...\n");
                    }                                   //Else keep in state
                    break;
            }

            // Shit out torque distribution and special command
            if(VDU_STAT == VDU_Run) {
                //Allow output torque
                Tx_Tcmd_CAN1();
            } else if(RST_cmd != 0) {
                //Send out reset cmd once
                Tx_CLRerr_CAN1();
            } else {
                //Force RTD off when not in VDU_Run
                Tx_Estop_CAN1();
            }

        }
        // End of high speed loop

        // Do low speed state reporting
        if (LST_EXFL == 1) {
            LST_EXFL = 0;
            Tx_Qdrv_CAN1();

            // Print out error mesagge if exist, 1Hz repeative
            if(FLT_print != 0) {
                FLT_print_cnt += FLT_print;
                if(FLT_print_cnt > 19) {
                    printf("POST FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Post,FR_FLT_Post,RL_FLT_Post,RR_FLT_Post);
                    printf("RUN FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Run,FR_FLT_Run,RL_FLT_Run,RR_FLT_Run);
                    printf("VDU\n0x%04X\n\n", VDU_FLT);
                    FLT_print_cnt = 0;
                }
            }
        }
        // End of low speed state reporting

    } // end of while
}

void Idle2Run(void)
{
    RTD_cmd = 1;
}

void Run2Idle(void)
{
    RTD_cmd = 0;
}

void POST(void)
{
    //Check IMU status
    if(0) {
        VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
    }
}

void RUNT(void)
{
    POST();

    //Check timeout
    if((FL_online*FR_online*RL_online*RR_online) == 0) {
        VDU_FLT |= MODOFL_VDUFLTCode;           //Module timeout
    }
    if(PSU_online == 0) {
        VDU_FLT |= PSUOFL_VDUFLTCode;           //PSU timeout
    }

    //Check IMU status
    //XXX

    //Check ShutDrv voltage
    //XXX
}

void Aux_read(void)
{
    //Capture analog in at once
    AUX_1_u16 = AUX_1.read_u16();
    AUX_2_u16 = AUX_2.read_u16();
    AUX_3_u16 = AUX_3.read_u16();
    AUX_4_u16 = AUX_4.read_u16();
    SDn_voltage = 18.81f*SDn_sense.read();
}

void IMU_read(void)
{

}

void AWD(void)
{

}

void Rx_CAN1(void)
{
    LED = 1;
    int16_t tmp;

    if(can1.read(can_msg_Rx)) {
        switch(can_msg_Rx.id) {                                 //Filtered input message
            // Start of 100Hz msg
            case FL_HSB_ID:
                //HSB from FL motor drive
                FL_DSM = can_msg_Rx.data[6] & 0x01;             //Get DSM_STAT
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                FL_W_ele = tmp*1.0f;
                tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                FL_Trq_fil3 = tmp * 0.01f;
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                FL_Trq_est = tmp * 0.01f;
                FL_online = 3;
                break;

            case FR_HSB_ID:
                //HSB from FR motor drive
                FR_DSM = can_msg_Rx.data[6] & 0x01;             //Get DSM_STAT
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                FR_W_ele = tmp*1.0f;
                tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                FR_Trq_fil3 = tmp * 0.01f;
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                FR_Trq_est = tmp * 0.01f;
                FR_online = 3;
                break;

            case RL_HSB_ID:
                //HSB from RL motor drive
                RL_DSM = can_msg_Rx.data[6] & 0x01;             //Get DSM_STAT
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                RL_W_ele = tmp*1.0f;
                tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                RL_Trq_fil3 = tmp * 0.01f;
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                RL_Trq_est = tmp * 0.01f;
                RL_online = 3;
                break;

            case RR_HSB_ID:
                //HSB from RR motor drive
                RR_DSM = can_msg_Rx.data[6] & 0x01;             //Get DSM_STAT
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                RR_W_ele = tmp*1.0f;
                tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                RR_Trq_fil3 = tmp * 0.01f;
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                RR_Trq_est = tmp * 0.01f;
                RR_online = 3;
                break;

            case HMI_cmd_ID:
                //HMI command from PSU
                AWD_HMI = can_msg_Rx.data[6] & 0x01;             //Get AWD switch
                RST_HMI = can_msg_Rx.data[5] & 0x01;             //Get RST request
                RTD_HMI = can_msg_Rx.data[4] & 0x01;             //Get RTD switch
                tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                Steer_HMI = tmp * 0.01f;
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                Trq_HMI = tmp * 0.01f;
                PSU_online = 3;
                break;
            // end of 100Hz msg

            // Start of 10Hz msg
            case FL_LSB_ID:
                //LSB from FL motor drive
                tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
                FL_Tmotor = tmp*0.01f;
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                FL_Tmodule = tmp*0.01f;
                FL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                break;

            case FR_LSB_ID:
                //LSB from FR motor drive
                tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
                FR_Tmotor = tmp*0.01f;
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                FR_Tmodule = tmp*0.01f;
                FR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                break;

            case RL_LSB_ID:
                //LSB from RL motor drive
                tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
                RL_Tmotor = tmp*0.01f;
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                RL_Tmodule = tmp*0.01f;
                RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                break;

            case RR_LSB_ID:
                //LSB from RR motor drive
                tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
                RL_Tmotor = tmp*0.01f;
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                RL_Tmodule = tmp*0.01f;
                RL_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                break;
                // end of 10Hz msg
        }
    }
    LED = 0;
}

void Tx_CLRerr_CAN1(void)
{
    RTD_cmd = 0;        //disable as default
    Tx_Tcmd_CAN1();
    RST_cmd = 0;        //on shot
}

void Tx_Estop_CAN1(void)
{
    RTD_cmd = 0;        //force disable
    Tx_Tcmd_CAN1();
}

void Tx_Tcmd_CAN1(void)   // 100 Hz
{
    int16_t tmp;
    // Need to change ID for real case 2019/11/14
    tmp = (int16_t) (FL_Tcmd * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    temp_msg[2] = RTD_cmd;
    temp_msg[3] = RST_cmd;
    can_msg_Tx = CANMessage(FL_HSB_ID,temp_msg,8,CANData,CANStandard);  // FL_CMD_ID, now only for testing
//    can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    tmp = (int16_t) (FR_Tcmd * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    temp_msg[2] = RTD_cmd;
    temp_msg[3] = RST_cmd;
    can_msg_Tx = CANMessage(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
//    can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    tmp = (int16_t) (RL_Tcmd * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    temp_msg[2] = RTD_cmd;
    temp_msg[3] = RST_cmd;
    can_msg_Tx = CANMessage(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
//    can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    tmp = (int16_t) (RR_Tcmd * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    temp_msg[2] = RTD_cmd;
    temp_msg[3] = RST_cmd;
    can_msg_Tx = CANMessage(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
//    can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

}

void Tx_Qdrv_CAN1(void)   // 10 Hz
{
    // Auxilary sensor reading shitting out
    temp_msg[0] = AUX_1_u16;
    temp_msg[1] = AUX_1_u16 >> 8U;
    temp_msg[2] = AUX_2_u16;
    temp_msg[3] = AUX_2_u16 >> 8U;
    temp_msg[4] = AUX_3_u16;
    temp_msg[5] = AUX_3_u16 >> 8U;
    temp_msg[6] = AUX_4_u16;
    temp_msg[7] = AUX_4_u16 >> 8U;
    can_msg_Tx = CANMessage(AUX_sense_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    // Qdrive internal state shitting out
    temp_msg[0] = 1;
    temp_msg[1] = 2;
    temp_msg[2] = 3;
    temp_msg[3] = 4;
    temp_msg[4] = 5;
    temp_msg[5] = 6;
    temp_msg[6] = 7;
    temp_msg[7] = 8;
    can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
    temp_msg[0] = 1;
    temp_msg[1] = 2;
    temp_msg[2] = 3;
    temp_msg[3] = 4;
    temp_msg[4] = 5;
    temp_msg[5] = 6;
    temp_msg[6] = 7;
    temp_msg[7] = 8;
    can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);
}

void CANpendTX(void)
{
    //Pend till TX box has empty slot, timeout will generate error
    uint32_t timeout = 0;
    while(!(CAN1->TSR & CAN_TSR_TME_Msk)) {
        //Wait till non empty
        timeout += 1;
        if(timeout > 10000) {
            // Put some timeout error handler
            break;
        }
    }
}

void CAN_init(void)
{
    //Set CAN system
    SET_BIT(CAN1->MCR, CAN_MCR_ABOM);               // Enable auto reboot after bus off
    can1.filter(FL_HSB_ID,0xFFFF,CANStandard,0);    // ID filter listing mode
    can1.filter(FR_HSB_ID,0xFFFF,CANStandard,1);
    can1.filter(RL_HSB_ID,0xFFFF,CANStandard,2);
    can1.filter(RR_HSB_ID,0xFFFF,CANStandard,3);
    can1.filter(FL_LSB_ID,0xFFFF,CANStandard,4);
    can1.filter(FR_LSB_ID,0xFFFF,CANStandard,5);
    can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
    can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
    can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8);
    can1.mode(CAN::GlobalTest);                     // Add only for testing 2019/11/13
    can1.attach(&Rx_CAN1, CAN::RxIrq);              //CAN1 Recieve Irq
}

void Module_WD(void)
{
    if (FL_online != 0) {
        FL_online -= 1;
    }
    if (FR_online != 0) {
        FR_online -= 1;
    }
    if (RL_online != 0) {
        RL_online -= 1;
    }
    if (RR_online != 0) {
        RR_online -= 1;
    }
    if (PSU_online != 0) {
        PSU_online -= 1;
    }
}

int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
    int16_t max = i1;
    if(i2 > max) max = i2;
    if(i3 > max) max = i3;
    if(i4 > max) max = i4;
    return max;
}
//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);