SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 20:e9daae390513
- Parent:
- 19:d68f21173c23
- Child:
- 21:e01a019fae2f
diff -r d68f21173c23 -r e9daae390513 main.cpp --- a/main.cpp Wed Jan 08 13:10:51 2020 +0000 +++ b/main.cpp Wed Mar 04 08:17:12 2020 +0000 @@ -109,6 +109,7 @@ //Check if state transition only when all module online if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; + RST_HMI = 0; //Ensure new RST action after error printf("POST Fault\n"); FLT_print = 1; } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) { @@ -143,6 +144,7 @@ if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; + RST_HMI = 0; //Ensure new RST action after error printf("Idle 2 Fault\n"); FLT_print = 1; } else if (RTD_HMI != 0) { //Or command to run threw PSU @@ -178,6 +180,7 @@ if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; + RST_HMI = 0; //Ensure new RST action after error printf("Run 2 Fault\n"); FLT_print = 1; } else if (RTD_HMI != 1) { //Or command to stop threw can bus @@ -229,7 +232,7 @@ // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.rate[0], imu.imuProcessedData.rate[1], imu.imuProcessedData.rate[2]); // pc.printf("%.3f,%.3f,%.3f\n\r", imu.imuProcessedData.accel[0], imu.imuProcessedData.accel[1], imu.imuProcessedData.accel[2]); // 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,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu); } // End of high speed loop @@ -330,8 +333,8 @@ VDU_FLT |= PSUOFL_VDUFLTCode; //PSU timeout } - //Check ShutDrv voltage - if(VDU_STAT == VDU_Run) { + //Check ShutDrv voltage when running + if(VDU_STAT == VDU_Run) { if(SDn_voltage < 8.0f) { VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv } @@ -446,14 +449,14 @@ // Start of 100Hz msg case FL_HSB_ID://1 //HSB from FL motor drive - FL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT + 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; 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; + FL_online = 5; //If fault if(FL_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault @@ -462,14 +465,14 @@ case FR_HSB_ID://2 //HSB from FR motor drive - FR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT + FR_DSM = can_msg_Rx.data[6] & 0x03; //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; + FR_online = 5; if(FR_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } @@ -477,14 +480,14 @@ case RL_HSB_ID://3 //HSB from RL motor drive - RL_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT + 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; 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; + RL_online = 5; if(RL_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } @@ -492,14 +495,14 @@ case RR_HSB_ID://4 //HSB from RR motor drive - RR_DSM = can_msg_Rx.data[6] & 0x01; //Get DSM_STAT + RR_DSM = can_msg_Rx.data[6] & 0x03; //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; + RR_online = 5; if(RR_DSM == 3U) { VDU_FLT |= DSM_VDUFLTCode; //DSM Fault } @@ -514,7 +517,7 @@ 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; + PSU_online = 5; break; // end of 100Hz msg @@ -567,7 +570,7 @@ void Tx_CLRerr_CAN1(void) { Tx_Estop_CAN1(); //disable as default - RST_cmd = 0; //clear out on shot + RST_cmd = 0; //clear out one shot } void Tx_Estop_CAN1(void) @@ -583,7 +586,7 @@ temp_msg[0] = tmp; temp_msg[1] = tmp >> 8U; temp_msg[2] = RTD_cmd; - temp_msg[3] = RST_cmd; +// temp_msg[3] = 0U; // 2020/3/4 add to disable HMI reseting Driver temp_msg[4] = 0U; temp_msg[5] = 0U; temp_msg[6] = 0U; @@ -595,8 +598,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; +// temp_msg[2] = RTD_cmd; +// temp_msg[3] = RST_cmd; can_msg_Tx = CANMessage(FR_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); @@ -604,8 +607,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; +// temp_msg[2] = RTD_cmd; +// temp_msg[3] = RST_cmd; can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); @@ -613,8 +616,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; +// temp_msg[2] = RTD_cmd; +// temp_msg[3] = RST_cmd; can_msg_Tx = CANMessage(RR_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx);