Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 31:d65576185bdf
- Parent:
- 26:8f3e4e1a3acc
- Child:
- 34:01dec68de3ed
diff -r a56c141d1d38 -r d65576185bdf SensorFusion.cpp --- a/SensorFusion.cpp Thu Apr 23 12:13:28 2015 +0000 +++ b/SensorFusion.cpp Tue Apr 28 09:08:03 2015 +0000 @@ -48,11 +48,12 @@ } static float const deg_to_radian = 0.0174532925f; -static float const radian_to_deg = 57.2957795131f; void SensorFusion::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(); @@ -60,9 +61,7 @@ gyro_reading.x, gyro_reading.y, gyro_reading.z, magneto_reading.x, magneto_reading.y, magneto_reading.z); - Vector3 const fused(q.getEulerAngles()); - - delegate->sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q); + delegate->sensorTick(q.getEulerAngles(), accel_reading, magneto_reading, gyro_degrees, q); } void SensorFusion::updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)