From 2fab15dcd57f3a075d3c6acc87146b01f0c5f9b4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 5 Oct 2016 11:52:34 +1100
Subject: [PATCH] AP_TECS: added use_synthetic_airspeed() API

used by quadplane during transitions
---
 libraries/AP_TECS/AP_TECS.cpp | 11 ++++++++---
 libraries/AP_TECS/AP_TECS.h   |  8 ++++++++
 2 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index 93b8d43c8f..c62f5ff4bc 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -366,7 +366,7 @@ void AP_TECS::_update_speed(float load_factor)
 
     // Get airspeed or default to halfway between min and max if
     // airspeed is not being used and set speed rate to zero
-    if (!_ahrs.airspeed_sensor_enabled() || !_ahrs.airspeed_estimate(&_EAS)) {
+    if (!(_use_synthetic_airspeed || _ahrs.airspeed_sensor_enabled()) || !_ahrs.airspeed_estimate(&_EAS)) {
         // If no airspeed available use average of min and max
         _EAS = 0.5f * (aparm.airspeed_min.get() + (float)aparm.airspeed_max.get());
     }
@@ -1034,9 +1034,14 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
     // Calculate specific energy quantitiues
     _update_energies();
 
-    // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
-    if (_ahrs.airspeed_sensor_enabled()) {
+    // Calculate throttle demand - use simple pitch to throttle if no
+    // airspeed sensor.
+    // Note that caller can demand the use of
+    // synthetic airspeed for one loop if needed. This is required
+    // during QuadPlane transition when pitch is constrained
+    if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed) {
         _update_throttle_with_airspeed();
+        _use_synthetic_airspeed = false;
     } else {
         _update_throttle_without_airspeed(throttle_nudge);
     }
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 673499ab5d..929d33a6a7 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -103,6 +103,11 @@ public:
     void set_pitch_max_limit(int8_t pitch_limit) {
         _pitch_max_limit = pitch_limit;
     }
+
+    // force use of synthetic airspeed for one loop
+    void use_synthetic_airspeed(void) {
+        _use_synthetic_airspeed = true;
+    }
     
     // this supports the TECS_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
@@ -305,6 +310,9 @@ private:
         float SKE_error;
         float SEB_delta;
     } logging;
+
+    // use synthetic airspeed for next loop
+    bool _use_synthetic_airspeed;
     
     // Update the airspeed internal state using a second order complementary filter
     void _update_speed(float load_factor);