![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
Class of MPU9250
Dependencies: AHRS_fillter mbed
Fork of MPU9250AHRS by
Diff: AHRS.cpp
- Revision:
- 7:3e74239e3778
- Parent:
- 6:5665d427bceb
- Child:
- 8:928673148b55
--- a/AHRS.cpp Wed Jan 20 02:34:07 2016 +0000 +++ b/AHRS.cpp Wed Jan 20 02:36:32 2016 +0000 @@ -329,25 +329,6 @@ q[2] = q3 * norm; q[3] = q4 * norm; - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - - float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch); - float Yh = my*cos(roll)+mz*sin(roll); - - float yawmag = atan2(Yh,Xh)+PI; - //////test.printf("Xh= %f Yh= %f ",Xh,Yh); - //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI); - //test.printf(",%f",yawmag*180.0f/PI); - - - - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; - } @@ -439,24 +420,6 @@ q[2] = q3 * norm; q[3] = q4 * norm; - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - - Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch); - Yh = my*cos(roll)+mz*sin(roll); - - yawmag = atan2(Yh,Xh)+PI; - ////////////test.printf("Xh= %f Yh= %f ",Xh,Yh); - //////test.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI); - //test.printf(",%f",yawmag*180.0f/PI); - - - - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - yaw += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 - roll *= 180.0f / PI; } void AHRS::TimeStart()