Class of MPU9250
Dependencies: AHRS_fillter mbed
Fork of MPU9250AHRS by
Diff: AHRS.h
- Revision:
- 6:5665d427bceb
- Child:
- 8:928673148b55
diff -r d31487b34216 -r 5665d427bceb AHRS.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AHRS.h Wed Jan 20 02:34:07 2016 +0000 @@ -0,0 +1,116 @@ +#ifndef _AHRS_ +#define _AHRS_ + +#include "MPU9250.h" + +// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) +// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute +// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. +// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms +// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + +class AHRS:public MPU9250 +{ +private: + float roll, pitch, yaw; + float q[4]; // vector to hold quaternion + float eInt[3]; // vector to hold integral error for Mahony method + + // parameters for 6 DoF sensor fusion calculations + float PI; + float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 + float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + float beta; // compute beta + float zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + + float norm; + float hx, hy, bx, bz; + float vx, vy, vz, wx, wy, wz; + float ex, ey, ez; + float pa, pb, pc; + + float q1, q2, q3, q4; + + // Auxiliary variables to avoid repeated arithmetic + float q1q1; + float q1q2; + float q1q3; + float q1q4; + float q2q2; + float q2q3; + float q2q4; + float q3q3; + float q3q4; + float q4q4; + + float Xh; + float Yh; + float yawmag; + + float deltat; + float delt_t; + float Now; + float sum; + float firstUpdate; + float lastUpdate; + uint32_t sumCount; + int count; + //int16_t tempCount; + + +public: + //MPU9250 imu; + //Serial test; + Timer t; + AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address) : MPU9250(sda,scl,tx,rx,address) { + for(int i=0; i<=3; i++) { + eInt[i] = 0; + q[i] = 0; // vector to hold quaternion + } + + q[0] = 1.0f; + + PI = 3.14159265358979323846f; + GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 + beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta + GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) + zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value + + //Time + sum = 0; + sumCount = 0; + count = 0; + + delt_t = 0; + lastUpdate = 0; + firstUpdate = 0; + Now = 0; + }; + //void MadgwickQuaternionUpdate(float , float , float , float , float , float , float , float , float , float); + //void MahonyQuaternionUpdate(float , float , float , float , float , float , float , float , float , float); + + void MadgwickQuaternionUpdate(); + void MahonyQuaternionUpdate(); + + float getRoll() { + return roll; + } + float getPitch() { + return pitch; + } + float getYaw() { + return yaw; + } + void printRollPitchYaw(); + + void TimeStart(); + void TimeRead() { + t.read_us(); + }; + void TimeCal(); + + void Run(); + +}; +#endif \ No newline at end of file