diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c old mode 100755 new mode 100644 index 7fa07a2a83..a61204dee9 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -548,7 +548,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 @@ -678,28 +678,21 @@ void processRx(timeUs_t currentTimeUs) // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR) bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs); bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; - bool canUseHorizonMode = true; - if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { - // bumpless transfer to Level mode - canUseHorizonMode = false; + /* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD + * MANUAL mode has priority over these modes except when ANGLE auto enabled */ + DISABLE_FLIGHT_MODE(ANGLE_MODE); + DISABLE_FLIGHT_MODE(HORIZON_MODE); + DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE); - if (!FLIGHT_MODE(ANGLE_MODE)) { + if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) { + if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) { ENABLE_FLIGHT_MODE(ANGLE_MODE); - } - } else { - DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support - } - - if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { - - DISABLE_FLIGHT_MODE(ANGLE_MODE); - - if (!FLIGHT_MODE(HORIZON_MODE)) { + } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) { ENABLE_FLIGHT_MODE(HORIZON_MODE); + } else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { + ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE); } - } else { - DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { @@ -710,18 +703,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); } @@ -740,9 +729,7 @@ void processRx(timeUs_t currentTimeUs) #if defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) { - if (!FLIGHT_MODE(HEADFREE_MODE)) { - ENABLE_FLIGHT_MODE(HEADFREE_MODE); - } + ENABLE_FLIGHT_MODE(HEADFREE_MODE); } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } @@ -760,7 +747,7 @@ void processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); } - }else{ + } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); } @@ -781,52 +768,52 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE)) { - if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { - ENABLE_STATE(ANTI_WINDUP); - } - else { - DISABLE_STATE(ANTI_WINDUP); - } - } - else { - DISABLE_STATE(ANTI_WINDUP); - pidResetErrorAccumulators(); - } - } - else { - DISABLE_STATE(ANTI_WINDUP); - } + if (STATE(AIRMODE_ACTIVE)) { + if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { + ENABLE_STATE(ANTI_WINDUP); + } + else { + DISABLE_STATE(ANTI_WINDUP); + } + } + else { + DISABLE_STATE(ANTI_WINDUP); + pidResetErrorAccumulators(); + } + } + else { + DISABLE_STATE(ANTI_WINDUP); + } } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE)) { - if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) { - ENABLE_STATE(ANTI_WINDUP); - } - else { - DISABLE_STATE(ANTI_WINDUP); - } - } - else { - DISABLE_STATE(ANTI_WINDUP); - pidResetErrorAccumulators(); - } - } - else { - DISABLE_STATE(ANTI_WINDUP); - if (rollPitchStatus != CENTERED) { - ENABLE_STATE(ANTI_WINDUP_DEACTIVATED); - } - } + if (STATE(AIRMODE_ACTIVE)) { + if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) { + ENABLE_STATE(ANTI_WINDUP); + } + else { + DISABLE_STATE(ANTI_WINDUP); + } + } + else { + DISABLE_STATE(ANTI_WINDUP); + pidResetErrorAccumulators(); + } + } + else { + DISABLE_STATE(ANTI_WINDUP); + if (rollPitchStatus != CENTERED) { + ENABLE_STATE(ANTI_WINDUP_DEACTIVATED); + } + } } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { - DISABLE_STATE(ANTI_WINDUP); - //This case applies only to MR when Airmode management is throttle threshold activated - if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) { - pidResetErrorAccumulators(); - } - } + DISABLE_STATE(ANTI_WINDUP); + //This case applies only to MR when Airmode management is throttle threshold activated + if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) { + pidResetErrorAccumulators(); + } + } //--------------------------------------------------------- if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); @@ -849,6 +836,8 @@ void processRx(timeUs_t currentTimeUs) } } #endif + // Sound a beeper if the flight mode state has changed + updateFlightModeChangeBeeper(); } // Function for loop trigger diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 5b6b8656d8..5728fd6933 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -102,6 +102,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 }, { .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 }, { .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 }, + { .boxId = BOXANGLEHOLD, .boxName = "ANGLE HOLD", .permanentId = 64 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -259,7 +260,7 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXNAVALTHOLD); } - if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) || + if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) || platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) { ADD_ACTIVE_BOX(BOXMANUAL); } @@ -279,6 +280,9 @@ void initActiveBoxIds(void) if (sensors(SENSOR_BARO)) { ADD_ACTIVE_BOX(BOXAUTOLEVEL); } + if (sensors(SENSOR_ACC)) { + ADD_ACTIVE_BOX(BOXANGLEHOLD); + } } /* @@ -432,6 +436,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); #endif + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)), BOXANGLEHOLD); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { if (activeBoxes[activeBoxIds[i]]) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 6b56fa556b..fa1827a6ad 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -81,6 +81,7 @@ typedef enum { BOXMULTIFUNCTION = 52, BOXMIXERPROFILE = 53, BOXMIXERTRANSITION = 54, + BOXANGLEHOLD = 55, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 175de20c48..daeb9cbbcd 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -91,31 +91,16 @@ armingFlag_e isArmingDisabledReason(void) } /** - * Enables the given flight mode. A beep is sounded if the flight mode - * has changed. Returns the new 'flightModeFlags' value. + * Called at Rx update rate. Beeper sounded if flight mode state has changed. */ -uint32_t enableFlightMode(flightModeFlags_e mask) +void updateFlightModeChangeBeeper(void) { - uint32_t oldVal = flightModeFlags; + static uint32_t previousFlightModeFlags = 0; - flightModeFlags |= (mask); - if (flightModeFlags != oldVal) + if (flightModeFlags != previousFlightModeFlags) { beeperConfirmationBeeps(1); - return flightModeFlags; -} - -/** - * Disables the given flight mode. A beep is sounded if the flight mode - * has changed. Returns the new 'flightModeFlags' value. - */ -uint32_t disableFlightMode(flightModeFlags_e mask) -{ - uint32_t oldVal = flightModeFlags; - - flightModeFlags &= ~(mask); - if (flightModeFlags != oldVal) - beeperConfirmationBeeps(1); - return flightModeFlags; + } + previousFlightModeFlags = flightModeFlags; } bool sensors(uint32_t mask) @@ -173,6 +158,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(HORIZON_MODE)) return FLM_HORIZON; + if (FLIGHT_MODE(ANGLEHOLD_MODE)) + return FLM_ANGLEHOLD; return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO; } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 8f7bcaa678..c532b77eee 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -104,12 +104,13 @@ typedef enum { TURN_ASSISTANT = (1 << 14), TURTLE_MODE = (1 << 15), SOARING_MODE = (1 << 16), + ANGLEHOLD_MODE = (1 << 17), } flightModeFlags_e; 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 { @@ -162,6 +163,7 @@ typedef enum { FLM_CRUISE, FLM_LAUNCH, FLM_FAILSAFE, + FLM_ANGLEHOLD, FLM_COUNT } flightModeForTelemetry_e; @@ -200,8 +202,7 @@ 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); void sensorsSet(uint32_t mask); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1011638959..4f870fa8f7 100644 --- a/src/main/fc/settings.yaml +++ b/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 - name: led_pin_pwm_mode values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] enum: led_pin_pwm_mode_e @@ -1186,7 +1186,7 @@ groups: min: 0 max: 200 - + - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t @@ -3586,7 +3586,7 @@ groups: min: 1000 max: 5000 default_value: 1500 - + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c3a47fa529..bc794735eb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -159,6 +159,8 @@ 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 restartAngleHoldMode = true; +static EXTENDED_FASTRAM bool angleHoldIsLevel = false; #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand #define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f @@ -838,7 +840,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); - + if (pidProfile()->pidItermLimitPercent != 0){ float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); @@ -1064,9 +1066,62 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis } +bool isAngleHoldLevel(void) +{ + return angleHoldIsLevel; +} + +void updateAngleHold(float *angleTarget, uint8_t axis) +{ + int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis(); + + if (!restartAngleHoldMode) { // set restart flag when anglehold is inactive + restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1; + } + + if ((FLIGHT_MODE(ANGLEHOLD_MODE) || axis == navAngleHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) { + /* angleHoldTarget stores attitude values using a zero datum when level. + * This requires angleHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0 + * when the craft is level even though attitude pitch is non zero in this case. + * angleTarget pitch is corrected back to fixedWingLevelTrim datum on return from function */ + + static int16_t angleHoldTarget[2]; + + if (restartAngleHoldMode) { // set target attitude to current attitude on activation + angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL]; + angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); + restartAngleHoldMode = false; + } + + // set flag indicating anglehold is level + if (FLIGHT_MODE(ANGLEHOLD_MODE)) { + angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0; + } else { + angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0; + } + + uint16_t bankLimit = pidProfile()->max_angle_inclination[axis]; + + // use Nav bank angle limits if Nav active + if (navAngleHoldAxis == FD_ROLL) { + bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle); + } else if (navAngleHoldAxis == FD_PITCH) { + bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + } + + int16_t levelTrim = axis == FD_PITCH ? DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : 0; + if (calculateRollPitchCenterStatus() == CENTERED) { + angleHoldTarget[axis] = ABS(angleHoldTarget[axis]) < 30 ? 0 : angleHoldTarget[axis]; // snap to level when within 3 degs of level + *angleTarget = constrain(angleHoldTarget[axis] - levelTrim, -bankLimit, bankLimit); + } else { + *angleTarget = constrain(attitude.raw[axis] + *angleTarget + levelTrim, -bankLimit, bankLimit); + angleHoldTarget[axis] = attitude.raw[axis] + levelTrim; + } + } +} + void FAST_CODE pidController(float dT) { - const float dT_inv = 1.0f / dT; if (!pidFiltersConfigured) { @@ -1115,21 +1170,30 @@ void FAST_CODE pidController(float dT) #endif } - // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK - const float horizonRateMagnitude = calcHorizonRateMagnitude(); + // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE + const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; levelingEnabled = false; + angleHoldIsLevel = false; + for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { - if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) { - //If axis angle override, get the correct angle from Logic Conditions + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) { + // If axis angle override, get the correct angle from Logic Conditions float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis)); - //Apply the Level PID controller + if (STATE(AIRPLANE)) { // update anglehold mode + updateAngleHold(&angleTarget, axis); + } + + // Apply the Level PID controller pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON levelingEnabled = true; + } else { + restartAngleHoldMode = true; } } + // Apply Turn Assistance if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH])); @@ -1137,6 +1201,7 @@ void FAST_CODE pidController(float dT) canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } + // Apply FPV camera mix if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } @@ -1272,7 +1337,7 @@ bool isFixedWingLevelTrimActive(void) return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) && - !navigationIsControllingAltitude(); + !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel); } void updateFixedWingLevelTrim(timeUs_t currentTimeUs) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 48f1352eae..e0a8b9d310 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -220,3 +220,4 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex); bool isFixedWingLevelTrimActive(void); void updateFixedWingLevelTrim(timeUs_t currentTimeUs); float getFixedWingLevelTrim(void); +bool isAngleHoldLevel(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1662094cab..b799ab5c56 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1726,7 +1726,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; - } else + } else #endif { if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { @@ -1760,7 +1760,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, " NF"); else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) { uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value - + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it if (isBfCompatibleVideoSystem(osdConfig())) { //BFcompat is unable to work with scaled values and it only has mAh symbol to work with @@ -1768,7 +1768,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = SYM_MAH; buff[6] = '\0'; unitsDrawn = true; - } else + } else #endif { if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { @@ -2258,7 +2258,7 @@ static bool osdDrawSingleElement(uint8_t item) p = " WP "; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { // If navigationRequiresAngleMode() returns false when ALTHOLD is active, - // it means it can be combined with ANGLE, HORIZON, ACRO, etc... + // it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc... // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. p = " AH "; } @@ -2266,6 +2266,8 @@ static bool osdDrawSingleElement(uint8_t item) p = "ANGL"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; + else if (FLIGHT_MODE(ANGLEHOLD_MODE)) + p = "ANGH"; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return true; @@ -5156,10 +5158,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO + // ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO // when it doesn't require ANGLE mode (required only in FW - // right now). If if requires ANGLE, its display is handled - // by OSD_FLYMODE. + // right now). If it requires ANGLE, its display is handled by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { @@ -5196,6 +5197,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(BOXANGLEHOLD)) { + int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis(); + if (isAngleHoldLevel()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_LEVEL); + } else if (navAngleHoldAxis == FD_ROLL) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_ROLL); + } else if (navAngleHoldAxis == FD_PITCH) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); + } + } } } } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f09c9d049b..23d2ec87c1 100644 --- a/src/main/io/osd.h +++ b/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_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)" +#define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)" +#define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)" #ifdef USE_DEV_TOOLS #define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" @@ -413,7 +416,7 @@ typedef struct osdConfig_s { #ifdef USE_WIND_ESTIMATOR bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance -#endif +#endif uint8_t coordinate_digits; bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index e79ddc0dfd..c040b7b762 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -300,6 +300,7 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask) case FLM_ALTITUDE_HOLD: case FLM_POSITION_HOLD: case FLM_MISSION: + case FLM_ANGLEHOLD: default: // Unsupported ATM, keep at ANGLE bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cb12579b83..c566c94bfa 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3894,9 +3894,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); } @@ -4567,3 +4565,21 @@ int32_t navigationGetHeadingError(void) { return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); } + +int8_t navCheckActiveAngleHoldAxis(void) +{ + int8_t activeAxis = -1; + + if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { + 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; +} diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ecc487cc9e..d4ee6e078e 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -627,6 +627,8 @@ bool isEstimatedAglTrusted(void); void checkManualEmergencyLandingControl(bool forcedActivation); float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue); +int8_t navCheckActiveAngleHoldAxis(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. diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 1e3ce0eb61..f0e54d9013 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -97,7 +97,7 @@ static int logicConditionCompute( int temporaryValue; #if defined(USE_VTX_CONTROL) vtxDeviceCapability_t vtxDeviceCapability; -#endif +#endif switch (operation) { @@ -154,7 +154,7 @@ static int logicConditionCompute( case LOGIC_CONDITION_NOR: return !(operandA || operandB); - break; + break; case LOGIC_CONDITION_NOT: return !operandA; @@ -170,7 +170,7 @@ static int logicConditionCompute( return false; } - //When both operands are not met, keep current value + //When both operands are not met, keep current value return currentValue; break; @@ -238,7 +238,7 @@ static int logicConditionCompute( gvSet(operandA, operandB); return operandB; break; - + case LOGIC_CONDITION_GVAR_INC: temporaryValue = gvGet(operandA) + operandB; gvSet(operandA, temporaryValue); @@ -270,7 +270,7 @@ static int logicConditionCompute( return operandA; } break; - + case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); return true; @@ -288,10 +288,10 @@ static int logicConditionCompute( break; case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: -#if defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) ) { logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); @@ -339,18 +339,18 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH); return true; break; - + case LOGIC_CONDITION_INVERT_YAW: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW); return true; break; - + case LOGIC_CONDITION_OVERRIDE_THROTTLE: logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE); return operandA; break; - + case LOGIC_CONDITION_SET_OSD_LAYOUT: logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT); @@ -366,18 +366,18 @@ static int logicConditionCompute( case LOGIC_CONDITION_SIN: temporaryValue = (operandB == 0) ? 500 : operandB; - return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; - + case LOGIC_CONDITION_COS: temporaryValue = (operandB == 0) ? 500 : operandB; - return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; break; - + case LOGIC_CONDITION_TAN: temporaryValue = (operandB == 0) ? 500 : operandB; - return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; case LOGIC_CONDITION_MIN: @@ -387,11 +387,11 @@ static int logicConditionCompute( case LOGIC_CONDITION_MAX: return (operandA > operandB) ? operandA : operandB; break; - + case LOGIC_CONDITION_MAP_INPUT: return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); break; - + case LOGIC_CONDITION_MAP_OUTPUT: return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); break; @@ -474,7 +474,7 @@ static int logicConditionCompute( } else { return false; } - break; + break; #ifdef LED_PIN case LOGIC_CONDITION_LED_PIN_PWM: @@ -486,10 +486,9 @@ static int logicConditionCompute( return operandA; break; #endif - default: return false; - break; + break; } } @@ -498,7 +497,7 @@ void logicConditionProcess(uint8_t i) { const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); if (logicConditions(i)->enabled && activatorValue && !cliMode) { - + /* * Process condition only when latch flag is not set * Latched LCs can only go from OFF to ON, not the other way @@ -507,13 +506,13 @@ void logicConditionProcess(uint8_t i) { const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); const int newValue = logicConditionCompute( - logicConditionStates[i].value, - logicConditions(i)->operation, - operandAValue, + logicConditionStates[i].value, + logicConditions(i)->operation, + operandAValue, operandBValue, i ); - + logicConditionStates[i].value = newValue; /* @@ -588,7 +587,7 @@ static int logicConditionGetWaypointOperandValue(int operand) { return distance; } break; - + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; break; @@ -634,11 +633,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s return constrain((uint32_t)getFlightTime(), 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m return constrain(GPS_distanceToHome, 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX); break; @@ -646,7 +645,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 return getBatteryVoltage(); break; @@ -662,7 +661,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 return getAmperage(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh return getMAhDrawn(); break; @@ -674,7 +673,7 @@ static int logicConditionGetFlightOperandValue(int operand) { return gpsSol.numSat; } break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 return STATE(GPS_FIX) ? 1 : 0; break; @@ -726,15 +725,15 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0; - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0; @@ -742,7 +741,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1 return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0; @@ -751,16 +750,16 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1 return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // return axisPID[YAW]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // return axisPID[ROLL]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // return axisPID[PITCH]; break; @@ -787,7 +786,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int return currentMixerProfileIndex + 1; break; @@ -802,14 +801,14 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS: return isEstimatedAglTrusted(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_AGL: return getEstimatedAglPosition(); - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW: return rangefinderGetLatestRawAltitude(); - break; + break; default: return 0; @@ -861,13 +860,18 @@ static int logicConditionGetFlightModeOperandValue(int operand) { return (bool) FLIGHT_MODE(HORIZON_MODE); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD: + return (bool) FLIGHT_MODE(ANGLEHOLD_MODE); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR: return (bool) FLIGHT_MODE(AIRMODE_ACTIVE); break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO: - return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || - (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false); + return (((bool) FLIGHT_MODE(ANGLE_MODE) || (bool) FLIGHT_MODE(HORIZON_MODE) || (bool) FLIGHT_MODE(ANGLEHOLD_MODE) || + (bool) FLIGHT_MODE(MANUAL_MODE) || (bool) FLIGHT_MODE(NAV_RTH_MODE) || (bool) FLIGHT_MODE(NAV_POSHOLD_MODE) || + (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (bool) FLIGHT_MODE(NAV_WP_MODE)) == false); break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1: @@ -905,7 +909,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { //Extract RC channel raw value if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) { retVal = rxGetChannelValue(operand - 1); - } + } break; case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT: @@ -933,7 +937,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { retVal = programmingPidGetOutput(operand); } break; - + case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: retVal = logicConditionGetWaypointOperandValue(operand); break; @@ -947,7 +951,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { /* * conditionId == -1 is always evaluated as true - */ + */ int logicConditionGetValue(int8_t conditionId) { if (conditionId >= 0) { return logicConditionStates[conditionId].value; @@ -1029,7 +1033,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { uint32_t getLoiterRadius(uint32_t loiterRadius) { #ifdef USE_PROGRAMMING_FRAMEWORK - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) { return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000); } else { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 09163b62ba..bb62881b78 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -158,6 +158,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO, // 14 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION, // 15 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD, // 16 } logicFlightModeOperands_e; typedef enum { diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index dca381adea..967092de15 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -352,6 +352,8 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "ANGL"; } else if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = "HOR"; + } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) { + flightMode = "ANGH"; } #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 30237c2855..187f959b17 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -191,6 +191,7 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) case FLM_ACRO_AIR: return COPTER_MODE_ACRO; case FLM_ANGLE: return COPTER_MODE_STABILIZE; case FLM_HORIZON: return COPTER_MODE_STABILIZE; + case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD; case FLM_RTH: return COPTER_MODE_RTL; @@ -220,6 +221,7 @@ static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) case FLM_ACRO_AIR: return PLANE_MODE_ACRO; case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; case FLM_HORIZON: return PLANE_MODE_STABILIZE; + case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; case FLM_POSITION_HOLD: return PLANE_MODE_LOITER; case FLM_RTH: return PLANE_MODE_RTL; @@ -662,7 +664,7 @@ void mavlinkSendHUDAndHeartbeat(void) // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 - thr, + thr, // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second @@ -1102,9 +1104,9 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) { // Only process scheduled data if we didn't serve any incoming request this cycle - if (!incomingRequestServed || + if (!incomingRequestServed || ( - (rxConfig()->receiverType == RX_TYPE_SERIAL) && + (rxConfig()->receiverType == RX_TYPE_SERIAL) && (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) && !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex) ) diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index bb5f8b0465..af58cd5c43 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -122,7 +122,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED; static int simRssi; static uint8_t accEvent = ACC_EVENT_NONE; static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " }; -static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" }; +static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS", "ANH" }; static const char gpsFixIndicators[] = { '!', '*', ' ' }; static bool checkGroundStationNumber(uint8_t* rv)