AP_AHRS: added true airspeed support in AHRS

use true airspeed for wind calculations, and allow other drivers to
ask for the current ratio

libraries/AP_AHRS/AP_AHRS.cpp

@@ -109,9 +109,11 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
                     // constrain the airspeed by the ground speed
                     // and AHRS_WIND_MAX
                     float gnd_speed = _gps->ground_speed_cm*0.01f;
-                    *airspeed_ret = constrain_float(*airspeed_ret, 
+                    float true_airspeed = *airspeed_ret * get_EAS2TAS();
+                    true_airspeed = constrain_float(true_airspeed,
                                                     gnd_speed - _wind_max, 
                                                     gnd_speed + _wind_max);
+                    *airspeed_ret = true_airspeed / get_EAS2TAS();
 		}
 		return true;
 	}
@@ -145,27 +147,6 @@ 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() || !_flags.wind_estimation) {
-		// we are not using the compass - no wind correction,
-		// as GPS gives course over ground already
-		return;
-	}
-
-	// if we are using a compass for navigation, then adjust the
-	// heading to account for wind	
-	Vector3f wind = wind_estimate();
-	Vector2f wind2d = Vector2f(wind.x, wind.y);
-	float speed;
-	if (airspeed_estimate(&speed)) {
-		Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
-		Vector2f nav_adjusted = nav_vector - wind2d;
-		nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
-	}
-}
-
 // return a ground speed estimate in m/s
 Vector2f AP_AHRS::groundspeed_vector(void)
 {
@@ -173,7 +154,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     Vector2f gndVelADS;
     Vector2f gndVelGPS;
     float airspeed;
-    bool gotAirspeed = airspeed_estimate(&airspeed);
+    bool gotAirspeed = airspeed_estimate_true(&airspeed);
     bool gotGPS = (_gps && _gps->status() >= GPS::GPS_OK_FIX_2D);
     if (gotAirspeed) {
 	    Vector3f wind = wind_estimate();

libraries/AP_AHRS/AP_AHRS.h

@@ -150,6 +150,24 @@ public:
     // if we have an estimate
     virtual bool airspeed_estimate(float *airspeed_ret);
 
+    // return a true airspeed estimate (navigation airspeed) if
+    // available. return true if we have an estimate
+    bool airspeed_estimate_true(float *airspeed_ret) {
+        if (!airspeed_estimate(airspeed_ret)) {
+            return false;
+        }
+        *airspeed_ret *= get_EAS2TAS();
+        return true;
+    }
+
+    // get apparent to true airspeed ratio
+    float get_EAS2TAS(void) const {
+        if (_airspeed) {
+            return _airspeed->get_EAS2TAS();
+        }
+        return 1.0f;
+    }
+
     // return true if airspeed comes from an airspeed sensor, as
     // opposed to an IMU estimate
     bool airspeed_sensor_enabled(void) const {
@@ -162,9 +180,6 @@ public:
     // return true if we will use compass for yaw
     virtual bool use_compass(void) const { return _compass && _compass->use_for_yaw(); }
 
-    // correct a bearing in centi-degrees for wind
-    void wind_correct_bearing(int32_t &nav_bearing_cd);
-
     // return true if yaw has been initialised
     bool yaw_initialised(void) const {
         return _flags.have_initial_yaw;
@@ -190,21 +205,22 @@ public:
     // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
     virtual void            add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
 
-    // settable parameters
-    AP_Float beta;
+    // for holding parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+    // these are public for ArduCopter
 	AP_Float _kp_yaw;
     AP_Float _kp;
     AP_Float gps_gain;
+
+protected:
+    // settable parameters
+    AP_Float beta;
     AP_Int8 _gps_use;
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
 
-    // for holding parameters
-    static const struct AP_Param::GroupInfo var_info[];
-
-protected:
-
     // flags structure
     struct ahrs_flags {
         uint8_t have_initial_yaw        : 1;    // whether the yaw value has been intialised with a reference

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -711,7 +711,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
     } else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) {
         // when flying straight use airspeed to get wind estimate if available
         Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
-        Vector3f wind = velocity - airspeed;
+        Vector3f wind = velocity - (airspeed * get_EAS2TAS());
         _wind = _wind * 0.92f + wind * 0.08f;
     }    
 }
@@ -799,9 +799,11 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
 		// constrain the airspeed by the ground speed
 		// and AHRS_WIND_MAX
         float gnd_speed = _gps->ground_speed_cm*0.01f;
-		*airspeed_ret = constrain_float(*airspeed_ret, 
+        float true_airspeed = *airspeed_ret * get_EAS2TAS();
+		true_airspeed = constrain_float(true_airspeed,
                                         gnd_speed - _wind_max, 
                                         gnd_speed + _wind_max);
+        *airspeed_ret = true_airspeed / get_EAS2TAS();
 	}
 	return ret;
 }