SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
Diff: main.cpp
- Revision:
- 19:d68f21173c23
- Parent:
- 18:780366f2534c
- Child:
- 20:e9daae390513
--- 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)