AP_TECS : Reduce tendency to overspeed during climbout

The old code relies on the action of the integrator to raise the nose above the minimum pitch angle which depending on the model and the tuning can be too slow to correct. Biasing the pitch angle to climbout minimum will reduce the taken before an overspeed condition is corrected.

libraries/AP_TECS/AP_TECS.cpp

@@ -515,9 +515,16 @@ void AP_TECS::_update_pitch(void)
     // Apply max and min values for integrator state that will allow for no more than 
 	// 5deg of saturation. This allows for some pitch variation due to gusts before the 
 	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+    // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle 
+    // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
+    // integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * GRAVITY_MSS);
     float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
-	_integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
+    if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF)
+    {
+    	temp += _PITCHminf * gainInv;
+    }
+    _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
 
     // Calculate pitch demand from specific energy balance signals
     _pitch_dem_unc = (temp + _integ7_state) / gainInv;