Update EM7180_MPU9250_MS5637.ino

EM7180_MPU9250_MS5637.ino

@@ -865,16 +865,18 @@ void loop()
   sum += deltat; // sum for averaging filter update rate
   sumCount++;
   
-  // Sensors x (y)-axis of the accelerometer is aligned with the -y (x)-axis of the magnetometer;
-  // the magnetometer z-axis (+ up) is aligned with z-axis (+ up) of accelerometer and gyro!
+   // 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.
-  // For the BMX-055, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like
-  // in the MPU9250 sensor. This rotation can be modified to allow any convenient orientation convention.
+  // 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
-    MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  mx,  my, mz);
-//  if(passThru)MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
-
+    MadgwickQuaternionUpdate(-ay, -ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f,  mx,  my, mz);
+//  if(passThru)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 0.5 s rate independent of data rates
     delt_t = millis() - count;
     if (delt_t > 500) { // update LCD once per half-second independent of read rate