1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00

OP-1760 Autotakeoff

Squashed commit as code reviews impossible otherwise due to dependencing on OP-1696
This commit is contained in:
abeck70 2015-04-12 21:39:31 +10:00
parent 73d3d3b869
commit a52969a76b
26 changed files with 1294 additions and 22 deletions

View File

@ -62,6 +62,7 @@ void plan_setup_returnToBase();
* @brief setup pathplanner/pathfollower for land
*/
void plan_setup_land();
void plan_setup_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for braking
@ -90,6 +91,12 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred);
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1 1
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2 2
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3 3
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH 0
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST 1
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN 2
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE 3
/**
* @brief setup pathfollower for positionvario
*/
@ -107,6 +114,7 @@ void plan_run_PositionRoam();
void plan_run_VelocityRoam();
void plan_run_HomeLeash();
void plan_run_AbsolutePosition();
void plan_run_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for AutoCruise

View File

@ -41,7 +41,9 @@
#include <attitudestate.h>
#include <vtolpathfollowersettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <sin_lookup.h>
#include <statusvtolautotakeoff.h>
#define UPDATE_EXPECTED 0.02f
#define UPDATE_MIN 1.0e-6f
@ -88,6 +90,7 @@ void plan_initialize()
VelocityStateInitialize();
VtolPathFollowerSettingsInitialize();
StabilizationBankInitialize();
StabilizationDesiredInitialize();
}
/**
@ -166,6 +169,137 @@ void plan_setup_returnToBase()
}
// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
// 1. Arming must be done whilst in the AutoTakeOff flight mode
// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
// 3. Wait for armed state
// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT 2.5f
static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = -velocity_down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)autotakeoffState;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down - AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
}
#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
void plan_setup_AutoTakeoff()
{
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
// We only allow takeoff if the state transition of disarmed to armed occurs
// whilst in the autotake flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
StabilizationDesiredData stabiDesired;
StabilizationDesiredGet(&stabiDesired);
// Are we inflight?
if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
// ok assume already in flight and just enter position hold
// if we are not actually inflight this will just be a violent autotakeoff
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
plan_setup_positionHold();
} else {
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
// Note that if this mode was invoked unintentionally whilst in flight, effectively
// all inputs get ignored and the vtol continues to fly to its previous
// stabi command.
}
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT 0.1f
void plan_run_AutoTakeoff()
{
StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
switch (autotakeoffState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (!flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
plan_setup_land();
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
// nothing to do. land has been requested. stay here for forever until mode change.
default:
break;
}
if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
if (priorState != autotakeoffState) {
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
}
static void plan_setup_land_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;

View File

@ -148,6 +148,7 @@ int32_t configuration_check()
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;

View File

@ -313,6 +313,9 @@ static bool okToArm(void)
case FLIGHTSTATUS_FLIGHTMODE_LAND:
return false;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
return true;
default:
return false;

View File

@ -403,6 +403,7 @@ static void manualControlTask(void)
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist) {
// Set the default thrust state
@ -527,6 +528,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
break;

View File

@ -101,6 +101,9 @@ void pathFollowerHandler(bool newinit)
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_setup_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_setup_AutoCruise();
break;
@ -145,6 +148,9 @@ void pathFollowerHandler(bool newinit)
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_run_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_run_AutoCruise();
break;

View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef VTOLAUTOTAKEOFFCONTROLLER_H
#define VTOLAUTOTAKEOFFCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class VtolAutoTakeoffFSM;
class VtolAutoTakeoffController : public PathFollowerControl {
private:
static VtolAutoTakeoffController *p_inst;
VtolAutoTakeoffController();
public:
static VtolAutoTakeoffController *instance()
{
if (!p_inst) {
p_inst = new VtolAutoTakeoffController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(uint8_t val);
VtolAutoTakeoffFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // VTOLAUTOTAKEOFFCONTROLLER_H

View File

@ -0,0 +1,158 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef VTOLAUTOTAKEOFFFSM_H
#define VTOLAUTOTAKEOFFFSM_H
extern "C" {
#include "statusvtolautotakeoff.h"
}
#include "pathfollowerfsm.h"
// AutoTakeoffing state machine
typedef enum {
AUTOTAKEOFF_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
AUTOTAKEOFF_STATE_CHECKSTATE, // Initial condition checks
AUTOTAKEOFF_STATE_SLOWSTART, // Slow start motors
AUTOTAKEOFF_STATE_THRUSTUP, // Ramp motors up to neutral thrust
AUTOTAKEOFF_STATE_TAKEOFF, // Ascend to target velocity
AUTOTAKEOFF_STATE_HOLD, // Hold position as completion of the sequence
AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
AUTOTAKEOFF_STATE_DISARMED, // Disarmed
AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to hold
AUTOTAKEOFF_STATE_SIZE
} PathFollowerFSM_AutoTakeoffState_T;
class VtolAutoTakeoffFSM : public PathFollowerFSM {
private:
static VtolAutoTakeoffFSM *p_inst;
VtolAutoTakeoffFSM();
public:
static VtolAutoTakeoffFSM *instance()
{
if (!p_inst) {
p_inst = new VtolAutoTakeoffFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus);
void Inactive(void);
void Activate(void);
void Update(void);
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);
protected:
// FSM instance data type
typedef struct {
StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
PathFollowerFSM_AutoTakeoffState_T currentState;
TakeOffLocationData takeOffLocation;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
float expectedAutoTakeoffPositionNorth;
float expectedAutoTakeoffPositionEast;
float thrustLimit;
float boundThrustMin;
float boundThrustMax;
uint8_t observationCount;
uint8_t observation2Count;
uint8_t flZeroStabiHorizontal;
uint8_t flConstrainThrust;
uint8_t flLowAltitude;
uint8_t flAltitudeHold;
} VtolAutoTakeoffFSMData_T;
// FSM state structure
typedef struct {
void(VtolAutoTakeoffFSM::*setup) (void); // Called to initialise the state
void(VtolAutoTakeoffFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_AutoTakeoffStateHandler_T;
// Private variables
VtolAutoTakeoffFSMData_T *mAutoTakeoffData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
FlightStatusData *flightStatus;
void setup_inactive(void);
void setup_checkstate(void);
void setup_slowstart(void);
void run_slowstart(uint8_t);
void setup_takeoff(void);
void run_takeoff(uint8_t);
void setup_hold(void);
void run_hold(uint8_t);
void setup_thrustup(void);
void run_thrustup(uint8_t);
void setup_thrustdown(void);
void run_thrustdown(uint8_t);
void setup_thrustoff(void);
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(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
int32_t runState();
int32_t runAlways();
void updateVtolAutoTakeoffFSMStatus();
void assessAltitude(void);
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
};
#endif // VTOLAUTOTAKEOFFFSM_H

View File

@ -81,12 +81,14 @@ extern "C" {
#include <pidstatus.h>
#include <homelocation.h>
#include <accelstate.h>
#include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <statusgrounddrive.h>
}
#include "pathfollowercontrol.h"
#include "vtollandcontroller.h"
#include "vtolautotakeoffcontroller.h"
#include "vtolvelocitycontroller.h"
#include "vtolbrakecontroller.h"
#include "vtolflycontroller.h"
@ -172,6 +174,7 @@ extern "C" int32_t PathFollowerInitialize()
PIDStatusInitialize();
StatusVtolLandInitialize();
StatusGroundDriveInitialize();
StatusVtolAutoTakeoffInitialize();
// VtolLandFSM additional objects
HomeLocationInitialize();
@ -206,6 +209,7 @@ void pathFollowerInitializeControllersForFrameType()
case FRAME_TYPE_HELI:
if (!multirotor_initialised) {
VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
VtolAutoTakeoffController::instance()->Initialize(&vtolPathFollowerSettings);
VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
@ -263,6 +267,10 @@ static void pathFollowerSetActiveController(void)
activeController = VtolLandController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_AUTOTAKEOFF:
activeController = VtolAutoTakeoffController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;

View File

@ -0,0 +1,286 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolautotakeoffcontroller.h"
#include "pathfollowerfsm.h"
#include "vtolautotakeofffsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
VtolAutoTakeoffController::VtolAutoTakeoffController()
: fsm(0), vtolPathFollowerSettings(0), mActive(false)
{}
// Called when mode first engaged
void VtolAutoTakeoffController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolAutoTakeoffController::IsActive(void)
{
return mActive;
}
uint8_t VtolAutoTakeoffController::Mode(void)
{
return PATHDESIRED_MODE_AUTOTAKEOFF;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolAutoTakeoffController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
}
void VtolAutoTakeoffController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
// AutoTakeoff Uses different vertical velocity PID.
void VtolAutoTakeoffController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kp,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Ki,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kd,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
// AutoTakeoff Uses a different FSM to its parent
int32_t VtolAutoTakeoffController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
if (fsm == 0) {
fsm = VtolAutoTakeoffFSM::instance();
VtolAutoTakeoffFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus);
controlDown.Initialize(fsm);
}
return 0;
}
void VtolAutoTakeoffController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
PositionStateData positionState;
PositionStateGet(&positionState);
if (fsm->PositionHoldState()) {
controlDown.UpdatePositionState(positionState.Down);
controlDown.ControlPosition();
}
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// autotakeoff flight mode has stored original horizontal position in pathdesired
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
if (fsm->PositionHoldState()) {
pathStatus->fractional_progress = 1.0f;
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
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;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolAutoTakeoffController::UpdateAutoPilot()
{
fsm->Update();
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);
}
PathStatusSet(pathStatus);
}
void VtolAutoTakeoffController::setArmedIfChanged(uint8_t val)
{
if (flightStatus->Armed != val) {
flightStatus->Armed = val;
FlightStatusSet(flightStatus);
}
}

View File

@ -0,0 +1,569 @@
/*
******************************************************************************
*
* @file vtolautotakeofffsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This autotakeoff state machine is a helper state machine to the
* VtolAutoTakeoffController.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolautotakeoff.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtolautotakeofffsm.h>
// Private constants
#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 AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
[AUTOTAKEOFF_STATE_INACTIVE] = { .setup = &VtolAutoTakeoffFSM::setup_inactive, .run = 0 },
[AUTOTAKEOFF_STATE_CHECKSTATE] = { .setup = &VtolAutoTakeoffFSM::setup_checkstate, .run = 0 },
[AUTOTAKEOFF_STATE_SLOWSTART] = { .setup = &VtolAutoTakeoffFSM::setup_slowstart, .run = &VtolAutoTakeoffFSM::run_slowstart },
[AUTOTAKEOFF_STATE_THRUSTUP] = { .setup = &VtolAutoTakeoffFSM::setup_thrustup, .run = &VtolAutoTakeoffFSM::run_thrustup },
[AUTOTAKEOFF_STATE_TAKEOFF] = { .setup = &VtolAutoTakeoffFSM::setup_takeoff, .run = &VtolAutoTakeoffFSM::run_takeoff },
[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 }
};
// pointer to a singleton instance
VtolAutoTakeoffFSM *VtolAutoTakeoffFSM::p_inst = 0;
VtolAutoTakeoffFSM::VtolAutoTakeoffFSM()
: mAutoTakeoffData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mAutoTakeoffData == 0) {
mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T));
PIOS_Assert(mAutoTakeoffData);
}
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolAutoTakeoffFSM::Inactive(void)
{
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0);
initFSM();
}
// Initialise the FSM
void VtolAutoTakeoffFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} else {
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
}
}
void VtolAutoTakeoffFSM::Activate()
{
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0);
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->flLowAltitude = true;
mAutoTakeoffData->flAltitudeHold = false;
mAutoTakeoffData->boundThrustMin = 0.0f;
mAutoTakeoffData->boundThrustMax = 0.0f;
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[AUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
assessAltitude();
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
void VtolAutoTakeoffFSM::Abort(void)
{
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{
switch (mAutoTakeoffData->currentState) {
case AUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case AUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case AUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolAutoTakeoffFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolAutoTakeoffFSM::runState(void)
{
uint8_t flTimeout = false;
mAutoTakeoffData->stateRunCount++;
if (mAutoTakeoffData->stateTimeoutCount > 0 && mAutoTakeoffData->stateRunCount > mAutoTakeoffData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run) {
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolAutoTakeoffFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolAutoTakeoffFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mAutoTakeoffData->boundThrustMin;
uhigh = mAutoTakeoffData->boundThrustMax;
if (mAutoTakeoffData->flConstrainThrust) {
uhigh = mAutoTakeoffData->thrustLimit;
}
}
void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mAutoTakeoffData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
if (mAutoTakeoffData->currentState == newState) {
return;
}
mAutoTakeoffData->currentState = newState;
if (newState != AUTOTAKEOFF_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mAutoTakeoffData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mAutoTakeoffData->stateTimeoutCount = 0;
if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup) {
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
}
updateVtolAutoTakeoffFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
{
mAutoTakeoffData->stateTimeoutCount = count;
}
void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
{
mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
if (mAutoTakeoffData->flLowAltitude) {
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_LOW;
} else {
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_HIGH;
}
StatusVtolAutoTakeoffSet(&mAutoTakeoffData->fsmAutoTakeoffStatus);
}
void VtolAutoTakeoffFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
mAutoTakeoffData->flLowAltitude = false;
} else {
mAutoTakeoffData->flLowAltitude = true;
}
}
// Action the required state from plans.c
void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOptions controlState)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = controlState;
switch (controlState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
setState(AUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
}
}
// State: INACTIVE
void VtolAutoTakeoffFSM::setup_inactive(void)
{
// Re-initialise local variables
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->flConstrainThrust = false;
}
// State: CHECKSTATE
void VtolAutoTakeoffFSM::setup_checkstate(void)
{
// Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
// 1. Already armed
// 2. Not in flight. This was checked in plans.c
// 3. User has placed throttle position to more than 50% to allow autotakeoff
// If pathplanner, we may need additional checks???
// E.g. if inflight, this mode is just positon hold??
// Sanity check arm state and thrust state
// TODO Orientation checks
// Start from a enforced thrust off condition
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
setState(AUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
// STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved
void VtolAutoTakeoffFSM::setup_slowstart(void)
{
setStateTimeout(TIMEOUT_SLOWSTART);
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum1 = (vtolPathFollowerSettings->ThrustLimits.Min - 0.05f) / (float)TIMEOUT_SLOWSTART;
mAutoTakeoffData->boundThrustMin = 0.05f;
mAutoTakeoffData->boundThrustMax = 0.05f;
PositionStateData positionState;
PositionStateGet(&positionState);
mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North;
mAutoTakeoffData->expectedAutoTakeoffPositionEast = positionState.East;
}
void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout)
{
// increase thrust setpoint step by step
if (mAutoTakeoffData->boundThrustMin < vtolPathFollowerSettings->ThrustLimits.Min) {
mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
}
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
if (mAutoTakeoffData->boundThrustMax > vtolPathFollowerSettings->ThrustLimits.Min) {
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Min;
}
if (flTimeout) {
setState(AUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved
void VtolAutoTakeoffFSM::setup_thrustup(void)
{
setStateTimeout(TIMEOUT_THRUSTUP);
mAutoTakeoffData->flZeroStabiHorizontal = false;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum1 = (0.8f * vtolPathFollowerSettings->ThrustLimits.Max - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
mAutoTakeoffData->sum2 = 0.8f * vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
}
void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
{
// increase thrust setpoint step by step
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
}
if (flTimeout) {
setState(AUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: TAKEOFF
void VtolAutoTakeoffFSM::setup_takeoff(void)
{
mAutoTakeoffData->flZeroStabiHorizontal = false;
}
void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
return;
}
// detect broad sideways drift.
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mAutoTakeoffData->expectedAutoTakeoffPositionNorth - positionState.North;
float east_error = mAutoTakeoffData->expectedAutoTakeoffPositionEast - positionState.East;
float down_error = pathDesired->End.Down - positionState.Down;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 3.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
return;
}
if (fabsf(down_error) < 0.5f) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
return;
}
}
// STATE: HOLD
void VtolAutoTakeoffFSM::setup_hold(void)
{
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->flAltitudeHold = true;
}
void VtolAutoTakeoffFSM::run_hold(__attribute__((unused)) uint8_t flTimeout)
{}
uint8_t VtolAutoTakeoffFSM::PositionHoldState(void)
{
return mAutoTakeoffData->flAltitudeHold;
}
// STATE: THRUSTDOWN
void VtolAutoTakeoffFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mAutoTakeoffData->flZeroStabiHorizontal = true;
mAutoTakeoffData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->thrustLimit = stabDesired.Thrust;
mAutoTakeoffData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Neutral;
}
void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mAutoTakeoffData->thrustLimit -= mAutoTakeoffData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolAutoTakeoffFSM::setup_thrustoff(void)
{
mAutoTakeoffData->thrustLimit = -1.0f;
mAutoTakeoffData->flConstrainThrust = true;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
}
void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(AUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolAutoTakeoffFSM::setup_disarmed(void)
{
// nothing to do
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->observationCount = 0;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
}
void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#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)
{}

View File

@ -40,8 +40,6 @@ extern "C" {
#include <homelocation.h>
#include <accelstate.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
@ -563,7 +561,7 @@ void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTim
void VtolLandFSM::setup_groundeffect(void)
{
setStateTimeout(TIMEOUT_GROUNDEFFECT);
mLandData->flZeroStabiHorizontal = true;
mLandData->flZeroStabiHorizontal = false;
PositionStateData positionState;
PositionStateGet(&positionState);
mLandData->expectedLandPositionNorth = positionState.North;

View File

@ -20,6 +20,7 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats

View File

@ -20,8 +20,9 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired

View File

@ -25,6 +25,7 @@
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats

View File

@ -90,6 +90,7 @@ modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type)
case MapDataDelegate::MODE_FOLLOWVECTOR:
case MapDataDelegate::MODE_VELOCITY:
case MapDataDelegate::MODE_LAND:
case MapDataDelegate::MODE_AUTOTAKEOFF:
case MapDataDelegate::MODE_BRAKE:
return OVERLAY_LINE;

View File

@ -114,6 +114,7 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa
combo->addItem("Set Accessory", MODE_SETACCESSORY);
combo->addItem("Disarm Alarm", MODE_DISARMALARM);
combo->addItem("Land", MODE_LAND);
combo->addItem("AutoTakeoff", MODE_AUTOTAKEOFF);
combo->addItem("Brake", MODE_BRAKE);
combo->addItem("Velocity", MODE_VELOCITY);

View File

@ -38,7 +38,7 @@ class MapDataDelegate : public QItemDelegate {
public:
typedef enum { MODE_GOTOENDPOINT = 0, MODE_FOLLOWVECTOR = 1, MODE_CIRCLERIGHT = 2, MODE_CIRCLELEFT = 3,
MODE_FIXEDATTITUDE = 8, MODE_SETACCESSORY = 9, MODE_DISARMALARM = 10, MODE_LAND = 11,
MODE_BRAKE = 12, MODE_VELOCITY = 13 } ModeOptions;
MODE_BRAKE = 12, MODE_VELOCITY = 13, MODE_AUTOTAKEOFF = 14 } ModeOptions;
typedef enum { ENDCONDITION_NONE = 0, ENDCONDITION_TIMEOUT = 1, ENDCONDITION_DISTANCETOTARGET = 2,
ENDCONDITION_LEGREMAINING = 3, ENDCONDITION_BELOWERROR = 4, ENDCONDITION_ABOVEALTITUDE = 5,

View File

@ -28,6 +28,7 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += \
$$UAVOBJECT_SYNTHETICS/statusgrounddrive.h \
$$UAVOBJECT_SYNTHETICS/statusvtolautotakeoff.h \
$$UAVOBJECT_SYNTHETICS/pidstatus.h \
$$UAVOBJECT_SYNTHETICS/statusvtolland.h \
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.h \
@ -138,6 +139,7 @@ HEADERS += \
SOURCES += \
$$UAVOBJECT_SYNTHETICS/statusgrounddrive.cpp \
$$UAVOBJECT_SYNTHETICS/statusvtolautotakeoff.cpp \
$$UAVOBJECT_SYNTHETICS/pidstatus.cpp \
$$UAVOBJECT_SYNTHETICS/statusvtolland.cpp \
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.cpp \

View File

@ -78,31 +78,31 @@
units=""
type="enum"
elements="6"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise"
options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"
defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6"
limits="\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;\
\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,\
%0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise,AutoTakeoff\
%0903NE:POI:PathPlanner:AutoCruise;"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
@ -112,6 +112,7 @@
<field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
<field name="ReturnToBaseNextCommand" units="" type="enum" elements="1" options="Hold,Land" defaultvalue="Hold"/>
<field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
<field name="AutoTakeOffVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
<access gcs="readwrite" flight="readwrite"/>

View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"/>
<!-- the options for FlightModeAssist needs to match the StablizationSettings' assist options -->
<field name="FlightModeAssist" units="" type="enum" elements="1" options="None,GPSAssist_PrimaryThrust,GPSAssist"/>
<field name="AssistedControlState" units="" type="enum" elements="1" options="Primary,Brake,Hold" defaultvalue="Primary"/>

View File

@ -4,7 +4,7 @@
<!-- ensure the following Mode options are exactly the same as in pathdesired mode -->
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity,AutoTakeoff" default="GoToEndpoint" />
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,

View File

@ -9,7 +9,7 @@
<!-- ensure the following Mode options are exactly the same as in pathaction mode -->
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity,AutoTakeoff" default="GoToEndpoint" />
<!-- Endpoint mode - move directly towards endpoint regardless of position -->
<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->

View File

@ -12,7 +12,7 @@
<field name="brakeRateActualDesiredRatio" units="" type="float" elements="1"/>
<field name="velocityIntoHold" units="m/s" type="float" elements="1"/>
<field name="Mode" units="" type="enum" elements="1" options="GoToEndpoint,FollowVector,CircleRight,CircleLeft,
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity" default="GoToEndpoint" />
FixedAttitude,SetAccessory,DisarmAlarm,Land,Brake,Velocity,AutoTakeoff" default="GoToEndpoint" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

View File

@ -0,0 +1,14 @@
<xml>
<object name="StatusVtolAutoTakeoff" singleinstance="true" settings="false" category="Navigation">
<description>Status of a AutoTakeoff autopilot</description>
<field name="State" units="" type="enum" elements="1" options="Inactive,CheckState,SlowStart,ThrustUp,Takeoff,Hold,ThrustDown,ThrustOff,Disarmed, Abort" default="0"/>
<field name="AltitudeAtState" units="m" type="float" elements="10" default="0"/>
<field name="StateExitReason" units="" type="enum" elements="10" options="None,ArrivedAtAlt,ZeroThrust,PositionError,Timeout" default="0"/>
<field name="AltitudeState" units="" type="enum" elements="1" options="High,Low" default="0"/>
<field name="ControlState" units="" type="enum" elements="1" options="WaitForArmed,WaitForMidThrottle,RequireUnarmedFirst,Initiate,PositionHold,Abort" default="0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="500"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

View File

@ -24,6 +24,7 @@
<field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="12.0, 0.0, 0.03, 15"/>
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
<field name="AutoTakeoffVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>