Diff: main.cpp

Revision:

8:4e8991196bb8

Parent:

7:4931dbfbc042

Child:

9:310663c014d8

--- a/main.cpp	Fri Oct 26 06:35:24 2018 +0000
+++ b/main.cpp	Sat Feb 23 18:07:47 2019 +0000
@@ -190,9 +190,18 @@
         sum += deltat;
         sumCount++;
     
+        // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer;
+        // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro!
+        // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter.
+        // We will assume that +y accel/gyro is North, then x accel/gyro is East. So if we want te quaternions properly aligned
+        // we need to feed into the madgwick function Ay, Ax, -Az, Gy, Gx, -Gz, Mx, My, and Mz. But because gravity is by convention
+        // positive down, we need to invert the accel data, so we pass -Ay, -Ax, Az, Gy, Gx, -Gz, Mx, My, and Mz into the Madgwick
+        // function to get North along the accel +y-axis, East along the accel +x-axis, and Down along the accel -z-axis.
+        // This orientation choice can be modified to allow any convenient (non-NED) orientation convention.
+        // This is ok by aircraft orientation standards!  
         // Pass gyro rate as rad/s
-        // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-        mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+        // mpu9250.MadgwickQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f,  mx,  my, mz);
+        mpu9250.MahonyQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f, mx, my, mz);
 
 
         // Serial print and/or display at 1.5 s rate independent of data rates