mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
7d66a075de
1. Fix autoyaw to 0 attitude on takeoff post launch. 2. An error condition abort now properly disarms and continues to contrain thrust and stabi during disarmed state. 3. Increase thrustdown time from 2 to 5 seconds on an error condition (e.g. if 3 m deviation from takeoff position on NE).
556 lines
20 KiB
C++
556 lines
20 KiB
C++
/*
|
|
******************************************************************************
|
|
*
|
|
* @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 (5 * 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 }
|
|
};
|
|
|
|
// 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, 0, sizeof(VtolAutoTakeoffFSMData_T));
|
|
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
|
|
pathDesired = ptr_pathDesired;
|
|
flightStatus = ptr_flightStatus;
|
|
initFSM();
|
|
|
|
return 0;
|
|
}
|
|
|
|
void VtolAutoTakeoffFSM::Inactive(void)
|
|
{
|
|
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
|
|
initFSM();
|
|
}
|
|
|
|
// Initialise the FSM
|
|
void VtolAutoTakeoffFSM::initFSM(void)
|
|
{
|
|
if (vtolPathFollowerSettings != 0) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
} else {
|
|
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
|
|
}
|
|
}
|
|
|
|
void VtolAutoTakeoffFSM::Activate()
|
|
{
|
|
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
|
|
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_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[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
|
|
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
|
|
assessAltitude();
|
|
|
|
// Check if we are already flying. This can happen in pathplanner mode
|
|
// going into a second loop of the waypoints.
|
|
StabilizationDesiredData stabDesired;
|
|
StabilizationDesiredGet(&stabDesired);
|
|
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
return;
|
|
}
|
|
|
|
// initially inactive and wait for a change in controlstate.
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
}
|
|
|
|
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
|
|
{
|
|
switch (mAutoTakeoffData->currentState) {
|
|
case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
|
|
return PFFSM_STATE_INACTIVE;
|
|
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_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(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
|
|
{
|
|
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
|
|
|
|
if (mAutoTakeoffData->currentState == newState) {
|
|
return;
|
|
}
|
|
mAutoTakeoffData->currentState = newState;
|
|
|
|
if (newState != STATUSVTOLAUTOTAKEOFF_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(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
break;
|
|
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, 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 need additional checks
|
|
// E.g. if inflight, this mode is just positon hol
|
|
StabilizationDesiredData stabDesired;
|
|
|
|
StabilizationDesiredGet(&stabDesired);
|
|
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
return;
|
|
}
|
|
|
|
|
|
// Start from a enforced thrust off condition
|
|
mAutoTakeoffData->flConstrainThrust = false;
|
|
mAutoTakeoffData->boundThrustMin = -0.1f;
|
|
mAutoTakeoffData->boundThrustMax = 0.0f;
|
|
|
|
setState(STATUSVTOLAUTOTAKEOFF_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
|
|
#define SLOWSTART_INITIAL_THRUST 0.05f // assumed to be less than vtol min
|
|
void VtolAutoTakeoffFSM::setup_slowstart(void)
|
|
{
|
|
setStateTimeout(TIMEOUT_SLOWSTART);
|
|
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
|
|
StabilizationDesiredData stabDesired;
|
|
StabilizationDesiredGet(&stabDesired);
|
|
float vtol_thrust_min = vtolPathFollowerSettings->ThrustLimits.Min;
|
|
if (vtol_thrust_min < SLOWSTART_INITIAL_THRUST) {
|
|
vtol_thrust_min = SLOWSTART_INITIAL_THRUST;
|
|
}
|
|
mAutoTakeoffData->sum1 = (vtol_thrust_min - SLOWSTART_INITIAL_THRUST) / (float)TIMEOUT_SLOWSTART;
|
|
mAutoTakeoffData->sum2 = vtol_thrust_min;
|
|
mAutoTakeoffData->boundThrustMin = SLOWSTART_INITIAL_THRUST;
|
|
mAutoTakeoffData->boundThrustMax = SLOWSTART_INITIAL_THRUST;
|
|
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 < mAutoTakeoffData->sum2) {
|
|
mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
|
|
}
|
|
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
|
|
if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
|
|
mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
|
|
}
|
|
|
|
if (flTimeout) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_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
|
|
#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
|
|
void VtolAutoTakeoffFSM::setup_thrustup(void)
|
|
{
|
|
setStateTimeout(TIMEOUT_THRUSTUP);
|
|
mAutoTakeoffData->flZeroStabiHorizontal = false;
|
|
StabilizationDesiredData stabDesired;
|
|
StabilizationDesiredGet(&stabDesired);
|
|
mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
|
|
mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
|
|
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(STATUSVTOLAUTOTAKEOFF_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(STATUSVTOLAUTOTAKEOFF_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(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
|
|
return;
|
|
}
|
|
if (fabsf(down_error) < 0.5f) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_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(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
|
|
}
|
|
|
|
if (flTimeout) {
|
|
setState(STATUSVTOLAUTOTAKEOFF_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(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
}
|
|
|
|
// STATE: DISARMED
|
|
void VtolAutoTakeoffFSM::setup_disarmed(void)
|
|
{
|
|
mAutoTakeoffData->thrustLimit = -1.0f;
|
|
mAutoTakeoffData->flConstrainThrust = true;
|
|
mAutoTakeoffData->flZeroStabiHorizontal = true;
|
|
mAutoTakeoffData->observationCount = 0;
|
|
mAutoTakeoffData->boundThrustMin = -0.1f;
|
|
mAutoTakeoffData->boundThrustMax = 0.0f;
|
|
|
|
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
}
|
|
|
|
void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
|
|
{
|
|
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
|
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
|
}
|
|
#ifdef DEBUG_GROUNDIMPACT
|
|
if (mAutoTakeoffData->observationCount++ > 100) {
|
|
setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
|
|
}
|
|
#endif
|
|
}
|