Iterm limiting for fixed wing (#574)

docs/Cli.md

@@ -255,6 +255,7 @@ Re-apply any new defaults as desired.
 | `nav_max_climb_rate`           | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s                                                                                                                             | 10      | 2000     | 500             | Master       | UINT16    |  
 | `flaperon_throw_offset`           | Defines throw range in us for both ailerons that will be applied (before scaling) when FLAPERON mode is activated. | 100      | 400     | 250             | Profile       | UINT16    |  
 | `flaperon_throw_inverted`           | Inverts throw offset on both ailerons. Can be used to create SPOILERON or just to change throw direction | OFF      | ON     | OFF             | Profile       | UINT8    |  
+| `fw_iterm_throw_limit`           | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us).   | 25      | 500     | 165             | Profile       | UINT16    |  
 
 ## New iNav spesific
 | `Variable`                                    | Description/Units                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | Min    | Max    | Default          | Type         | Datatype |

src/main/config/config.c

@@ -160,7 +160,7 @@ size_t custom_flash_memory_address = 0;
 #define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address)
 #else
 // use the last flash pages for storage
-#ifndef CONFIG_START_FLASH_ADDRESS 
+#ifndef CONFIG_START_FLASH_ADDRESS
 #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
 #endif
 #endif
@@ -231,6 +231,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
 
     pidProfile->max_angle_inclination[FD_ROLL] = 300;    // 30 degrees
     pidProfile->max_angle_inclination[FD_PITCH] = 300;    // 30 degrees
+
+    pidProfile->fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT;
+
 }
 
 #ifdef NAV

src/main/config/config_profile.h

@@ -44,5 +44,6 @@ typedef struct profile_s {
 
     uint16_t flaperon_throw_offset;
     uint8_t flaperon_throw_inverted;
+
 #endif
 } profile_t;

src/main/flight/pid.c

@@ -344,6 +344,12 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
         pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf);
     }
 
+#ifdef USE_SERVOS
+    if (STATE(FIXED_WING)) {
+        pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile->fixedWingItermThrowLimit, pidProfile->fixedWingItermThrowLimit);
+    }
+#endif
+
     axisPID[axis] = newOutputLimited;
 
 #ifdef BLACKBOX

src/main/flight/pid.h

@@ -27,6 +27,10 @@
 #define MAG_HOLD_RATE_LIMIT_MAX 250
 #define MAG_HOLD_RATE_LIMIT_DEFAULT 90
 
+#define FW_ITERM_THROW_LIMIT_DEFAULT 165
+#define FW_ITERM_THROW_LIMIT_MIN 25
+#define FW_ITERM_THROW_LIMIT_MAX 500
+
 #define AXIS_ACCEL_MIN_LIMIT    50
 
 typedef enum {
@@ -65,6 +69,10 @@ typedef struct pidProfile_s {
     int16_t max_angle_inclination[ANGLE_INDEX_COUNT];       // Max possible inclination (roll and pitch axis separately
 
     uint8_t mag_hold_rate_limit;            //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller
+
+#ifdef USE_SERVOS
+    uint16_t fixedWingItermThrowLimit;
+#endif
 } pidProfile_t;
 
 extern int16_t axisPID[];

src/main/io/serial_cli.c

@@ -757,6 +757,7 @@ const clivalue_t valueTable[] = {
     { "gimbal_mode",                VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 },
     { "servo_center_pulse",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX }, 0 },
     { "servo_pwm_rate",             VAR_UINT16 | MASTER_VALUE,  &masterConfig.servo_pwm_rate, .config.minmax = { 50,  498 }, 0 },
+    { "fw_iterm_throw_limit",       VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.fixedWingItermThrowLimit, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN,  FW_ITERM_THROW_LIMIT_MAX}, 0 },
 #endif
 
     { "mode_range_logic_operator",  VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP,  &masterConfig.profile[0].modeActivationOperator, .config.lookup = { TABLE_AUX_OPERATOR }, 0 },