SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 14:bcf5cb4d08a5
- Parent:
- 13:ac024885d0bf
- Child:
- 18:780366f2534c
diff -r ac024885d0bf -r bcf5cb4d08a5 main.cpp --- a/main.cpp Tue Nov 19 11:54:12 2019 +0000 +++ b/main.cpp Fri Dec 20 04:51:03 2019 +0000 @@ -16,7 +16,7 @@ AnalogIn Brk_sense(PA_4); //Brake sensor readings CAN can1(PB_8, PB_9, 1000000); //1Mbps, contain critical torque command message SPI spi2(PB_15, PB_14, PB_13); //1Mbps default, MOSI MISO SCLK, forIMU -ImuDriver <spi2, PA_13, PA_14, PA_15> imu; //SPI instance, reset, data ready, slave select +ImuDriver <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg); //SPI instance, reset, data ready, slave select Serial pc(USBTX, USBRX, 115200); Ticker ticker1; //100Hz task CANMessage can_msg_Rx; @@ -76,22 +76,15 @@ POST(); //Check if state transition only when all module online -// if((FL_online*FR_online*RL_online*RR_online*PSU_online)!= 0) { - if(1) { //Force online check pass only for debug 2019/11/14 - - if ( //Check if any error - (FL_DSM == 3U)|| - (FR_DSM == 3U)|| - (RL_DSM == 3U)|| - (RR_DSM == 3U)|| - (VDU_FLT != 0)) { - VDU_STAT = VDU_Fault; - FLT_print = 1; - } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) { - //All module online & POST pass - VDU_STAT = VDU_Idle; - printf("All module online, VDU now Idle\n"); - } + if (VDU_FLT != 0) { //Check if any error + VDU_STAT = VDU_Fault; + printf("POST Fault\n"); + FLT_print = 1; + } else if ((FL_online*FR_online*RL_online*RR_online*PSU_online)!=0) { +// } else if (1) { //2019/11/30 only use for force online debug + //All module online & POST pass + VDU_STAT = VDU_Idle; + printf("All module online, VDU now Idle\n"); } //Else keep in state VDU_PowerOn break; @@ -117,12 +110,7 @@ RUNT(); //Run test AWD(); //AWD main program - if ( //Check if any error - (FL_DSM == 3U)|| - (FR_DSM == 3U)|| - (RL_DSM == 3U)|| - (RR_DSM == 3U)|| - (VDU_FLT != 0)) { + if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; printf("Idle 2 Fault\n"); FLT_print = 1; @@ -157,12 +145,7 @@ //printf("%d\n\r", (int16_t)Tmodule);// //printf("%d\n\r", (int16_t)Vbus); - if ( //Check if any error - (FL_DSM == 3U)|| - (FR_DSM == 3U)|| - (RL_DSM == 3U)|| - (RR_DSM == 3U)|| - (VDU_FLT != 0)) { + if (VDU_FLT != 0) { //Check if any error VDU_STAT = VDU_Fault; printf("Run 2 Fault\n"); FLT_print = 1; @@ -190,9 +173,9 @@ if (RST_HMI == 1) { //PSU reset to clear error RST_cmd = 1; - FLT_print = 0; + FLT_print = 0; //Stop error printing VDU_STAT = VDU_PowerOn; - printf("Module reset\nVDU restarting...\n"); + printf("VDU rebooting...\n"); } //Else keep in state break; } @@ -209,6 +192,11 @@ Tx_Estop_CAN1(); } + //2019/12/18 Add here to test IMU, newer version use inturrupt get data +// pc.printf("%.3f,%.3f,%.3f\n\r", imu.ahrsProcessedData.attitude[0], imu.ahrsProcessedData.attitude[1], imu.ahrsProcessedData.attitude[2]); +// 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]); + } // End of high speed loop @@ -228,6 +216,8 @@ FLT_print_cnt = 0; } } + + LED = !LED; } // End of low speed state reporting @@ -247,7 +237,7 @@ void POST(void) { //Check IMU status abnormal - if(fabsf(YR_imu) > pi) { //half turn per sec, strange + if(fabsf(YR_imu) > 250) { //half turn per sec, strange VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error } } @@ -286,12 +276,12 @@ void IMU_read(void) { - //Get readings threw SPI, unfinished 2019/11/15 - YR_imu = 0.0f; - Ax_imu = 0.0f; - Ay_imu = 0.0f; - Roll_imu = 0.0f; - Pitch_imu = 0.0f; + //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]; + Roll_imu = imu.ahrsProcessedData.attitude[0]; + Pitch_imu = imu.ahrsProcessedData.attitude[1]; } void AWD(void) @@ -456,6 +446,10 @@ temp_msg[1] = tmp >> 8U; temp_msg[2] = RTD_cmd; temp_msg[3] = RST_cmd; + temp_msg[4] = 0U; + temp_msg[5] = 0U; + temp_msg[6] = 0U; + temp_msg[7] = 0U; can_msg_Tx = CANMessage(FL_CMD_ID,temp_msg,8,CANData,CANStandard); CANpendTX(); can1.write(can_msg_Tx); @@ -509,6 +503,11 @@ temp_msg[0] = VDU_FLT; 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[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); @@ -557,7 +556,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.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 +// can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13 can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq }