From e3792f9b264d1e0641e2c0a903dc56d6ea2f76db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Mar 2014 12:30:17 +1100 Subject: [PATCH] AP_NavEKF: added a mechanism for per-vehicle-type defaults in EKF --- libraries/AP_NavEKF/AP_NavEKF.cpp | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 61bd800461..c8c15de7aa 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -15,9 +15,31 @@ #include "AP_NavEKF.h" #include #include +#include #include +/* + parameter defaults for different types of vehicle. The + APM_BUILD_DIRECTORY is taken from the main vehicle directory name + where the code is built. Note that this trick won't work for arduino + builds on APM2, but NavEKF doesn't run on APM2, so that's OK + */ +#if APM_BUILD_DIRECTORY == APM_BUILD_ArduCopter +// copter defaults +#define VELNE_NOISE_DEFAULT 0.30f + +#elif APM_BUILD_DIRECTORY == APM_BUILD_APMrover2 +// rover defaults +#define VELNE_NOISE_DEFAULT 0.30f + +#else +// generic defaults (and for plane) +#define VELNE_NOISE_DEFAULT 0.30f + +#endif // APM_BUILD_DIRECTORY + + extern const AP_HAL::HAL& hal; #define earthRate 0.000072921f // earth rotation rate (rad/sec) @@ -31,7 +53,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @Range: 0.05 - 5.0 // @Increment: 0.05 // @User: advanced - AP_GROUPINFO("VELNE_NOISE", 0, NavEKF, _gpsHorizVelNoise, 0.30f), + AP_GROUPINFO("VELNE_NOISE", 0, NavEKF, _gpsHorizVelNoise, VELNE_NOISE_DEFAULT), // @Param: VELD_NOISE // @DisplayName: GPS vertical velocity measurement noise (m/s)