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:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(FlightStatusArmedOptions val);
int8_t UpdateStabilizationDesired(void);
VtolAutoTakeoffFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;

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

@ -73,7 +73,6 @@ public:
void BoundThrust(float &ulow, float &uhigh);
PathFollowerFSMState_T GetCurrentState(void);
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
void Abort(void);
uint8_t PositionHoldState(void);
void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
@ -138,9 +137,6 @@ protected:
void setup_disarmed(void);
void run_disarmed(uint8_t);
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void);
void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
int32_t runState();
@ -150,7 +146,6 @@ protected:
void assessAltitude(void);
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
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 <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -208,7 +209,7 @@ void VtolAutoTakeoffController::UpdateVelocityDesired()
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
int8_t VtolAutoTakeoffController::UpdateStabilizationDesired()
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
@ -236,13 +237,8 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude,
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
@ -259,29 +255,13 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
int8_t result = UpdateStabilizationDesired();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
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 TIMEOUT_SLOWSTART (2 * 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
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_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown },
[AUTOTAKEOFF_STATE_THRUSTOFF] = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff, .run = &VtolAutoTakeoffFSM::run_thrustoff },
[AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed },
[AUTOTAKEOFF_STATE_ABORT] = { .setup = &VtolAutoTakeoffFSM::setup_abort, .run = &VtolAutoTakeoffFSM::run_abort }
[AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed }
};
// pointer to a singleton instance
@ -161,21 +160,12 @@ void VtolAutoTakeoffFSM::Activate()
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
void VtolAutoTakeoffFSM::Abort(void)
{
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{
switch (mAutoTakeoffData->currentState) {
case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
@ -339,7 +329,7 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
}
}
@ -540,51 +530,26 @@ void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout
// STATE: DISARMED
void VtolAutoTakeoffFSM::setup_disarmed(void)
{
// nothing to do
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->thrustLimit = -1.0f;
mAutoTakeoffData->flConstrainThrust = true;
mAutoTakeoffData->flZeroStabiHorizontal = true;
mAutoTakeoffData->observationCount = 0;
mAutoTakeoffData->boundThrustMin = -0.1f;
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)
{
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
#ifdef DEBUG_GROUNDIMPACT
if (mAutoTakeoffData->observationCount++ > 100) {
setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
#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)
{}