Diff: Quaternion/Quaternion.h

Revision:

0:ebbc3cd3a61e

diff -r 000000000000 -r ebbc3cd3a61e Quaternion/Quaternion.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Quaternion/Quaternion.h	Sat Dec 05 18:39:33 2015 +0000
@@ -0,0 +1,32 @@
+#ifndef Quaternion_h
+#define Quaternion_h
+#include "mbed.h"
+
+
+#define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
+#define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
+
+class Quaternion {
+
+    public:
+        Quaternion();
+        void update6DOF(float gx, float gy, float gz, float ax, float ay, float az);
+        void update9DOF(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+        void getQ(float * q);
+        void getEulerAngles(float * angles);
+        void getYawPitchRoll(double * ypr);
+        void getGravity(float * gravity);
+        void getLinearAcceleration(float * linearAccel, float ax, float ay, float az);
+    
+    private:
+        float invSqrt(float x);
+        volatile float q0, q1, q2, q3;
+        volatile float twoKp;   // 2 * proportional gain (Kp)
+        volatile float twoKi;   // 2 * integral gain (Ki)
+        volatile float integralFBx,  integralFBy, integralFBz;
+        unsigned long lastUpdate, now;
+        float sampleFreq;
+        void updateSampleFrequency();
+};
+
+#endif
\ No newline at end of file