File content as of revision 27:52260996ce32:

#include "mbed.h"
#include "main.h"

// define this for newversion test mode
//#define IMU_direct

#ifndef IMU_direct
#include "imu_driver.hpp"
#else
typedef struct AhrsRawData {
    uint16_t    status;
    int16_t     rate[3];
    int16_t     accel[3];
    uint16_t    temperature;
    int16_t     attitude[3];
} AhrsData;
#define Read_VG (0x3D)
#define ACCL2F  (4000.0f)
#define GYRO2F  (100.0f)
#define AHRS2F  (90.0f)
#define VG_len  11U
#endif

#define pi  3.14159265359f
#define d2r 0.01745329252f
#define dt  0.01f

#define st2r 0.033f                                 //steer signal to actual rad

DigitalOut  Aux_Rly(PC_10,0);                       //Control aux relay, 1 active
DigitalOut  Fault_Ind(PC_12,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_3);
AnalogIn    AUX_3(PC_2);
AnalogIn    AUX_4(PC_1);
AnalogIn    SDn_sense(PB_0);                        //Shutdown circuit driving end detection
AnalogIn    Brk_sense(PA_4);                        //Brake sensor readings
CAN         can1(PB_8, PB_9, 1000000);              //1Mbps, contain critical torque command message
SPI         spi2(PB_15, PB_14, PB_13);              //1Mbps default, MOSI MISO SCLK, forIMU
#ifndef IMU_direct
ImuDriver   <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg);  //SPI instance, reset, data ready, slave select
#else
AhrsData    ahrsdata;
DigitalOut  cs(PB_1,1);
InterruptIn drdy(PB_2);
#endif
Serial      pc(USBTX, USBRX, 115200);
Ticker      ticker1;                                //100Hz task
CANMessage  can_msg_Rx;
CANMessage  can_msg_Tx;

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();                 // Note now in Gloable test mode only for testing 2019/11/17

    //Start House keeping task
    printf("VDU start up, pend for module online\n");
#ifdef IMU_direct
    drdy.fall(&IMU_isr);                            //IMU interrupt service
    spi2.format(16, 3);
    //spi2.frequency();                             //As default
#endif

    wait_ms(100);
    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, Max Temperature
            Cooler();
            IMU_read();
            Aux_read();
            Module_WD();

            // 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();
//                    printf("%d,%d,%d,%d,%d\n",FL_online,FR_online,RL_online,RR_online,PSU_online);
                    //Check if state transition only when all module online
                    if (VDU_FLT != 0) {                 //Check if any error
                        VDU_STAT = VDU_Fault;
                        RST_HMI = 0;                    //Ensure new RST action after error
                        printf("POST Fault\n");
                        FLT_print = 1;
                    } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) {
//                    } else if (1) {                     //2019/11/30 only use for force online debug
                        //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 (VDU_FLT != 0) {                 //Check if any error
                        VDU_STAT = VDU_Fault;
                        RST_HMI = 0;                    //Ensure new RST action after error
                        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 (VDU_FLT != 0) {                 //Check if any error
                        VDU_STAT = VDU_Fault;
                        RST_HMI = 0;                    //Ensure new RST action after error
                        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_HMI = 0;
                        RST_cmd = 1;
                        FLT_print = 0;                  //Stop error printing
//                        FL_online = 5;              // 0 indicate detection timeout
//                        FR_online = 5;              // reset value is 3 to hold for 0.03sec
//                        RL_online = 5;              // -1 per 100Hz task
//                        RR_online = 5;
//                        PSU_online = 5;
                        VDU_FLT = 0;
                        VDU_STAT = VDU_Reset;
                        printf("VDU rebooting...\n");
                        printf("QDrive rebooting...\n");
                    }                                   //Else keep in state
                    break;

                case VDU_Reset:
                    /* Controller Reset state
                     * Description:
                     *  A state after reset by HMI when Fault latched
                     *  Wait till four driver processing, timeout protected
                     * Do:
                     *  Nothing, just a soft delay
                     * Check:
                     *  Timeout condition met
                     * To VDU_Idle:
                     *  A valid reset
                     * To VDU_Fault:
                     *  A fail reset
                     */

                    RUNT();                             //Run test
                    if(!RL_DSM) {
//                    if(!(FL_DSM|FR_DSM|RL_DSM|RR_DSM)) {  // 2020/3/13 for real case
                        printf("...\n");
                        VDU_FLT &= ~(DSM_VDUFLTCode);   //Clear if fine
                    }

                    Reset_to += 1;                      //Time out check
                    if (Reset_to > 30) {
                        Reset_to = 0;
                        if (VDU_FLT != 0) {             //Check if any error
                            VDU_STAT = VDU_Fault;       //Back to fault state wait for next reset
                            printf("Reset fail 2 Fault\n");
                            FLT_print = 1;
                        } else {                        //A success reset
                            VDU_STAT = VDU_Idle;
                            printf("Reset ok 2 Idle\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) {
                //Send out reset cmd once
                Tx_CLRerr_CAN1();
            } else {
                //Force RTD off when not in VDU_Run
                Tx_Estop_CAN1();
            }

            //2019/12/18 Add here to test IMU, newer version use inturrupt get data
//            pc.printf("%.3f,%.3f,%.3f\n\r", imu.ahrsProcessedData.attitude[0], imu.ahrsProcessedData.attitude[1], imu.ahrsProcessedData.attitude[2]);
//            pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]);
//            pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.accel[0], imu.imuProcessedData.accel[1], imu.imuProcessedData.accel[2]);
//            pc.printf("%.3f,%.3f,%.3f\n\r", YR_imu, Ax_imu, Ay_imu);
//            pc.printf("%.3f,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu);
//            pc.printf("%.3f\n\r", Trq_HMI*100.0f);
//            pc.printf("%.3f\n\r", Steer_HMI);
//            pc.printf("%.1f\n\r", Vx_wss);

        }
        // End of high speed loop

        // Do low speed state reporting 10 Hz
        if (LST_EXFL == 1) {
            LST_EXFL = 0;
            //Cooling
            Cooler();

            //Print low speed data on CAN
            Tx_Qdrv_CAN1();

            // Print out error mesagge if exist, 0.5Hz repeative
            if(FLT_print != 0) {
                //Merge Faults

                //USE THIS FOR FULL FUNCTION
                FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post;
                FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run;

                //ONLY FOR TEST TODO
//                FLT_Post = RL_FLT_Post;     // Use only for single module test
//                FLT_Run = RL_FLT_Run;       //2020/3/10

                //UART
                FLT_print_cnt += FLT_print;
                if(FLT_print_cnt > 19) {
                    if(FLT_Post!=0)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);
                    if(FLT_Run!=0)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);
                    if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT);

                    //Only temperature printing 2020/6/30
                    printf("Tmotor FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
                    printf("Tmodule FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);

                    FLT_print_cnt = 0;
                }

                //LED
                if(Ind_refresh) {
                    // Refresh data for LED indication after run threw
                    FLT_Post_ind = FLT_Post;
                    FLT_Run_ind = FLT_Run;
                    VDU_FLT_ind = VDU_FLT;
                    Ind_refresh = 0;    // Set after run threw all error bits
                }
            } else {
                // Clear & stop LED when no faults
                FLT_Post_ind = 0;
                FLT_Run_ind = 0;
                VDU_FLT_ind = 0;
                Repeat = 0U;
                Phase = 0U;
                Blk_n = 0U;
            }

            // Blinky output
            if (VDU_STAT != VDU_PowerOn) {
                // Case after poweron (all module online) or fault(s) occur
                Ind_refresh = IndicationKernel(
                                  &Pattern,
                                  &Repeat,
                                  &Phase,
                                  &FLT_Post_ind,
                                  &FLT_Run_ind,
                                  &VDU_FLT_ind);
                LED = Indication(Pattern, &Repeat, &Blk_n);
                Fault_Ind = LED;
            } else {
                // Case no fault while waiting for all module online
                LED = !LED;     //Fast 5Hz blinky indicate the activity
                Fault_Ind = LED;
            }


        }
        // 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 abnormal
//    if(fabsf(YR_imu) > 250) {                   //half turn per sec, strange
//        VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
//    }
}

void RUNT(void)
{
    //POST
    POST();

    //Check module response 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 ShutDrv voltage when running
    if(VDU_STAT == VDU_Run) {
        if(SDn_voltage < 8.0f) {
            //2020/4/5 TODO remove in real case
            VDU_FLT |= ShDVol_VDUFLTCode;       //Shutdown circuit unclosed or uv
        }
    }

    //Check IMU status abnormal add in 2020/10/07
    if(fabsf(YR_imu) > 250.0f) {                //half turn per sec, strange
        VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
    }
}

void Aux_read(void)
{
    //Capture analog in at once imu_driver ver
    AUX_1_u16 = AUX_1.read_u16() >> 4U;
    AUX_2_u16 = AUX_2.read_u16() >> 4U;
    AUX_3_u16 = AUX_3.read_u16() >> 4U;
    AUX_4_u16 = AUX_4.read_u16() >> 4U;
    SDn_voltage = 18.81f*SDn_sense.read();      //Read in Shutdown circuit voltage in output end
    Brk_voltage = 5.5f*Brk_sense.read();
}

#ifdef IMU_direct
void IMU_isr(void)
{
    //Start data transfer
    uint8_t data_rx = 0;
    uint16_t reg_VG = Read_VG<<8U;
    cs = 0;
    spi2.write(reg_VG);
    while(data_rx < VG_len) {
        uint16_t spi_data = spi2.write(0x0000);
        switch(data_rx) {
            case 0:
                ahrsdata.status = spi_data;
                break;
            case 1:
            case 2:
            case 3:
                ahrsdata.rate[data_rx - 1] = spi_data;
                break;
            case 4:
            case 5:
            case 6:
                ahrsdata.accel[data_rx - 4] = spi_data;
                break;
            case 7:
                ahrsdata.temperature = spi_data;
                break;
            case 8:
            case 9:
            case 10:
                ahrsdata.attitude[data_rx - 8] = spi_data;
                break;
            default:
                break;
        }
        ++data_rx;
    }
    cs = 1;
}
#endif

void IMU_read(void)
{
#ifndef IMU_direct
    //Get readings threw back ground, direction not checked 2019/12/20
    // Direction checked 2020/10/29
    YR_imu = imu.imuProcessedData.rate[2];
    Ax_imu = imu.imuProcessedData.accel[0];
    Ay_imu = imu.imuProcessedData.accel[1];
    Roll_imu = imu.ahrsProcessedData.attitude[0];
    Pitch_imu = imu.ahrsProcessedData.attitude[1];
    Yaw_imu = imu.ahrsProcessedData.attitude[2];
#else
    YR_imu = ahrsdata.rate[2]/GYRO2F;
    Ax_imu = ahrsdata.accel[0]/ACCL2F;
    Ay_imu = ahrsdata.accel[1]/ACCL2F;
    Roll_imu = ahrsdata.attitude[0]/AHRS2F;
    Pitch_imu = ahrsdata.attitude[1]/AHRS2F;
#endif
}

void AWD(void)
{
    float Tayc_tmp = 0;                     //active part of yaw control system
    float Tlsd_tmp = 0;                     //limit slip differetial torque
    float YdelW_ele = 0;                    //Ideal L-R electric speed difference

    //Get estimate from wheel only
    Vx_wss = (FL_W_ele+FR_W_ele+RL_W_ele+RR_W_ele)*0.25f/4.0f/11.0f*0.235f;      // average, pole pair, reducer, wheel radius

    //Simple comp filter is fine without GSS onboard
    Vx_fil += (Ax_imu-0.01f)*9.807f*0.01f;              //-0.01 here is to be calibrated
    Vx_fil = Vx_fil*0.98f + Vx_wss*0.02f;               //0.98, 0.02 is coefficient

    if(AWD_HMI) {
        // A simple version is put here for reading
        //YR_ref = Steer_HMI*st2r*Vb_est/(Base+EG*Vb_est*Vb_est);

        /*Still in test section, ignore*/
        //sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
        //FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
        //FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
        //RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
        //RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);

        /*A basic static distribution*/
        FL_Tcmd = 0.11f*Trq_HMI;
        FR_Tcmd = 0.11f*Trq_HMI;
        RL_Tcmd = 0.25f*Trq_HMI;
        RR_Tcmd = 0.25f*Trq_HMI;

        /*A basic Yaw control*/
        YR_ref = (Steer_HMI*st2r)*Vx_fil/Base;          // Center calibration moved to can recieve
        Tayc_tmp = (YR_ref - YR_imu*d2r)*10.0f;         // map 1 rad/s YR difference to ~ 10Nm
        Tayc_tmp = constrain(Tayc_tmp,-5.0f,5.0f);
        FL_Tcmd += -Tayc_tmp;
        FR_Tcmd += Tayc_tmp;
        RL_Tcmd += -Tayc_tmp;
        RR_Tcmd += Tayc_tmp;

        /*A basic ideal differential controller*/
        YdelW_ele = YR_ref*Track/0.235f*11.0f*4.0f;  // dir, rate to speed, wheel radius, reducer, pole pair
        //YdelW_ele > 0 when left turning

        /*A basic static distribution + differential for test 2020/7/19*/
        //Front differential
        Tlsd_tmp = (FL_W_ele - FR_W_ele + YdelW_ele)*0.0025f;   // map 1000 rad/s difference to ~ 5Nm
        Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
        FL_Tcmd += -Tlsd_tmp;
        FR_Tcmd += Tlsd_tmp;


        //Rear differential
        Tlsd_tmp = (RL_W_ele - RR_W_ele + YdelW_ele)*0.0025f;   // map 1000 rad/s difference to ~ 5Nm
        Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
        RL_Tcmd += -Tlsd_tmp;
        RR_Tcmd += Tlsd_tmp;

    } else {
        //2020/8/19 for fast test pure rear
        //2021/3/4 change front max to 11Nm to test?
        Tlsd_tmp = (FL_W_ele - FR_W_ele)*0.01f;     // map 1000 rad/s difference to ~ 10Nm
        Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
        if(Tlsd_tmp>0.0f) {  //L > R
            FL_Tcmd = 0.11f*Trq_HMI - Tlsd_tmp;
            FR_Tcmd = 0.11f*Trq_HMI;
        } else {        //L < R, Tlsd_tmp < 0
            FL_Tcmd = 0.11f*Trq_HMI;
            FR_Tcmd = 0.11f*Trq_HMI + Tlsd_tmp;
        }
        //Rear differential
        Tlsd_tmp = (RL_W_ele - RR_W_ele)*0.005f;    // map 1000 rad/s difference to ~ 5Nm
        Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
        if(Tlsd_tmp>0.0f) {  //L > R
            RL_Tcmd = 0.25f*Trq_HMI - Tlsd_tmp;
            RR_Tcmd = 0.25f*Trq_HMI;
        } else {        //L < R, Tlsd_tmp < 0
            RL_Tcmd = 0.25f*Trq_HMI;
            RR_Tcmd = 0.25f*Trq_HMI + Tlsd_tmp;
        }



//        // A basic static distribution 2020/7/19
//        FL_Tcmd = 0.15f*Trq_HMI;
//        FR_Tcmd = 0.15f*Trq_HMI;
//        RL_Tcmd = 0.25f*Trq_HMI;
//        RR_Tcmd = 0.25f*Trq_HMI;
    }//last line of: if(AWD_HMI){}

////Add to force normal drive
//    FL_Tcmd = 0.25f*Trq_HMI;
//    FR_Tcmd = 0.25f*Trq_HMI;
//    RL_Tcmd = 0.25f*Trq_HMI;
//    RR_Tcmd = 0.25f*Trq_HMI;

//Add to force rear drive
//    FL_Tcmd = 0.2f*Trq_HMI;
//    FR_Tcmd = 0.2f*Trq_HMI;
//    RL_Tcmd = 0.5f*Trq_HMI;
//    RR_Tcmd = 0.5f*Trq_HMI;

//Direction define, move to can sendout
//    FL_Tcmd = -1.0f*FL_Tcmd;
//    FR_Tcmd = 1.0f*FR_Tcmd;
//    RL_Tcmd = -1.0f*RL_Tcmd;
//    RR_Tcmd = 1.0f*RR_Tcmd;

//Add to force only one motor drive (DYNO)
//    FL_Tcmd = 0.0f*Trq_HMI;
//    FR_Tcmd = 0.0f*Trq_HMI;
//    RL_Tcmd = 0.1f*Trq_HMI;
//    RR_Tcmd = 0.0f*Trq_HMI;

}

void ASL(void)
{
    //Filter out each motor W_ele and get approximate accel, compare with IMU
    //Policy is set as "degrade only" coefficient exp(K_ASL*delAccel)
    //Fill out if enough time
}

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://1
                //HSB from FL motor drive
                FL_DSM = can_msg_Rx.data[6] & 0x03;             //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 = 5;
                //If fault
                if(FL_DSM == 3U) {
                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                }
                break;

            case FR_HSB_ID://2
                //HSB from FR motor drive
                FR_DSM = can_msg_Rx.data[6] & 0x03;             //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 = 5;
                if(FR_DSM == 3U) {
                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                }
                break;

            case RL_HSB_ID://3
                //HSB from RL motor drive
                RL_DSM = can_msg_Rx.data[6] & 0x03;             //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 = 5;
                if(RL_DSM == 3U) {
                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                }
                break;

            case RR_HSB_ID://4
                //HSB from RR motor drive
                RR_DSM = can_msg_Rx.data[6] & 0x03;             //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 = 5;
                if(RR_DSM == 3U) {
                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                }
                break;

            case HMI_cmd_ID://5
                //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 - 0.0f;                 //0.7f here is to calibrated center
                tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                Trq_HMI = tmp * 0.01f;
                PSU_online = 5;
                break;
            // end of 100Hz msg

            // Start of 10Hz msg
            case FL_LSB_ID://6
                //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://7
                //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://8
                //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://9
                //LSB from RR motor drive
                tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
                RR_Tmotor = tmp*0.01f;
                tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
                RR_Tmodule = tmp*0.01f;
                RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
                RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                break;

            case PedalStat_ID://10
                RTD_SW = 0x01&can_msg_Rx.data[1];
                break;
                // end of 10Hz msg
        }
    }
//    LED = 0;
}

void Tx_CLRerr_CAN1(void)
{
    Tx_Estop_CAN1();        //disable as default
    RST_cmd = 0;            //clear out one shot
}

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

void Tx_Tcmd_CAN1(void)     // 100 Hz
{
    int16_t tmp;
    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;
//    temp_msg[3] = 0U;       // 2020/3/4 add to disable HMI reseting Driver
    temp_msg[4] = 0U;
    temp_msg[5] = 0U;
    temp_msg[6] = 0U;
    temp_msg[7] = 0U;
    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;
    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;
    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;
    can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    // IMU attitude readings shitting out
    tmp = (int16_t) (YR_imu * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    tmp = (int16_t) (Roll_imu * 100.0f);
    temp_msg[2] = tmp;
    temp_msg[3] = tmp >> 8U;
    tmp = (int16_t) (Pitch_imu * 100.0f);
    temp_msg[4] = tmp;
    temp_msg[5] = tmp >> 8U;
    temp_msg[6] = (int8_t)(Ax_imu * 50.0f);
    temp_msg[7] = (int8_t)(Ay_imu * 50.0f);
    can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);

    // Some internal control variables shitting out
    tmp = (int16_t) (Vx_fil * 100.0f);
    temp_msg[0] = tmp;
    temp_msg[1] = tmp >> 8U;
    can_msg_Tx = CANMessage(RegularVar_ID,temp_msg,2,CANData,CANStandard);
    CANpendTX();
    can1.write(can_msg_Tx);
}

void Tx_Qdrv_CAN1(void)   // 10 Hz
{
    //int16_t tmp;
    // 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] = VDU_FLT;
    temp_msg[1] = VDU_FLT >> 8U;
    temp_msg[2] = VDU_STAT;
    temp_msg[3] = (int8_t)(SDn_voltage*10.0f);
    temp_msg[4] = (int8_t)(Brk_voltage*50.0f);
    temp_msg[5] = 0U;
    temp_msg[6] = 0U;
    temp_msg[7] = 0U;
    can_msg_Tx = CANMessage(Qdrv_stat_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);   // PSU online monitoring
    can1.filter(PedalStat_ID,0xFFFF,CANStandard,9); // PSU online monitoring
//    can1.mode(CAN::GlobalTest);                     // Add only for testing 2019/11/13
    can1.attach(&Rx_CAN1, CAN::RxIrq);              // CAN1 Recieve Irq
}

void Module_WD(void)
{
    //Module online dissipitive indicator
    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;
    }
}
uint8_t Indication(                     // Blink indicator in pattern * repeat
    uint8_t pattern,
    uint16_t*repeat,
    uint8_t*blk_n)
{
    uint8_t out = 0;
    if(*repeat==0) return out;          // reject repeat = 0 case, out = 0
    out = (pattern>>(*blk_n)) & 1U;     // blink from LSB to MSB
    if(*blk_n < 7U) {
        *blk_n += 1U;
    } else {                            // a full pattern was passed
        *blk_n = 0U;
        *repeat >>= 1U;
    }
    return out;
}

uint8_t IndicationKernel(               // Generate blink pattern, return 1 if all ind cleared
    uint8_t*pattern,
    uint16_t*repeat,
    uint8_t*phase,
    uint16_t*Post_ind,
    uint16_t*Run_ind,
    uint16_t*VDU_ind)
{
    //Blink indicator in pattern
    //If FLT_Run = 0b0010-0110, pattern will be --......--...--..
    uint8_t refresh = 0;
    if(*repeat!=0) return refresh;              // skip straight to Indication()

    if(*Post_ind != 0) {
        switch(*phase) {
            case 0U:
                *repeat = Post_rep;
                *pattern = L_Pulse;
                *phase = 1U;
                break;

            case 1U:
                *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit
                *Post_ind &= ~*repeat;              // this bit is used out
                *pattern = S_Pulse;
                *phase = 2U;
                break;

            case 2U:
                *repeat = 1U;
                *pattern = N_Pulse;
                *phase = 0U;
                break;
        }
        return refresh;
    }

    if(*Run_ind != 0) {
        switch(*phase) {
            case 0U:
                *repeat = Run_rep;
                *pattern = L_Pulse;
                *phase = 1U;
                break;

            case 1U:
                *repeat = (*Run_ind)&(-*Run_ind);  // extract LSB bit
                *Run_ind &= ~*repeat;              // this bit is used out
                *pattern = S_Pulse;
                *phase = 2U;
                break;

            case 2U:
                *repeat = 1U;
                *pattern = N_Pulse;
                *phase = 0U;
                break;
        }
        return refresh;
    }

    if(*VDU_ind != 0) {
        switch(*phase) {
            case 0U:
                *repeat = VDU_rep;
                *pattern = L_Pulse;
                *phase = 1U;
                break;

            case 1U:
                *repeat = (*VDU_ind)&(-*VDU_ind);  // extract LSB bit
                *VDU_ind &= ~*repeat;              // this bit is used out
                *pattern = S_Pulse;
                *phase = 2U;
                break;

            case 2U:
                *repeat = 1U;
                *pattern = N_Pulse;
                *phase = 0U;
                break;
        }
        return refresh;
    }

    // else all XXX_ind is cleared out, set refresh
    refresh = 1U;
    return refresh;
}

void Cooler(void)
{
    //Cooling auto control, unfinish 2019/11/15
    Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
    Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);

    //2020/6/14 only for test use AWD_HMI
    if(RTD_SW) {
        Aux_Rly = 0;
    } else {
        Aux_Rly = 1;
    }
}

int16_t max_uval(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;
}

float max_fval(float i1, float i2, float i3, float i4)
{
    float max = i1;
    if(i2 > max) max = i2;
    if(i3 > max) max = i3;
    if(i4 > max) max = i4;
    return max;
}