Add CRUISE mode which is equivalent to CRSH+AH

iNavFlight · Apr 29, 2021 · aeae949 · aeae949
1 parent f2692f3
commit aeae949
Show file tree

Hide file tree

Showing 8 changed files with 26 additions and 6 deletions.
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
@@ -90,6 +90,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
     { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
+    { BOXNAVCRUISE, "NAV CRUISE", 53 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -220,6 +221,7 @@ void initActiveBoxIds(void)
             activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
             if (STATE(AIRPLANE)) {
                 activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD;
+                activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
             }
         }
     }
@@ -346,6 +348,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)),         BOXNAVALTHOLD);
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)),         BOXNAVPOSHOLD);
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)),     BOXNAVCOURSEHOLD);
+    CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)),     BOXNAVCRUISE);
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)),             BOXNAVRTH);
     CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)),              BOXNAVWP);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)),         BOXAIRMODE);

diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c
@@ -212,6 +212,7 @@ void updateUsedModeActivationConditionFlags(void)
     isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
                         isModeActivationConditionPresent(BOXNAVRTH) ||
                         isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
+                        isModeActivationConditionPresent(BOXNAVCRUISE) ||
                         isModeActivationConditionPresent(BOXNAVWP);
 #endif
 }

diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
@@ -70,6 +70,7 @@ typedef enum {
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
     BOXFLIPOVERAFTERCRASH = 43,
+    BOXNAVCRUISE     = 44,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
@@ -129,4 +130,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 
 void updateActivatedModes(void);
 void updateUsedModeActivationConditionFlags(void);
-void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);
+void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);
diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
@@ -155,9 +155,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
     if (FLIGHT_MODE(NAV_POSHOLD_MODE))
         return FLM_POSITION_HOLD;
 
-    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
         return FLM_CRUISE;
 
+    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
+        return FLM_COURSE_HOLD;
+
     if (FLIGHT_MODE(NAV_WP_MODE))
         return FLM_MISSION;
 

diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
@@ -152,6 +152,7 @@ typedef enum {
     FLM_POSITION_HOLD,
     FLM_RTH,
     FLM_MISSION,
+    FLM_COURSE_HOLD,
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -3200,7 +3200,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
                 return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
         }
 
-        // PH has priority over CRUISE
+        // CRUISE has priority over COURSE_HOLD and AH
+        if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
+            if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))
+                return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
+        }
+
+        // PH has priority over COURSE_HOLD
         // CRUISE has priority on AH
         if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
 
@@ -3290,8 +3296,8 @@ bool navigationTerrainFollowingEnabled(void)
 
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 {
-    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
-    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
+    const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)));
+    const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
 
     if (usedBypass) {
         *usedBypass = false;

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
@@ -559,10 +559,14 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
             return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE);
             break;
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD:
             return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
             break;
 
+        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
+            return (bool)(FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE));
+            break;
+
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
             return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE);
             break;

diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
@@ -135,6 +135,7 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR,                                // 8
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1,                              // 9
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2,                              // 10
+    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD,                        // 11
 } logicFlightModeOperands_e;
 
 typedef enum {