diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index 1615ac91b..7dbf79336 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -35,6 +35,7 @@ #include #include #include +#include // Private constants #define ARMED_THRESHOLD 0.50f @@ -267,6 +268,8 @@ static bool okToArm(void) } } + StabilizationDesiredStabilizationModeData stabDesired; + uint8_t flightMode; FlightStatusFlightModeGet(&flightMode); switch (flightMode) { @@ -277,8 +280,14 @@ static bool okToArm(void) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: - return true; - + // Prevent arming if unsafe due to the current Thrust Mode + StabilizationDesiredStabilizationModeGet(&stabDesired); + if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD || + stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) { + return false; + } else { + return true; + } break; default: