TECS: added support for throttle nudging

libraries/AP_SpdHgtControl/AP_SpdHgtControl.h

@@ -41,7 +41,11 @@ public:
 
 	// Update of the pitch and throttle demands
 	// Should be called at 10Hz or faster
-	virtual void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) = 0;
+	virtual void update_pitch_throttle( int32_t hgt_dem_cm,
+										int32_t EAS_dem_cm,
+										bool climbOutDem,
+										int32_t ptchMinCO_cd,
+										int16_t throttle_nudge) = 0;
 
 	// demanded throttle in percentage
 	// should return 0 to 100

libraries/AP_TECS/AP_TECS.cpp

@@ -424,10 +424,10 @@ void AP_TECS::_update_throttle(void)
     _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
 }
 
-void AP_TECS::_update_throttle_option(void)
+void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
 {
 	// Calculate throttle demand by interpolating between pitch and throttle limits
-    float nomThr = aparm.throttle_cruise * 0.01f;	
+    float nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f;	
 	if (_climbOutDem)
 	{
 		_throttle_dem = _THRmaxf;
@@ -576,7 +576,11 @@ void AP_TECS::_update_STE_rate_lim(void)
     _STEdot_min = - _minSinkRate * GRAVITY_MSS;
 }
 
-void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd) 
+void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
+									int32_t EAS_dem_cm, 
+									bool climbOutDem, 
+									int32_t ptchMinCO_cd, 
+									int16_t throttle_nudge) 
 {
     // Calculate time in seconds since last update
     uint32_t now = hal.scheduler->micros();
@@ -617,7 +621,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool
 	if (_ahrs->airspeed_sensor_enabled()) {
         _update_throttle();
 	} else {
-        _update_throttle_option();
+        _update_throttle_option(throttle_nudge);
 	}
 
     // Detect bad descent due to demanded airspeed being too high

libraries/AP_TECS/AP_TECS.h

@@ -45,7 +45,11 @@ public:
 	void update_50hz(float hgt_afe);
 
 	// Update the control loop calculations
-    void update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, bool climbOutDem, int32_t ptchMinCO_cd);
+    void update_pitch_throttle(int32_t hgt_dem_cm, 
+                               int32_t EAS_dem_cm, 
+                               bool climbOutDem, 
+                               int32_t ptchMinCO_cd,
+                               int16_t throttle_nudge);
 
 	// demanded throttle in percentage
 	// should return 0 to 100
@@ -243,7 +247,7 @@ private:
 	void _update_throttle(void);
 
 	// Update Demanded Throttle Non-Airspeed
-	void _update_throttle_option(void);
+	void _update_throttle_option(int16_t throttle_nudge);
 
 	// Detect Bad Descent
 	void _detect_bad_descent(void);