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)