update method, add AltHold + OSD messages

src/main/fc/fc_core.c

@@ -712,7 +712,7 @@ void processRx(timeUs_t currentTimeUs)
         } else {
             DISABLE_FLIGHT_MODE(MANUAL_MODE);
         }
-    }else{
+    } else {
         DISABLE_FLIGHT_MODE(MANUAL_MODE);
     }
 

src/main/flight/pid.c

@@ -159,6 +159,7 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
+static EXTENDED_FASTRAM bool attiHoldIsLevel = false;
 
 #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
 #define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
@@ -1057,9 +1058,58 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
 
 }
 
-void FAST_CODE pidController(float dT)
+bool isAttiholdLevel(void)
+{
+    return attiHoldIsLevel;
+}
+
+void updateAttihold(float *angleTarget, uint8_t axis)
 {
+    int8_t navAttiHoldAxis = navCheckActiveAttiHoldAxis();
+    static bool restartAttiMode = true;
+
+    if (!restartAttiMode) {     // reset flags for when attitude hold is inactive
+        restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE) && navAttiHoldAxis == -1;
+        attiHoldIsLevel = restartAttiMode ? false: attiHoldIsLevel;
+    }
+
+    if ((FLIGHT_MODE(ATTIHOLD_MODE) || axis == navAttiHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
+        static int16_t attiHoldTarget[2];
+
+        if (restartAttiMode) {      // set target attitude to current attitude on activation
+            attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
+            attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
+            restartAttiMode = false;
+        }
+
+        // set flag indicating attitude hold is level
+        if (FLIGHT_MODE(ATTIHOLD_MODE)) {
+            attiHoldIsLevel = attiHoldTarget[FD_ROLL] == 0 && attiHoldTarget[FD_PITCH] == 0;
+        } else {
+            attiHoldIsLevel = attiHoldTarget[navAttiHoldAxis] == 0;
+        }
+
+        uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
+
+        // use Nav bank angle limits if Nav active
+        if (navAttiHoldAxis == FD_ROLL) {
+            bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
+        } else if (navAttiHoldAxis == FD_PITCH) {
+            bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
+        }
 
+        if (calculateRollPitchCenterStatus() == CENTERED) {
+            attiHoldTarget[axis] = ABS(attiHoldTarget[axis]) < 30 ? 0 : attiHoldTarget[axis];   // snap to level when within 3 degs of level
+            *angleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit);
+        } else {
+            *angleTarget = constrain(attitude.raw[axis] + *angleTarget, -bankLimit, bankLimit);
+            attiHoldTarget[axis] = attitude.raw[axis];
+        }
+    }
+}
+
+void FAST_CODE pidController(float dT)
+{
     const float dT_inv = 1.0f / dT;
 
     if (!pidFiltersConfigured) {
@@ -1112,38 +1162,13 @@ 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) && !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 (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
-                    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 {
-                    angleTarget = constrain(attitude.raw[axis] + angleTarget, -bankLimit, bankLimit);
-                    attiHoldTarget[axis] = attitude.raw[axis];
-                }
+            if (STATE(AIRPLANE)) {  // update attitude hold mode
+                updateAttihold(&angleTarget, axis);
             }
 
             // Apply the Level PID controller

src/main/flight/pid.h

@@ -223,3 +223,4 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex);
 
 void updateFixedWingLevelTrim(timeUs_t currentTimeUs);
 float getFixedWingLevelTrim(void);
+bool isAttiholdLevel(void);

src/main/io/osd.c

@@ -5173,6 +5173,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (STATE(LANDING_DETECTED)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
                     }
+                    if (IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
+                        int8_t navAttiHoldAxis = navCheckActiveAttiHoldAxis();
+                        if (isAttiholdLevel()) {
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_LEVEL);
+                        } else if (navAttiHoldAxis == FD_ROLL) {
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_ROLL);
+                        } else if (navAttiHoldAxis == FD_PITCH) {
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ATTI_PITCH);
+                        }
+                    }
                 }
             }
         } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {

src/main/io/osd.h

@@ -118,6 +118,9 @@
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
 #define OSD_MSG_SAVING_SETTNGS      "** SAVING SETTINGS **"
 #define OSD_MSG_SETTINGS_SAVED      "** SETTINGS SAVED **"
+#define OSD_MSG_ATTI_ROLL           "(ATTI ROLL)"
+#define OSD_MSG_ATTI_PITCH          "(ATTI PITCH)"
+#define OSD_MSG_ATTI_LEVEL          "(ATTI LEVEL)"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"

src/main/navigation/navigation.c

@@ -949,7 +949,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
         }
     },
-    
+
     /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/
     [NAV_STATE_MIXERAT_INITIALIZE] = {
         .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE,
@@ -992,7 +992,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEvent = {
             [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            
+
         }
     },
 };
@@ -1514,7 +1514,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
     if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
-    
+
     if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){
         return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
     }
@@ -4533,3 +4533,21 @@ int32_t navigationGetHeadingError(void)
 {
     return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
 }
+
+int8_t navCheckActiveAttiHoldAxis(void)
+{
+    int8_t activeAxis = -1;
+
+    if (IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
+        navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+        bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
+
+        if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+            activeAxis = FD_PITCH;
+        } else if (altholdActive) {
+            activeAxis = FD_ROLL;
+        }
+    }
+
+    return activeAxis;
+}

src/main/navigation/navigation.h

@@ -625,6 +625,8 @@ bool isEstimatedAglTrusted(void);
 void checkManualEmergencyLandingControl(bool forcedActivation);
 float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
 
+int8_t navCheckActiveAttiHoldAxis(void);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.