Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 17:e9d42864c8a1
- Parent:
- 16:3e2468d4f4c1
- Child:
- 19:9e9753b87cfe
diff -r 3e2468d4f4c1 -r e9d42864c8a1 SensorFusion.cpp --- a/SensorFusion.cpp Fri Mar 20 10:30:01 2015 +0000 +++ b/SensorFusion.cpp Thu Mar 26 15:43:49 2015 +0000 @@ -43,6 +43,20 @@ 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) * radian_to_deg; +} + void SensorFusion::sensorUpdate(Vector3 gyro_degrees) { Vector3 const gyro_reading = gyro_degrees * deg_to_radian; @@ -53,7 +67,7 @@ gyro_reading.x, gyro_reading.y, gyro_reading.z, magneto_reading.x, magneto_reading.y, magneto_reading.z); - Vector3 const fused = q.getEulerAngles() * radian_to_deg; + Vector3 const fused = eulerAngles(q); sensorTick(fused, accel_reading, magneto_reading, gyro_degrees, q); }