Diff: main.cpp

Revision:

4:43a6ec1af346

Parent:

3:2d6ff72599f1

Child:

5:c4ae0656a736

--- a/main.cpp	Mon Apr 24 04:05:46 2017 +0000
+++ b/main.cpp	Sun Apr 30 19:48:22 2017 +0000
@@ -2,6 +2,7 @@
 #include "LSM9DS1.h"
 #include "Wireless.h"
 //#include "USBKeyboard.h"
+//#include "MahonyAHRS.h"
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
 // a declination to get a more accurate heading. Calculate
@@ -15,25 +16,229 @@
 DigitalOut led4(LED4);
 Serial pc(USBTX, USBRX);
 DigitalIn pb1(p17);
-Timeout walkingTimer;
-WirelessModule wireless(p9, p10, FOOT_STEP);
-bool isWalking = false;
+//USBKeyboard keyboard;
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
+#include <math.h>
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq  952.0f      // sample frequency in Hz
+#define betaDef     0.1f        // 2 * proportional gain
+
+//---------------------------------------------------------------------------------------------------
+// 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 MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+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))) {
+
+        // 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;
+    }
+
+    // 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;
+
+        // 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;
+}
+
+//====================================================================================================
+// END OF CODE
+//====================================================================================================
 
 
-//Function that timeout calls when detected that person stops walking
-void printStop()
-{
-    // pc.printf("stop\n\r");
-    wireless.sendDirection(DIR_NONE);
-    isWalking = false;
-}
+
 
-//Rotates raw heading so 0 degrees is forward direction
+
 float correctHeading(float currHeading, float forward)
 {
     float newHeading = currHeading - forward;
@@ -66,7 +271,38 @@
 
     //pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
     //pc.printf("Magnetic Heading: %f degress\n\r",heading);
-    return heading;
+    return abs(heading);
+}
+
+bool isWalking = false;
+
+Ticker walkingTimer;
+Ticker resetStart;
+WirelessModule wireless(p9, p10, FOOT_STEP);
+float ax ;
+float ay ;
+float az ;
+float gx ;
+float gy ;
+float gz ;
+float mx ;
+float my ;
+float mz ;
+LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+
+void printStop()
+{
+    // pc.printf("stop\n\r");
+    wireless.sendDirection(DIR_NONE);
+    isWalking = false;
+}
+void resetForward()
+{
+    q0 = 1.0f;
+    q1 = 0.0f;
+    q2 = 0.0f;
+    q3 = 0.0f;
+    MadgwickAHRSupdate(IMU.calcGyro(gx), IMU.calcGyro(gy), IMU.calcGyro(gz), IMU.calcAccel(ax), IMU.calcAccel(ay), IMU.calcAccel(az), IMU.calcMag(mx), IMU.calcMag(my), IMU.calcMag(mz));
 }
 
 
@@ -75,34 +311,40 @@
 int main()
 {
     //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+
     pb1.mode(PullUp);
     IMU.begin();
     float forward;
     if (!IMU.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
-    led3 = 1;
-    wait(1);
+    led4 = 1;
     IMU.calibrate(1);
-    led3 = 0;
-    led1 = 1;
+    led4 = 0;
     wait(0.5);
+    led1 = 1;
     led4 = 1;
-    wait(0.5);
     IMU.calibrateMag(0);
     led4 = 0;
     led2 = 1;
-
-    //Get forward direction relative to user
     pc.printf("Press button to set forward direction");
     while(pb1 == 1) {
         IMU.readMag();
         IMU.readAccel();
+        ax = IMU.calcAccel(IMU.ax);
+        ay = IMU.calcAccel(IMU.ay);
+        az = IMU.calcAccel(IMU.az);
+        gx = IMU.calcGyro(IMU.gx);
+        gy = IMU.calcGyro(IMU.gy);
+        gz = IMU.calcGyro(IMU.gz);
+        mx = IMU.calcMag(IMU.mx);
+        my = IMU.calcMag(IMU.my);
+        mz = IMU.calcMag(IMU.mz);
         forward = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
                                 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));;
     }
-    pc.printf("forward: %f\n\r", forward);
+    led3 = 1;
+    resetStart.attach(resetForward, 0.1);
     while(1) {
         while(!IMU.tempAvailable());
         IMU.readTemp();
@@ -112,37 +354,44 @@
         IMU.readAccel();
         while(!IMU.gyroAvailable());
         IMU.readGyro();
-        if(abs(IMU.calcGyro(IMU.gy)) > 100) {
 
+        MadgwickAHRSupdate(IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz), IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        float Yaw_m=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1)*180/PI;
+        float Pitch_m=-1*asin(2*q1*q3+2*q0*q2)*180/PI;
+        float Roll_m=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1)*180/PI;
 
-            IMU.readAccel();
+        if( Yaw_m < 0 ) Yaw_m += 360.0;
+        //pc.printf("yaw: %f\n\r", Yaw_m);
+        //pc.printf("Yaw: %f\n\r Roll: %f\n\r Pitch: %f\n\n\n\r", Yaw_m, Roll_m, Pitch_m);
 
-            IMU.readMag();
-
+        if(abs(IMU.calcGyro(IMU.gy)) > 100) {
 
             //Calculate heading relative to forward direction
             float currHeading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-            currHeading = correctHeading(currHeading, forward) + 45;
+            currHeading = correctHeading(currHeading, forward);
+            pc.printf("heading: %f\n\r", currHeading);
+            //pc.printf("corrected heading: %f\n\r", currHeading);
             //Start timeout to detect when stopped walking
             walkingTimer.attach(printStop, 0.3);
 
             //Detect direction and send command to main mbed
-            if((currHeading > 270 && currHeading < 360) && !isWalking) {
-
+            if((currHeading > 225 && currHeading < 315) && !isWalking) {
+                pc.printf("left\n\r");
                 wireless.sendDirection(DIR_LEFT);
 
                 isWalking = true;
-            } else if((currHeading > 90 && currHeading < 180) && !isWalking) {
-
-               wireless.sendDirection(DIR_RIGHT);
+            } else if((currHeading > 45 && currHeading < 135) && !isWalking) {
+                pc.printf("right\n\r");
+                wireless.sendDirection(DIR_RIGHT);
                 isWalking = true;
-            } else if((currHeading > 180 && currHeading < 270) && !isWalking) {
-
+            } else if((currHeading > 135 && currHeading < 225) && !isWalking) {
+                pc.printf("down\n\r");
                 wireless.sendDirection(DIR_DOWN);
                 isWalking = true;
-            } else if((currHeading > 360 || currHeading < 90) && !isWalking) {
+            } else if((currHeading > 315 || currHeading < 45) && !isWalking) {
+                pc.printf("up\n\r");
+                wireless.sendDirection(DIR_UP);
 
-                wireless.sendDirection(DIR_UP);
                 isWalking = true;
             }
         }