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: use the accelerometer chosen by DCM for each step

Browse Source
This commit is contained in:
Andrew Tridgell 2014-02-27 09:41:48 +11:00
parent b53496d470
commit a9e683dada
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
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();
initAccVec = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
// Normalise the acceleration vector
@ -2469,7 +2469,7 @@ 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();
accel = _ahrs->get_ins().get_accel(_ahrs->get_active_accel_instance());
// trapezoidal integration
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;