Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@41:731e3cfac19b, 2015-05-20 (annotated)

Committer:
uadhikari
Date:
Wed May 20 10:13:14 2015 +0000
Revision:
41:731e3cfac19b
Parent:
39:1fa9c0e1ffde
Child:
42:160a37bdaa64
Refactored SensorFusion class

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 39:1fa9c0e1ffde 7 #define SIXAXIS
pvaibhav 34:01dec68de3ed 8
pvaibhav 21:5a0c9406e119 9 SensorFusion::SensorFusion(I2C &i2c) :
pvaibhav 21:5a0c9406e119 10 delegate(&defaultDelegate),
pvaibhav 21:5a0c9406e119 11 accel(i2c), gyro(i2c), magneto(i2c),
pvaibhav 15:4488660e1a3b 12 q(1, 0, 0, 0), // output quaternion
pvaibhav 15:4488660e1a3b 13 deltat(0.010), // sec
pvaibhav 39:1fa9c0e1ffde 14 beta(0.3), // correction gain
pvaibhav 34:01dec68de3ed 15 fused(0, 0, 0)
pvaibhav 15:4488660e1a3b 16 {
pvaibhav 20:503cbe360419 17 }
pvaibhav 20:503cbe360419 18
uadhikari 41:731e3cfac19b 19 SensorFusion::SensorFusion(const SensorFusion& s) :
uadhikari 41:731e3cfac19b 20 delegate(s.delegate),
uadhikari 41:731e3cfac19b 21 accel(s.accel),
uadhikari 41:731e3cfac19b 22 gyro(s.gyro),
uadhikari 41:731e3cfac19b 23 magneto(s.magneto),
uadhikari 41:731e3cfac19b 24 q(s.q),
uadhikari 41:731e3cfac19b 25 deltat(s.deltat),
uadhikari 41:731e3cfac19b 26 beta(s.beta),
uadhikari 41:731e3cfac19b 27 fused(s.fused)
uadhikari 41:731e3cfac19b 28 {}
uadhikari 41:731e3cfac19b 29
uadhikari 41:731e3cfac19b 30 SensorFusion::~SensorFusion(){};
uadhikari 41:731e3cfac19b 31
uadhikari 41:731e3cfac19b 32
uadhikari 41:731e3cfac19b 33 SixAxesSensor::~SixAxesSensor(){};
uadhikari 41:731e3cfac19b 34
uadhikari 41:731e3cfac19b 35 NineAxesSensor::~NineAxesSensor(){};
uadhikari 41:731e3cfac19b 36
uadhikari 41:731e3cfac19b 37 NineAxesSensor::NineAxesSensor(const NineAxesSensor& c) : SensorFusion(c){};
uadhikari 41:731e3cfac19b 38
uadhikari 41:731e3cfac19b 39 SixAxesSensor::SixAxesSensor(const SixAxesSensor& c) : SensorFusion(c){};
uadhikari 41:731e3cfac19b 40
pvaibhav 20:503cbe360419 41 void SensorFusion::setDelegate(SensorFusion::Delegate &d)
pvaibhav 20:503cbe360419 42 {
pvaibhav 20:503cbe360419 43 delegate = &d;
pvaibhav 15:4488660e1a3b 44 }
pvaibhav 15:4488660e1a3b 45
uadhikari 41:731e3cfac19b 46 void SensorFusion::startAccelerometer(){
pvaibhav 15:4488660e1a3b 47 accel.powerOn();
uadhikari 41:731e3cfac19b 48 accel.start();
uadhikari 41:731e3cfac19b 49 };
uadhikari 41:731e3cfac19b 50
uadhikari 41:731e3cfac19b 51 void SensorFusion::startGyrometer(){
uadhikari 41:731e3cfac19b 52 gyro.powerOn();
uadhikari 41:731e3cfac19b 53 gyro.start();
uadhikari 41:731e3cfac19b 54 };
uadhikari 41:731e3cfac19b 55
uadhikari 41:731e3cfac19b 56 bool SensorFusion::startMagnetometer(){
pvaibhav 15:4488660e1a3b 57 magneto.powerOn();
uadhikari 41:731e3cfac19b 58 if (magneto.performSelfTest() == false){
uadhikari 41:731e3cfac19b 59 //Should it be left powered on
pvaibhav 15:4488660e1a3b 60 return false;
pvaibhav 15:4488660e1a3b 61 }
uadhikari 41:731e3cfac19b 62 magneto.start();
uadhikari 41:731e3cfac19b 63 return true;
uadhikari 41:731e3cfac19b 64
uadhikari 41:731e3cfac19b 65 };
pvaibhav 15:4488660e1a3b 66
uadhikari 41:731e3cfac19b 67 void SensorFusion::stopAccelerometer(){
uadhikari 41:731e3cfac19b 68 accel.stop();
uadhikari 41:731e3cfac19b 69 accel.powerOff();
uadhikari 41:731e3cfac19b 70 };
pvaibhav 15:4488660e1a3b 71
uadhikari 41:731e3cfac19b 72 void SensorFusion::stopGyrometer(){
pvaibhav 15:4488660e1a3b 73 gyro.stop();
uadhikari 41:731e3cfac19b 74 gyro.powerOff();
uadhikari 41:731e3cfac19b 75 };
uadhikari 41:731e3cfac19b 76
uadhikari 41:731e3cfac19b 77 void SensorFusion::stopMagnetometer(){
pvaibhav 15:4488660e1a3b 78 magneto.stop();
uadhikari 41:731e3cfac19b 79 magneto.powerOff();
uadhikari 41:731e3cfac19b 80 };
pvaibhav 15:4488660e1a3b 81
pvaibhav 15:4488660e1a3b 82 static float const deg_to_radian = 0.0174532925f;
pvaibhav 15:4488660e1a3b 83
pvaibhav 34:01dec68de3ed 84 void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max)
pvaibhav 34:01dec68de3ed 85 {
pvaibhav 34:01dec68de3ed 86 magneto.getCalibration(min, max);
pvaibhav 15:4488660e1a3b 87 }
pvaibhav 15:4488660e1a3b 88
uadhikari 41:731e3cfac19b 89 /* NineAxesSensor*/
pvaibhav 35:fb6e4601adf3 90
uadhikari 41:731e3cfac19b 91 NineAxesSensor::NineAxesSensor(I2C &i2c) : SensorFusion(i2c){}
pvaibhav 35:fb6e4601adf3 92
uadhikari 41:731e3cfac19b 93 bool NineAxesSensor::start(){
uadhikari 41:731e3cfac19b 94 startAccelerometer();
uadhikari 41:731e3cfac19b 95
uadhikari 41:731e3cfac19b 96 bool magnetoMeterSelfTestResult = startMagnetometer();
uadhikari 41:731e3cfac19b 97 if( magnetoMeterSelfTestResult == false){
uadhikari 41:731e3cfac19b 98 return false;
uadhikari 41:731e3cfac19b 99 }
uadhikari 41:731e3cfac19b 100
uadhikari 41:731e3cfac19b 101 //Since everything is synced to gyro interrupt, start it last
uadhikari 41:731e3cfac19b 102 gyro.setDelegate(*this);
uadhikari 41:731e3cfac19b 103 startGyrometer();
uadhikari 41:731e3cfac19b 104 return true;
uadhikari 41:731e3cfac19b 105 };
pvaibhav 35:fb6e4601adf3 106
uadhikari 41:731e3cfac19b 107 void NineAxesSensor::stop(){
uadhikari 41:731e3cfac19b 108 stopAccelerometer();
uadhikari 41:731e3cfac19b 109 stopMagnetometer();
uadhikari 41:731e3cfac19b 110 stopGyrometer();
pvaibhav 35:fb6e4601adf3 111 }
pvaibhav 35:fb6e4601adf3 112
uadhikari 41:731e3cfac19b 113 void NineAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){
uadhikari 41:731e3cfac19b 114 float q1 = q.w,
uadhikari 41:731e3cfac19b 115 q2 = q.v.x,
uadhikari 41:731e3cfac19b 116 q3 = q.v.y,
uadhikari 41:731e3cfac19b 117 q4 = q.v.z; // short name local variable for readability
pvaibhav 15:4488660e1a3b 118 float norm;
pvaibhav 15:4488660e1a3b 119 float s1, s2, s3, s4;
pvaibhav 15:4488660e1a3b 120
pvaibhav 15:4488660e1a3b 121 // Auxiliary variables to avoid repeated arithmetic
pvaibhav 15:4488660e1a3b 122 const float _2q1 = 2.0f * q1;
pvaibhav 15:4488660e1a3b 123 const float _2q2 = 2.0f * q2;
pvaibhav 15:4488660e1a3b 124 const float _2q3 = 2.0f * q3;
pvaibhav 15:4488660e1a3b 125 const float _2q4 = 2.0f * q4;
pvaibhav 15:4488660e1a3b 126 const float _2q1q3 = 2.0f * q1 * q3;
pvaibhav 15:4488660e1a3b 127 const float _2q3q4 = 2.0f * q3 * q4;
pvaibhav 15:4488660e1a3b 128 const float q1q1 = q1 * q1;
pvaibhav 15:4488660e1a3b 129 const float q1q2 = q1 * q2;
pvaibhav 15:4488660e1a3b 130 const float q1q3 = q1 * q3;
pvaibhav 15:4488660e1a3b 131 const float q1q4 = q1 * q4;
pvaibhav 15:4488660e1a3b 132 const float q2q2 = q2 * q2;
pvaibhav 15:4488660e1a3b 133 const float q2q3 = q2 * q3;
pvaibhav 15:4488660e1a3b 134 const float q2q4 = q2 * q4;
pvaibhav 15:4488660e1a3b 135 const float q3q3 = q3 * q3;
pvaibhav 15:4488660e1a3b 136 const float q3q4 = q3 * q4;
pvaibhav 15:4488660e1a3b 137 const float q4q4 = q4 * q4;
pvaibhav 15:4488660e1a3b 138
pvaibhav 15:4488660e1a3b 139 // Normalise accelerometer measurement
pvaibhav 15:4488660e1a3b 140 norm = sqrt(ax * ax + ay * ay + az * az);
pvaibhav 15:4488660e1a3b 141 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 142 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 143 ax *= norm;
pvaibhav 15:4488660e1a3b 144 ay *= norm;
pvaibhav 15:4488660e1a3b 145 az *= norm;
pvaibhav 15:4488660e1a3b 146
pvaibhav 15:4488660e1a3b 147 // Normalise magnetometer measurement
pvaibhav 15:4488660e1a3b 148 norm = sqrt(mx * mx + my * my + mz * mz);
pvaibhav 15:4488660e1a3b 149 if (norm == 0.0f) return; // handle NaN
pvaibhav 15:4488660e1a3b 150 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 151 mx *= norm;
pvaibhav 15:4488660e1a3b 152 my *= norm;
pvaibhav 15:4488660e1a3b 153 mz *= norm;
pvaibhav 15:4488660e1a3b 154
pvaibhav 15:4488660e1a3b 155 // Reference direction of Earth's magnetic field
pvaibhav 15:4488660e1a3b 156 const float _2q1mx = 2.0f * q1 * mx;
pvaibhav 15:4488660e1a3b 157 const float _2q1my = 2.0f * q1 * my;
pvaibhav 15:4488660e1a3b 158 const float _2q1mz = 2.0f * q1 * mz;
pvaibhav 15:4488660e1a3b 159 const float _2q2mx = 2.0f * q2 * mx;
pvaibhav 15:4488660e1a3b 160 const float hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
pvaibhav 15:4488660e1a3b 161 const float hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
pvaibhav 15:4488660e1a3b 162 const float _2bx = sqrt(hx * hx + hy * hy);
pvaibhav 15:4488660e1a3b 163 const float _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
pvaibhav 15:4488660e1a3b 164 const float _4bx = 2.0f * _2bx;
pvaibhav 15:4488660e1a3b 165 const float _4bz = 2.0f * _2bz;
pvaibhav 15:4488660e1a3b 166
pvaibhav 15:4488660e1a3b 167 // Gradient decent algorithm corrective step
pvaibhav 15:4488660e1a3b 168 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 169 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 170 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 171 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 172 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
pvaibhav 15:4488660e1a3b 173 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 174 s1 *= norm;
pvaibhav 15:4488660e1a3b 175 s2 *= norm;
pvaibhav 15:4488660e1a3b 176 s3 *= norm;
pvaibhav 15:4488660e1a3b 177 s4 *= norm;
pvaibhav 15:4488660e1a3b 178
pvaibhav 15:4488660e1a3b 179 // Compute rate of change of quaternion
pvaibhav 15:4488660e1a3b 180 const float qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
pvaibhav 15:4488660e1a3b 181 const float qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
pvaibhav 15:4488660e1a3b 182 const float qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
pvaibhav 15:4488660e1a3b 183 const float qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
pvaibhav 15:4488660e1a3b 184
pvaibhav 15:4488660e1a3b 185 // Integrate to yield quaternion
pvaibhav 15:4488660e1a3b 186 q1 += qDot1 * deltat;
pvaibhav 15:4488660e1a3b 187 q2 += qDot2 * deltat;
pvaibhav 15:4488660e1a3b 188 q3 += qDot3 * deltat;
pvaibhav 15:4488660e1a3b 189 q4 += qDot4 * deltat;
pvaibhav 15:4488660e1a3b 190 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
pvaibhav 15:4488660e1a3b 191 norm = 1.0f/norm;
pvaibhav 15:4488660e1a3b 192 q.w = q1 * norm;
pvaibhav 15:4488660e1a3b 193 q.v.x = q2 * norm;
pvaibhav 15:4488660e1a3b 194 q.v.y = q3 * norm;
uadhikari 41:731e3cfac19b 195 q.v.z = q4 * norm;
uadhikari 41:731e3cfac19b 196 }
uadhikari 41:731e3cfac19b 197
uadhikari 41:731e3cfac19b 198 void NineAxesSensor::sensorUpdate(Vector3 gyro_degrees){
uadhikari 41:731e3cfac19b 199 Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
uadhikari 41:731e3cfac19b 200 Vector3 const accel_reading = accel.read();
uadhikari 41:731e3cfac19b 201 Vector3 const magneto_reading = magneto.read();
uadhikari 41:731e3cfac19b 202
uadhikari 41:731e3cfac19b 203 updateFilter( accel_reading.x, accel_reading.y, accel_reading.z,
uadhikari 41:731e3cfac19b 204 gyro_reading.x, gyro_reading.y, gyro_reading.z,
uadhikari 41:731e3cfac19b 205 magneto_reading.x, magneto_reading.y, magneto_reading.z);
uadhikari 41:731e3cfac19b 206
uadhikari 41:731e3cfac19b 207 delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
uadhikari 41:731e3cfac19b 208 }
uadhikari 41:731e3cfac19b 209
uadhikari 41:731e3cfac19b 210 /* SixAxesSensor */
uadhikari 41:731e3cfac19b 211
uadhikari 41:731e3cfac19b 212 SixAxesSensor::SixAxesSensor(I2C &i2c) : SensorFusion(i2c){}
uadhikari 41:731e3cfac19b 213
uadhikari 41:731e3cfac19b 214 bool SixAxesSensor::start(){
uadhikari 41:731e3cfac19b 215 startAccelerometer();
uadhikari 41:731e3cfac19b 216 startGyrometer();
uadhikari 41:731e3cfac19b 217 gyro.setDelegate(*this);
uadhikari 41:731e3cfac19b 218 return true;
uadhikari 41:731e3cfac19b 219 };
uadhikari 41:731e3cfac19b 220
uadhikari 41:731e3cfac19b 221 void SixAxesSensor::stop(){
uadhikari 41:731e3cfac19b 222 stopAccelerometer();
uadhikari 41:731e3cfac19b 223 stopGyrometer();
pvaibhav 15:4488660e1a3b 224 }
uadhikari 41:731e3cfac19b 225
uadhikari 41:731e3cfac19b 226 void SixAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz){
uadhikari 41:731e3cfac19b 227 float q0 = q.w,
uadhikari 41:731e3cfac19b 228 q1 = q.v.x,
uadhikari 41:731e3cfac19b 229 q2 = q.v.y,
uadhikari 41:731e3cfac19b 230 q3 = q.v.z; // short name local variable for readability
uadhikari 41:731e3cfac19b 231
uadhikari 41:731e3cfac19b 232 float recipNorm;
uadhikari 41:731e3cfac19b 233 float s0, s1, s2, s3;
uadhikari 41:731e3cfac19b 234 float qDot1, qDot2, qDot3, qDot4;
uadhikari 41:731e3cfac19b 235 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
uadhikari 41:731e3cfac19b 236
uadhikari 41:731e3cfac19b 237 // Rate of change of quaternion from gyroscope
uadhikari 41:731e3cfac19b 238 qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz);
uadhikari 41:731e3cfac19b 239 qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy);
uadhikari 41:731e3cfac19b 240 qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx);
uadhikari 41:731e3cfac19b 241 qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx);
uadhikari 41:731e3cfac19b 242
uadhikari 41:731e3cfac19b 243 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
uadhikari 41:731e3cfac19b 244 if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
uadhikari 41:731e3cfac19b 245
uadhikari 41:731e3cfac19b 246 // Normalise accelerometer measurement
uadhikari 41:731e3cfac19b 247 recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az);
uadhikari 41:731e3cfac19b 248 ax *= recipNorm;
uadhikari 41:731e3cfac19b 249 ay *= recipNorm;
uadhikari 41:731e3cfac19b 250 az *= recipNorm;
uadhikari 41:731e3cfac19b 251
uadhikari 41:731e3cfac19b 252 // Auxiliary variables to avoid repeated arithmetic
uadhikari 41:731e3cfac19b 253 _2q0 = 2.0 * q0;
uadhikari 41:731e3cfac19b 254 _2q1 = 2.0 * q1;
uadhikari 41:731e3cfac19b 255 _2q2 = 2.0 * q2;
uadhikari 41:731e3cfac19b 256 _2q3 = 2.0 * q3;
uadhikari 41:731e3cfac19b 257 _4q0 = 4.0 * q0;
uadhikari 41:731e3cfac19b 258 _4q1 = 4.0 * q1;
uadhikari 41:731e3cfac19b 259 _4q2 = 4.0 * q2;
uadhikari 41:731e3cfac19b 260 _8q1 = 8.0 * q1;
uadhikari 41:731e3cfac19b 261 _8q2 = 8.0 * q2;
uadhikari 41:731e3cfac19b 262 q0q0 = q0 * q0;
uadhikari 41:731e3cfac19b 263 q1q1 = q1 * q1;
uadhikari 41:731e3cfac19b 264 q2q2 = q2 * q2;
uadhikari 41:731e3cfac19b 265 q3q3 = q3 * q3;
uadhikari 41:731e3cfac19b 266
uadhikari 41:731e3cfac19b 267 // Gradient decent algorithm corrective step
uadhikari 41:731e3cfac19b 268 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
uadhikari 41:731e3cfac19b 269 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
uadhikari 41:731e3cfac19b 270 s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
uadhikari 41:731e3cfac19b 271 s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay;
uadhikari 41:731e3cfac19b 272 recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
uadhikari 41:731e3cfac19b 273 s0 *= recipNorm;
uadhikari 41:731e3cfac19b 274 s1 *= recipNorm;
uadhikari 41:731e3cfac19b 275 s2 *= recipNorm;
uadhikari 41:731e3cfac19b 276 s3 *= recipNorm;
uadhikari 41:731e3cfac19b 277
uadhikari 41:731e3cfac19b 278 // Apply feedback step
uadhikari 41:731e3cfac19b 279 qDot1 -= beta * s0;
uadhikari 41:731e3cfac19b 280 qDot2 -= beta * s1;
uadhikari 41:731e3cfac19b 281 qDot3 -= beta * s2;
uadhikari 41:731e3cfac19b 282 qDot4 -= beta * s3;
uadhikari 41:731e3cfac19b 283 }
uadhikari 41:731e3cfac19b 284
uadhikari 41:731e3cfac19b 285 // Integrate rate of change of quaternion to yield quaternion
uadhikari 41:731e3cfac19b 286 q0 += qDot1 * deltat;
uadhikari 41:731e3cfac19b 287 q1 += qDot2 * deltat;
uadhikari 41:731e3cfac19b 288 q2 += qDot3 * deltat;
uadhikari 41:731e3cfac19b 289 q3 += qDot4 * deltat;
uadhikari 41:731e3cfac19b 290
uadhikari 41:731e3cfac19b 291 // Normalise quaternion
uadhikari 41:731e3cfac19b 292 recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
uadhikari 41:731e3cfac19b 293 q0 *= recipNorm;
uadhikari 41:731e3cfac19b 294 q1 *= recipNorm;
uadhikari 41:731e3cfac19b 295 q2 *= recipNorm;
uadhikari 41:731e3cfac19b 296 q3 *= recipNorm;
uadhikari 41:731e3cfac19b 297
uadhikari 41:731e3cfac19b 298 // return
uadhikari 41:731e3cfac19b 299 q.w = q0;
uadhikari 41:731e3cfac19b 300 q.v.x = q1;
uadhikari 41:731e3cfac19b 301 q.v.y = q2;
uadhikari 41:731e3cfac19b 302 q.v.z = q3;
uadhikari 41:731e3cfac19b 303 }
uadhikari 41:731e3cfac19b 304
uadhikari 41:731e3cfac19b 305 void SixAxesSensor::sensorUpdate(Vector3 gyro_degrees){
uadhikari 41:731e3cfac19b 306 Vector3 const gyro_reading = gyro_degrees * deg_to_radian;
uadhikari 41:731e3cfac19b 307 Vector3 const accel_reading = accel.read();
uadhikari 41:731e3cfac19b 308 Vector3 const magneto_reading(0, 0, 0);
uadhikari 41:731e3cfac19b 309
uadhikari 41:731e3cfac19b 310 updateFilter( accel_reading.x, accel_reading.y, accel_reading.z,
uadhikari 41:731e3cfac19b 311 gyro_reading.x, gyro_reading.y, gyro_reading.z);
uadhikari 41:731e3cfac19b 312
uadhikari 41:731e3cfac19b 313 delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q);
uadhikari 41:731e3cfac19b 314 }
uadhikari 41:731e3cfac19b 315
uadhikari 41:731e3cfac19b 316 bool SensorFusion::start(){
uadhikari 41:731e3cfac19b 317 return false;
uadhikari 41:731e3cfac19b 318 }
uadhikari 41:731e3cfac19b 319
uadhikari 41:731e3cfac19b 320 void SensorFusion::stop(){
uadhikari 41:731e3cfac19b 321
uadhikari 41:731e3cfac19b 322 }
uadhikari 41:731e3cfac19b 323
uadhikari 41:731e3cfac19b 324 void SensorFusion::sensorUpdate(Vector3 gyro_degrees){
uadhikari 41:731e3cfac19b 325
uadhikari 41:731e3cfac19b 326 }
uadhikari 41:731e3cfac19b 327
uadhikari 41:731e3cfac19b 328
uadhikari 41:731e3cfac19b 329
uadhikari 41:731e3cfac19b 330
uadhikari 41:731e3cfac19b 331