Diff: main.cpp

Revision:

8:005b0a85be70

Parent:

7:d27f97ac2bea

--- a/main.cpp	Tue Nov 15 04:20:38 2022 +0000
+++ b/main.cpp	Tue Nov 15 05:42:41 2022 +0000
@@ -1,10 +1,16 @@
-// uLCD-144-G2 demo program for uLCD-4GL LCD driver library
-//
+/**
+ * Uses the Fusion library for AHRS algorithm:
+ *      https://github.com/xioTechnologies/Fusion
+ */
+
+
 #include <cmath>
 #include "mbed.h"
 #include "uLCD_4DGL.h"
 #include "LSM9DS1.h"
 #define PI 3.14159
+#include "Fusion/Fusion.h"
+#define SAMPLE_RATE (100) // replace this with actual sample rate
 
 uLCD_4DGL uLCD(p9,p10,p30);
 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
@@ -23,10 +29,10 @@
 
     if (az == 0.0)  roll = (ay < 0.0) ? 180 : 0.0;
     else            roll = atan2(ay, az);
-        
+
     if (my == 0.0)  yaw = (mx < 0.0) ? 180.0 : 0.0;
     else            yaw = atan2(mx, my);
-    
+
     if (yaw > 180.0)        yaw -=PI;
     else if (yaw < -180.0)  yaw += PI;
 
@@ -34,35 +40,95 @@
 
 int main()
 {
+    ////////////////////
+    // INITIALIZATION //
+    ////////////////////
+
     imu.begin();
     if (!imu.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
+    imu.setGyroScale(2000);
+    imu.setAccelScale(16);
     imu.calibrate(1);
 
+    // Initialise algorithms
+    FusionAhrs ahrs;
+    FusionAhrsInitialise(&ahrs);
+
+    // Set AHRS algorithm settings
+    const FusionAhrsSettings settings = {
+        .gain = 0.5f,
+        .accelerationRejection = 10.0f,
+        .magneticRejection = 20.0f,
+        .rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */
+    };
+    FusionAhrsSetSettings(&ahrs, &settings);
+
+    Timer timer;
+    timer.start();
+
+
+
+    //////////////
+    // IMU LOOP //
+    //////////////
 
     while (true) {
         // Read IMU
         while(!imu.accelAvailable());
         imu.readAccel();
 
+        while (!imu.gyroAvailable());
+        imu.readGyro();
+
         while (!imu.magAvailable());
         imu.readMag();
 
-
+        // Elementary Algorithm:
+        //float roll, pitch, yaw;
+        //getAttitude(imu, roll, pitch, yaw);
+        //pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
 
-        float roll, pitch, yaw;
-        getAttitude(imu, roll, pitch, yaw);
+        // Construct algorithm inputs
+        FusionVector gyroscope = {          // Gyro data in degrees / s
+            imu.calcGyro(imu.gx),
+            imu.calcGyro(imu.gy),
+            imu.calcGyro(imu.gz)
+        };
+        FusionVector accelerometer = {      // Accelerometer data in g
+            imu.calcAccel(imu.ax),
+            imu.calcAccel(imu.ay),
+            imu.calcAccel(imu.az)
+        };
+        FusionVector magnetometer = {       // Accelerometer data in g
+            imu.calcMag(imu.mx),
+            imu.calcMag(imu.my),
+            imu.calcMag(imu.mz)
+        };
 
-        pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
+        // Update AHRS algorithm
+        float dt = timer.read();
+        timer.reset();
+        FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
 
+        // Print algorithm outputs
+        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
+        const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);
+
+        printf("R %5.1f   P %5.1f   Y %5.1f\n\r",
+               euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
+
+        float roll = euler.angle.roll   / 360 * 2 * PI,
+              pitch = euler.angle.pitch / 360 * 2 * PI,
+              yaw = euler.angle.yaw     / 360 * 2 * PI;
 
         uLCD.cls();
 
         float x = 64 + roll * 60;
         float y = 64 + pitch * 60;
-        float dx = 12 * sin(yaw);
-        float dy = 12 * cos(yaw);
+        float dx = 12 * cos(yaw);
+        float dy = 12 * sin(yaw);
 
         uLCD.circle(x, y, 2, GREEN);
         uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);