Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 43:6251c0169f4f
- Parent:
- 40:8e852115fe55
- Child:
- 44:5483079fa156
- Child:
- 46:fd5a62296b12
diff -r 8e852115fe55 -r 6251c0169f4f SensorFusion.cpp --- a/SensorFusion.cpp Tue May 26 11:28:37 2015 +0000 +++ b/SensorFusion.cpp Wed May 27 11:45:00 2015 +0000 @@ -10,10 +10,10 @@ accel(i2c), gyro(i2c), deltat(0.010), // seconds - beta(1), - lowpassX(0.93), - lowpassY(0.93), - lowpassZ(0.93) + beta(50), + lowpassX(0.96), + lowpassY(0.96), + lowpassZ(0.96) { gyro.setDelegate(*this); } @@ -136,7 +136,43 @@ q.v.z = q3; } -/* +SensorFusion9::SensorFusion9(I2C &i2c) : SensorFusion6(i2c), magneto(i2c) +{ + gyro.setDelegate(*this); +} + +bool SensorFusion9::start() +{ + magneto.powerOn(); + magneto.start(); + + return SensorFusion6::start(); +} + +void SensorFusion9::stop() +{ + SensorFusion6::stop(); + magneto.stop(); + magneto.powerOff(); +} + +void SensorFusion9::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(); + + Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x), + lowpassY.filter(accel_reading.y), + lowpassZ.filter(accel_reading.z)); + + updateFilter( filtered_accel.x, filtered_accel.y, filtered_accel.z, + gyro_reading.x, gyro_reading.y, gyro_reading.z, + magneto_reading.x, magneto_reading.y, magneto_reading.z); + + delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, magneto_reading, gyro_degrees, q); +} + void SensorFusion9::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 @@ -219,4 +255,4 @@ q.v.y = q3 * norm; q.v.z = q4 * norm; } -*/ +