Revision 6:fbe401163489, committed 2019-11-14

Comitter:

open4416

Date:

Thu Nov 14 14:29:31 2019 +0000

Parent:

5:8116016abee0

Child:

7:f674ef860c9c

Commit message:

Main structure done, left blank to fill in

Changed in this revision

--- a/main.cpp	Wed Nov 13 13:56:55 2019 +0000
+++ b/main.cpp	Thu Nov 14 14:29:31 2019 +0000
@@ -19,9 +19,19 @@
 #define Qdrv_stat_ID 0xE1   // Tx
 #define IMU_sense_ID 0xE2   // Tx
 
+#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    SD_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
@@ -54,94 +64,339 @@
 uint8_t FR_online = 0;              // reset value is 10 to hold for 0.1sec
 uint8_t RL_online = 0;              // -1 per 100Hz task
 uint8_t RR_online = 0;
+uint8_t PSU_online = 0;
 
-//Decoded out data storage
-float FL_Temp_mtr = 0;              // motor temperature degC, 10Hz refresh
+//Feedback data decoded out storage
+float FL_Temp_mtr = 0;              // motor temperature degC, 10Hz recieving
 float FR_Temp_mtr = 0;
 float RL_Temp_mtr = 0;
 float RR_Temp_mtr = 0;
-float FL_Temp_ivt = 0;              // inverter temperature degC, 10Hz refresh
+float FL_Temp_ivt = 0;              // inverter temperature degC, 10Hz recieving
 float FR_Temp_ivt = 0;
 float RL_Temp_ivt = 0;
 float RR_Temp_ivt = 0;
-uint16_t FL_FLT_Run = 0;            // RUN fault code, 10Hz refresh
+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 refresh
+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 refresh
+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 refresh
+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 refresh
+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 refresh
+uint8_t FL_DSM = 0;                 // DSM state, 100Hz recieving
 uint8_t FR_DSM = 0;
 uint8_t RL_DSM = 0;
 uint8_t RR_DSM = 0;
-CAN_HandleTypeDef hcan1;
 
 //From Pedal Box msg
-uint8_t RTD_HMI = 0;                //1 = requesting
-uint8_t AWD_HMI = 0;                //1 = requesting
+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
 
-//100Hz tasking
-uint8_t timer_flag = 0;
+//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;
 
-void Rx_CAN1(void);
-void Rx_Decode(void);
-void Tx_Tcmd_CAN1(void);
-void Tx_Qdrv_CAN1(void);
-void Error_CAN1(void);
+//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
 
-int16_t max_val(int16_t i1, int16_t i2, int16_t i3);
+//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)
 {
-    timer_flag = 1;
+    HSTick += 1;
+    LSTick += 1;
+    if (HSTick > 9) {           // 100Hz
+        HST_EXFL = 1;
+        HSTick = 0;
+    }
+    if (LSTick > 99) {          // 10Hz
+        LST_EXFL = 1;
+        LSTick = 0;
+    }
 }
 
 int main()
 {
-    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(PSU_cmd_ID,0xFFFF,CANStandard,8);
+    //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
 
-//    can1.mode(CAN::GlobalTest);                   // Add only for testing 2019/11/13
-    can1.attach(&Rx_CAN1, CAN::RxIrq);                  //CAN1 Recieve Irq
+                    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 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);
 
-    //House keeping task
-    ticker1.attach_us(&timer1_interrupt, 10000);        //10 ms
-    while(1) {
-        if(timer_flag == 1) {
+                    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 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
+            if(VDU_STAT == VDU_Run) {
+                //Allow output torque
+                Tx_Tcmd_CAN1();
+            } else if(RST_cmd != 0) {
+                Tx_CLRerr_CAN1();
+            } else {
+                //Force RTD off when not in VDU_Run
+                Tx_Estop_CAN1();
+            }
+
+        }
+
+
+        // Do low speed state reporting
+        if (LST_EXFL == 1) {
+            LST_EXFL = 0;
             Tx_Qdrv_CAN1();
-//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);
-//            pc.printf("Min: %.3f\n", Module_Min*0.0001f);
-//            pc.printf("Max: %.3f\n", Module_Max*0.0001f);
-//            pc.printf("Max1: %.3f\n", Module_Max_BMS1*0.0001f);
-//            pc.printf("Max2: %.3f\n", Module_Max_BMS2*0.0001f);
-//            pc.printf("Temp: %.1f\n", Max_temp*0.01f);
-            timer_flag = 0;
+
+            // Print out error mesagge if exist, 1Hz repeative
+            if(FLT_print != 0) {
+                FLT_print_cnt += 1;
+                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 Err:0x%04X\n\n", VDU_FLT);
+                    FLT_print_cnt = 0;
+                }
+            }
+
         }
     }
 }
 
+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)
+{
+}
+
+void IMU_read(void)
+{
+}
+
+void AWD(void)
+{
+
+}
+
 void Rx_CAN1(void)
 {
     LED = 1;
@@ -188,17 +443,75 @@
     LED = 0;
 }
 
-void Rx_Decode(void)
+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)
+void Tx_Tcmd_CAN1(void)   // 100 Hz
 {
+    // Need to change ID for real case 2019/11/13
+    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(FL_HSB_ID,temp_msg,8,CANData,CANStandard);  // FL_CMD_ID, now only for testing
+    CANpendTX();
+    can1.write(can_msg_Tx);
+
+    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(FR_HSB_ID,temp_msg,8,CANData,CANStandard);
+    CANpendTX();
+    can1.write(can_msg_Tx);
+
+    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(RL_HSB_ID,temp_msg,8,CANData,CANStandard);
+    CANpendTX();
+    can1.write(can_msg_Tx);
+
+    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(RR_HSB_ID,temp_msg,8,CANData,CANStandard);
+    CANpendTX();
+    can1.write(can_msg_Tx);
+
 }
 
-void Tx_Qdrv_CAN1(void)
+void Tx_Qdrv_CAN1(void)   // 10 Hz
 {
-    uint32_t ID = FL_HSB_ID;
+    // Auxilary sensor reading shitting out
     temp_msg[0] = 1;
     temp_msg[1] = 2;
     temp_msg[2] = 3;
@@ -207,38 +520,93 @@
     temp_msg[5] = 6;
     temp_msg[6] = 7;
     temp_msg[7] = 8;
-    can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
-    if(!can1.write(can_msg_Tx)) {
-        //Report fault
-    }
+    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);
 
-    ID = RR_HSB_ID;
-    can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
-    if(!can1.write(can_msg_Tx)) {
-        //Report fault
+    // 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 Send2CAN1(void)
-//{
-//    uint32_t ID = 0xF1;
-//    temp_msg[0] = Module_Total;
-//    temp_msg[1] = Module_Total >> 8;
-//    temp_msg[2] = Module_Min;
-//    temp_msg[3] = Module_Min >> 8;
-//    temp_msg[4] = Module_Max;
-//    temp_msg[5] = Module_Max >> 8;
-//    temp_msg[6] = Max_temp;
-//    temp_msg[7] = Max_temp >> 8;
-//    send_CAN1();
-//    can_msg_Tx = CANMessage(ID,temp_msg,8,CANData,CANStandard);
-//    can1.write(can_msg_Tx);
-//}
+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(PSU_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
+}
 
-int16_t max_val(int16_t i1, int16_t i2, int16_t i3)
+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;
-}
\ No newline at end of file
+}
+//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);