AHRS: added airspeed_estimate() function

this allows the APM code to use an airspeed estimate for navigation

libraries/AP_AHRS/AP_AHRS.cpp

@@ -43,3 +43,26 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 
     AP_GROUPEND
 };
+
+// get pitch rate in earth frame, in radians/s
+float AP_AHRS::get_pitch_rate_earth(void) 
+{
+	Vector3f omega = get_gyro();
+	return cos(roll) * omega.y - sin(roll) * omega.z;
+}
+
+// get roll rate in earth frame, in radians/s
+float AP_AHRS::get_roll_rate_earth(void)  {
+	Vector3f omega = get_gyro();
+	return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll));
+}
+
+// return airspeed estimate if available
+bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
+{
+	if (_airspeed && _airspeed->use()) {
+		*airspeed_ret = _airspeed->get_airspeed();
+		return true;
+	}
+	return false;
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -75,14 +75,9 @@ public:
     int32_t pitch_sensor;
     int32_t yaw_sensor;
 
-    float get_pitch_rate_earth(void) {
+    // roll and pitch rates in earth frame, in radians/s
-	    Vector3f omega = get_gyro();
+    float get_pitch_rate_earth(void);
-	    return cos(roll) * omega.y - sin(roll) * omega.z;
+    float get_roll_rate_earth(void);
-    }
-    float get_roll_rate_earth(void)  {
-	    Vector3f omega = get_gyro();
-	    return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll));
-    }
 
     // return a smoothed and corrected gyro vector
     virtual Vector3f get_gyro(void) = 0;
@@ -129,6 +124,10 @@ public:
         return Vector3f(0,0,0);
     }
 
+    // return an airspeed estimate if available. return true
+    // if we have an estimate
+    bool airspeed_estimate(float *airspeed_ret);
+
     // return true if yaw has been initialised
     bool yaw_initialised(void) {
         return _have_initial_yaw;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -696,3 +696,19 @@ bool AP_AHRS_DCM::get_position(struct Location *loc)
     return true;
 }
 
+// return an airspeed estimate if available
+bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
+{
+	if (_airspeed && _airspeed->use()) {
+		*airspeed_ret = _airspeed->get_airspeed();
+		return true;
+	}
+
+	// estimate it via GPS speed and wind
+	if (have_gps()) {
+		*airspeed_ret = _last_airspeed;
+		return true;
+	}
+
+	return false;
+}

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -53,6 +53,10 @@ public:
         return _wind;
     }
 
+    // return an airspeed estimate if available. return true
+    // if we have an estimate
+    bool airspeed_estimate(float *airspeed_ret);
+
 private:
     float _ki;
     float _ki_yaw;