Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.cpp@42:160a37bdaa64, 2015-05-20 (annotated)

Committer:
uadhikari
Date:
Wed May 20 17:20:27 2015 +0000
Revision:
42:160a37bdaa64
Parent:
41:731e3cfac19b 
added runtime enable/disable of  Magnetometer, some warnings persist

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