Add sticks restriction to continuous trim

docs/Settings.md

@@ -482,6 +482,16 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho
 
 ---
 
+### control_deadband
+
+Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 2 | 250 |
+
+---
+
 ### cpu_underclock
 
 This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz
@@ -4664,7 +4674,7 @@ Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER
 
 ### pos_hold_deadband
 
-Stick deadband in [r/c points], applied after r/c deadband and expo
+Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes.
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/fc/fc_core.c

@@ -216,7 +216,7 @@ static void updateArmingStatus(void)
 
 	/* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
 	if (isNavLaunchEnabled()) {
-	  if (areSticksDeflectedMoreThanPosHoldDeadband()) {
+	  if (areSticksDeflected()) {
 	    ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
 	  } else {
 	    DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);

src/main/fc/rc_controls.c

@@ -71,12 +71,13 @@ stickPositions_e rcStickPositions;
 
 FASTRAM int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 3);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = SETTING_DEADBAND_DEFAULT,
     .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT,
     .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT,
+    .control_deadband = SETTING_CONTROL_DEADBAND_DEFAULT,
     .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT,
     .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT,
     .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT,
@@ -97,9 +98,9 @@ bool areSticksInApModePosition(uint16_t ap_mode)
     return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
 }
 
-bool areSticksDeflectedMoreThanPosHoldDeadband(void)
+bool areSticksDeflected(void)
 {
-    return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
+    return ABS(rcCommand[ROLL]) > rcControlsConfig()->control_deadband || ABS(rcCommand[PITCH]) > rcControlsConfig()->control_deadband || ABS(rcCommand[YAW]) > rcControlsConfig()->control_deadband;
 }
 
 throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)

src/main/fc/rc_controls.h

@@ -84,7 +84,8 @@ extern int16_t rcCommand[4];
 typedef struct rcControlsConfig_s {
     uint8_t deadband;                       // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.
     uint8_t yaw_deadband;                   // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
-    uint8_t pos_hold_deadband;              // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
+    uint8_t pos_hold_deadband;              // Deadband for position hold
+    uint8_t control_deadband;               // General deadband to check if sticks are deflected
     uint8_t alt_hold_deadband;              // Defines the neutral zone of throttle stick during altitude hold
     uint16_t mid_throttle_deadband;           // default throttle deadband from MIDRC
     uint8_t airmodeHandlingType;            // Defaults to ANTI_WINDUP triggered at sticks centered
@@ -106,7 +107,7 @@ stickPositions_e getRcStickPositions(void);
 bool checkStickPosition(stickPositions_e stickPos);
 
 bool areSticksInApModePosition(uint16_t ap_mode);
-bool areSticksDeflectedMoreThanPosHoldDeadband(void);
+bool areSticksDeflected(void);
 throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus);

src/main/fc/settings.yaml

@@ -1505,7 +1505,12 @@ groups:
         min: 0
         max: 100
       - name: pos_hold_deadband
-        description: "Stick deadband in [r/c points], applied after r/c deadband and expo"
+        description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used for adjustments in navigation modes."
+        default_value: 10
+        min: 2
+        max: 250
+      - name: control_deadband
+        description: "Stick deadband in [r/c points], applied after r/c deadband and expo. Used to check if sticks are centered."
         default_value: 10
         min: 2
         max: 250

src/main/flight/pid.c

@@ -1314,15 +1314,9 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
 
     //Iterm should freeze when sticks are deflected
-    bool areSticksDeflected = false;
-    for (int stick = ROLL; stick <= YAW; stick++) {
-        areSticksDeflected = areSticksDeflected ||
-            rxGetChannelValue(stick) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
-            rxGetChannelValue(stick) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband);
-    }
     if (
         !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
-        areSticksDeflected ||
+        areSticksDeflected() ||
         (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
         navigationIsControllingAltitude()
     ) {

src/main/flight/servos.c

@@ -490,6 +490,7 @@ void processServoAutotrimMode(void)
 #define SERVO_AUTOTRIM_CENTER_MIN       1300
 #define SERVO_AUTOTRIM_CENTER_MAX       1700
 #define SERVO_AUTOTRIM_UPDATE_SIZE      5
+#define SERVO_AUTOTRIM_ATIITUDE_LIMIT   50       // 5 degrees
 
 void processContinuousServoAutotrim(const float dT)
 {
@@ -497,21 +498,22 @@ void processContinuousServoAutotrim(const float dT)
     static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
     static uint32_t servoMiddleUpdateCount;
 
-    const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, sqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
-    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
-    const float targetRateMagnitude = getTotalRateTarget();
-    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
 
     if (ARMING_FLAG(ARMED)) {
         trimState = AUTOTRIM_COLLECTING;
         if ((millis() - lastUpdateTimeMs) > 500) {
             const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
             const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
-            const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
+            const bool sticksAreCentered = !areSticksDeflected();
+            const bool planeIsFlyingLevel = (ABS(attitude.values.pitch) + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT 
+                                            && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATIITUDE_LIMIT;
             if (
                 planeIsFlyingStraight && 
                 noRotationCommanded && 
                 planeIsFlyingLevel &&
+                sticksAreCentered &&
                 !FLIGHT_MODE(MANUAL_MODE) && 
                 isGPSHeadingValid() // TODO: proper flying detection
             ) { 

src/main/navigation/navigation.c

@@ -1728,7 +1728,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
     //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
     if (feature(FEATURE_FW_LAUNCH)) {
         throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
-        if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
+        if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflected())) {
             abortFixedWingLaunch();
             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
         }

src/main/navigation/navigation_fw_launch.c

@@ -251,7 +251,7 @@ static inline bool isLaunchMaxAltitudeReached(void)
 
 static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
 {
-    return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
+    return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflected();
 }
 
 static void resetPidsIfNeeded(void) {
@@ -435,7 +435,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
 
-    if (areSticksDeflectedMoreThanPosHoldDeadband()) {
+    if (areSticksDeflected()) {
         return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
     }
     if (elapsedTimeMs > endTimeMs) {