Add Course Hold pitch hold & Cleanup

src/main/fc/fc_core.c

@@ -531,7 +531,7 @@ void tryArm(void)
     if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
-        enableFlightMode(TURTLE_MODE);
+        ENABLE_FLIGHT_MODE(TURTLE_MODE);
         return;
     }
 #endif
@@ -667,18 +667,14 @@ void processRx(timeUs_t currentTimeUs)
 
     /* Flaperon mode */
     if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
-        if (!FLIGHT_MODE(FLAPERON)) {
-            ENABLE_FLIGHT_MODE(FLAPERON);
-        }
+        ENABLE_FLIGHT_MODE(FLAPERON);
     } else {
         DISABLE_FLIGHT_MODE(FLAPERON);
     }
 
     /* Turn assistant mode */
     if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
-        if (!FLIGHT_MODE(TURN_ASSISTANT)) {
-            ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
-        }
+         ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
     } else {
         DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
     }
@@ -697,9 +693,7 @@ void processRx(timeUs_t currentTimeUs)
 #if defined(USE_MAG)
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
-            if (!FLIGHT_MODE(HEADFREE_MODE)) {
-                ENABLE_FLIGHT_MODE(HEADFREE_MODE);
-            }
+            ENABLE_FLIGHT_MODE(HEADFREE_MODE);
         } else {
             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
         }

src/main/fc/runtime_config.c

@@ -90,24 +90,6 @@ armingFlag_e isArmingDisabledReason(void)
     return 0;
 }
 
-/**
- * Enables the given flight mode. Returns the new 'flightModeFlags' value.
- */
-uint32_t enableFlightMode(flightModeFlags_e mask)
-{
-    flightModeFlags |= (mask);
-    return flightModeFlags;
-}
-
-/**
- * Disables the given flight mode. Returns the new 'flightModeFlags' value.
- */
-uint32_t disableFlightMode(flightModeFlags_e mask)
-{
-    flightModeFlags &= ~(mask);
-    return flightModeFlags;
-}
-
 /**
  * Called at Rx update rate. Beeper sounded if flight mode state has changed.
  */

src/main/fc/runtime_config.h

@@ -109,8 +109,8 @@ typedef enum {
 
 extern uint32_t flightModeFlags;
 
-#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask)
-#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
+#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask))
+#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask))
 #define FLIGHT_MODE(mask) (flightModeFlags & (mask))
 
 typedef enum {
@@ -201,8 +201,6 @@ extern simulatorData_t simulatorData;
 
 #endif
 
-uint32_t enableFlightMode(flightModeFlags_e mask);
-uint32_t disableFlightMode(flightModeFlags_e mask);
 void updateFlightModeChangeBeeper(void);
 
 bool sensors(uint32_t mask);

src/main/fc/settings.yaml

@@ -192,7 +192,7 @@ tables:
     enum: gpsBaudRate_e
   - name: nav_mc_althold_throttle
     values: ["STICK", "MID_STICK", "HOVER"]
-    enum: navMcAltHoldThrottle_e    
+    enum: navMcAltHoldThrottle_e
 
 constants:
   RPYL_PID_MIN: 0

src/main/flight/pid.c

@@ -1111,26 +1111,32 @@ void FAST_CODE pidController(float dT)
     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
     levelingEnabled = false;
 
+    // Pitch hold enabled during Course Hold mode if AttiHold mode selected (but not active since Angle has priority)
+    bool navPitchHoldActive = IS_RC_MODE_ACTIVE(BOXATTIHOLD) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE);
+
     static bool restartAttiMode = true;
     if (!restartAttiMode) {
-        restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE);  // set restart flag if attihold_mode not active
+        restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE) && !navPitchHoldActive;   // set restart flag if attitude hold not active
     }
 
     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
             // If axis angle override, get the correct angle from Logic Conditions
             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
 
-            if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) {
+            if (STATE(AIRPLANE) && (FLIGHT_MODE(ATTIHOLD_MODE) || (navPitchHoldActive && axis == FD_PITCH)) && !isFlightAxisAngleOverrideActive(axis)) {
                 static int16_t attiHoldTarget[2];
 
-                if (restartAttiMode) {      // set target attitude to current attitude when initialised
+                if (restartAttiMode) {  // set target attitude to current attitude when initialised
                     attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
                     attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
                     restartAttiMode = false;
                 }
 
                 uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
+                if (navPitchHoldActive) {   // use Nav bank limits if Nav active, limited to pitch only
+                    bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
+                }
                 if (calculateRollPitchCenterStatus() == CENTERED) {
                     angleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit);
                 } else {

src/main/flight/pid.h

@@ -106,8 +106,8 @@ typedef struct pidProfile_s {
     pidBank_t bank_mc;
 
     uint8_t dterm_lpf_type;                 // Dterm LPF type: PT1, BIQUAD
-    uint16_t dterm_lpf_hz;                  
-    
+    uint16_t dterm_lpf_hz;
+
     uint8_t yaw_lpf_hz;
 
     uint8_t heading_hold_rate_limit;        // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
@@ -129,7 +129,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
-    
+
     float       navVelXyDTermLpfHz;
     uint8_t navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
     uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity

src/main/navigation/navigation.c

@@ -3727,9 +3727,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             /* Soaring mode, disables altitude control in Position hold and Course hold modes.
              * Pitch allowed to freefloat within defined Angle mode deadband */
             if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) {
-                if (!FLIGHT_MODE(SOARING_MODE)) {
-                    ENABLE_FLIGHT_MODE(SOARING_MODE);
-                }
+                ENABLE_FLIGHT_MODE(SOARING_MODE);
             } else {
                 DISABLE_FLIGHT_MODE(SOARING_MODE);
             }