Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS_NavEKF: use gyro drift states from EKF in get_gyro

Browse Source
This commit is contained in:
Jonathan Challinger 2014-04-15 13:21:12 -07:00 committed by Andrew Tridgell
parent 61987f6655
commit 966d66ef40
1 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -28,7 +28,29 @@ extern const AP_HAL::HAL& hal;
// return the smoothed gyro vector corrected for drift
const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
{
if (!using_EKF()) {
return AP_AHRS_DCM::get_gyro();
}
Vector3f angRate;
// average the available gyro sensors
angRate.zero();
uint8_t gyro_count = 0;
for (uint8_t i = 0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
angRate += _ins.get_gyro(i);
gyro_count++;
}
}
if (gyro_count != 0) {
angRate /= gyro_count;
}
Vector3f gyrobias;
EKF.getGyroBias(gyrobias);
return angRate+gyrobias;
}
const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const