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

AP_NavEKF2: Don't switch magnetometers when on the ground or just started

Browse Source 
Large magnetometer innovations on the ground could be caused by factors that will disappear when flying, eg:

a) Bad initial gyro bias
b) External magnetic field disturbances (adjacent metal structures, placement of hatches with magnets, etc)

To avoid unnecessary switches, we inhibit switching until off-ground and when sufficient time has lapsed from power on to learn gyro bias offsets.
This commit is contained in:
Paul Riseborough 2015-11-09 13:50:27 +11:00 committed by Andrew Tridgell
parent 3099d94e78
commit 3d8e74df26
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -192,8 +192,10 @@ void NavEKF2_core::readMagData()
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (magTimeout && (maxCount > 1)) {
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;