From d37c7883940c55a98db473fcfcfa191f541f4464 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sun, 12 Oct 2014 22:17:38 +1100
Subject: [PATCH] AP_NavEKF: Track baro alt when pre-armed
This will help prevent spurious alt disparity warning messages for copter
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 33 +++++++++++++++----------------
1 file changed, 16 insertions(+), 17 deletions(-)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 26b303b1c..5fde16ab4 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -699,23 +699,6 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false;
}
- // check for and read new height data
- readHgtData();
-
- // command fusion of height data
- if (newDataHgt)
- {
- // reset data arrived flag
- newDataHgt = false;
- // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
- memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
- hgtUpdateCount = 0;
- // enable fusion
- fuseHgtData = true;
- } else {
- fuseHgtData = false;
- }
-
} else {
// 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
@@ -726,7 +709,23 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false;
}
fuseVelData = false;
+ }
+
+ // check for and read new height data
+ readHgtData();
+
+ // command fusion of height data
+ if (newDataHgt)
+ {
+ // reset data arrived flag
+ newDataHgt = false;
+ // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
+ memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
+ hgtUpdateCount = 0;
+ // enable fusion
fuseHgtData = true;
+ } else {
+ fuseHgtData = false;
}
// perform fusion
--
GitLab