diff --git a/flight/modules/PathFollower/inc/vtollandfsm.h b/flight/modules/PathFollower/inc/vtollandfsm.h index dec143c41..4c582a00a 100644 --- a/flight/modules/PathFollower/inc/vtollandfsm.h +++ b/flight/modules/PathFollower/inc/vtollandfsm.h @@ -47,7 +47,6 @@ typedef enum { LAND_STATE_THRUSTDOWN, // Thrust down sequence LAND_STATE_THRUSTOFF, // Thrust is now off LAND_STATE_DISARMED, // Disarmed - LAND_STATE_ABORT, // Abort on error triggerig fallback to hold LAND_STATE_SIZE } PathFollowerFSM_LandState_T; @@ -75,7 +74,6 @@ public: void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); float BoundVelocityDown(float); void CheckPidScaler(pid_scaler *scaler); - void Abort(void); protected: @@ -130,8 +128,6 @@ protected: void run_thrustoff(uint8_t); void setup_disarmed(void); void run_disarmed(uint8_t); - void setup_abort(void); - void run_abort(uint8_t); void initFSM(void); void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason); int32_t runState(); @@ -140,7 +136,6 @@ protected: void assessAltitude(void); void setStateTimeout(int32_t count); - void fallback_to_hold(void); static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE]; }; diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index f9d7a5064..267299bc6 100644 --- a/flight/modules/PathFollower/vtollandfsm.cpp +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -97,8 +97,7 @@ VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAN [LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect }, [LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown }, [LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff }, - [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed }, - [LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort } + [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed } }; // pointer to a singleton instance @@ -177,25 +176,16 @@ void VtolLandFSM::Activate() #endif } else { // move to error state and callback to position hold - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); + setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE); } } -void VtolLandFSM::Abort(void) -{ - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); -} - PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void) { switch (mLandData->currentState) { case STATUSVTOLLAND_STATE_INACTIVE: return PFFSM_STATE_INACTIVE; - break; - case STATUSVTOLLAND_STATE_ABORT: - return PFFSM_STATE_ABORT; - break; case STATUSVTOLLAND_STATE_DISARMED: return PFFSM_STATE_DISARMED; @@ -432,7 +422,7 @@ void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout) } if (flTimeout) { - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); } } @@ -638,11 +628,11 @@ void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) // STATE: THRUSTOFF void VtolLandFSM::setup_thrustoff(void) { - mLandData->thrustLimit = -1.0f; + mLandData->thrustLimit = -1.0f; mLandData->flZeroStabiHorizontal = true; - mLandData->flConstrainThrust = true; - mLandData->boundThrustMin = -0.1f; - mLandData->boundThrustMax = 0.0f; + mLandData->flConstrainThrust = true; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = 0.0f; } void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) @@ -659,48 +649,24 @@ void VtolLandFSM::setup_disarmed(void) mLandData->observationCount = 0; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; + + // force disarm unless in pathplanner mode + // to clear, a new pathfollower mode must be selected that is not land, + // and also a non-pathfollower mode selection will set this to uninitialised. if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); } } void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout) { + if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + } + #ifdef DEBUG_GROUNDIMPACT if (mLandData->observationCount++ > 100) { setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); } #endif } - -void VtolLandFSM::fallback_to_hold(void) -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - pathDesired->End.North = positionState.North; - pathDesired->End.East = positionState.East; - pathDesired->End.Down = positionState.Down; - pathDesired->Start.North = positionState.North; - pathDesired->Start.East = positionState.East; - pathDesired->Start.Down = positionState.Down; - pathDesired->StartingVelocity = 0.0f; - pathDesired->EndingVelocity = 0.0f; - pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT; - - PathDesiredSet(pathDesired); -} - -// abort repeatedly overwrites pathfollower's objective on a landing abort and -// continues to do so until a flight mode change. -void VtolLandFSM::setup_abort(void) -{ - mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; - mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; - mLandData->flConstrainThrust = false; - mLandData->flZeroStabiHorizontal = false; - fallback_to_hold(); -} - -void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout) -{}