From cdbc5a3f35ca07b23515aafb78ac7a7d16d7154b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 5 Jan 2014 20:05:48 +1100 Subject: [PATCH] AP_NavEKF: added position observations to static mode --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index df1a576390..c9be64f5b4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1091,7 +1091,7 @@ void NavEKF::FuseVelPosNED() observation[5] = -hgtMea; // zero observations if in static mode (used for pre-arm and bench testing) if (staticMode) { - for (uint8_t i=0; i<=2; i++) observation[i] = 0.0f; + for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f; // cancel static mode (it needs to be set every frame if required) staticMode = false; }