Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 35:fb6e4601adf3
- Parent:
- 34:01dec68de3ed
- Child:
- 37:63d355f2cf6a
diff -r 01dec68de3ed -r fb6e4601adf3 SensorFusion.cpp --- a/SensorFusion.cpp Wed May 06 07:50:02 2015 +0000 +++ b/SensorFusion.cpp Fri May 08 06:20:18 2015 +0000 @@ -4,6 +4,7 @@ #include "Logger.h" #include "Utils.h" +#define SIXAXIS SensorFusion::SensorFusion(I2C &i2c) : delegate(&defaultDelegate), @@ -25,11 +26,13 @@ accel.powerOn(); accel.start(); + #ifdef NINEAXIS magneto.powerOn(); if (magneto.performSelfTest() == false) { return false; } magneto.start(); + #endif // Since everything is synced to gyro interrupt, start it last gyro.setDelegate(*this); @@ -42,11 +45,15 @@ void SensorFusion::stop() { gyro.stop(); + #ifdef NINEAXIS magneto.stop(); + #endif accel.stop(); gyro.powerOff(); + #ifdef NINEAXIS magneto.powerOff(); + #endif accel.powerOff(); } @@ -58,11 +65,16 @@ Vector3 const gyro_reading = gyro_degrees * deg_to_radian; Vector3 const accel_reading = accel.read(); +#ifdef NINEAXIS Vector3 const magneto_reading = magneto.read(); - updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, gyro_reading.x, gyro_reading.y, gyro_reading.z, magneto_reading.x, magneto_reading.y, magneto_reading.z); +#else + Vector3 const magneto_reading(0, 0, 0); + updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, + gyro_reading.x, gyro_reading.y, gyro_reading.z); +#endif delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); } @@ -72,33 +84,86 @@ magneto.getCalibration(min, max); } +// 6 axis version +void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz) +{ + float q0 = q.w, q1 = q.v.x, q2 = q.v.y, q3 = q.v.z; // short name local variable for readability + + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { + + // Normalise accelerometer measurement + recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0 * q0; + _2q1 = 2.0 * q1; + _2q2 = 2.0 * q2; + _2q3 = 2.0 * q3; + _4q0 = 4.0 * q0; + _4q1 = 4.0 * q1; + _4q2 = 4.0 * q2; + _8q1 = 8.0 * q1; + _8q2 = 8.0 * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay; + recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * deltat; + q1 += qDot2 * deltat; + q2 += qDot3 * deltat; + q3 += qDot4 * deltat; + + // Normalise quaternion + recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + // return + q.w = q0; + q.v.x = q1; + q.v.y = q2; + q.v.z = q3; +} + 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; @@ -178,5 +243,4 @@ q.v.x = q2 * norm; q.v.y = q3 * norm; q.v.z = q4 * norm; -#endif }