Browse Source

AP_NavEKF2: Fix bug in initial alignment calculation

The bug caused the initial roll angle to be incorrect if the vehicle was powered up when inverted, causing long alignment times.
mission-4.1.18
Paul Riseborough 9 years ago committed by Randy Mackay
parent
commit
38b3625ed8
  1. 2
      libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

2
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@ -298,7 +298,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) @@ -298,7 +298,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
pitch = asinf(initAccVec.x);
// calculate initial roll angle
roll = -asinf(initAccVec.y / cosf(pitch));
roll = atan2f(-initAccVec.y , -initAccVec.z);
}
// calculate initial roll and pitch orientation

Loading…
Cancel
Save