Diff: sensor_fusion.h

Revision:

25:6e43bbe76aec

Parent:

24:cbdf0f7d33bd

--- a/sensor_fusion.h	Tue Sep 20 04:18:34 2016 +0000
+++ b/sensor_fusion.h	Sat Sep 24 06:10:54 2016 +0000
@@ -16,6 +16,7 @@
 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
 float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
 float pitch, yaw, roll;
+float mag_dec = 7.97;                            // Adelaide Magnetic declination http://www.magnetic-declination.com/Australia/Adelaide/116230.html
 float deltat = 0.0f;                             // integration interval for both filter schemes
 int lastUpdate = 0, firstUpdate = 0, Now = 0;    // used to calculate integration interval
 
@@ -208,4 +209,15 @@
 
 }
 
+void calcEuler()
+{
+    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+    pitch *= 180.0f / PI;
+    yaw   *= 180.0f / PI;
+    yaw   -= mag_dec; // Declination from BLE/SD storage at GPS location
+    roll  *= 180.0f / PI;
+}
+
 #endif
\ No newline at end of file