From 61488327449e252fe868f91c405806204f33076c Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 29 Apr 2021 16:48:19 +0200 Subject: [PATCH] Add CRUISE mode which is equivalent to CRSH+AH --- src/main/fc/fc_msp_box.c | 3 +++ src/main/fc/rc_modes.c | 1 + src/main/fc/rc_modes.h | 3 ++- src/main/fc/runtime_config.c | 5 ++++- src/main/fc/runtime_config.h | 1 + src/main/navigation/navigation.c | 12 +++++++++--- src/main/programming/logic_condition.c | 6 +++++- src/main/programming/logic_condition.h | 1 + 8 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 9ba7e4a0f49..7da748f0af1 100644 --- 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 }, { BOXTURTLE, "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 index 48dface10e0..b832f88b34c 100755 --- 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 index 4a840a4d3a7..506ab8f9b3b 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -70,6 +70,7 @@ typedef enum { BOXMSPRCOVERRIDE = 41, BOXPREARM = 42, BOXTURTLE = 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); \ No newline at end of file +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 index 6f7241244d1..57cc63f8bc5 100644 --- 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 index 55f3c3df560..e77e7c18b4e 100644 --- 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 index eba7382530f..2de796238fd 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3212,7 +3212,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)) { @@ -3302,8 +3308,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 index 89a1fc6fe6e..01e2dada95a 100644 --- 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 index 005c7a85f4f..3b795ba43e0 100644 --- 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 {