zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_AHRS: don't use disabled gyro in rate controllers 

obey INS_USE* parameters in gyro estimate
master
Andrew Tridgell 9 years ago
parent
0e32c047c3
commit
664ce5c16e
1 changed files with 2 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save