diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b7c50c7571..eb39a21a97 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -506,7 +506,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_STATE_WAYPOINT_RTH_LAND] = { .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index c11e089930..d5ccf32f79 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -407,7 +407,16 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) { pitchCorrection += posControl.rcAdjustment[PITCH]; throttleCorrection += DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle; - throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); + + if (navStateFlags & NAV_CTL_LAND) { + /* + * During LAND we do not allow to raise THROTTLE when nose is up + * to reduce speed + */ + throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); + } else { + throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); + } } // Speed controller - only apply in POS mode diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 05ab263661..219eb3b9a9 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -230,6 +230,9 @@ typedef enum { NAV_RC_ALT = (1 << 11), NAV_RC_POS = (1 << 12), NAV_RC_YAW = (1 << 13), + + /* Additional flags */ + NAV_CTL_LAND = (1 << 14), } navigationFSMStateFlags_t; typedef struct {