filter for imu
Dependencies: MPU9250_Interface
AHRS.h
- Committer:
- soulx
- Date:
- 2016-01-20
- Revision:
- 0:bf9febe45e1d
File content as of revision 0:bf9febe45e1d:
#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; Serial pc2; AHRS(PinName sda, PinName scl, PinName tx, PinName rx, int address) : MPU9250(sda,scl,tx,rx,address),pc2(tx,rx) { 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