diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 75425d9905..ec1263d723 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -160,7 +160,7 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ // correct a bearing in centi-degrees for wind void AP_AHRS::wind_correct_bearing(int32_t &nav_bearing_cd) { - if (!use_compass()) { + if (!use_compass() || !_flags.wind_estimation) { // we are not using the compass - no wind correction, // as GPS gives course over ground already return; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 2e8c4c5818..cbbf6a5c9f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -53,6 +53,11 @@ public: void set_fly_forward(bool b) { _flags.fly_forward = b; } + + void set_wind_estimation(bool b) { + _flags.wind_estimation = b; + } + void set_compass(Compass *compass) { _compass = compass; if (_compass != NULL) { @@ -195,6 +200,7 @@ protected: uint8_t fast_ground_gains : 1; // should we raise the gain on the accelerometers for faster convergence, used when disarmed for ArduCopter uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed) + uint8_t wind_estimation : 1; // 1 if we should do wind estimation } _flags; // pointer to compass object, if available diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index cbcde47dc8..c9f085d633 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -638,6 +638,10 @@ AP_AHRS_DCM::drift_correction(float deltat) // update our wind speed estimate void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) { + if (!_flags.wind_estimation) { + return; + } + // this is based on the wind speed estimation code from MatrixPilot by // Bill Premerlani. Adaption for ArduPilot by Jon Challinger // See http://gentlenav.googlecode.com/files/WindEstimation.pdf @@ -761,6 +765,10 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) return true; } + if (!_flags.wind_estimation) { + return false; + } + // estimate it via GPS speed and wind if (have_gps()) { *airspeed_ret = _last_airspeed;