Revision 8:f8b1402c8f3c, committed 2019-11-15

Comitter:

open4416

Date:

Fri Nov 15 08:19:32 2019 +0000

Parent:

7:f674ef860c9c

Child:

9:c99eeafa6fa3

Commit message:

Initial release of workable version

Changed in this revision

--- a/main.cpp	Thu Nov 14 17:06:09 2019 +0000
+++ b/main.cpp	Fri Nov 15 08:19:32 2019 +0000
@@ -1,28 +1,8 @@
 #include "mbed.h"
+#include "main.h"
+#define pi  3.14159265359f
+#define d2r 0.01745329252f
 #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
@@ -33,113 +13,12 @@
 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
+SPI     spi2(PB_15, PB_14, PB_13);                  //1Mbps, MOSI MISO SCLK, forIMU
 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;
@@ -332,6 +211,7 @@
         // Do low speed state reporting
         if (LST_EXFL == 1) {
             LST_EXFL = 0;
+            Cooler();
             Tx_Qdrv_CAN1();
 
             // Print out error mesagge if exist, 1Hz repeative
@@ -362,17 +242,18 @@
 
 void POST(void)
 {
-    //Check IMU status
-    if(0) {
+    //Check IMU status abnormal
+    if(fabsf(YR_imu) > pi) {                    //half turn per sec, strange
         VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
     }
 }
 
 void RUNT(void)
 {
+    //POST
     POST();
 
-    //Check timeout
+    //Check module response timeout
     if((FL_online*FR_online*RL_online*RR_online) == 0) {
         VDU_FLT |= MODOFL_VDUFLTCode;           //Module timeout
     }
@@ -380,11 +261,12 @@
         VDU_FLT |= PSUOFL_VDUFLTCode;           //PSU timeout
     }
 
-    //Check IMU status
-    //XXX
-
     //Check ShutDrv voltage
-    //XXX
+    if(VDU_STAT == VDU_Run) {
+        if(SDn_voltage < 8.0f) {
+            VDU_FLT |= ShDVol_VDUFLTCode;       //Shutdown circuit unclosed or uv
+        }
+    }
 }
 
 void Aux_read(void)
@@ -394,17 +276,44 @@
     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();
+    SDn_voltage = 18.81f*SDn_sense.read();      //Read in Shutdown circuit voltage in output end
 }
 
 void IMU_read(void)
 {
-
+    //Get readings threw SPI, unfinished 2019/11/15
+    YR_imu = 0.0f;
+    Ax_imu = 0.0f;
+    Ay_imu = 0.0f;
+    Roll_imu = 0.0f;
+    Pitch_imu = 0.0f;
 }
 
 void AWD(void)
 {
+    if(AWD_HMI) {
+        // A simple version is put here for reading
+        Vb_est = 0.25f * (FL_W_ele + FR_W_ele + RL_W_ele + RR_W_ele);
+        YR_ref = Steer_HMI*d2r*Vb_est/(Base+EG*Vb_est*Vb_est);
+        Mz_reg = (YR_ref - YR_imu) * K_yaw;         //K_yaw unfinished 2019/11/15
+        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);
+    } else {
+        FL_Tcmd = 0.25f*Trq_HMI;
+        FR_Tcmd = 0.25f*Trq_HMI;
+        RL_Tcmd = 0.25f*Trq_HMI;
+        RR_Tcmd = 0.25f*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)
@@ -415,7 +324,7 @@
     if(can1.read(can_msg_Rx)) {
         switch(can_msg_Rx.id) {                                 //Filtered input message
             // Start of 100Hz msg
-            case FL_HSB_ID:
+            case FL_HSB_ID://1
                 //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];
@@ -427,7 +336,7 @@
                 FL_online = 3;
                 break;
 
-            case FR_HSB_ID:
+            case FR_HSB_ID://2
                 //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];
@@ -439,7 +348,7 @@
                 FR_online = 3;
                 break;
 
-            case RL_HSB_ID:
+            case RL_HSB_ID://3
                 //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];
@@ -451,7 +360,7 @@
                 RL_online = 3;
                 break;
 
-            case RR_HSB_ID:
+            case RR_HSB_ID://4
                 //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];
@@ -463,7 +372,7 @@
                 RR_online = 3;
                 break;
 
-            case HMI_cmd_ID:
+            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
@@ -477,7 +386,7 @@
             // end of 100Hz msg
 
             // Start of 10Hz msg
-            case FL_LSB_ID:
+            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;
@@ -487,7 +396,7 @@
                 FL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                 break;
 
-            case FR_LSB_ID:
+            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;
@@ -497,7 +406,7 @@
                 FR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                 break;
 
-            case RL_LSB_ID:
+            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;
@@ -507,14 +416,14 @@
                 RL_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
                 break;
 
-            case RR_LSB_ID:
+            case RR_LSB_ID://9
                 //LSB from RR motor drive
                 tmp = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
-                RL_Tmotor = tmp*0.01f;
+                RR_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];
+                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;
                 // end of 10Hz msg
         }
@@ -524,9 +433,8 @@
 
 void Tx_CLRerr_CAN1(void)
 {
-    RTD_cmd = 0;        //disable as default
-    Tx_Tcmd_CAN1();
-    RST_cmd = 0;        //on shot
+    Tx_Estop_CAN1();    //disable as default
+    RST_cmd = 0;        //clear out on shot
 }
 
 void Tx_Estop_CAN1(void)
@@ -538,14 +446,12 @@
 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);
+    can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard);
     CANpendTX();
     can1.write(can_msg_Tx);
 
@@ -554,8 +460,7 @@
     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);
+    can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard);
     CANpendTX();
     can1.write(can_msg_Tx);
 
@@ -564,8 +469,7 @@
     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);
+    can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
     CANpendTX();
     can1.write(can_msg_Tx);
 
@@ -574,8 +478,7 @@
     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);
+    can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard);
     CANpendTX();
     can1.write(can_msg_Tx);
 
@@ -583,6 +486,7 @@
 
 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;
@@ -597,27 +501,25 @@
     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;
+    temp_msg[0] = VDU_FLT;
+    temp_msg[1] = VDU_FLT >> 8U;
+    temp_msg[2] = VDU_STAT;
     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;
+    tmp = (int16_t) (YR_imu * 10000.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;
+    temp_msg[7] = (int8_t)Ay_imu;
     can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
     CANpendTX();
     can1.write(can_msg_Tx);
@@ -649,9 +551,9 @@
     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.filter(HMI_cmd_ID,0xFFFF,CANStandard,8);   // PSU online monitoring
     can1.mode(CAN::GlobalTest);                     // Add only for testing 2019/11/13
-    can1.attach(&Rx_CAN1, CAN::RxIrq);              //CAN1 Recieve Irq
+    can1.attach(&Rx_CAN1, CAN::RxIrq);              // CAN1 Recieve Irq
 }
 
 void Module_WD(void)
@@ -673,6 +575,16 @@
     }
 }
 
+void Cooler(void)
+{
+    //Cooling auto control, unfinish 2019/11/15
+    if(0) {
+        Aux_Rly = 1;
+    } else {
+        Aux_Rly = 0;
+    }
+}
+
 int16_t max_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4)
 {
     int16_t max = i1;
@@ -681,4 +593,4 @@
     if(i4 > max) max = i4;
     return max;
 }
-//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);
+//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Fri Nov 15 08:19:32 2019 +0000
@@ -0,0 +1,147 @@
+#define Track   1.240f      //Average wheel track
+#define Base    1.530f      //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      // <0: over steer, >0:under steer
+#define K_yaw   0.000f      //Gain for yaw regulator
+#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
+#define ShDVol_VDUFLTCode       0x0008U     //Shutdown voltage abnormal
+
+//CAN msg bank
+char    temp_msg[8] = {0,0,0,0,0,0,0,0};
+
+//IMU readings
+float YR_imu = 0;                   // rad/s yawrate estimated threw IMU sensor fusion
+float Roll_imu = 0;                 // deg
+float Pitch_imu = 0;                // deg
+float Ax_imu = 0;                   // m/s/s
+float Ay_imu = 0;                   // m/s/s
+
+//AWD variable
+float YR_ref = 0;                   // rad/s yawrate reference generate from ideal single track model
+float Vb_est = 0;                   // m/s estimated body Velocity
+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 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, 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_val(int16_t i1, int16_t i2, int16_t i3, int16_t i4);
\ No newline at end of file