Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 41:731e3cfac19b
- Parent:
- 39:1fa9c0e1ffde
- Child:
- 42:160a37bdaa64
--- a/SensorFusion.cpp Tue May 19 14:18:30 2015 +0000 +++ b/SensorFusion.cpp Wed May 20 10:13:14 2015 +0000 @@ -16,155 +16,105 @@ { } +SensorFusion::SensorFusion(const SensorFusion& s) : + delegate(s.delegate), + accel(s.accel), + gyro(s.gyro), + magneto(s.magneto), + q(s.q), + deltat(s.deltat), + beta(s.beta), + fused(s.fused) +{} + +SensorFusion::~SensorFusion(){}; + + +SixAxesSensor::~SixAxesSensor(){}; + +NineAxesSensor::~NineAxesSensor(){}; + +NineAxesSensor::NineAxesSensor(const NineAxesSensor& c) : SensorFusion(c){}; + +SixAxesSensor::SixAxesSensor(const SixAxesSensor& c) : SensorFusion(c){}; + void SensorFusion::setDelegate(SensorFusion::Delegate &d) { delegate = &d; } -bool SensorFusion::start() -{ +void SensorFusion::startAccelerometer(){ accel.powerOn(); - accel.start(); - - #ifdef NINEAXIS + accel.start(); +}; + +void SensorFusion::startGyrometer(){ + gyro.powerOn(); + gyro.start(); +}; + +bool SensorFusion::startMagnetometer(){ magneto.powerOn(); - if (magneto.performSelfTest() == false) { + if (magneto.performSelfTest() == false){ + //Should it be left powered on return false; } - magneto.start(); - #endif + magneto.start(); + return true; + +}; - // Since everything is synced to gyro interrupt, start it last - gyro.setDelegate(*this); - gyro.powerOn(); - gyro.start(); - - return true; -} +void SensorFusion::stopAccelerometer(){ + accel.stop(); + accel.powerOff(); +}; -void SensorFusion::stop() -{ +void SensorFusion::stopGyrometer(){ gyro.stop(); - #ifdef NINEAXIS + gyro.powerOff(); +}; + +void SensorFusion::stopMagnetometer(){ magneto.stop(); - #endif - accel.stop(); - - gyro.powerOff(); - #ifdef NINEAXIS - magneto.powerOff(); - #endif - accel.powerOff(); -} + magneto.powerOff(); +}; static float const deg_to_radian = 0.0174532925f; -void SensorFusion::sensorUpdate(Vector3 gyro_degrees) -{ - - Vector3 const gyro_reading = gyro_degrees * deg_to_radian; - - Vector3 const accel_reading = accel.read(); -#ifdef NINEAXIS - Vector3 const magneto_reading = magneto.read(); - updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, - gyro_reading.x, gyro_reading.y, gyro_reading.z, - magneto_reading.x, magneto_reading.y, magneto_reading.z); -#else - Vector3 const magneto_reading(0, 0, 0); - updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, - gyro_reading.x, gyro_reading.y, gyro_reading.z); -#endif - - delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); -} - void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max) { magneto.getCalibration(min, max); } -// 6 axis version -void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz) -{ - float q0 = q.w, q1 = q.v.x, q2 = q.v.y, q3 = q.v.z; // short name local variable for readability - - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; +/* NineAxesSensor*/ - // Rate of change of quaternion from gyroscope - qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { - - // Normalise accelerometer measurement - recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; +NineAxesSensor::NineAxesSensor(I2C &i2c) : SensorFusion(i2c){} - // Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0 * q0; - _2q1 = 2.0 * q1; - _2q2 = 2.0 * q2; - _2q3 = 2.0 * q3; - _4q0 = 4.0 * q0; - _4q1 = 4.0 * q1; - _4q2 = 4.0 * q2; - _8q1 = 8.0 * q1; - _8q2 = 8.0 * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - // Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay; - recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; +bool NineAxesSensor::start(){ + startAccelerometer(); + + bool magnetoMeterSelfTestResult = startMagnetometer(); + if( magnetoMeterSelfTestResult == false){ + return false; + } + + //Since everything is synced to gyro interrupt, start it last + gyro.setDelegate(*this); + startGyrometer(); + return true; +}; - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * deltat; - q1 += qDot2 * deltat; - q2 += qDot3 * deltat; - q3 += qDot4 * deltat; - - // Normalise quaternion - recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - // return - q.w = q0; - q.v.x = q1; - q.v.y = q2; - q.v.z = q3; +void NineAxesSensor::stop(){ + stopAccelerometer(); + stopMagnetometer(); + stopGyrometer(); } -void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) -{ - float q1 = q.w, q2 = q.v.x, q3 = q.v.y, q4 = q.v.z; // short name local variable for readability +void NineAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){ + float q1 = q.w, + q2 = q.v.x, + q3 = q.v.y, + q4 = q.v.z; // short name local variable for readability float norm; float s1, s2, s3, s4; @@ -242,5 +192,140 @@ q.w = q1 * norm; q.v.x = q2 * norm; q.v.y = q3 * norm; - q.v.z = q4 * norm; + q.v.z = q4 * norm; +} + +void NineAxesSensor::sensorUpdate(Vector3 gyro_degrees){ + Vector3 const gyro_reading = gyro_degrees * deg_to_radian; + Vector3 const accel_reading = accel.read(); + Vector3 const magneto_reading = magneto.read(); + + updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, + gyro_reading.x, gyro_reading.y, gyro_reading.z, + magneto_reading.x, magneto_reading.y, magneto_reading.z); + + delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); +} + +/* SixAxesSensor */ + +SixAxesSensor::SixAxesSensor(I2C &i2c) : SensorFusion(i2c){} + +bool SixAxesSensor::start(){ + startAccelerometer(); + startGyrometer(); + gyro.setDelegate(*this); + return true; +}; + +void SixAxesSensor::stop(){ + stopAccelerometer(); + stopGyrometer(); } + +void SixAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz){ + float q0 = q.w, + q1 = q.v.x, + q2 = q.v.y, + q3 = q.v.z; // short name local variable for readability + + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { + + // Normalise accelerometer measurement + recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0 * q0; + _2q1 = 2.0 * q1; + _2q2 = 2.0 * q2; + _2q3 = 2.0 * q3; + _4q0 = 4.0 * q0; + _4q1 = 4.0 * q1; + _4q2 = 4.0 * q2; + _8q1 = 8.0 * q1; + _8q2 = 8.0 * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay; + recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * deltat; + q1 += qDot2 * deltat; + q2 += qDot3 * deltat; + q3 += qDot4 * deltat; + + // Normalise quaternion + recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + // return + q.w = q0; + q.v.x = q1; + q.v.y = q2; + q.v.z = q3; +} + +void SixAxesSensor::sensorUpdate(Vector3 gyro_degrees){ + Vector3 const gyro_reading = gyro_degrees * deg_to_radian; + Vector3 const accel_reading = accel.read(); + Vector3 const magneto_reading(0, 0, 0); + + updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, + gyro_reading.x, gyro_reading.y, gyro_reading.z); + + delegate->sensorTick(deltat, q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); +} + + bool SensorFusion::start(){ + return false; + } + + void SensorFusion::stop(){ + + } + + void SensorFusion::sensorUpdate(Vector3 gyro_degrees){ + + } + + + + +