From b47a11edf62646d68ba61faad93eacf204635079 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 10 Mar 2014 04:39:59 +1100 Subject: [PATCH] AP_NavEKF : improved static and on-ground mode selection logic --- libraries/AP_NavEKF/AP_NavEKF.cpp | 17 ++++++++--------- libraries/AP_NavEKF/AP_NavEKF.h | 1 + 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index eecb8f34cf..fa63f5cd5b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -541,14 +541,9 @@ void NavEKF::UpdateFilter() // define rules used to set staticMode // staticMode enables ground operation without GPS by fusing zeros for position and height measurements - // staticMode is always used when on-grond and magnetometer not used - // if exisiting staticMode without a compass, a forced yaw alignment is performed - if (onGround && (static_mode_demanded() || !use_compass())) { + if (static_mode_demanded()) { staticMode = true; } else { - if (!use_compass() && staticMode) { - ForceYawAlignment(); - } staticMode = false; } @@ -2598,14 +2593,18 @@ void NavEKF::OnGroundCheck() uint8_t lowAirSpd = (!airspeed || !airspeed->use() || airspeed->get_airspeed() * airspeed->get_EAS2TAS() < 8.0f); uint8_t lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f); uint8_t lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f)); - // inhibit onGround mode if magnetomter calibration at low speed and height is enabled. - // A static check is still applied to prevent excessive compass drift when sitting stationary - if ((_magCal == 1) && (accNavMagHoriz > 0.5f)) { + // inhibit onGround mode if magnetomter calibration is enabled, movement is detected and static mode isn't demanded + if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) { onGround = false; } else { // Go with a majority vote from three criteria onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2); + // force a yaw alignment if exiting onGround without a compass + if (!onGround && prevOnGround && !use_compass()) { + ForceYawAlignment(); + } } + prevOnGround = onGround; } void NavEKF::CovarianceInit(float roll, float pitch, float yaw) diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 75ad6e3614..33a86b2d58 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -360,6 +360,7 @@ private: ftype dt; // time lapsed since the last covariance prediction (sec) ftype hgtRate; // state for rate of change of height filter bool onGround; // boolean true when the flight vehicle is on the ground (not flying) + bool prevOnGround; // value of onGround from previous update Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements bool fuseVelData; // this boolean causes the velNED measurements to be fused