diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index f83b4644e..fe80141a6 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -346,18 +346,6 @@ static bool forcedDisArm(void) return true; } -#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES - // check landing state if active - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { - StatusVtolLandData statusland; - StatusVtolLandGet(&statusland); - if (statusland.State == STATUSVTOLLAND_STATE_DISARMED) { - return true; - } - } -#endif return false; } diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index f51b76fe0..f9d7a5064 100644 --- a/flight/modules/PathFollower/vtollandfsm.cpp +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -31,6 +31,7 @@ extern "C" { #include #include +#include #include #include #include @@ -638,6 +639,7 @@ void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) void VtolLandFSM::setup_thrustoff(void) { mLandData->thrustLimit = -1.0f; + mLandData->flZeroStabiHorizontal = true; mLandData->flConstrainThrust = true; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; @@ -653,10 +655,13 @@ void VtolLandFSM::setup_disarmed(void) { // nothing to do mLandData->flConstrainThrust = false; - mLandData->flZeroStabiHorizontal = false; + mLandData->flZeroStabiHorizontal = true; mLandData->observationCount = 0; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; + if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + } } void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)