Browse Source

AP_NavEKF: use radians values from AHRS directly

master
Andrew Tridgell 11 years ago
parent
commit
4771a4fc77
  1. 6
      libraries/AP_NavEKF/AP_NavEKF.cpp

6
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -60,9 +60,9 @@ void NavEKF::InitialiseFilter(void) @@ -60,9 +60,9 @@ void NavEKF::InitialiseFilter(void)
{
// Calculate initial filter quaternion states from ahrs solution
Quaternion initQuat;
ahrsEul[0] = _ahrs.roll_sensor*0.01f*DEG_TO_RAD;
ahrsEul[1] = _ahrs.pitch_sensor*0.01f*DEG_TO_RAD;
ahrsEul[2] = _ahrs.yaw_sensor*0.01f*DEG_TO_RAD;
ahrsEul[0] = _ahrs.roll;
ahrsEul[1] = _ahrs.pitch;
ahrsEul[2] = _ahrs.yaw;
eul2quat(initQuat, ahrsEul);
// Calculate initial Tbn matrix and rotate Mag measurements into NED

Loading…
Cancel
Save