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

AP_NavEKF: don't assume the number of gyros == number of accels

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-02 13:38:35 +11:00
parent ebb3cc3348
commit 77f91e6250
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2559,12 +2559,24 @@ void NavEKF::readIMUData()
if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) { if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) {
accel1 = _ahrs->get_ins().get_accel(0); accel1 = _ahrs->get_ins().get_accel(0);
accel2 = _ahrs->get_ins().get_accel(1); accel2 = _ahrs->get_ins().get_accel(1);
angRate = (_ahrs->get_ins().get_gyro(0) + _ahrs->get_ins().get_gyro(1)) * 0.5f;
} else { } else {
accel1 = _ahrs->get_ins().get_accel(); accel1 = _ahrs->get_ins().get_accel();
accel2 = accel1; accel2 = accel1;
angRate = _ahrs->get_ins().get_gyro();
} }
// average the available gyro sensors
angRate.zero();
uint8_t gyro_count = 0;
for (uint8_t i = 0; i<_ahrs->get_ins().get_gyro_count(); i++) {
if (_ahrs->get_ins().get_gyro_health(i)) {
angRate += _ahrs->get_ins().get_gyro(i);
gyro_count++;
}
}
if (gyro_count != 0) {
angRate /= gyro_count;
}
// trapezoidal integration // trapezoidal integration
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f; dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;
lastAngRate = angRate; lastAngRate = angRate;