Revision 7:f674ef860c9c, committed 2019-11-14

Comitter:

open4416

Date:

Thu Nov 14 17:06:09 2019 +0000

Parent:

6:fbe401163489

Child:

8:f8b1402c8f3c

Commit message:

Fill in CAN receiving part

Changed in this revision

--- a/main.cpp	Thu Nov 14 14:29:31 2019 +0000
+++ b/main.cpp	Thu Nov 14 17:06:09 2019 +0000
@@ -2,22 +2,22 @@
 #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
-#define RL_HSB_ID 0xA2      // Rx
-#define RR_HSB_ID 0xA3      // Rx
+#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
-#define RL_LSB_ID 0xB2      // Rx
-#define RR_LSB_ID 0xB3      // Rx
-#define PSU_cmd_ID 0xC4     // Rx, 100Hz
+#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
-#define RL_CMD_ID 0xC2      // Tx
-#define RR_CMD_ID 0xC3      // Tx
+#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
-#define IMU_sense_ID 0xE2   // Tx
+#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
@@ -31,7 +31,7 @@
 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
+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
@@ -39,16 +39,8 @@
 CANMessage  can_msg_Rx;
 CANMessage  can_msg_Tx;
 
-//Recive bank for HSB & LFB
+//CAN msg bank
 char    temp_msg[8] = {0,0,0,0,0,0,0,0};
-char    FL_HFB[8];                                  //data space to speed up can_Rx speed
-char    FR_HFB[8];
-char    RL_HFB[8];
-char    RR_HFB[8];
-char    FL_LFB[8];
-char    FR_LFB[8];
-char    RL_LFB[8];
-char    RR_LFB[8];
 
 //CAN to motor drive module, 100Hz
 //msg ID: 0xC0~0xC3
@@ -61,20 +53,20 @@
 
 //Module online flag
 uint8_t FL_online = 0;              // 0 indicate detection timeout
-uint8_t FR_online = 0;              // reset value is 10 to hold for 0.1sec
+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_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 recieving
-float FR_Temp_ivt = 0;
-float RL_Temp_ivt = 0;
-float RR_Temp_ivt = 0;
+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;
@@ -104,6 +96,8 @@
 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
@@ -112,6 +106,11 @@
 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 {
@@ -194,8 +193,8 @@
                     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((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)||
@@ -229,7 +228,7 @@
                      */
 
                     //Forced RTD_HMI for debug purpose 2019/11/14
-//                    RTD_HMI = 1;                        //Should be set if can bus received data
+                    RTD_HMI = 1;                        //Should be set if can bus received data
                     //Forced RTD_HMI for debug purpose 2019/11/14
 
                     RUNT();                             //Run test
@@ -242,6 +241,7 @@
                         (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
@@ -281,6 +281,7 @@
                         (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();
@@ -313,11 +314,12 @@
                     break;
             }
 
-            // Shit out torque distribution
+            // 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
@@ -325,7 +327,7 @@
             }
 
         }
-
+        // End of high speed loop
 
         // Do low speed state reporting
         if (LST_EXFL == 1) {
@@ -334,17 +336,18 @@
 
             // Print out error mesagge if exist, 1Hz repeative
             if(FLT_print != 0) {
-                FLT_print_cnt += 1;
+                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 Err:0x%04X\n\n", VDU_FLT);
+                    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)
@@ -386,10 +389,17 @@
 
 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)
@@ -400,45 +410,114 @@
 void Rx_CAN1(void)
 {
     LED = 1;
+    int16_t tmp;
+
     if(can1.read(can_msg_Rx)) {
-//        switch(can_msg_Rx.id) {                                                  //Filtered the input message and do corresponding action
-//            case 0xF0:
-//                //Internal state address
-//                RTD_State = can_msg_Rx.data[7] & 0x01;                           //get bit "0", result ( 1 = RTD, 0 = OFF )
-//                CAN_flag_1 = 1;
-//                break;
-//
-//            case 0xA0 :
-//                //Temperature infrom address 1
-//                IGBT_Temp_A = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
-//                IGBT_Temp_B = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-//                IGBT_Temp_C = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
-//                IGBT_Temp = max_val(IGBT_Temp_A,IGBT_Temp_B,IGBT_Temp_C);
-//                CAN_flag_1 = 1;
-//                break;
-//
-//            case 0xA2 :
-//                //Temperature infrom address 3
-//                Motor_Temp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
-//                CAN_flag_1 = 1;
-//                break;
-//
-//            case 0xA5:
-//                Motor_Speed = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-//                break;
-//
-//            case 0xA6 :
-//                //Current infrom address
-//                DC_Current = can_msg_Rx.data[7] << 8 | can_msg_Rx.data[6];
-//                CAN_flag_1 = 1;
-//                break;
-//
-//            case 0xA7 :
-//                //Voltage infrom address
-//                DC_Voltage = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
-//                CAN_flag_1 = 1;
-//                break;
-//        }
+        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;
 }
@@ -458,52 +537,45 @@
 
 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;
+    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);
 
-    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) (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);
 
-    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) (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);
 
-    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) (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);
 
@@ -512,14 +584,14 @@
 void Tx_Qdrv_CAN1(void)   // 10 Hz
 {
     // Auxilary sensor reading 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] = 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);
@@ -577,7 +649,7 @@
     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.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
 }