From a694b781c7f58e2055125c30c2544971aa3a069a Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 5 Jul 2013 07:48:28 +1000
Subject: [PATCH] AP_TECS: Add non airspeed sensor pitch to throttle mode lost
 during integration

---
 libraries/AP_TECS/AP_TECS.cpp | 32 +++++++++++++++++++++++++++++---
 libraries/AP_TECS/AP_TECS.h   |  3 +++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index 4eb4e4c64b..621a199692 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -424,6 +424,28 @@ void AP_TECS::_update_throttle(void)
     _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
 }
 
+void AP_TECS::_update_throttle_option(void)
+{
+	// Calculate throttle demand by interpolating between pitch and throttle limits
+    float nomThr = aparm.throttle_cruise * 0.01f;	
+	if (_climbOutDem)
+	{
+		_throttle_dem = _THRmaxf;
+	}
+	else if (_pitch_dem > 0.0 && _PITCHmaxf > 0.0)
+	{
+		_throttle_dem = nomThr + (_THRmaxf - nomThr) * _pitch_dem / _PITCHmaxf;
+	}
+	else if (_pitch_dem < 0.0 && _PITCHminf < 0.0)
+	{
+		_throttle_dem = nomThr + (_THRminf - nomThr) * _pitch_dem / _PITCHminf;
+	}
+	else
+	{
+		_throttle_dem = nomThr;
+	}
+}
+
 void AP_TECS::_detect_bad_descent(void) 
 {
 	// Detect a demanded airspeed too high for the aircraft to achieve. This will be
@@ -591,10 +613,14 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool
     // Calculate specific energy quantitiues
     _update_energies();
 
-    // Calculate throttle demand
-    _update_throttle();
+    // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
+	if (_ahrs->airspeed_sensor_enabled()) {
+        _update_throttle();
+	} else {
+        _update_throttle_option();
+	}
 
-	// Detect bad descent due to demanded airspeed being too high
+    // Detect bad descent due to demanded airspeed being too high
 	_detect_bad_descent();
 
 	// Calculate pitch demand
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 03e44d967e..96a3745968 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -241,6 +241,9 @@ private:
 	// Update Demanded Throttle
 	void _update_throttle(void);
 
+	// Update Demanded Throttle Non-Airspeed
+	void _update_throttle_option(void);
+
 	// Detect Bad Descent
 	void _detect_bad_descent(void);