Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@16:3e2468d4f4c1, 2015-03-20 (annotated)

Committer:
pvaibhav
Date:
Fri Mar 20 10:30:01 2015 +0000
Revision:
16:3e2468d4f4c1
Parent:
15:4488660e1a3b
Child:
17:e9d42864c8a1
Barometer driver converted to new Sensor infrastructure

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 16:3e2468d4f4c1 9 beta(0.5) // 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 15:4488660e1a3b 46 void SensorFusion::sensorUpdate(Vector3 gyro_degrees)
pvaibhav 15:4488660e1a3b 47 {
pvaibhav 15:4488660e1a3b 48 Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
pvaibhav 15:4488660e1a3b 49 Vector3 const accel_reading = accel.read();
pvaibhav 15:4488660e1a3b 50 Vector3 const magneto_reading = magneto.read();
pvaibhav 15:4488660e1a3b 51
pvaibhav 15:4488660e1a3b 52 updateFilter( accel_reading.x, accel_reading.y, accel_reading.z,
pvaibhav 15:4488660e1a3b 53 gyro_reading.x, gyro_reading.y, gyro_reading.z,
pvaibhav 15:4488660e1a3b 54 magneto_reading.x, magneto_reading.y, magneto_reading.z);
pvaibhav 15:4488660e1a3b 55
pvaibhav 15:4488660e1a3b 56 Vector3 const fused = q.getEulerAngles() * radian_to_deg;
pvaibhav 15:4488660e1a3b 57
pvaibhav 15:4488660e1a3b 58 sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q);
pvaibhav 15:4488660e1a3b 59 }
pvaibhav 15:4488660e1a3b 60
pvaibhav 15:4488660e1a3b 61 void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
pvaibhav 15:4488660e1a3b 62 {
pvaibhav 15:4488660e1a3b 63 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 64 float norm;
pvaibhav 15:4488660e1a3b 65 float s1, s2, s3, s4;
pvaibhav 15:4488660e1a3b 66
pvaibhav 15:4488660e1a3b 67 // Auxiliary variables to avoid repeated arithmetic
pvaibhav 15:4488660e1a3b 68 const float _2q1 = 2.0f * q1;
pvaibhav 15:4488660e1a3b 69 const float _2q2 = 2.0f * q2;
pvaibhav 15:4488660e1a3b 70 const float _2q3 = 2.0f * q3;
pvaibhav 15:4488660e1a3b 71 const float _2q4 = 2.0f * q4;
pvaibhav 15:4488660e1a3b 72 const float _2q1q3 = 2.0f * q1 * q3;
pvaibhav 15:4488660e1a3b 73 const float _2q3q4 = 2.0f * q3 * q4;
pvaibhav 15:4488660e1a3b 74 const float q1q1 = q1 * q1;
pvaibhav 15:4488660e1a3b 75 const float q1q2 = q1 * q2;
pvaibhav 15:4488660e1a3b 76 const float q1q3 = q1 * q3;
pvaibhav 15:4488660e1a3b 77 const float q1q4 = q1 * q4;
pvaibhav 15:4488660e1a3b 78 const float q2q2 = q2 * q2;
pvaibhav 15:4488660e1a3b 79 const float q2q3 = q2 * q3;
pvaibhav 15:4488660e1a3b 80 const float q2q4 = q2 * q4;
pvaibhav 15:4488660e1a3b 81 const float q3q3 = q3 * q3;
pvaibhav 15:4488660e1a3b 82 const float q3q4 = q3 * q4;
pvaibhav 15:4488660e1a3b 83 const float q4q4 = q4 * q4;
pvaibhav 15:4488660e1a3b 84
pvaibhav 15:4488660e1a3b 85 // Normalise accelerometer measurement
pvaibhav 15:4488660e1a3b 86 norm = sqrt(ax * ax + ay * ay + az * az);
pvaibhav 15:4488660e1a3b 87 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 88 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 89 ax *= norm;
pvaibhav 15:4488660e1a3b 90 ay *= norm;
pvaibhav 15:4488660e1a3b 91 az *= norm;
pvaibhav 15:4488660e1a3b 92
pvaibhav 15:4488660e1a3b 93 // Normalise magnetometer measurement
pvaibhav 15:4488660e1a3b 94 norm = sqrt(mx * mx + my * my + mz * mz);
pvaibhav 15:4488660e1a3b 95 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 96 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 97 mx *= norm;
pvaibhav 15:4488660e1a3b 98 my *= norm;
pvaibhav 15:4488660e1a3b 99 mz *= norm;
pvaibhav 15:4488660e1a3b 100
pvaibhav 15:4488660e1a3b 101 // Reference direction of Earth's magnetic field
pvaibhav 15:4488660e1a3b 102 const float _2q1mx = 2.0f * q1 * mx;
pvaibhav 15:4488660e1a3b 103 const float _2q1my = 2.0f * q1 * my;
pvaibhav 15:4488660e1a3b 104 const float _2q1mz = 2.0f * q1 * mz;
pvaibhav 15:4488660e1a3b 105 const float _2q2mx = 2.0f * q2 * mx;
pvaibhav 15:4488660e1a3b 106 const float hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
pvaibhav 15:4488660e1a3b 107 const float hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
pvaibhav 15:4488660e1a3b 108 const float _2bx = sqrt(hx * hx + hy * hy);
pvaibhav 15:4488660e1a3b 109 const float _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
pvaibhav 15:4488660e1a3b 110 const float _4bx = 2.0f * _2bx;
pvaibhav 15:4488660e1a3b 111 const float _4bz = 2.0f * _2bz;
pvaibhav 15:4488660e1a3b 112
pvaibhav 15:4488660e1a3b 113 // Gradient decent algorithm corrective step
pvaibhav 15:4488660e1a3b 114 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 115 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 116 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 117 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 118 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
pvaibhav 15:4488660e1a3b 119 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 120 s1 *= norm;
pvaibhav 15:4488660e1a3b 121 s2 *= norm;
pvaibhav 15:4488660e1a3b 122 s3 *= norm;
pvaibhav 15:4488660e1a3b 123 s4 *= norm;
pvaibhav 15:4488660e1a3b 124
pvaibhav 15:4488660e1a3b 125 // Compute rate of change of quaternion
pvaibhav 15:4488660e1a3b 126 const float qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
pvaibhav 15:4488660e1a3b 127 const float qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
pvaibhav 15:4488660e1a3b 128 const float qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
pvaibhav 15:4488660e1a3b 129 const float qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
pvaibhav 15:4488660e1a3b 130
pvaibhav 15:4488660e1a3b 131 // Integrate to yield quaternion
pvaibhav 15:4488660e1a3b 132 q1 += qDot1 * deltat;
pvaibhav 15:4488660e1a3b 133 q2 += qDot2 * deltat;
pvaibhav 15:4488660e1a3b 134 q3 += qDot3 * deltat;
pvaibhav 15:4488660e1a3b 135 q4 += qDot4 * deltat;
pvaibhav 15:4488660e1a3b 136 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
pvaibhav 15:4488660e1a3b 137 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 138 q.w = q1 * norm;
pvaibhav 15:4488660e1a3b 139 q.v.x = q2 * norm;
pvaibhav 15:4488660e1a3b 140 q.v.y = q3 * norm;
pvaibhav 15:4488660e1a3b 141 q.v.z = q4 * norm;
pvaibhav 15:4488660e1a3b 142 }