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 19:d68f21173c23, committed 2020-01-08
- Comitter:
- open4416
- Date:
- Wed Jan 08 13:10:51 2020 +0000
- Parent:
- 18:780366f2534c
- Child:
- 20:e9daae390513
- Commit message:
- Add test code for IMU, in case future update
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Dec 28 14:16:21 2019 +0000
+++ b/main.cpp Wed Jan 08 13:10:51 2020 +0000
@@ -1,6 +1,26 @@
#include "mbed.h"
#include "main.h"
+
+// define this for newversion test mode
+//#define IMU_direct
+
+#ifndef IMU_direct
#include "imu_driver.hpp"
+#else
+typedef struct AhrsRawData {
+ uint16_t status;
+ int16_t rate[3];
+ int16_t accel[3];
+ uint16_t temperature;
+ int16_t attitude[3];
+} AhrsData;
+#define Read_VG (0x3D)
+#define ACCL2F (4000.0f)
+#define GYRO2F (100.0f)
+#define AHRS2F (90.0f)
+#define VG_len 11U
+#endif
+
#define pi 3.14159265359f
#define d2r 0.01745329252f
#define dt 0.01f
@@ -16,7 +36,13 @@
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
+#ifndef IMU_direct
ImuDriver <spi2, PC_4, PB_2, PB_1> imu(ImuExtiRcvBothMsg); //SPI instance, reset, data ready, slave select
+#else
+AhrsData ahrsdata;
+DigitalOut cs(PB_1,1);
+InterruptIn drdy(PB_2);
+#endif
Serial pc(USBTX, USBRX, 115200);
Ticker ticker1; //100Hz task
CANMessage can_msg_Rx;
@@ -43,6 +69,11 @@
//Start House keeping task
printf("VDU start up, pend for module online\n");
+#ifdef IMU_direct
+ drdy.fall(&IMU_isr); //IMU interrupt service
+ spi2.format(16, 3);
+ //spi2.frequency(); //As default
+#endif
ticker1.attach_us(&timer1_interrupt, 1000); //1 ms Systick
while(1) {
@@ -197,6 +228,8 @@
// 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]);
+// 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);
}
// End of high speed loop
@@ -307,7 +340,7 @@
void Aux_read(void)
{
- //Capture analog in at once
+ //Capture analog in at once imu_driver ver
AUX_1_u16 = AUX_1.read_u16() >> 4U;
AUX_2_u16 = AUX_2.read_u16() >> 4U;
AUX_3_u16 = AUX_3.read_u16() >> 4U;
@@ -316,14 +349,64 @@
Brk_voltage = 5.5f*Brk_sense.read();
}
+#ifdef IMU_direct
+void IMU_isr(void)
+{
+ //Start data transfer
+ uint8_t data_rx = 0;
+ uint16_t reg_VG = Read_VG<<8U;
+ cs = 0;
+ spi2.write(reg_VG);
+ while(data_rx < VG_len) {
+ uint16_t spi_data = spi2.write(0x0000);
+ switch(data_rx) {
+ case 0:
+ ahrsdata.status = spi_data;
+ break;
+ case 1:
+ case 2:
+ case 3:
+ ahrsdata.rate[data_rx - 1] = spi_data;
+ break;
+ case 4:
+ case 5:
+ case 6:
+ ahrsdata.accel[data_rx - 4] = spi_data;
+ break;
+ case 7:
+ ahrsdata.temperature = spi_data;
+ break;
+ case 8:
+ case 9:
+ case 10:
+ ahrsdata.attitude[data_rx - 8] = spi_data;
+ break;
+ default:
+ break;
+ }
+ ++data_rx;
+ }
+ cs = 1;
+}
+#endif
+
void IMU_read(void)
{
+#ifndef IMU_direct
//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];
+ Yaw_imu = imu.ahrsProcessedData.attitude[2];
+#else
+ YR_imu = ahrsdata.rate[2]/GYRO2F;
+ Ax_imu = ahrsdata.accel[0]/ACCL2F;
+ Ay_imu = ahrsdata.accel[1]/ACCL2F;
+ Roll_imu = ahrsdata.attitude[0]/AHRS2F;
+ Pitch_imu = ahrsdata.attitude[1]/AHRS2F;
+#endif
}
void AWD(void)
--- a/main.h Sat Dec 28 14:16:21 2019 +0000 +++ b/main.h Wed Jan 08 13:10:51 2020 +0000 @@ -7,6 +7,8 @@ #define EG 0.000f //EG<0: over steer, EG>0:under steer #define K_yaw 0.000f //Gain for yaw regulator #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// CAN ID define #define FL_HSB_ID 0xA0 // Rx, 100Hz #define FR_HSB_ID 0xA1 // Rx, 100Hz #define RL_HSB_ID 0xA2 // Rx, 100Hz @@ -37,6 +39,7 @@ float YR_imu = 0; // deg/s yawrate estimated threw IMU sensor fusion float Roll_imu = 0; // deg float Pitch_imu = 0; // deg +float Yaw_imu = 0; // deg float Ax_imu = 0; // m/s/s float Ay_imu = 0; // m/s/s @@ -141,18 +144,19 @@ uint8_t Phase = 0U; // 0:Type ind, 1:Num ind, 2:Blank uint8_t Blk_n = 0U; //Category repetive code -#define Post_rep 0b00000001; -#define Run_rep 0b00000010; -#define VDU_rep 0b00000100; +#define Post_rep 0b00000001 +#define Run_rep 0b00000010 +#define VDU_rep 0b00000100 //Blink Pattern -#define F_Blink 0b01010101; // blink from LSB to MSB -#define L_Pulse 0b01111110; -#define S_Pulse 0b01000000; -#define N_Pulse 0b00000000; +#define F_Blink 0b01010101 // blink from LSB to MSB +#define L_Pulse 0b01111110 +#define S_Pulse 0b01000000 +#define N_Pulse 0b00000000 //Prototype void CAN_init(void); //Initial CAN frequency filter... void Module_WD(void); //Software watchdog indicate module state +void IMU_isr(void); //Use if test mode void IMU_read(void); //Update IMU readings by once void Aux_read(void); //Update AUX reafings by once void Idle2Run(void); //Initializing before running