SHENG-HEN HSIEH
/
VDU_2021
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 14:bcf5cb4d08a5, committed 2019-12-20
- Comitter:
- open4416
- Date:
- Fri Dec 20 04:51:03 2019 +0000
- Parent:
- 13:ac024885d0bf
- Child:
- 15:a56b58459086
- Commit message:
- Add AHRS/IMU in, done by Jacky
Changed in this revision
--- a/imu_driver.lib Tue Nov 19 11:54:12 2019 +0000 +++ b/imu_driver.lib Fri Dec 20 04:51:03 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/Arithemetica/code/imu_driver/#866dd73e4ab3 +https://os.mbed.com/users/Arithemetica/code/imu_driver/#4a23d075acb2
--- 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
}
--- a/main.h Tue Nov 19 11:54:12 2019 +0000
+++ b/main.h Fri Dec 20 04:51:03 2019 +0000
@@ -33,7 +33,7 @@
char temp_msg[8] = {0,0,0,0,0,0,0,0};
//IMU readings
-float YR_imu = 0; // rad/s yawrate estimated threw IMU sensor fusion
+float YR_imu = 0; // deg/s yawrate estimated threw IMU sensor fusion
float Roll_imu = 0; // deg
float Pitch_imu = 0; // deg
float Ax_imu = 0; // m/s/s