SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 20:e9daae390513
- Parent:
- 19:d68f21173c23
- Child:
- 21:e01a019fae2f
--- 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);