Revision 3:fd549671e512, committed 2016-03-13

Comitter:

cvmp94

Date:

Sun Mar 13 21:40:10 2016 +0000

Parent:

2:e8c2301f7523

Commit message:

initial commit

Changed in this revision

diff -r e8c2301f7523 -r fd549671e512 MadgwickUpdate.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MadgwickUpdate.h	Sun Mar 13 21:40:10 2016 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "math.h"
+
+float deltat = 0.0f;
+int lastUpdate = 0, firstUpdate = 0, Now = 0;
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+
+float PI = 3.14159265358979323846f;
+float GyroMeasError = PI * (60.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
+float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
+
+void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) {
+    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];         // short name local variable for readability
+    float norm;                                               // vector norm
+    float f1, f2, f3;                                         // objective funcyion elements
+    float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
+    float qDot1, qDot2, qDot3, qDot4;
+    float hatDot1, hatDot2, hatDot3, hatDot4;
+    float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz;  // gyro bias error
+
+    // Auxiliary variables to avoid repeated arithmetic
+    float _halfq1 = 0.5f * q1;
+    float _halfq2 = 0.5f * q2;
+    float _halfq3 = 0.5f * q3;
+    float _halfq4 = 0.5f * q4;
+    float _2q1 = 2.0f * q1;
+    float _2q2 = 2.0f * q2;
+    float _2q3 = 2.0f * q3;
+    float _2q4 = 2.0f * q4;
+
+    // Normalise accelerometer measurement
+    norm = sqrt(ax * ax + ay * ay + az * az);
+    if (norm == 0.0f) return; // handle NaN
+    norm = 1.0f/norm;
+    ax *= norm;
+    ay *= norm;
+    az *= norm;
+    
+    // Compute the objective function and Jacobian
+    f1 = _2q2 * q4 - _2q1 * q3 - ax;
+    f2 = _2q1 * q2 + _2q3 * q4 - ay;
+    f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
+    J_11or24 = _2q3;
+    J_12or23 = _2q4;
+    J_13or22 = _2q1;
+    J_14or21 = _2q2;
+    J_32 = 2.0f * J_14or21;
+    J_33 = 2.0f * J_11or24;
+  
+    // Compute the gradient (matrix multiplication)
+    hatDot1 = J_14or21 * f2 - J_11or24 * f1;
+    hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
+    hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
+    hatDot4 = J_14or21 * f1 + J_11or24 * f2;
+    
+    // Normalize the gradient
+    norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
+    hatDot1 /= norm;
+    hatDot2 /= norm;
+    hatDot3 /= norm;
+    hatDot4 /= norm;
+    
+    // Compute estimated gyroscope biases
+    gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
+    gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
+    gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
+    
+    // Compute and remove gyroscope biases
+    gbiasx += gerrx * deltat * zeta;
+    gbiasy += gerry * deltat * zeta;
+    gbiasz += gerrz * deltat * zeta;
+//           gx -= gbiasx;
+//           gy -= gbiasy;
+//           gz -= gbiasz;
+    
+    // Compute the quaternion derivative
+    qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
+    qDot2 =  _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
+    qDot3 =  _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
+    qDot4 =  _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;
+
+    // Compute then integrate estimated quaternion derivative
+    q1 += (qDot1 -(beta * hatDot1)) * deltat;
+    q2 += (qDot2 -(beta * hatDot2)) * deltat;
+    q3 += (qDot3 -(beta * hatDot3)) * deltat;
+    q4 += (qDot4 -(beta * hatDot4)) * deltat;
+
+    // Normalize the quaternion
+    norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+    norm = 1.0f/norm;
+    q[0] = q1 * norm;
+    q[1] = q2 * norm;
+    q[2] = q3 * norm;
+    q[3] = q4 * norm;
+    
+}
\ No newline at end of file

diff -r e8c2301f7523 -r fd549671e512 main.cpp
--- a/main.cpp	Wed Feb 03 18:24:29 2016 +0000
+++ b/main.cpp	Sun Mar 13 21:40:10 2016 +0000
@@ -1,29 +1,84 @@
 #include "LSM9DS1.h"
+#include "MadgwickUpdate.h"
 
-DigitalOut myled(LED1);
 Serial pc(USBTX, USBRX);
 
+Timer t;
+int lastPrint = 0;
+int printInterval = 500;
+
+RawSerial dev( p13, p14 );
+
+int calibrate = 1;
+
+void dev_recv()
+{
+    while(dev.readable()) {
+        if ( dev.getc() == 'r' ){
+                calibrate = 1;
+        }
+    }
+}
+
 int main() {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
+    
+    LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
+    
+    t.start();
+
+    dev.baud(9600);
+    dev.attach(&dev_recv, Serial::RxIrq);
+
     lol.begin();
     if (!lol.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
-    lol.calibrate();
+
     while(1) {
-        lol.readTemp();
-        lol.readMag();
-        lol.readGyro();
+
+        if( calibrate ){
+            lol.calibrate();
+            firstUpdate = t.read_us();
+            lastUpdate = firstUpdate;
+            q[0] = 1.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 0.0f; 
+            calibrate = 0;
+        }
         
-        //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
+        if ( lol.accelAvailable() || lol.gyroAvailable() ) {
+         
+            lol.readAccel();
+            lol.readGyro();
+            
+            Now = t.read_us();
+            deltat = (float)((Now - lastUpdate)/1000000.0f);
+            lastUpdate = Now;
+            
+            float ax = lol.calcAccel(lol.ax);
+            float ay = lol.calcAccel(lol.ay);
+            float az = lol.calcAccel(lol.az);
+            
+            float gx = lol.calcGyro(lol.gx);
+            float gy = lol.calcGyro(lol.gy);
+            float gz = lol.calcGyro(lol.gz);
+            
+            
+            // switch x and y to convert to right handed coordinate system
+            MadgwickQuaternionUpdate( ay, ax, az, gy*PI/180.0f, gx*PI/180.0f, gz*PI/180.0f );
+            
+            int nowMs = t.read_ms();
+            if( nowMs - lastPrint > printInterval ){
+                lastPrint = nowMs;
+                pc.printf("gyro: %f %f %f\n\r", gx, gy, gz);
+                pc.printf("accel: %f %f %f\n\r", ax, ay, az);
+                pc.printf("quat: %f %f %f %f\n\r", q[0], q[1], q[2], q[3]);
+                char* str = (char*) &q;
+                int i = 0;
+                while (i < 16){
+                    dev.putc( str[i] );
+                    i += 1;   
+                }
+            }
+            
+        }
     }
 }