From 5f941655a83e4e943f68953e6ee09b2ee876dca8 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Nov 2014 21:58:11 +1100 Subject: [PATCH] AP_NavEKF: changes to support to 20Hz flow fusion rate --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 85b6bd2959..9635a91c1c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -331,7 +331,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @Range: 0 - 500 // @Increment: 10 // @User: advanced - AP_GROUPINFO("FLOW_DELAY", 28, NavEKF, _msecFLowDelay, 20), + AP_GROUPINFO("FLOW_DELAY", 28, NavEKF, _msecFLowDelay, 10), // @Param: RNG_GATE // @DisplayName: Range finder measurement gate size @@ -397,7 +397,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _msecMagAvg = 100; // average number of msec between magnetometer measurements _msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements _msecBetaMax = 200; // maximum number of msec between synthetic sideslip measurements - _msecFlowAvg = 100; // average number of msec between optical flow measurements + _msecFlowAvg = 50; // average number of msec between optical flow measurements dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. // Misc initial conditions