From 5c10d81f60f6ed2221c19978bfebf20aed27d9f4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 31 Jul 2017 19:58:19 +1000
Subject: [PATCH] Plane: always setup target airspeed

this sets up a target airspeed even when flying without an airspeed
sensor. This is needed for quadplanes without airspeed sensors where
we use synthetic airspeed during the transition in TECS
---
 ArduPlane/navigation.cpp | 14 +++++++++-----
 ArduPlane/sensors.cpp    |  6 +++++-
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index df0430dcb5..0dbcd16777 100644
--- a/ArduPlane/navigation.cpp
+++ b/ArduPlane/navigation.cpp
@@ -101,8 +101,11 @@ void Plane::navigate()
 
 void Plane::calc_airspeed_errors()
 {
-    float airspeed_measured_cm = airspeed.get_airspeed_cm();
-
+    float airspeed_measured = 0;
+    
+    // we use the airspeed estimate function not direct sensor as TECS
+    // may be using synthetic airspeed
+    ahrs.airspeed_estimate(&airspeed_measured);
 
     // FBW_B airspeed target
     if (control_mode == FLY_BY_WIRE_B || 
@@ -125,9 +128,10 @@ void Plane::calc_airspeed_errors()
     // but only when this is faster than the target airspeed commanded
     // above.
     if (control_mode >= FLY_BY_WIRE_B && (aparm.min_gndspeed_cm > 0)) {
-        int32_t min_gnd_target_airspeed = airspeed_measured_cm + groundspeed_undershoot;
-        if (min_gnd_target_airspeed > target_airspeed_cm)
+        int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
+        if (min_gnd_target_airspeed > target_airspeed_cm) {
             target_airspeed_cm = min_gnd_target_airspeed;
+        }
     }
 
     // Bump up the target airspeed based on throttle nudging
@@ -141,7 +145,7 @@ void Plane::calc_airspeed_errors()
 
     // use the TECS view of the target airspeed for reporting, to take
     // account of the landing speed
-    airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured_cm * 0.01f;
+    airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured;
 }
 
 void Plane::calc_gndspeed_undershoot()
diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp
index 42a8853f5e..9ea89e5bde 100644
--- a/ArduPlane/sensors.cpp
+++ b/ArduPlane/sensors.cpp
@@ -85,7 +85,6 @@ void Plane::read_airspeed(void)
         if (should_log(MASK_LOG_IMU)) {
             Log_Write_Airspeed();
         }
-        calc_airspeed_errors();
 
         // supply a new temperature to the barometer from the digital
         // airspeed sensor if we can
@@ -95,6 +94,11 @@ void Plane::read_airspeed(void)
         }
     }
 
+    // we calculate airspeed errors (and thus target_airspeed_cm) even
+    // when airspeed is disabled as TECS may be using synthetic
+    // airspeed for a quadplane transition
+    calc_airspeed_errors();
+    
     // update smoothed airspeed estimate
     float aspeed;
     if (ahrs.airspeed_estimate(&aspeed)) {