Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 26:8f3e4e1a3acc
- Parent:
- 25:abb0f208e6a9
- Child:
- 31:d65576185bdf
--- a/SensorFusion.cpp Fri Apr 17 12:08:00 2015 +0000 +++ b/SensorFusion.cpp Tue Apr 21 12:27:24 2015 +0000 @@ -50,20 +50,6 @@ static float const deg_to_radian = 0.0174532925f; static float const radian_to_deg = 57.2957795131f; - -Vector3 SensorFusion::eulerAngles(Quaternion const &q) const { - float const q0 = q.w; - float const q1 = q.v.x; - float const q2 = q.v.y; - float const q3 = q.v.z; - - float const roll = asin(2*(q0*q2-q3*q1)); - float const pitch = atan2(2*(q0*q1+q2*q3), 1 - 2*(q1*q1+q2*q2)); - float const yaw = atan2(2*(q0*q3+q1*q2), 1 - 2*(q2*q2+q3*q3)); - - return Vector3(pitch, roll, yaw); -} - void SensorFusion::sensorUpdate(Vector3 gyro_degrees) { Vector3 const gyro_reading = gyro_degrees * deg_to_radian; @@ -74,7 +60,7 @@ gyro_reading.x, gyro_reading.y, gyro_reading.z, magneto_reading.x, magneto_reading.y, magneto_reading.z); - Vector3 const fused = eulerAngles(q); + Vector3 const fused(q.getEulerAngles()); delegate->sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q); }