From de28cc8b28024fa0bd42c7d1e2c12c2ebc92a587 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Sep 2012 11:27:12 +1000 Subject: [PATCH] AHRS: added AHRS_WIND_MAX option this allows APM to cope better with airspeed sensor failure, but ensuring airspeed stays within AHRS_WIND_MAX of ground speed --- libraries/AP_AHRS/AP_AHRS.cpp | 15 +++++++++++++++ libraries/AP_AHRS/AP_AHRS.h | 1 + libraries/AP_AHRS/AP_AHRS_DCM.cpp | 17 ++++++++++++----- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 91ed8675c3..bae347e954 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -41,6 +41,14 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // @Increment: .01 AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.4), + // @Param: WIND_MAX + // @DisplayName: Maximum wind + // @Description: This sets the maximum allowable difference between ground speed and airspeed. This allows the plane to cope with a failing airspeed sensor. A value of zero means to use the airspeed as is. + // @Range: 0 127 + // QUnits: m/s + // @Increment: 1 + AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0), + AP_GROUPEND }; @@ -62,6 +70,13 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) { if (_airspeed && _airspeed->use()) { *airspeed_ret = _airspeed->get_airspeed(); + if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { + // constrain the airspeed by the ground speed + // and AHRS_WIND_MAX + *airspeed_ret = constrain(*airspeed_ret, + _gps->ground_speed*0.01 - _wind_max, + _gps->ground_speed*0.01 + _wind_max); + } return true; } return false; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 5a44430f6a..a0e0a61e31 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -143,6 +143,7 @@ public: AP_Float _kp; AP_Float gps_gain; AP_Int8 _gps_use; + AP_Int8 _wind_max; // for holding parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 03c2f0bda5..c01295e7be 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -632,8 +632,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed(); Vector3f wind = velocity - airspeed; _wind = _wind * 0.92 + wind * 0.08; - } - + } } @@ -699,16 +698,24 @@ bool AP_AHRS_DCM::get_position(struct Location *loc) // return an airspeed estimate if available bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) { + bool ret = false; if (_airspeed && _airspeed->use()) { *airspeed_ret = _airspeed->get_airspeed(); - return true; + ret = true; } // estimate it via GPS speed and wind if (have_gps()) { *airspeed_ret = _last_airspeed; - return true; + ret = true; } - return false; + if (ret && _wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { + // constrain the airspeed by the ground speed + // and AHRS_WIND_MAX + *airspeed_ret = constrain(*airspeed_ret, + _gps->ground_speed*0.01 - _wind_max, + _gps->ground_speed*0.01 + _wind_max); + } + return ret; }