Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@31:d65576185bdf, 2015-04-28 (annotated)

Committer:
pvaibhav
Date:
Tue Apr 28 09:08:03 2015 +0000
Revision:
31:d65576185bdf
Parent:
26:8f3e4e1a3acc
Child:
34:01dec68de3ed
- Gyro doesn't disable interrupt anymore (fix for SML2 v0.95); - Accelerometer frequency reduced to 125 Hz

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 21:5a0c9406e119 6 SensorFusion::SensorFusion(I2C &i2c) :
pvaibhav 21:5a0c9406e119 7 delegate(&defaultDelegate),
pvaibhav 21:5a0c9406e119 8 accel(i2c), gyro(i2c), magneto(i2c),
pvaibhav 15:4488660e1a3b 9 q(1, 0, 0, 0), // output quaternion
pvaibhav 15:4488660e1a3b 10 deltat(0.010), // sec
pvaibhav 25:abb0f208e6a9 11 beta(0.5) // correction gain
pvaibhav 15:4488660e1a3b 12 {
pvaibhav 20:503cbe360419 13 }
pvaibhav 20:503cbe360419 14
pvaibhav 20:503cbe360419 15 void SensorFusion::setDelegate(SensorFusion::Delegate &d)
pvaibhav 20:503cbe360419 16 {
pvaibhav 20:503cbe360419 17 delegate = &d;
pvaibhav 15:4488660e1a3b 18 }
pvaibhav 15:4488660e1a3b 19
pvaibhav 15:4488660e1a3b 20 bool SensorFusion::start()
pvaibhav 15:4488660e1a3b 21 {
pvaibhav 15:4488660e1a3b 22 accel.powerOn();
pvaibhav 15:4488660e1a3b 23 accel.start();
pvaibhav 15:4488660e1a3b 24
pvaibhav 15:4488660e1a3b 25 magneto.powerOn();
pvaibhav 15:4488660e1a3b 26 if (magneto.performSelfTest() == false) {
pvaibhav 15:4488660e1a3b 27 return false;
pvaibhav 15:4488660e1a3b 28 }
pvaibhav 15:4488660e1a3b 29 magneto.start();
pvaibhav 15:4488660e1a3b 30
pvaibhav 15:4488660e1a3b 31 // Since everything is synced to gyro interrupt, start it last
pvaibhav 15:4488660e1a3b 32 gyro.setDelegate(*this);
pvaibhav 15:4488660e1a3b 33 gyro.powerOn();
pvaibhav 15:4488660e1a3b 34 gyro.start();
pvaibhav 15:4488660e1a3b 35
pvaibhav 15:4488660e1a3b 36 return true;
pvaibhav 15:4488660e1a3b 37 }
pvaibhav 15:4488660e1a3b 38
pvaibhav 15:4488660e1a3b 39 void SensorFusion::stop()
pvaibhav 15:4488660e1a3b 40 {
pvaibhav 15:4488660e1a3b 41 gyro.stop();
pvaibhav 15:4488660e1a3b 42 magneto.stop();
pvaibhav 15:4488660e1a3b 43 accel.stop();
pvaibhav 15:4488660e1a3b 44
pvaibhav 15:4488660e1a3b 45 gyro.powerOff();
pvaibhav 15:4488660e1a3b 46 magneto.powerOff();
pvaibhav 15:4488660e1a3b 47 accel.powerOff();
pvaibhav 15:4488660e1a3b 48 }
pvaibhav 15:4488660e1a3b 49
pvaibhav 15:4488660e1a3b 50 static float const deg_to_radian = 0.0174532925f;
pvaibhav 15:4488660e1a3b 51
pvaibhav 15:4488660e1a3b 52 void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
pvaibhav 15:4488660e1a3b 53 {
pvaibhav 31:d65576185bdf 54
pvaibhav 15:4488660e1a3b 55 Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
pvaibhav 31:d65576185bdf 56
pvaibhav 15:4488660e1a3b 57 Vector3 const accel_reading = accel.read();
pvaibhav 15:4488660e1a3b 58 Vector3 const magneto_reading = magneto.read();
pvaibhav 15:4488660e1a3b 59
pvaibhav 15:4488660e1a3b 60 updateFilter( accel_reading.x, accel_reading.y, accel_reading.z,
pvaibhav 15:4488660e1a3b 61 gyro_reading.x, gyro_reading.y, gyro_reading.z,
pvaibhav 15:4488660e1a3b 62 magneto_reading.x, magneto_reading.y, magneto_reading.z);
pvaibhav 15:4488660e1a3b 63
pvaibhav 31:d65576185bdf 64 delegate->sensorTick(q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
pvaibhav 15:4488660e1a3b 65 }
pvaibhav 15:4488660e1a3b 66
pvaibhav 15:4488660e1a3b 67 void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
pvaibhav 15:4488660e1a3b 68 {
pvaibhav 15:4488660e1a3b 69 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 70 float norm;
pvaibhav 15:4488660e1a3b 71 float s1, s2, s3, s4;
pvaibhav 15:4488660e1a3b 72
pvaibhav 15:4488660e1a3b 73 // Auxiliary variables to avoid repeated arithmetic
pvaibhav 15:4488660e1a3b 74 const float _2q1 = 2.0f * q1;
pvaibhav 15:4488660e1a3b 75 const float _2q2 = 2.0f * q2;
pvaibhav 15:4488660e1a3b 76 const float _2q3 = 2.0f * q3;
pvaibhav 15:4488660e1a3b 77 const float _2q4 = 2.0f * q4;
pvaibhav 15:4488660e1a3b 78 const float _2q1q3 = 2.0f * q1 * q3;
pvaibhav 15:4488660e1a3b 79 const float _2q3q4 = 2.0f * q3 * q4;
pvaibhav 15:4488660e1a3b 80 const float q1q1 = q1 * q1;
pvaibhav 15:4488660e1a3b 81 const float q1q2 = q1 * q2;
pvaibhav 15:4488660e1a3b 82 const float q1q3 = q1 * q3;
pvaibhav 15:4488660e1a3b 83 const float q1q4 = q1 * q4;
pvaibhav 15:4488660e1a3b 84 const float q2q2 = q2 * q2;
pvaibhav 15:4488660e1a3b 85 const float q2q3 = q2 * q3;
pvaibhav 15:4488660e1a3b 86 const float q2q4 = q2 * q4;
pvaibhav 15:4488660e1a3b 87 const float q3q3 = q3 * q3;
pvaibhav 15:4488660e1a3b 88 const float q3q4 = q3 * q4;
pvaibhav 15:4488660e1a3b 89 const float q4q4 = q4 * q4;
pvaibhav 15:4488660e1a3b 90
pvaibhav 15:4488660e1a3b 91 // Normalise accelerometer measurement
pvaibhav 15:4488660e1a3b 92 norm = sqrt(ax * ax + ay * ay + az * az);
pvaibhav 15:4488660e1a3b 93 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 94 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 95 ax *= norm;
pvaibhav 15:4488660e1a3b 96 ay *= norm;
pvaibhav 15:4488660e1a3b 97 az *= norm;
pvaibhav 15:4488660e1a3b 98
pvaibhav 15:4488660e1a3b 99 // Normalise magnetometer measurement
pvaibhav 15:4488660e1a3b 100 norm = sqrt(mx * mx + my * my + mz * mz);
pvaibhav 15:4488660e1a3b 101 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 102 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 103 mx *= norm;
pvaibhav 15:4488660e1a3b 104 my *= norm;
pvaibhav 15:4488660e1a3b 105 mz *= norm;
pvaibhav 15:4488660e1a3b 106
pvaibhav 15:4488660e1a3b 107 // Reference direction of Earth's magnetic field
pvaibhav 15:4488660e1a3b 108 const float _2q1mx = 2.0f * q1 * mx;
pvaibhav 15:4488660e1a3b 109 const float _2q1my = 2.0f * q1 * my;
pvaibhav 15:4488660e1a3b 110 const float _2q1mz = 2.0f * q1 * mz;
pvaibhav 15:4488660e1a3b 111 const float _2q2mx = 2.0f * q2 * mx;
pvaibhav 15:4488660e1a3b 112 const float hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
pvaibhav 15:4488660e1a3b 113 const float hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
pvaibhav 15:4488660e1a3b 114 const float _2bx = sqrt(hx * hx + hy * hy);
pvaibhav 15:4488660e1a3b 115 const float _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
pvaibhav 15:4488660e1a3b 116 const float _4bx = 2.0f * _2bx;
pvaibhav 15:4488660e1a3b 117 const float _4bz = 2.0f * _2bz;
pvaibhav 15:4488660e1a3b 118
pvaibhav 15:4488660e1a3b 119 // Gradient decent algorithm corrective step
pvaibhav 15:4488660e1a3b 120 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 121 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 122 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 123 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 124 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
pvaibhav 15:4488660e1a3b 125 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 126 s1 *= norm;
pvaibhav 15:4488660e1a3b 127 s2 *= norm;
pvaibhav 15:4488660e1a3b 128 s3 *= norm;
pvaibhav 15:4488660e1a3b 129 s4 *= norm;
pvaibhav 15:4488660e1a3b 130
pvaibhav 15:4488660e1a3b 131 // Compute rate of change of quaternion
pvaibhav 15:4488660e1a3b 132 const float qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
pvaibhav 15:4488660e1a3b 133 const float qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
pvaibhav 15:4488660e1a3b 134 const float qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
pvaibhav 15:4488660e1a3b 135 const float qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
pvaibhav 15:4488660e1a3b 136
pvaibhav 15:4488660e1a3b 137 // Integrate to yield quaternion
pvaibhav 15:4488660e1a3b 138 q1 += qDot1 * deltat;
pvaibhav 15:4488660e1a3b 139 q2 += qDot2 * deltat;
pvaibhav 15:4488660e1a3b 140 q3 += qDot3 * deltat;
pvaibhav 15:4488660e1a3b 141 q4 += qDot4 * deltat;
pvaibhav 15:4488660e1a3b 142 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
pvaibhav 15:4488660e1a3b 143 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 144 q.w = q1 * norm;
pvaibhav 15:4488660e1a3b 145 q.v.x = q2 * norm;
pvaibhav 15:4488660e1a3b 146 q.v.y = q3 * norm;
pvaibhav 15:4488660e1a3b 147 q.v.z = q4 * norm;
pvaibhav 15:4488660e1a3b 148 }