IMU
Fork of IMUfilter by
IMUfilter.h
- Committer:
- YSB
- Date:
- 2013-07-05
- Revision:
- 2:fae851819c43
- Parent:
- 1:8a920397b510
File content as of revision 2:fae851819c43:
#ifndef IMU_FILTER_H #define IMU_FILTER_H #include "mbed.h" #define PI 3.1415926536 #define g0 9.812865328//Gravity at Earth's surface in m/s/s #define SAMPLES 4//Number of samples to average. #define CALIBRATION_SAMPLES 64//128//Number of samples to be averaged for a null bias calculation//during calibration. #define toDegrees(x) (x * 57.2957795)//Convert from radians to degrees. #define toRadians(x) (x * 0.01745329252)//Convert from degrees to radians. #define GYROSCOPE_GAIN (1 / 14.375)//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). #define ACCELEROMETER_GAIN (0.004 * g0)//Full scale resolution on the ADXL345 is 4mg/LSB. #define GYRO_RATE 0.005//Sampling gyroscope at 200Hz. #define ACC_RATE 0.005//Sampling accelerometer at 200Hz. #define FILTER_RATE 0.1//Updating filter at 40Hz. class IMUfilter { public: /** * Constructor. * * Initializes filter variables. * * @param rate The rate at which the filter should be updated. * @param gyroscopeMeasurementError The error of the gyroscope in degrees * per second. This used to calculate a tuning constant for the filter. * Try changing this value if there are jittery readings, or they change * too much or too fast when rotating the IMU. */ IMUfilter(double rate, double gyroscopeMeasurementError); /** * Update the filter variables. * * @param w_x X-axis gyroscope reading in rad/s. * @param w_y Y-axis gyroscope reading in rad/s. * @param w_z Z-axis gyroscope reading in rad/s. * @param a_x X-axis accelerometer reading in m/s/s. * @param a_y Y-axis accelerometer reading in m/s/s. * @param a_z Z-axis accelerometer reading in m/s/s. */ void updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z); /** * Compute the Euler angles based on the current filter data. */ void computeEuler(void); /** * Get the current roll. * * @return The current roll angle in radians. */ double getRoll(void); /** * Get the current pitch. * * @return The current pitch angle in radians. */ double getPitch(void); /** * Get the current yaw. * * @return The current yaw angle in radians. */ double getYaw(void); /** * Reset the filter. */ void reset(void); private: int firstUpdate; //Quaternion orientation of earth frame relative to auxiliary frame. double AEq_1; double AEq_2; double AEq_3; double AEq_4; //Estimated orientation quaternion elements with initial conditions. double SEq_1; double SEq_2; double SEq_3; double SEq_4; //Sampling period double deltat; //Gyroscope measurement error (in degrees per second). double gyroMeasError; //Compute beta (filter tuning constant.. double beta; double phi; double theta; double psi; }; #endif /* IMU_FILTER_H */