Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 42:160a37bdaa64
- Parent:
- 41:731e3cfac19b
--- a/SensorFusion.cpp Wed May 20 10:13:14 2015 +0000 +++ b/SensorFusion.cpp Wed May 20 17:20:27 2015 +0000 @@ -2,115 +2,85 @@ #define DEBUG "SensorFusion" #include "Logger.h" - #include "Utils.h" #define SIXAXIS - + SensorFusion::SensorFusion(I2C &i2c) : + magnetoMeterEnabled(false), delegate(&defaultDelegate), + gyroDelegateWithMagneto(this), + gyroDelegateWithoutMagneto(this), accel(i2c), gyro(i2c), magneto(i2c), - q(1, 0, 0, 0), // output quaternion - deltat(0.010), // sec - beta(0.3), // correction gain + q(1, 0, 0, 0), // output quaternion + deltat(0.010), // sec + beta(0.3), // correction gain fused(0, 0, 0) -{ -} - -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) +void SensorFusion::setDelegate(Delegate &d) { delegate = &d; } -void SensorFusion::startAccelerometer(){ - accel.powerOn(); - accel.start(); -}; - -void SensorFusion::startGyrometer(){ - gyro.powerOn(); - gyro.start(); -}; +void SensorFusion::enableMagnetometer(){ + magnetoMeterEnabled = true; + stop(); + start(); +} -bool SensorFusion::startMagnetometer(){ - magneto.powerOn(); - if (magneto.performSelfTest() == false){ - //Should it be left powered on - return false; - } - magneto.start(); - return true; - -}; - -void SensorFusion::stopAccelerometer(){ - accel.stop(); - accel.powerOff(); -}; - -void SensorFusion::stopGyrometer(){ - gyro.stop(); - gyro.powerOff(); -}; - -void SensorFusion::stopMagnetometer(){ - magneto.stop(); - magneto.powerOff(); -}; - -static float const deg_to_radian = 0.0174532925f; - -void SensorFusion::getMagnetometerCalibration(Vector3 &min, Vector3 &max) -{ - magneto.getCalibration(min, max); +void SensorFusion::disableMagnetometer(){ + magnetoMeterEnabled = false; + stop(); + start(); } -/* NineAxesSensor*/ - -NineAxesSensor::NineAxesSensor(I2C &i2c) : SensorFusion(i2c){} - -bool NineAxesSensor::start(){ - startAccelerometer(); +bool SensorFusion::start() +{ + //Reset quarternion q + q.w = 1.0; + q.v.x = 0.0; + q.v.y = 0.0; + q.v.z = 0.0; - bool magnetoMeterSelfTestResult = startMagnetometer(); - if( magnetoMeterSelfTestResult == false){ - return false; + accel.powerOn(); + accel.start(); + + if(magnetoMeterEnabled){ + magneto.powerOn(); + if (magneto.performSelfTest() == false) { + return false; + } + magneto.start(); + gyro.setDelegate(gyroDelegateWithMagneto); + }else{ + gyro.setDelegate(gyroDelegateWithoutMagneto); } //Since everything is synced to gyro interrupt, start it last - gyro.setDelegate(*this); - startGyrometer(); + gyro.powerOn(); + gyro.start(); + return true; -}; - -void NineAxesSensor::stop(){ - stopAccelerometer(); - stopMagnetometer(); - stopGyrometer(); } -void NineAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){ +void SensorFusion::stop() +{ + gyro.stop(); + gyro.powerOff(); + + if(magnetoMeterEnabled){ + magneto.stop(); + magneto.powerOff(); + } + + accel.stop(); + accel.powerOff(); +} + +static float const deg_to_radian = 0.0174532925f; + +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, @@ -164,11 +134,12 @@ const float _4bx = 2.0f * _2bx; const float _4bz = 2.0f * _2bz; - // Gradient decent algorithm corrective step + // Gradient decent algorithm corrective step 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); 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); 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); 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); + norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude norm = 1.0f/norm; s1 *= norm; @@ -189,41 +160,28 @@ q4 += qDot4 * deltat; norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion norm = 1.0f/norm; + q.w = q1 * norm; q.v.x = q2 * norm; q.v.y = q3 * 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); + q.v.z = q4 * norm; } -/* SixAxesSensor */ - -SixAxesSensor::SixAxesSensor(I2C &i2c) : SensorFusion(i2c){} +SensorFusion::NineAxisSensorFusion::NineAxisSensorFusion(SensorFusion* _ref) : senFuseRef(_ref){} -bool SixAxesSensor::start(){ - startAccelerometer(); - startGyrometer(); - gyro.setDelegate(*this); - return true; -}; - -void SixAxesSensor::stop(){ - stopAccelerometer(); - stopGyrometer(); +void SensorFusion::NineAxisSensorFusion::sensorUpdate(Vector3 gyro_degrees){ + Vector3 const gyro_reading = gyro_degrees * deg_to_radian; + Vector3 const accel_reading = senFuseRef->accel.read(); + Vector3 const magneto_reading = senFuseRef->magneto.read(); + + senFuseRef->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); + senFuseRef->delegate->sensorTick(senFuseRef->deltat, senFuseRef->q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, senFuseRef->q); } - -void SixAxesSensor::updateFilter(float ax, float ay, float az, float gx, float gy, float gz){ + +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, @@ -299,33 +257,21 @@ 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); + q.v.z = q3; } - - bool SensorFusion::start(){ - return false; - } - - void SensorFusion::stop(){ - - } - - void SensorFusion::sensorUpdate(Vector3 gyro_degrees){ - - } - - - - - + +SensorFusion::SixAxisSensorFusion::SixAxisSensorFusion(SensorFusion* _ref) : senFuseRef(_ref){}; + +void SensorFusion::SixAxisSensorFusion::sensorUpdate(Vector3 gyro_degrees){ + Vector3 const gyro_reading = gyro_degrees * deg_to_radian; + Vector3 const accel_reading = senFuseRef->accel.read(); + Vector3 const magneto_reading(0, 0, 0); + + senFuseRef->updateFilter( accel_reading.x, accel_reading.y, accel_reading.z, + gyro_reading.x, gyro_reading.y, gyro_reading.z); + //parent's delegate + senFuseRef->delegate->sensorTick(senFuseRef->deltat, senFuseRef->q.getEulerAngles(), + accel_reading, magneto_reading, gyro_degrees, senFuseRef->q); +} + + \ No newline at end of file