Diff: main.cpp

Revision:

10:28fa811afbfb

Parent:

9:e700b2d586d6

Child:

11:3f0b35a0855c

--- a/main.cpp	Fri Jun 17 06:07:57 2016 +0000
+++ b/main.cpp	Fri Jun 17 14:30:55 2016 +0000
@@ -7,10 +7,9 @@
  *
  **/
 
-
 #include "mbed.h"
 #include "MPU9250.h"
-
+#include "KalmanFilter.h"
 /*
     MOSI (Master Out Slave In)  p5
     MISO (Master In Slave Out   p6
@@ -20,65 +19,317 @@
 
 //  https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
 
-int main()
-{
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq  500.0f      // sample frequency in Hz
+#define betaDef     1.0f        // 2 * proportional gain
+#define PI          3.14159265358979f
+#define DEGREE2RAD  PI/180.0f
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float beta = betaDef;                              // 2 * proportional gain (Kp)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+float invSqrt(float x);
+
+//====================================================================================================
+// Functions
+
+//---------------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float hx, hy;
+    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+        return;
+    }
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
 
-    Serial pc(USBTX, USBRX);
-    pc.baud(115200);
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Normalise magnetometer measurement
+        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0mx = 2.0f * q0 * mx;
+        _2q0my = 2.0f * q0 * my;
+        _2q0mz = 2.0f * q0 * mz;
+        _2q1mx = 2.0f * q1 * mx;
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _2q0q2 = 2.0f * q0 * q2;
+        _2q2q3 = 2.0f * q2 * q3;
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
 
-    SPI spi(p5, p6, p7);
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _4q0 = 4.0f * q0;
+        _4q1 = 4.0f * q1;
+        _4q2 = 4.0f * q2;
+        _8q1 = 8.0f * q1;
+        _8q2 = 8.0f * q2;
+        q0q0 = q0 * q0;
+        q1q1 = q1 * q1;
+        q2q2 = q2 * q2;
+        q3q3 = q3 * q3;
 
-    //define the mpu9250 object
-    mpu9250_spi *imu[2];
+        // Gradient decent algorithm corrective step
+        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
 
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float invSqrt(float x) {
+    float halfx = 0.5f * x;
+    float y = x;
+    long i = *(long*)&y;
+    i = 0x5f3759df - (i>>1);
+    y = *(float*)&i;
+    y = y * (1.5f - (halfx * y * y));
+    return y;
+}
+
+
+
+//define the mpu9250 object
+mpu9250_spi     *imu[2];
+Serial          pc(USBTX, USBRX);
+SPI             spi(p5, p6, p7);
+KalmanFilter    *kf[12];
+Ticker          ticker;
+
+float x,y,z,gxOfs,gyOfs,gzOfs;
+// Calibration wait
+
+void init(void){
+    
+    pc.baud(921600);
+    
     imu[0] = new mpu9250_spi(spi, p8);
     imu[1] = new mpu9250_spi(spi, p9);
-
+    
+    for(int i=0; i<12; i++)     
+        kf[i] = new KalmanFilter(1e-3, 0.001);
+        
     for(int i=0; i<2; i++) {
-
+    
         imu[0]->deselect();
         imu[1]->deselect();
         imu[i]->select();
 
         if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
             printf("\nCouldn't initialize MPU9250 via SPI!");
+            wait(90);
         }
         printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
-        wait(1);
-        printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
-        wait(1);
+        printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_1000DPS));    //Set full scale range for gyros
         printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
-        wait(1);
         printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
-        wait(0.1);
         imu[i]->AK8963_calib_Magnetometer();
         wait(0.1);
     }
-    imu[0]->select();
+    
+    gxOfs = 0;
+    gyOfs = 0;
+    gzOfs = 0;
+    
+    imu[0]->deselect();
     imu[1]->deselect();
-    while(1) {
+    
+    imu[0]->select();
+    
+    for(int i=0; i<1000; i++) {
+        
+        imu[0]->read_all();
+        
+        gxOfs += imu[0]->gyroscope_data[0];
+        gyOfs += imu[0]->gyroscope_data[1];
+        gzOfs += imu[0]->gyroscope_data[2];
+        
+        wait_us(1000000.0f/sampleFreq);
+    }
+    
+    gxOfs /= 1000;
+    gyOfs /= 1000;
+    gzOfs /= 1000;
+    
+}
 
-        //myled = 1;
-        
-        //wait_us(1);
+void eventFunc(void)
+{
+    for(int i=0; i<1; i++) {
         
-        for(int i=0; i<2; i++) {
-            
-            imu[0]->deselect();
-            imu[1]->deselect();
-            
-            imu[i]->select();
-            imu[i]->read_acc();
-            imu[i]->read_rot();
-            
-            printf("%10.3f,%10.3f,%10.3f %10.3f,%10.3f,%10.3f ",
-                   imu[i]->gyroscope_data[0],
-                   imu[i]->gyroscope_data[1],
-                   imu[i]->gyroscope_data[2],
-                   imu[i]->accelerometer_data[0],
-                   imu[i]->accelerometer_data[1],
-                   imu[i]->accelerometer_data[2]
-                  );
+        imu[0]->deselect();
+        imu[1]->deselect();
+        
+        imu[i]->select();
+        imu[i]->read_all();
+
+        MadgwickAHRSupdate(
+            (imu[i]->gyroscope_data[0]-gxOfs)*DEGREE2RAD,
+            (imu[i]->gyroscope_data[1]-gyOfs)*DEGREE2RAD,
+            (imu[i]->gyroscope_data[2]-gzOfs)*DEGREE2RAD,
+            imu[i]->accelerometer_data[0],
+            imu[i]->accelerometer_data[1],
+            imu[i]->accelerometer_data[2],
+            imu[i]->Magnetometer[0],
+            imu[i]->Magnetometer[1],
+            imu[i]->Magnetometer[2]
+        );
+        
+        printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f\n", q0,q1,q2,q3, q0,q1,q2,q3, q0,q1,q2,q3, q0,q1,q2,q3 );
+    }  
+}
+
+int main()
+{
+    init();
+    
+    ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); // 512Hz
+    
+    while(1) {
+        
+        //name.readable();
             /*
             imu[i]->read_all();
             printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
@@ -95,7 +346,5 @@
                   );*/
             //myled = 0;
             //wait(0.5);
-        }
-        printf("\n");
     }
 }