Diff: main.cpp

Revision:

0:bc5b57f59735

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 21 14:20:17 2017 +0000
@@ -0,0 +1,265 @@
+#include "mbed.h"
+#include "MPU9250.h"
+#include "TextLCD.h"
+
+struct UART_buf
+{ 
+    uint8_t STA;
+    uint8_t MODE; 
+    uint8_t CMD;
+    uint8_t LEN;
+    uint8_t DATA[32];
+    uint8_t END; 
+     
+};
+
+union Data_DB{
+    int16_t data16;
+    uint8_t data8[2];
+}Data_Tr;
+
+MPU9250 mpu9250;
+
+Ticker Sensor_Timer;
+
+Serial SerialUART(PA_2, PA_3); // tx, rx
+  
+// rs, rw, e, d0-d3
+TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11); 
+
+uint8_t Buffer[37];
+volatile uint8_t Sensor_flag = 0;
+
+UART_buf RX_BUF;
+
+void SerialUARTRX_ISR(void);
+void Timer_setting(uint8_t cmd, uint8_t value);
+void Sensor_Read(void);
+
+int main()
+{
+    SerialUART.baud(115200);  
+
+    //Set up I2C
+    i2c.frequency(400000);  // use fast (400 kHz) I2C  
+    
+    // Read the WHO_AM_I register, this is a good test of communication
+    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
+    //SerialUART.printf("I AM 0x%x\n\r", whoami); SerialUART.printf("I SHOULD BE 0x71\n\r");
+    
+    if (whoami == 0x71) // WHO_AM_I should always be 0x68
+    {  
+        /*SerialUART.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+        SerialUART.printf("MPU9250 is online...\n\r");*/
+        lcd.printf("MPU9250 is 0x%x\n",whoami);
+        lcd.printf("   Connected    ");
+
+        wait(1);
+        
+        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+        /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
+        SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
+        SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
+        SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
+        SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
+        SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  */
+        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
+        /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]);
+        SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]);
+        SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]);
+        SerialUART.printf("x accel bias = %f\n\r", accelBias[0]);
+        SerialUART.printf("y accel bias = %f\n\r", accelBias[1]);
+        SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/
+        wait(2);
+        mpu9250.initMPU9250(); 
+        //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        mpu9250.initAK8963(magCalibration);
+        /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        SerialUART.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
+        pSerialUARTc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
+        if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14  bits\n\r");
+        if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16  bits\n\r");
+        if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r");
+        if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/
+        wait(1);
+    }
+    else
+    {
+        //SerialUART.printf("Could not connect to MPU9250: \n\r");
+        //SerialUART.printf("%#x \n",  whoami);
+        
+        lcd.printf("MPU9250 is 0x%x\n",whoami);
+        lcd.printf(" No connection  ");
+       
+        while(1) ; // Loop forever if communication doesn't happen
+    }
+
+    mpu9250.getAres(); // Get accelerometer sensitivity
+    mpu9250.getGres(); // Get gyro sensitivity
+    mpu9250.getMres(); // Get magnetometer sensitivity
+/*    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
+//    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+//    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
+//    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+
+    SerialUART.attach(&SerialUARTRX_ISR);
+    
+    //Timer_setting(0x06, 200);
+    Sensor_Timer.attach(&Sensor_Read, 0.05);
+
+    while(1)
+    {
+        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+        
+            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
+            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+            mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
+            // Now we'll calculate the accleration value into actual g's
+            if(Sensor_flag)
+            {
+                Sensor_flag = 0;
+                
+                ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+                ay = (float)accelCount[1]*aRes - accelBias[1];   
+                az = (float)accelCount[2]*aRes - accelBias[2];  
+                
+                // Calculate the gyro value into actual degrees per second
+                gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
+                gy = (float)gyroCount[1]*gRes - gyroBias[1];  
+                gz = (float)gyroCount[2]*gRes - gyroBias[2];   
+                
+                // Calculate the magnetometer values in milliGauss
+                // Include factory calibration per data sheet and user environmental corrections
+                mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+                my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
+                mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+                
+                Buffer[0] = 0x76;
+                Buffer[1] = 0x02;
+                Buffer[2] = 0x01;
+                Buffer[3] = 18;
+                Data_Tr.data16 = (int16_t)gx;
+                Buffer[4] = Data_Tr.data8[1];
+                Buffer[5] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)gy;
+                Buffer[6] = Data_Tr.data8[1];
+                Buffer[7] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)gz;
+                Buffer[8] = Data_Tr.data8[1];
+                Buffer[9] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)(ax * 1000);
+                Buffer[10] = Data_Tr.data8[1];
+                Buffer[11] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)(ay * 1000);
+                Buffer[12] = Data_Tr.data8[1];
+                Buffer[13] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)(az * 1000);
+                Buffer[14] = Data_Tr.data8[1];
+                Buffer[15] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)mx;
+                Buffer[16] = Data_Tr.data8[1];
+                Buffer[17] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)my;
+                Buffer[18] = Data_Tr.data8[1];
+                Buffer[19] = Data_Tr.data8[0];
+                Data_Tr.data16 = (int16_t)mz;
+                Buffer[20] = Data_Tr.data8[1];
+                Buffer[21] = Data_Tr.data8[0];
+                Buffer[22] = 0x3E;
+
+                for(int i=0; i<23; i++)
+                    SerialUART.putc(Buffer[i]);
+            }
+        }
+    }
+}
+
+void SerialUARTRX_ISR(void)
+{
+    static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
+    uint8_t rx_da = SerialUART.getc();
+    switch(RX_Status)
+    {
+        case 0:
+            if(rx_da == 0x76)
+            {
+                RX_BUF.STA = rx_da;
+                RX_Status++;
+            }
+            break;
+        case 1:
+            RX_BUF.MODE = rx_da;
+            RX_Status++;
+            break;
+        case 2:
+            RX_BUF.CMD = rx_da;
+            RX_Status++;
+            break;
+        case 3:
+            RX_BUF.LEN = rx_da;
+            RX_Len = RX_BUF.LEN;
+            RX_Status++;
+            if(RX_Len == 0)
+                RX_Status++;
+            break;
+        case 4:
+            RX_BUF.DATA[RX_count] = rx_da;
+            RX_count++;
+            if(RX_count == RX_Len)
+            {
+                RX_Status++;
+                RX_count = 0;
+                RX_Len = 32;
+            }
+            break;
+        case 5:
+            if(rx_da == 0x3E)
+            {
+                RX_BUF.END = rx_da;
+                RX_Status = 0;
+                switch(RX_BUF.MODE)
+                {
+                    case 0x04:
+                        Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
+                        break;
+                }
+            }
+            break;
+    }
+}
+
+void Timer_setting(uint8_t cmd, uint8_t value)
+{
+    double Time_value = 0;
+    switch(cmd)
+    {
+        case 0x01:
+            Time_value = 30;
+            break;
+        case 0x02:
+            Time_value = 60;
+            break;
+        case 0x03:
+            Time_value = 120;
+            break;
+        case 0x04:
+            Time_value = 300;
+            break;
+        case 0x05:
+            Time_value = 600;
+            break;
+        case 0x06:
+            Time_value = value;
+            Time_value = 1.0/Time_value;
+            break;
+    }
+    Sensor_Timer.attach(&Sensor_Read, Time_value);
+}
+
+void Sensor_Read(void)
+{
+    Sensor_flag = 1;
+}
\ No newline at end of file