Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 34:01dec68de3ed
- Parent:
- 31:d65576185bdf
- Child:
- 35:fb6e4601adf3
--- a/SensorFusion.cpp Tue May 05 09:59:11 2015 +0000 +++ b/SensorFusion.cpp Wed May 06 07:50:02 2015 +0000 @@ -3,12 +3,15 @@ #define DEBUG "SensorFusion" #include "Logger.h" +#include "Utils.h" + SensorFusion::SensorFusion(I2C &i2c) : delegate(&defaultDelegate), accel(i2c), gyro(i2c), magneto(i2c), q(1, 0, 0, 0), // output quaternion deltat(0.010), // sec - beta(0.5) // correction gain + beta(0.5), // correction gain + fused(0, 0, 0) { } @@ -61,11 +64,41 @@ gyro_reading.x, gyro_reading.y, gyro_reading.z, magneto_reading.x, magneto_reading.y, magneto_reading.z); - delegate->sensorTick(q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); + delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); +} + +void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max) +{ + magneto.getCalibration(min, max); } void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) { +#define FULL_MADGWICK +#ifdef SIMPLE_COMPLEMENTARY + + // normalise + /*float const anorm = sqrt(ax * ax + ay * ay + az * az); + ax /= anorm; + ay /= anorm; + az /= anorm; + */ + float const signAz = (az < 0) - (0 < az); + float const aroll = utils::rad2deg(atan2(-ax, sqrt(ay * ay + az * az))); + float const apitch = utils::rad2deg(atan2(ay, signAz * sqrt(az * az + ax * ax))); + + // integrate and fuse + float const beta = 0.98; + fused.x = fmod(beta * (fused.x + (-gx * deltat)) + (1 - beta) * aroll, 360); + fused.y = fmod(beta * (fused.y + (-gy * deltat)) + (1 - beta) * apitch, 360); + fused.z = fmod(fused.z + (gz * deltat), 360); + + //fused.x = fmod(fused.x + 180, 360) - 180; + //fused.y = fmod(fused.y + 180, 360) - 180; + +#endif + +#ifdef FULL_MADGWICK float q1 = q.w, q2 = q.v.x, q3 = q.v.y, q4 = q.v.z; // short name local variable for readability float norm; float s1, s2, s3, s4; @@ -145,4 +178,5 @@ q.v.x = q2 * norm; q.v.y = q3 * norm; q.v.z = q4 * norm; +#endif }