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

@@ -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;

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[];

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;
 }