From 3026ccee13006cb99357c180608ba9b9e7b9ef88 Mon Sep 17 00:00:00 2001 From: priseborough Date: Tue, 29 Apr 2014 12:59:19 +1000 Subject: [PATCH] AP_NavEKF: Use baro data during launch transients whilst in static mode Baro data can be used as it will still be valid during the launch and is not a synthesised measurement. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 004b6722f8..6ef7504cee 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -686,17 +686,16 @@ void NavEKF::SelectVelPosFusion() } } else { - // in static mode we only fuse position and height to improve long term numerical stability - // and only when the rate of change of velocity is less than 0.5g. This prevents attitude errors - // due to launch acceleration. position is assumed to be zero. - fuseVelData = false; + // in static mode use synthetic position measurements set to zero + // only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration + // do not use velocity fusion to reduce the effect of movement on attitude if (accNavMag < 4.9f) { fusePosData = true; - fuseHgtData = true; } else { fusePosData = false; - fuseHgtData = false; } + fuseVelData = false; + fuseHgtData = true; } // check for and read new height data