SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 14:bcf5cb4d08a5
- Parent:
- 13:ac024885d0bf
- Child:
- 18:780366f2534c
--- 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
}