Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@34:01dec68de3ed, 2015-05-06 (annotated)

Committer:
pvaibhav
Date:
Wed May 06 07:50:02 2015 +0000
Revision:
34:01dec68de3ed
Parent:
31:d65576185bdf
Child:
35:fb6e4601adf3
motor voltage under 0.5V supported, magneto calibration initial values changed for smartplane2, complementary filter added to sensor fusion (default off), dt now sent with fusion parameters.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 15:4488660e1a3b 1 #include "SensorFusion.h"
pvaibhav 15:4488660e1a3b 2
pvaibhav 15:4488660e1a3b 3 #define DEBUG "SensorFusion"
pvaibhav 15:4488660e1a3b 4 #include "Logger.h"
pvaibhav 15:4488660e1a3b 5
pvaibhav 34:01dec68de3ed 6 #include "Utils.h"
pvaibhav 34:01dec68de3ed 7
pvaibhav 21:5a0c9406e119 8 SensorFusion::SensorFusion(I2C &i2c) :
pvaibhav 21:5a0c9406e119 9 delegate(&defaultDelegate),
pvaibhav 21:5a0c9406e119 10 accel(i2c), gyro(i2c), magneto(i2c),
pvaibhav 15:4488660e1a3b 11 q(1, 0, 0, 0), // output quaternion
pvaibhav 15:4488660e1a3b 12 deltat(0.010), // sec
pvaibhav 34:01dec68de3ed 13 beta(0.5), // correction gain
pvaibhav 34:01dec68de3ed 14 fused(0, 0, 0)
pvaibhav 15:4488660e1a3b 15 {
pvaibhav 20:503cbe360419 16 }
pvaibhav 20:503cbe360419 17
pvaibhav 20:503cbe360419 18 void SensorFusion::setDelegate(SensorFusion::Delegate &d)
pvaibhav 20:503cbe360419 19 {
pvaibhav 20:503cbe360419 20 delegate = &d;
pvaibhav 15:4488660e1a3b 21 }
pvaibhav 15:4488660e1a3b 22
pvaibhav 15:4488660e1a3b 23 bool SensorFusion::start()
pvaibhav 15:4488660e1a3b 24 {
pvaibhav 15:4488660e1a3b 25 accel.powerOn();
pvaibhav 15:4488660e1a3b 26 accel.start();
pvaibhav 15:4488660e1a3b 27
pvaibhav 15:4488660e1a3b 28 magneto.powerOn();
pvaibhav 15:4488660e1a3b 29 if (magneto.performSelfTest() == false) {
pvaibhav 15:4488660e1a3b 30 return false;
pvaibhav 15:4488660e1a3b 31 }
pvaibhav 15:4488660e1a3b 32 magneto.start();
pvaibhav 15:4488660e1a3b 33
pvaibhav 15:4488660e1a3b 34 // Since everything is synced to gyro interrupt, start it last
pvaibhav 15:4488660e1a3b 35 gyro.setDelegate(*this);
pvaibhav 15:4488660e1a3b 36 gyro.powerOn();
pvaibhav 15:4488660e1a3b 37 gyro.start();
pvaibhav 15:4488660e1a3b 38
pvaibhav 15:4488660e1a3b 39 return true;
pvaibhav 15:4488660e1a3b 40 }
pvaibhav 15:4488660e1a3b 41
pvaibhav 15:4488660e1a3b 42 void SensorFusion::stop()
pvaibhav 15:4488660e1a3b 43 {
pvaibhav 15:4488660e1a3b 44 gyro.stop();
pvaibhav 15:4488660e1a3b 45 magneto.stop();
pvaibhav 15:4488660e1a3b 46 accel.stop();
pvaibhav 15:4488660e1a3b 47
pvaibhav 15:4488660e1a3b 48 gyro.powerOff();
pvaibhav 15:4488660e1a3b 49 magneto.powerOff();
pvaibhav 15:4488660e1a3b 50 accel.powerOff();
pvaibhav 15:4488660e1a3b 51 }
pvaibhav 15:4488660e1a3b 52
pvaibhav 15:4488660e1a3b 53 static float const deg_to_radian = 0.0174532925f;
pvaibhav 15:4488660e1a3b 54
pvaibhav 15:4488660e1a3b 55 void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
pvaibhav 15:4488660e1a3b 56 {
pvaibhav 31:d65576185bdf 57
pvaibhav 15:4488660e1a3b 58 Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
pvaibhav 31:d65576185bdf 59
pvaibhav 15:4488660e1a3b 60 Vector3 const accel_reading = accel.read();
pvaibhav 15:4488660e1a3b 61 Vector3 const magneto_reading = magneto.read();
pvaibhav 15:4488660e1a3b 62
pvaibhav 15:4488660e1a3b 63 updateFilter( accel_reading.x, accel_reading.y, accel_reading.z,
pvaibhav 15:4488660e1a3b 64 gyro_reading.x, gyro_reading.y, gyro_reading.z,
pvaibhav 15:4488660e1a3b 65 magneto_reading.x, magneto_reading.y, magneto_reading.z);
pvaibhav 15:4488660e1a3b 66
pvaibhav 34:01dec68de3ed 67 delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
pvaibhav 34:01dec68de3ed 68 }
pvaibhav 34:01dec68de3ed 69
pvaibhav 34:01dec68de3ed 70 void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max)
pvaibhav 34:01dec68de3ed 71 {
pvaibhav 34:01dec68de3ed 72 magneto.getCalibration(min, max);
pvaibhav 15:4488660e1a3b 73 }
pvaibhav 15:4488660e1a3b 74
pvaibhav 15:4488660e1a3b 75 void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
pvaibhav 15:4488660e1a3b 76 {
pvaibhav 34:01dec68de3ed 77 #define FULL_MADGWICK
pvaibhav 34:01dec68de3ed 78 #ifdef SIMPLE_COMPLEMENTARY
pvaibhav 34:01dec68de3ed 79
pvaibhav 34:01dec68de3ed 80 // normalise
pvaibhav 34:01dec68de3ed 81 /*float const anorm = sqrt(ax * ax + ay * ay + az * az);
pvaibhav 34:01dec68de3ed 82 ax /= anorm;
pvaibhav 34:01dec68de3ed 83 ay /= anorm;
pvaibhav 34:01dec68de3ed 84 az /= anorm;
pvaibhav 34:01dec68de3ed 85 */
pvaibhav 34:01dec68de3ed 86 float const signAz = (az < 0) - (0 < az);
pvaibhav 34:01dec68de3ed 87 float const aroll = utils::rad2deg(atan2(-ax, sqrt(ay * ay + az * az)));
pvaibhav 34:01dec68de3ed 88 float const apitch = utils::rad2deg(atan2(ay, signAz * sqrt(az * az + ax * ax)));
pvaibhav 34:01dec68de3ed 89
pvaibhav 34:01dec68de3ed 90 // integrate and fuse
pvaibhav 34:01dec68de3ed 91 float const beta = 0.98;
pvaibhav 34:01dec68de3ed 92 fused.x = fmod(beta * (fused.x + (-gx * deltat)) + (1 - beta) * aroll, 360);
pvaibhav 34:01dec68de3ed 93 fused.y = fmod(beta * (fused.y + (-gy * deltat)) + (1 - beta) * apitch, 360);
pvaibhav 34:01dec68de3ed 94 fused.z = fmod(fused.z + (gz * deltat), 360);
pvaibhav 34:01dec68de3ed 95
pvaibhav 34:01dec68de3ed 96 //fused.x = fmod(fused.x + 180, 360) - 180;
pvaibhav 34:01dec68de3ed 97 //fused.y = fmod(fused.y + 180, 360) - 180;
pvaibhav 34:01dec68de3ed 98
pvaibhav 34:01dec68de3ed 99 #endif
pvaibhav 34:01dec68de3ed 100
pvaibhav 34:01dec68de3ed 101 #ifdef FULL_MADGWICK
pvaibhav 15:4488660e1a3b 102 float q1 = q.w, q2 = q.v.x, q3 = q.v.y, q4 = q.v.z; // short name local variable for readability
pvaibhav 15:4488660e1a3b 103 float norm;
pvaibhav 15:4488660e1a3b 104 float s1, s2, s3, s4;
pvaibhav 15:4488660e1a3b 105
pvaibhav 15:4488660e1a3b 106 // Auxiliary variables to avoid repeated arithmetic
pvaibhav 15:4488660e1a3b 107 const float _2q1 = 2.0f * q1;
pvaibhav 15:4488660e1a3b 108 const float _2q2 = 2.0f * q2;
pvaibhav 15:4488660e1a3b 109 const float _2q3 = 2.0f * q3;
pvaibhav 15:4488660e1a3b 110 const float _2q4 = 2.0f * q4;
pvaibhav 15:4488660e1a3b 111 const float _2q1q3 = 2.0f * q1 * q3;
pvaibhav 15:4488660e1a3b 112 const float _2q3q4 = 2.0f * q3 * q4;
pvaibhav 15:4488660e1a3b 113 const float q1q1 = q1 * q1;
pvaibhav 15:4488660e1a3b 114 const float q1q2 = q1 * q2;
pvaibhav 15:4488660e1a3b 115 const float q1q3 = q1 * q3;
pvaibhav 15:4488660e1a3b 116 const float q1q4 = q1 * q4;
pvaibhav 15:4488660e1a3b 117 const float q2q2 = q2 * q2;
pvaibhav 15:4488660e1a3b 118 const float q2q3 = q2 * q3;
pvaibhav 15:4488660e1a3b 119 const float q2q4 = q2 * q4;
pvaibhav 15:4488660e1a3b 120 const float q3q3 = q3 * q3;
pvaibhav 15:4488660e1a3b 121 const float q3q4 = q3 * q4;
pvaibhav 15:4488660e1a3b 122 const float q4q4 = q4 * q4;
pvaibhav 15:4488660e1a3b 123
pvaibhav 15:4488660e1a3b 124 // Normalise accelerometer measurement
pvaibhav 15:4488660e1a3b 125 norm = sqrt(ax * ax + ay * ay + az * az);
pvaibhav 15:4488660e1a3b 126 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 127 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 128 ax *= norm;
pvaibhav 15:4488660e1a3b 129 ay *= norm;
pvaibhav 15:4488660e1a3b 130 az *= norm;
pvaibhav 15:4488660e1a3b 131
pvaibhav 15:4488660e1a3b 132 // Normalise magnetometer measurement
pvaibhav 15:4488660e1a3b 133 norm = sqrt(mx * mx + my * my + mz * mz);
pvaibhav 15:4488660e1a3b 134 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 135 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 136 mx *= norm;
pvaibhav 15:4488660e1a3b 137 my *= norm;
pvaibhav 15:4488660e1a3b 138 mz *= norm;
pvaibhav 15:4488660e1a3b 139
pvaibhav 15:4488660e1a3b 140 // Reference direction of Earth's magnetic field
pvaibhav 15:4488660e1a3b 141 const float _2q1mx = 2.0f * q1 * mx;
pvaibhav 15:4488660e1a3b 142 const float _2q1my = 2.0f * q1 * my;
pvaibhav 15:4488660e1a3b 143 const float _2q1mz = 2.0f * q1 * mz;
pvaibhav 15:4488660e1a3b 144 const float _2q2mx = 2.0f * q2 * mx;
pvaibhav 15:4488660e1a3b 145 const float hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
pvaibhav 15:4488660e1a3b 146 const float hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
pvaibhav 15:4488660e1a3b 147 const float _2bx = sqrt(hx * hx + hy * hy);
pvaibhav 15:4488660e1a3b 148 const float _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
pvaibhav 15:4488660e1a3b 149 const float _4bx = 2.0f * _2bx;
pvaibhav 15:4488660e1a3b 150 const float _4bz = 2.0f * _2bz;
pvaibhav 15:4488660e1a3b 151
pvaibhav 15:4488660e1a3b 152 // Gradient decent algorithm corrective step
pvaibhav 15:4488660e1a3b 153 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
pvaibhav 15:4488660e1a3b 154 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
pvaibhav 15:4488660e1a3b 155 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
pvaibhav 15:4488660e1a3b 156 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
pvaibhav 15:4488660e1a3b 157 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
pvaibhav 15:4488660e1a3b 158 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 159 s1 *= norm;
pvaibhav 15:4488660e1a3b 160 s2 *= norm;
pvaibhav 15:4488660e1a3b 161 s3 *= norm;
pvaibhav 15:4488660e1a3b 162 s4 *= norm;
pvaibhav 15:4488660e1a3b 163
pvaibhav 15:4488660e1a3b 164 // Compute rate of change of quaternion
pvaibhav 15:4488660e1a3b 165 const float qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
pvaibhav 15:4488660e1a3b 166 const float qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
pvaibhav 15:4488660e1a3b 167 const float qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
pvaibhav 15:4488660e1a3b 168 const float qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
pvaibhav 15:4488660e1a3b 169
pvaibhav 15:4488660e1a3b 170 // Integrate to yield quaternion
pvaibhav 15:4488660e1a3b 171 q1 += qDot1 * deltat;
pvaibhav 15:4488660e1a3b 172 q2 += qDot2 * deltat;
pvaibhav 15:4488660e1a3b 173 q3 += qDot3 * deltat;
pvaibhav 15:4488660e1a3b 174 q4 += qDot4 * deltat;
pvaibhav 15:4488660e1a3b 175 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
pvaibhav 15:4488660e1a3b 176 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 177 q.w = q1 * norm;
pvaibhav 15:4488660e1a3b 178 q.v.x = q2 * norm;
pvaibhav 15:4488660e1a3b 179 q.v.y = q3 * norm;
pvaibhav 15:4488660e1a3b 180 q.v.z = q4 * norm;
pvaibhav 34:01dec68de3ed 181 #endif
pvaibhav 15:4488660e1a3b 182 }