Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: only use the active accel from DCM if fly_forward is set

Browse Source
This commit is contained in:
Andrew Tridgell 2014-02-27 17:40:13 +11:00
parent aaaae9a222
commit 736201689b
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -408,7 +408,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
Vector3f initMagXYZ;
// we should average readings over several calls to this function
initAccVec = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
initAccVec = _ahrs->get_ins().get_accel();
initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
// Normalise the acceleration vector
@ -2469,7 +2469,17 @@ void NavEKF::readIMUData()
// Limit IMU delta time to prevent numerical problems elsewhere
dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(), 0.001f, 1.0f);
angRate = _ahrs->get_ins().get_gyro();
accel = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
// we use get_fly_forward() to detect if this is a copter. If it
// is a copter then we need to use the same accel all the time to
// prevent small altitude jumps. If it is a plane or rover then we
// are better off using the accel that DCM has set as active to
// cope with larger aliasing effects
if (_ahrs->get_fly_forward()) {
accel = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
} else {
accel = _ahrs->get_ins().get_accel();
}
// trapezoidal integration
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;