Revision 25:3c6e83b449b2, committed 2021-02-21

Comitter:

open4416

Date:

Sun Feb 21 04:19:41 2021 +0000

Parent:

24:518ec8a4fb6d

Child:

26:ad4fbceeb4ae

Commit message:

Typical version (tested for few months)

Changed in this revision

--- a/main.cpp	Sun Jul 19 03:27:01 2020 +0000
+++ b/main.cpp	Sun Feb 21 04:19:41 2021 +0000
@@ -25,6 +25,8 @@
 #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
@@ -279,6 +281,8 @@
 //            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
@@ -297,12 +301,12 @@
                 //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;
+                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
+//                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;
@@ -374,9 +378,9 @@
 void POST(void)
 {
     //Check IMU status abnormal
-    if(fabsf(YR_imu) > 250) {                   //half turn per sec, strange
-        VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
-    }
+//    if(fabsf(YR_imu) > 250) {                   //half turn per sec, strange
+//        VDU_FLT |= IMUSTA_VDUFLTCode;           //IMU status error
+//    }
 }
 
 void RUNT(void)
@@ -396,9 +400,14 @@
     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
+            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)
@@ -457,6 +466,7 @@
 {
 #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];
@@ -474,22 +484,91 @@
 
 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;
+    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
+        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.15f*Trq_HMI - Tlsd_tmp;
+            FR_Tcmd = 0.15f*Trq_HMI;
+        } else {        //L < R, Tlsd_tmp < 0
+            FL_Tcmd = 0.15f*Trq_HMI;
+            FR_Tcmd = 0.15f*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;
@@ -498,16 +577,16 @@
 //    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;
+//    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
-    FL_Tcmd = -1.0f*FL_Tcmd;
-    FR_Tcmd = 1.0f*FR_Tcmd;
-    RL_Tcmd = -1.0f*RL_Tcmd;
-    RR_Tcmd = 1.0f*RR_Tcmd;
+//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;
 }
 
 void ASL(void)
@@ -529,16 +608,15 @@
                 //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;
+                FL_W_ele = tmp*-1.0f;
                 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-                FL_Trq_fil3 = tmp * 0.01f;
+                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_Trq_est = tmp * -0.01f;
                 FL_online = 5;
                 //If fault
                 if(FL_DSM == 3U) {
-                    // USE THIS FOR FULL FUNCTION
-//                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
+                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                 }
                 break;
 
@@ -553,8 +631,7 @@
                 FR_Trq_est = tmp * 0.01f;
                 FR_online = 5;
                 if(FR_DSM == 3U) {
-                    // USE THIS FOR FULL FUNCTION
-//                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
+                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                 }
                 break;
 
@@ -562,11 +639,11 @@
                 //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;
+                RL_W_ele = tmp*-1.0f;
                 tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
-                RL_Trq_fil3 = tmp * 0.01f;
+                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_Trq_est = tmp * -0.01f;
                 RL_online = 5;
                 if(RL_DSM == 3U) {
                     VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
@@ -584,8 +661,7 @@
                 RR_Trq_est = tmp * 0.01f;
                 RR_online = 5;
                 if(RR_DSM == 3U) {
-                    // USE THIS FOR FULL FUNCTION
-//                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
+                    VDU_FLT |= DSM_VDUFLTCode;                  //DSM Fault
                 }
                 break;
 
@@ -595,7 +671,7 @@
                 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;
+                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;
@@ -642,6 +718,10 @@
                 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
         }
     }
@@ -663,7 +743,7 @@
 void Tx_Tcmd_CAN1(void)     // 100 Hz
 {
     int16_t tmp;
-    tmp = (int16_t) (FL_Tcmd * 100.0f);
+    tmp = (int16_t) (FL_Tcmd * -100.0f);
     temp_msg[0] = tmp;
     temp_msg[1] = tmp >> 8U;
     temp_msg[2] = RTD_cmd;
@@ -684,7 +764,7 @@
     CANpendTX();
     can1.write(can_msg_Tx);
 
-    tmp = (int16_t) (RL_Tcmd * 100.0f);
+    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);
@@ -698,11 +778,34 @@
     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;
+    //int16_t tmp;
     // Auxilary sensor reading shitting out
     temp_msg[0] = AUX_1_u16;
     temp_msg[1] = AUX_1_u16 >> 8U;
@@ -721,29 +824,13 @@
     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*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);
-
-    // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
-    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);
 }
 
 void CANpendTX(void)
@@ -773,6 +860,7 @@
     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
 }
@@ -910,10 +998,10 @@
     Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
 
     //2020/6/14 only for test use AWD_HMI
-    if(AWD_HMI) {
+    if(RTD_SW) {
+        Aux_Rly = 0;
+    } else {
         Aux_Rly = 1;
-    } else {
-        Aux_Rly = 0;
     }
 }
 
@@ -934,4 +1022,3 @@
     if(i4 > max) max = i4;
     return max;
 }
-//            pc.printf("SOC: %.2f\n", Module_Total*0.01f);
\ No newline at end of file

--- a/main.h	Sun Jul 19 03:27:01 2020 +0000
+++ b/main.h	Sun Feb 21 04:19:41 2021 +0000
@@ -1,11 +1,11 @@
 #define Track   1.240f      //Average wheel track
-#define Base    1.530f      //wheel base
+#define Base    1.560f      //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      //EG<0: over steer, EG>0:under steer
-#define K_yaw   0.000f      //Gain for yaw regulator
+#define K_yaw   1.000f      //Gain for yaw regulator
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 // CAN ID define
@@ -24,7 +24,10 @@
 #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 IMU_sense_ID 0xE2   // Tx, 100Hz
+#define RegularVar_ID 0xE3  // Tx, 100Hz
+#define PedalStat_ID 0xE4   // Tx, 10Hz
+
 
 #define MODOFL_VDUFLTCode       0x0001U     //Drive module timeout
 #define PSUOFL_VDUFLTCode       0x0002U     //Pedal unit timeout
@@ -40,12 +43,13 @@
 float Roll_imu = 0;                 // deg
 float Pitch_imu = 0;                // deg
 float Yaw_imu = 0;                  // deg
-float Ax_imu = 0;                   // m/s/s
-float Ay_imu = 0;                   // m/s/s
+float Ax_imu = 0;                   // g
+float Ay_imu = 0;                   // g
 
 //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 Vx_wss = 0;                   // m/s estimated body Velocity X (wheelspeed only)
+float Vx_fil = 0;                   // m/s estimated body Velocity X
 float Mz_reg = 0;                   // Nm addon yaw torque regulating yawrate, gain given by "K_yaw"
 float sig = 0;                      // Front rear distribution factor
 
@@ -110,7 +114,8 @@
 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
+float Steer_HMI = 0.0f;             // Steering wheel signal, *st2r to get rad
+uint8_t RTD_SW = 0;                 // 1 = RTD switch on
 
 //10/100Hz tasking
 uint8_t HSTick = 5;                 // High speed tick
@@ -146,8 +151,8 @@
 uint8_t Phase = 0U;                     // 0:Type ind, 1:Num ind, 2:Blank
 uint8_t Blk_n = 0U;
 //Category repetive code
-#define Post_rep 0b00000001
-#define Run_rep  0b00000010
+#define Post_rep 0b00000010
+#define Run_rep  0b00000001
 #define VDU_rep  0b00000100
 //Blink Pattern
 #define F_Blink 0b01010101              // blink from LSB to MSB