SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 25:3c6e83b449b2
- Parent:
- 24:518ec8a4fb6d
- Child:
- 26:ad4fbceeb4ae
--- 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