aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: if we have 3 gyros then only use first two

Browse Source 
the 3rd gyro on a PH2 has a lot more noise as it is not vibration
isolated
This commit is contained in:
Andrew Tridgell 2015-05-09 21:00:49 +10:00
parent f3d1242d60
commit 3529e02675
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -101,11 +101,13 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
// value
_omega.zero();
// average across all healthy gyros. This reduces noise on systems
// with more than one gyro
// average across first two healthy gyros. This reduces noise on
// systems with more than one gyro. We don't use the 3rd gyro
// unless another is unhealthy as 3rd gyro on PH2 has a lot more
// noise
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
if (_ins.get_gyro_health(i) && healthy_count < 2) {
_omega += _ins.get_gyro(i);
healthy_count++;
}

2
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -107,7 +107,7 @@ void AP_AHRS_NavEKF::update(void)
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i)) {
if (_ins.get_gyro_health(i) && healthy_count < 2) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}