From 56432f04d3c5325a6dea21a6e90917d737d8fe34 Mon Sep 17 00:00:00 2001
From: floaledm <flomartel@gmail.com>
Date: Tue, 25 Oct 2016 20:50:02 -0500
Subject: [PATCH] AP_Frsky_Telem: fixed airspeed; now same as VFR_HUD

---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
index 0215237a1a..f7e89b0011 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
@@ -735,15 +735,14 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
 
     // if we can't get velocity then we use zero for vertical velocity
     _ahrs.get_velocity_NED(velNED);
-
     // vertical velocity in dm/s
     velandyaw = prep_number(roundf(velNED.z * 10), 2, 1);
     // horizontal velocity in dm/s (use airspeed if available, otherwise use groundspeed)
-    float airspeed;
-    if (_ahrs.airspeed_estimate_true(&airspeed)) {
-        velandyaw |= prep_number(roundf(airspeed * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
-    } else {
-        velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
+    float aspeed;
+    if (_ahrs.airspeed_estimate(&aspeed)) { // send airspeed estimate from ahrs if available
+        velandyaw |= prep_number(roundf(aspeed * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
+    } else { // otherwise send groundspeed estimate from ahrs
+        velandyaw |= prep_number(roundf(_ahrs.groundspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
     }
     // yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
     velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;