SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 18:780366f2534c
- Parent:
- 14:bcf5cb4d08a5
- Child:
- 19:d68f21173c23
--- a/main.cpp Fri Dec 20 06:35:07 2019 +0000
+++ b/main.cpp Sat Dec 28 14:16:21 2019 +0000
@@ -172,6 +172,7 @@
RUNT(); //Run test
if (RST_HMI == 1) { //PSU reset to clear error
+ RST_HMI = 0;
RST_cmd = 1;
FLT_print = 0; //Stop error printing
VDU_STAT = VDU_PowerOn;
@@ -203,21 +204,62 @@
// Do low speed state reporting 10 Hz
if (LST_EXFL == 1) {
LST_EXFL = 0;
+ //Cooling
Cooler();
+
+ //Print low speed data on CAN
Tx_Qdrv_CAN1();
- // Print out error mesagge if exist, 1Hz repeative
+ // Print out error mesagge if exist, 0.5Hz repeative
if(FLT_print != 0) {
+ //Merge Faults
+ 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;
+
+ //UART
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\n0x%04X\n\n", VDU_FLT);
+ if(FLT_Post!=0)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);
+ if(FLT_Run!=0)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);
+ if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT);
FLT_print_cnt = 0;
}
+
+ //LED
+ if(Ind_refresh) {
+ // Refresh data for LED indication after run threw
+ FLT_Post_ind = FLT_Post;
+ FLT_Run_ind = FLT_Run;
+ VDU_FLT_ind = VDU_FLT;
+ Ind_refresh = 0; // Set after run threw all error bits
+ }
+ } else {
+ // Clear & stop LED when no faults
+ FLT_Post_ind = 0;
+ FLT_Run_ind = 0;
+ VDU_FLT_ind = 0;
+ Repeat = 0U;
+ Phase = 0U;
+ Blk_n = 0U;
}
- LED = !LED;
+ // Blinky output
+ if (VDU_STAT != VDU_PowerOn) {
+ // Case after poweron (all module online) or fault(s) occur
+ Ind_refresh = IndicationKernel(
+ &Pattern,
+ &Repeat,
+ &Phase,
+ &FLT_Post_ind,
+ &FLT_Run_ind,
+ &VDU_FLT_ind);
+ LED = Indication(Pattern, &Repeat, &Blk_n);
+ Fault_Ind = LED;
+ } else {
+ // Case no fault while waiting for all module online
+ LED = !LED; //Fast 5Hz blinky indicate the activity
+ Fault_Ind = LED;
+ }
}
// End of low speed state reporting
@@ -237,7 +279,7 @@
void POST(void)
{
//Check IMU status abnormal
- if(fabsf(YR_imu) > 250) { //half turn per sec, strange
+ if(fabsf(YR_imu) > 250) { //half turn per sec, strange
VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
}
}
@@ -276,7 +318,7 @@
void IMU_read(void)
{
- //Get readings threw back ground, direction not checked 2019/12/20
+ //Get readings threw back ground, direction not checked 2019/12/20
YR_imu = imu.imuProcessedData.rate[2];
Ax_imu = imu.imuProcessedData.accel[0];
Ay_imu = imu.imuProcessedData.accel[1];
@@ -313,7 +355,7 @@
void Rx_CAN1(void)
{
- LED = 1;
+// LED = 1;
int16_t tmp;
if(can1.read(can_msg_Rx)) {
@@ -329,6 +371,10 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
FL_Trq_est = tmp * 0.01f;
FL_online = 3;
+ //If fault
+ if(FL_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case FR_HSB_ID://2
@@ -341,6 +387,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
FR_Trq_est = tmp * 0.01f;
FR_online = 3;
+ if(FR_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case RL_HSB_ID://3
@@ -353,6 +402,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
RL_Trq_est = tmp * 0.01f;
RL_online = 3;
+ if(RL_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case RR_HSB_ID://4
@@ -365,6 +417,9 @@
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
RR_Trq_est = tmp * 0.01f;
RR_online = 3;
+ if(RR_DSM == 3U) {
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ }
break;
case HMI_cmd_ID://5
@@ -423,7 +478,7 @@
// end of 10Hz msg
}
}
- LED = 0;
+// LED = 0;
}
void Tx_CLRerr_CAN1(void)
@@ -579,6 +634,112 @@
PSU_online -= 1;
}
}
+uint8_t Indication( // Blink indicator in pattern * repeat
+ uint8_t pattern,
+ uint16_t*repeat,
+ uint8_t*blk_n)
+{
+ uint8_t out = 0;
+ if(*repeat==0) return out; // reject repeat = 0 case, out = 0
+ out = (pattern>>(*blk_n)) & 1U; // blink from LSB to MSB
+ if(*blk_n < 7U) {
+ *blk_n += 1U;
+ } else { // a full pattern was passed
+ *blk_n = 0U;
+ *repeat >>= 1U;
+ }
+ return out;
+}
+
+uint8_t IndicationKernel( // Generate blink pattern, return 1 if all ind cleared
+ uint8_t*pattern,
+ uint16_t*repeat,
+ uint8_t*phase,
+ uint16_t*Post_ind,
+ uint16_t*Run_ind,
+ uint16_t*VDU_ind)
+{
+ //Blink indicator in pattern
+ //If FLT_Run = 0b0010-0110, pattern will be --......--...--..
+ uint8_t refresh = 0;
+ if(*repeat!=0) return refresh; // skip straight to Indication()
+
+ if(*Post_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = Post_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*Post_ind)&(-*Post_ind); // extract LSB bit
+ *Post_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ if(*Run_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = Run_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*Run_ind)&(-*Run_ind); // extract LSB bit
+ *Run_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ if(*VDU_ind != 0) {
+ switch(*phase) {
+ case 0U:
+ *repeat = VDU_rep;
+ *pattern = L_Pulse;
+ *phase = 1U;
+ break;
+
+ case 1U:
+ *repeat = (*VDU_ind)&(-*VDU_ind); // extract LSB bit
+ *VDU_ind &= ~*repeat; // this bit is used out
+ *pattern = S_Pulse;
+ *phase = 2U;
+ break;
+
+ case 2U:
+ *repeat = 1U;
+ *pattern = N_Pulse;
+ *phase = 0U;
+ break;
+ }
+ return refresh;
+ }
+
+ // else all XXX_ind is cleared out, set refresh
+ refresh = 1U;
+ return refresh;
+}
void Cooler(void)
{