AP_TECS: add TECS_LAND_I_GAIN and TECS_TKOFF_I_GAIN

// more integral gain options for land

libraries/AP_TECS/AP_TECS.cpp

@@ -210,6 +210,22 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("LAND_TDAMP", 23, AP_TECS, _land_throttle_damp, 0),
 
+    // @Param: LAND_IGAIN
+    // @DisplayName: Controller integrator during landing
+    // @Description: This is the integrator gain on the control loop during landing. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values lower than TECS_INTEG_GAIN work best
+    // @Range: 0.0 0.5
+    // @Increment: 0.02
+    // @User: Advanced
+    AP_GROUPINFO("LAND_IGAIN", 24, AP_TECS, _integGain_land, 0),
+
+    // @Param: TKOFF_IGAIN
+    // @DisplayName: Controller integrator during takeoff
+    // @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best
+    // @Range: 0.0 0.5
+    // @Increment: 0.02
+    // @User: Advanced
+    AP_GROUPINFO("TKOFF_IGAIN", 25, AP_TECS, _integGain_takeoff, 0),
+
     AP_GROUPEND
 };
 
@@ -623,7 +639,7 @@ void AP_TECS::_update_throttle(void)
 
         // Calculate integrator state, constraining state
         // Set integrator to a max throttle value during climbout
-        _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
+        _integ6_state = _integ6_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
         if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)
         {
             _integ6_state = integ_max;
@@ -646,6 +662,21 @@ void AP_TECS::_update_throttle(void)
     _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf);
 }
 
+float AP_TECS::_get_i_gain(void)
+{
+    float i_gain = _integGain;
+    if (_flight_stage == FLIGHT_TAKEOFF) {
+        if (!is_zero(_integGain_takeoff)) {
+            i_gain = _integGain_takeoff;
+        }
+    } else if (_is_doing_auto_land) {
+        if (!is_zero(_integGain_land)) {
+            i_gain = _integGain_land;
+        }
+    }
+    return i_gain;
+}
+
 void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
 {
     // Calculate throttle demand by interpolating between pitch and throttle limits
@@ -741,7 +772,7 @@ void AP_TECS::_update_pitch(void)
     float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
 
     // Calculate integrator state, constraining input if pitch limits are exceeded
-    float integ7_input = SEB_error * _integGain;
+    float integ7_input = SEB_error * _get_i_gain();
     if (_pitch_dem > _PITCHmaxf)
     {
         integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem);
@@ -885,6 +916,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
 
     _is_doing_auto_land = is_doing_auto_land;
     _distance_beyond_land_wp = distance_beyond_land_wp;
+    _flight_stage = flight_stage;
 
     // Convert inputs
     _hgt_dem = hgt_dem_cm * 0.01f;
@@ -955,7 +987,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
     // convert to radians
     _PITCHmaxf = radians(_PITCHmaxf);
     _PITCHminf = radians(_PITCHminf);
-    _flight_stage = flight_stage;
 
     // initialise selected states and variables if DT > 1 second or in climbout
     _initialise_states(ptchMinCO_cd, hgt_afe);

libraries/AP_TECS/AP_TECS.h

@@ -152,6 +152,8 @@ private:
     AP_Float _thrDamp;
     AP_Float _land_throttle_damp;
     AP_Float _integGain;
+    AP_Float _integGain_takeoff;
+    AP_Float _integGain_land;
     AP_Float _vertAccLim;
     AP_Float _rollComp;
     AP_Float _spdWeight;
@@ -318,6 +320,9 @@ private:
     // Update Demanded Throttle Non-Airspeed
     void _update_throttle_option(int16_t throttle_nudge);
 
+    // get integral gain which is flight_stage dependent
+    float _get_i_gain(void);
+
     // Detect Bad Descent
     void _detect_bad_descent(void);