rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

REVONANO Landing fixes

Browse Source 
1. Forced disarm via guidance alarm critical on disarm state
2. Continue to zero stabi including yaw during disarm state just in case it doesn't actually disarm.
This commit is contained in:
abeck70 2015-05-25 22:21:53 +10:00
parent 7d66a075de
commit c5715b26af
2 changed files with 16 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/PathFollower/inc/vtollandfsm.h
View File

@ -47,7 +47,6 @@ typedef enum {
LAND_STATE_THRUSTDOWN, // Thrust down sequence LAND_STATE_THRUSTDOWN, // Thrust down sequence
LAND_STATE_THRUSTOFF, // Thrust is now off LAND_STATE_THRUSTOFF, // Thrust is now off
LAND_STATE_DISARMED, // Disarmed LAND_STATE_DISARMED, // Disarmed
LAND_STATE_ABORT, // Abort on error triggerig fallback to hold
LAND_STATE_SIZE LAND_STATE_SIZE
} PathFollowerFSM_LandState_T; } PathFollowerFSM_LandState_T;
@ -75,7 +74,6 @@ public:
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
float BoundVelocityDown(float); float BoundVelocityDown(float);
void CheckPidScaler(pid_scaler *scaler); void CheckPidScaler(pid_scaler *scaler);
void Abort(void);
protected: protected:
@ -130,8 +128,6 @@ protected:
void run_thrustoff(uint8_t); void run_thrustoff(uint8_t);
void setup_disarmed(void); void setup_disarmed(void);
void run_disarmed(uint8_t); void run_disarmed(uint8_t);
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void); void initFSM(void);
void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason); void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
int32_t runState(); int32_t runState();
@ -140,7 +136,6 @@ protected:
void assessAltitude(void); void assessAltitude(void);
void setStateTimeout(int32_t count); void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE]; static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
}; };

66
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -97,8 +97,7 @@ VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAN
[LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect }, [LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect },
[LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown }, [LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown },
[LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff }, [LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff },
[LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed }, [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed }
[LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort }
}; };
// pointer to a singleton instance // pointer to a singleton instance
@ -177,25 +176,16 @@ void VtolLandFSM::Activate()
#endif #endif
} else { } else {
// move to error state and callback to position hold // 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) PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
{ {
switch (mLandData->currentState) { switch (mLandData->currentState) {
case STATUSVTOLLAND_STATE_INACTIVE: case STATUSVTOLLAND_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE; return PFFSM_STATE_INACTIVE;
break;
case STATUSVTOLLAND_STATE_ABORT:
return PFFSM_STATE_ABORT;
break; break;
case STATUSVTOLLAND_STATE_DISARMED: case STATUSVTOLLAND_STATE_DISARMED:
return PFFSM_STATE_DISARMED; return PFFSM_STATE_DISARMED;
@ -432,7 +422,7 @@ void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
} }
if (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 // STATE: THRUSTOFF
void VtolLandFSM::setup_thrustoff(void) void VtolLandFSM::setup_thrustoff(void)
{ {
mLandData->thrustLimit = -1.0f; mLandData->thrustLimit = -1.0f;
mLandData->flZeroStabiHorizontal = true; mLandData->flZeroStabiHorizontal = true;
mLandData->flConstrainThrust = true; mLandData->flConstrainThrust = true;
mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f; mLandData->boundThrustMax = 0.0f;
} }
void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
@ -659,48 +649,24 @@ void VtolLandFSM::setup_disarmed(void)
mLandData->observationCount = 0; mLandData->observationCount = 0;
mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f; 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) { 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) 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 #ifdef DEBUG_GROUNDIMPACT
if (mLandData->observationCount++ > 100) { if (mLandData->observationCount++ > 100) {
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
} }
#endif #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)
{}