Merge pull request #10090 from breadoven/abo_osd_sys_msg_cleanup

OSD system message improvements

Author: breadoven

src/main/fc/rc_modes.c

@@ -114,14 +114,28 @@ void processAirmode(void) {
     } else if (STATE(MULTIROTOR)) {
         processAirmodeMultirotor();
     }
-
 }
 
 bool isUsingNavigationModes(void)
 {
     return isUsingNAVModes;
 }
 
+bool isFwAutoModeActive(boxId_e mode)
+{
+    /* Sets activation priority of fixed wing auto tune/trim modes: Autotune -> Autotrim -> Autolevel */
+
+    if (mode == BOXAUTOTUNE) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
+    } else if (mode == BOXAUTOTRIM) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
+    } else if (mode == BOXAUTOLEVEL) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && !IS_RC_MODE_ACTIVE(BOXAUTOTRIM);
+    }
+
+    return false;
+}
+
 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
 {
     return bitArrayGet(rcModeActivationMask.bits, boxId);

src/main/fc/rc_modes.h

@@ -140,3 +140,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 
 void updateActivatedModes(void);
 void updateUsedModeActivationConditionFlags(void);
+bool isFwAutoModeActive(boxId_e mode);

src/main/flight/pid.c

@@ -1385,7 +1385,7 @@ pidBank_t * pidBankMutable(void) {
 
 bool isFixedWingLevelTrimActive(void)
 {
-    return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
+    return isFwAutoModeActive(BOXAUTOLEVEL) && !areSticksDeflected() &&
            (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
            !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
            !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
@@ -1409,7 +1409,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
     previousArmingState = ARMING_FLAG(ARMED);
 
     // return if not active or disarmed
-    if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
+    if (!isFwAutoModeActive(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
         return;
     }
 

src/main/flight/pid_autotune.c

@@ -40,6 +40,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/rc_adjustments.h"
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
@@ -130,7 +131,7 @@ void autotuneStart(void)
 
 void autotuneUpdateState(void)
 {
-    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
+    if (isFwAutoModeActive(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
         if (!FLIGHT_MODE(AUTO_TUNE)) {
             autotuneStart();
             ENABLE_FLIGHT_MODE(AUTO_TUNE);
@@ -202,7 +203,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 
         if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
             if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
-                
+
                 // Target 80% control surface deflection to leave some room for P and I to work
                 float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
 

src/main/flight/servos.c

@@ -82,7 +82,7 @@ void Reset_servoMixers(servoMixer_t *instance)
 #ifdef USE_PROGRAMMING_FRAMEWORK
             ,.conditionId = -1
 #endif
-        );       
+        );
     }
 }
 
@@ -96,7 +96,7 @@ void pgResetFn_servoParams(servoParam_t *instance)
             .max = DEFAULT_SERVO_MAX,
             .middle = DEFAULT_SERVO_MIDDLE,
             .rate = 100
-        );        
+        );
     }
 }
 
@@ -194,7 +194,7 @@ void servosInit(void)
 }
 
 int getServoCount(void)
-{   
+{
     if (mixerUsesServos) {
         return 1 + maxServoIndex - minServoIndex;
     }
@@ -246,7 +246,7 @@ static void filterServos(void)
 void writeServos(void)
 {
     filterServos();
-    
+
 #if !defined(SITL_BUILD)
     int servoIndex = 0;
     bool zeroServoValue = false;
@@ -449,7 +449,7 @@ void processServoAutotrimMode(void)
     static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
     static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
 
-    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+    if (isFwAutoModeActive(BOXAUTOTRIM)) {
         switch (trimState) {
             case AUTOTRIM_IDLE:
                 if (ARMING_FLAG(ARMED)) {
@@ -544,7 +544,7 @@ void processServoAutotrimMode(void)
 void processContinuousServoAutotrim(const float dT)
 {
     static timeMs_t lastUpdateTimeMs;
-    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
+    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
     static uint32_t servoMiddleUpdateCount;
 
     const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
@@ -556,16 +556,16 @@ void processContinuousServoAutotrim(const float dT)
             const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
             const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
             const bool sticksAreCentered = !areSticksDeflected();
-            const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT 
+            const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT
                                             && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT;
             if (
-                planeIsFlyingStraight && 
-                noRotationCommanded && 
+                planeIsFlyingStraight &&
+                noRotationCommanded &&
                 planeIsFlyingLevel &&
                 sticksAreCentered &&
-                !FLIGHT_MODE(MANUAL_MODE) && 
+                !FLIGHT_MODE(MANUAL_MODE) &&
                 isGPSHeadingValid() // TODO: proper flying detection
-            ) { 
+            ) {
                 // Plane is flying straight and level: trim servos
                 for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
                     // For each stabilized axis, add 5 units of I-term to all associated servo midpoints
@@ -610,7 +610,7 @@ void processContinuousServoAutotrim(const float dT)
     DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
     DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
     DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
-    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);    
+    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);
 }
 
 void processServoAutotrim(const float dT) {