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 Fixes to AutoTakeoff

Browse Source 
1. Fix autoyaw to 0 attitude on takeoff post launch.
2. An error condition abort now properly disarms and continues to contrain thrust and stabi during disarmed state.
3. Increase thrustdown time from 2 to 5 seconds on an error condition (e.g. if 3 m deviation from takeoff position on NE).
This commit is contained in:
abeck70 2015-05-25 22:19:01 +10:00
parent 50d60649aa
commit 7d66a075de
4 changed files with 24 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h
View File

@ -63,8 +63,7 @@ public:
private: private:
void UpdateVelocityDesired(void); void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); int8_t UpdateStabilizationDesired(void);
void setArmedIfChanged(FlightStatusArmedOptions val);
VtolAutoTakeoffFSM *fsm; VtolAutoTakeoffFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings; VtolPathFollowerSettingsData *vtolPathFollowerSettings;

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

@ -73,7 +73,6 @@ public:
void BoundThrust(float &ulow, float &uhigh); void BoundThrust(float &ulow, float &uhigh);
PathFollowerFSMState_T GetCurrentState(void); PathFollowerFSMState_T GetCurrentState(void);
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
void Abort(void);
uint8_t PositionHoldState(void); uint8_t PositionHoldState(void);
void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState); void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
@ -138,9 +137,6 @@ protected:
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(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason); void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
int32_t runState(); int32_t runState();
@ -150,7 +146,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_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE]; static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
}; };

40
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
View File

@ -30,6 +30,7 @@ extern "C" {
#include <math.h> #include <math.h>
#include <pid.h> #include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <sin_lookup.h> #include <sin_lookup.h>
#include <pathdesired.h> #include <pathdesired.h>
@ -208,7 +209,7 @@ void VtolAutoTakeoffController::UpdateVelocityDesired()
VelocityDesiredSet(&velocityDesired); VelocityDesiredSet(&velocityDesired);
} }
int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction) int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
{ {
uint8_t result = 1; uint8_t result = 1;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
@ -236,13 +237,8 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude,
ManualControlCommandData manualControl; ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl); ManualControlCommandGet(&manualControl);
if (yaw_attitude) { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control // default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
@ -259,29 +255,13 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
UpdateVelocityDesired(); UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm int8_t result = UpdateStabilizationDesired();
bool yaw_attitude = true; if (result) {
float yaw = 0.0f; AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
fsm->GetYaw(yaw_attitude, yaw); pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} }
PathStatusSet(pathStatus); PathStatusSet(pathStatus);
} }
void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
{
if (flightStatus->Armed != val) {
flightStatus->Armed = val;
FlightStatusSet(flightStatus);
}
}

61
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -65,7 +65,7 @@ extern "C" {
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) #define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND) #define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND) #define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND) #define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND)
#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f #define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = { VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
@ -77,8 +77,7 @@ VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM
[AUTOTAKEOFF_STATE_HOLD] = { .setup = &VtolAutoTakeoffFSM::setup_hold, .run = &VtolAutoTakeoffFSM::run_hold }, [AUTOTAKEOFF_STATE_HOLD] = { .setup = &VtolAutoTakeoffFSM::setup_hold, .run = &VtolAutoTakeoffFSM::run_hold },
[AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown }, [AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown },
[AUTOTAKEOFF_STATE_THRUSTOFF] = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff, .run = &VtolAutoTakeoffFSM::run_thrustoff }, [AUTOTAKEOFF_STATE_THRUSTOFF] = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff, .run = &VtolAutoTakeoffFSM::run_thrustoff },
[AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed }, [AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed }
[AUTOTAKEOFF_STATE_ABORT] = { .setup = &VtolAutoTakeoffFSM::setup_abort, .run = &VtolAutoTakeoffFSM::run_abort }
}; };
// pointer to a singleton instance // pointer to a singleton instance
@ -161,21 +160,12 @@ void VtolAutoTakeoffFSM::Activate()
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} }
void VtolAutoTakeoffFSM::Abort(void)
{
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void) PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{ {
switch (mAutoTakeoffData->currentState) { switch (mAutoTakeoffData->currentState) {
case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE: case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE; return PFFSM_STATE_INACTIVE;
break;
case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break; break;
case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED: case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED; return PFFSM_STATE_DISARMED;
@ -339,7 +329,7 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
} }
} }
@ -540,51 +530,26 @@ void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout
// STATE: DISARMED // STATE: DISARMED
void VtolAutoTakeoffFSM::setup_disarmed(void) void VtolAutoTakeoffFSM::setup_disarmed(void)
{ {
// nothing to do mAutoTakeoffData->thrustLimit = -1.0f;
mAutoTakeoffData->flConstrainThrust = false; mAutoTakeoffData->flConstrainThrust = true;
mAutoTakeoffData->flZeroStabiHorizontal = false; mAutoTakeoffData->flZeroStabiHorizontal = true;
mAutoTakeoffData->observationCount = 0; mAutoTakeoffData->observationCount = 0;
mAutoTakeoffData->boundThrustMin = -0.1f; mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f; mAutoTakeoffData->boundThrustMax = 0.0f;
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
} }
void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout) void VtolAutoTakeoffFSM::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 (mAutoTakeoffData->observationCount++ > 100) { if (mAutoTakeoffData->observationCount++ > 100) {
setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} }
#endif #endif
} }
void VtolAutoTakeoffFSM::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 VtolAutoTakeoffFSM::setup_abort(void)
{
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->flZeroStabiHorizontal = false;
fallback_to_hold();
}
void VtolAutoTakeoffFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
{}