mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-21 13:28:58 +01:00
OP-1760 Autotakeoff
Squashed commit as code reviews impossible otherwise due to dependencing on OP-1696
This commit is contained in:
abeck70
parent
73d3d3b869
commit
a52969a76b
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -313,6 +313,9 @@ static bool okToArm(void)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
return false;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
|
||||
return true;
|
||||
|
||||
default:
|
||||
return false;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
76
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h
Normal file
76
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h
Normal 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
|
||||
158
flight/modules/PathFollower/inc/vtolautotakeofffsm.h
Normal file
158
flight/modules/PathFollower/inc/vtolautotakeofffsm.h
Normal 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
|
||||
@ -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;
|
||||
|
||||
286
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
Normal file
286
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
Normal 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);
|
||||
}
|
||||
}
|
||||
569
flight/modules/PathFollower/vtolautotakeofffsm.cpp
Normal file
569
flight/modules/PathFollower/vtolautotakeofffsm.cpp
Normal 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)
|
||||
{}
|
||||
@ -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;
|
||||
|
||||
@ -20,6 +20,7 @@
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += statusgrounddrive
|
||||
UAVOBJSRCFILENAMES += statusvtolautotakeoff
|
||||
UAVOBJSRCFILENAMES += pidstatus
|
||||
UAVOBJSRCFILENAMES += statusvtolland
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
|
||||
@ -20,8 +20,9 @@
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += statusgrounddrive
|
||||
UAVOBJSRCFILENAMES += pidstatus
|
||||
UAVOBJSRCFILENAMES += statusvtolland
|
||||
UAVOBJSRCFILENAMES += statusvtolautotakeoff
|
||||
UAVOBJSRCFILENAMES += pidstatus
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
|
||||
@ -25,6 +25,7 @@
|
||||
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += statusgrounddrive
|
||||
UAVOBJSRCFILENAMES += statusvtolautotakeoff
|
||||
UAVOBJSRCFILENAMES += pidstatus
|
||||
UAVOBJSRCFILENAMES += statusvtolland
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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 \
|
||||
|
||||
@ -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"/>
|
||||
|
||||
@ -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"/>
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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 -->
|
||||
|
||||
@ -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"/>
|
||||
|
||||
14
shared/uavobjectdefinition/statusvtolautotakeoff.xml
Normal file
14
shared/uavobjectdefinition/statusvtolautotakeoff.xml
Normal 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>
|
||||
@ -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"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user