4180 final project
Dependencies: LSM9DS0 USBDevice mbed
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