From 1f78a73cdbb5c327875e3932ef660adeb4d81960 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Nov 2014 22:10:29 +1100 Subject: [PATCH] AP_NavEKF: Make flow measurement delay vehicle specific This takes into account the inter-sampling delay between the flow driver and the APM software which depends on the rate at which the dirver is being checked. This is 50Hz for plane and rover, and 200Hz for Copter. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 9635a91c1c..8f7014b46b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -45,6 +45,7 @@ #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 #define GLITCH_RADIUS_DEFAULT 15 +#define FLOW_MEAS_DELAY 10 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) // rover defaults @@ -66,6 +67,7 @@ #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 #define GLITCH_RADIUS_DEFAULT 15 +#define FLOW_MEAS_DELAY 25 #else // generic defaults (and for plane) @@ -87,6 +89,7 @@ #define MAG_CAL_DEFAULT 0 #define GLITCH_ACCEL_DEFAULT 150 #define GLITCH_RADIUS_DEFAULT 15 +#define FLOW_MEAS_DELAY 25 #endif // APM_BUILD_DIRECTORY @@ -331,7 +334,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @Range: 0 - 500 // @Increment: 10 // @User: advanced - AP_GROUPINFO("FLOW_DELAY", 28, NavEKF, _msecFLowDelay, 10), + AP_GROUPINFO("FLOW_DELAY", 28, NavEKF, _msecFLowDelay, FLOW_MEAS_DELAY), // @Param: RNG_GATE // @DisplayName: Range finder measurement gate size