Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@19:9e9753b87cfe, 2015-04-02 (annotated)

Committer:
pvaibhav
Date:
Thu Apr 02 14:13:01 2015 +0000
Revision:
19:9e9753b87cfe
Parent:
17:e9d42864c8a1
Child:
20:503cbe360419
SensorFusion beta increased, Barometer does not subtract height offset anymore.

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