flight/modules/PathFollower/vtolautotakeofffsm.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtolautotakeofffsm.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      This autotakeoff state machine is a helper state machine to the
   7  *             VtolAutoTakeoffController.
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 extern "C" {
  28 #include <openpilot.h>
  29
  30 #include <math.h>
  31 #include <pid.h>
  32 #include <pathdesired.h>
  33 #include <paths.h>
  34 #include "plans.h"
  35 #include <sanitycheck.h>
  36
  37 #include <homelocation.h>
  38 #include <accelstate.h>
  39 #include <vtolpathfollowersettings.h>
  40 #include <flightstatus.h>
  41 #include <flightmodesettings.h>
  42 #include <pathstatus.h>
  43 #include <positionstate.h>
  44 #include <velocitystate.h>
  45 #include <velocitydesired.h>
  46 #include <stabilizationdesired.h>
  47 #include <attitudestate.h>
  48 #include <takeofflocation.h>
  49 #include <manualcontrolcommand.h>
  50 #include <systemsettings.h>
  51 #include <stabilizationbank.h>
  52 #include <stabilizationdesired.h>
  53 #include <vtolselftuningstats.h>
  54 #include <statusvtolautotakeoff.h>
  55 #include <pathsummary.h>
  56 }
  57
  58 // C++ includes
  59 #include <vtolautotakeofffsm.h>
  60
  61
  62 // Private constants
  63 #define TIMER_COUNT_PER_SECOND         (1000 / vtolPathFollowerSettings->UpdatePeriod)
  64 #define TIMEOUT_SLOWSTART              (2 * TIMER_COUNT_PER_SECOND)
  65 #define TIMEOUT_THRUSTUP               (1 * TIMER_COUNT_PER_SECOND)
  66 #define TIMEOUT_THRUSTDOWN             (5 * TIMER_COUNT_PER_SECOND)
  67 #define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
  68
  69 VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
  70     [AUTOTAKEOFF_STATE_INACTIVE]   = { .setup = &VtolAutoTakeoffFSM::setup_inactive,   .run = 0                                   },
  71     [AUTOTAKEOFF_STATE_CHECKSTATE] = { .setup = &VtolAutoTakeoffFSM::setup_checkstate, .run = 0                                   },
  72     [AUTOTAKEOFF_STATE_SLOWSTART]  = { .setup = &VtolAutoTakeoffFSM::setup_slowstart,  .run = &VtolAutoTakeoffFSM::run_slowstart  },
  73     [AUTOTAKEOFF_STATE_THRUSTUP]   = { .setup = &VtolAutoTakeoffFSM::setup_thrustup,   .run = &VtolAutoTakeoffFSM::run_thrustup   },
  74     [AUTOTAKEOFF_STATE_TAKEOFF]    = { .setup = &VtolAutoTakeoffFSM::setup_takeoff,    .run = &VtolAutoTakeoffFSM::run_takeoff    },
  75     [AUTOTAKEOFF_STATE_HOLD] =       { .setup = &VtolAutoTakeoffFSM::setup_hold,       .run = &VtolAutoTakeoffFSM::run_hold       },
  76     [AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown },
  77     [AUTOTAKEOFF_STATE_THRUSTOFF]  = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff,  .run = &VtolAutoTakeoffFSM::run_thrustoff  },
  78     [AUTOTAKEOFF_STATE_DISARMED]   = { .setup = &VtolAutoTakeoffFSM::setup_disarmed,   .run = &VtolAutoTakeoffFSM::run_disarmed   }
  79 };
  80
  81 // pointer to a singleton instance
  82 VtolAutoTakeoffFSM *VtolAutoTakeoffFSM::p_inst = 0;
  83
  84
  85 VtolAutoTakeoffFSM::VtolAutoTakeoffFSM()
  86     : mAutoTakeoffData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
  87 {}
  88
  89 // Private types
  90
  91 // Private functions
  92 // Public API methods
  93 /**
  94  * Initialise the module, called on startup
  95  * \returns 0 on success or -1 if initialisation failed
  96  */
  97 int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
  98                                        PathDesiredData *ptr_pathDesired,
  99                                        FlightStatusData *ptr_flightStatus)
 100 {
 101     PIOS_Assert(ptr_vtolPathFollowerSettings);
 102     PIOS_Assert(ptr_pathDesired);
 103     PIOS_Assert(ptr_flightStatus);
 104
 105     if (mAutoTakeoffData == 0) {
 106         mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T));
 107         PIOS_Assert(mAutoTakeoffData);
 108     }
 109     memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
 110     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 111     pathDesired  = ptr_pathDesired;
 112     flightStatus = ptr_flightStatus;
 113     initFSM();
 114
 115     return 0;
 116 }
 117
 118 void VtolAutoTakeoffFSM::Inactive(void)
 119 {
 120     memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
 121     initFSM();
 122 }
 123
 124 // Initialise the FSM
 125 void VtolAutoTakeoffFSM::initFSM(void)
 126 {
 127     if (vtolPathFollowerSettings != 0) {
 128         setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 129     } else {
 130         mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
 131     }
 132 }
 133
 134 void VtolAutoTakeoffFSM::Activate()
 135 {
 136     memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
 137     mAutoTakeoffData->currentState   = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
 138     mAutoTakeoffData->flLowAltitude  = true;
 139     mAutoTakeoffData->flAltitudeHold = false;
 140     mAutoTakeoffData->boundThrustMin = 0.0f;
 141     mAutoTakeoffData->boundThrustMax = 0.0f;
 142     mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
 143     TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
 144     mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
 145     mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
 146     assessAltitude();
 147
 148     // Check if we are already flying.  This can happen in pathplanner mode
 149     // going into a second loop of the waypoints.
 150     StabilizationDesiredData stabDesired;
 151     StabilizationDesiredGet(&stabDesired);
 152     if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
 153         setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 154         return;
 155     }
 156
 157     // initially inactive and wait for a change in controlstate.
 158     setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 159 }
 160
 161 PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
 162 {
 163     switch (mAutoTakeoffData->currentState) {
 164     case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
 165         return PFFSM_STATE_INACTIVE;
 166
 167         break;
 168     case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
 169         return PFFSM_STATE_DISARMED;
 170
 171         break;
 172     default:
 173         return PFFSM_STATE_ACTIVE;
 174
 175         break;
 176     }
 177 }
 178
 179 void VtolAutoTakeoffFSM::Update()
 180 {
 181     runState();
 182     if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
 183         runAlways();
 184     }
 185 }
 186
 187 int32_t VtolAutoTakeoffFSM::runState(void)
 188 {
 189     uint8_t flTimeout = false;
 190
 191     mAutoTakeoffData->stateRunCount++;
 192
 193     if (mAutoTakeoffData->stateTimeoutCount > 0 && mAutoTakeoffData->stateRunCount > mAutoTakeoffData->stateTimeoutCount) {
 194         flTimeout = true;
 195     }
 196
 197     // If the current state has a static function, call it
 198     if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run) {
 199         (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run)(flTimeout);
 200     }
 201     return 0;
 202 }
 203
 204 int32_t VtolAutoTakeoffFSM::runAlways(void)
 205 {
 206     void assessAltitude(void);
 207
 208     return 0;
 209 }
 210
 211 // PathFollower implements the PID scheme and has a objective
 212 // set by a PathDesired object.  Based on the mode, pathfollower
 213 // uses FSM's as helper functions that manage state and event detection.
 214 // PathFollower calls into FSM methods to alter its commands.
 215
 216 void VtolAutoTakeoffFSM::BoundThrust(float &ulow, float &uhigh)
 217 {
 218     ulow  = mAutoTakeoffData->boundThrustMin;
 219     uhigh = mAutoTakeoffData->boundThrustMax;
 220
 221
 222     if (mAutoTakeoffData->flConstrainThrust) {
 223         uhigh = mAutoTakeoffData->thrustLimit;
 224     }
 225 }
 226
 227 void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
 228 {
 229     if (mAutoTakeoffData->flZeroStabiHorizontal && stabDesired) {
 230         stabDesired->Pitch = 0.0f;
 231         stabDesired->Roll  = 0.0f;
 232         stabDesired->Yaw   = 0.0f;
 233     }
 234 }
 235
 236 // Set the new state and perform setup for subsequent state run calls
 237 // This is called by state run functions on event detection that drive
 238 // state transitions.
 239 void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
 240 {
 241     mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
 242
 243     if (mAutoTakeoffData->currentState == newState) {
 244         return;
 245     }
 246     mAutoTakeoffData->currentState = newState;
 247
 248     if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
 249         PositionStateData positionState;
 250         PositionStateGet(&positionState);
 251         float takeOffDown = 0.0f;
 252         if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
 253             takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
 254         }
 255         mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
 256         assessAltitude();
 257     }
 258
 259     // Restart state timer counter
 260     mAutoTakeoffData->stateRunCount     = 0;
 261
 262     // Reset state timeout to disabled/zero
 263     mAutoTakeoffData->stateTimeoutCount = 0;
 264
 265     if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup) {
 266         (this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
 267     }
 268
 269     updateVtolAutoTakeoffFSMStatus();
 270 }
 271
 272
 273 // Timeout utility function for use by state init implementations
 274 void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
 275 {
 276     mAutoTakeoffData->stateTimeoutCount = count;
 277 }
 278
 279 void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
 280 {
 281     mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
 282     if (mAutoTakeoffData->flLowAltitude) {
 283         mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_LOW;
 284     } else {
 285         mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_HIGH;
 286     }
 287     StatusVtolAutoTakeoffSet(&mAutoTakeoffData->fsmAutoTakeoffStatus);
 288 }
 289
 290
 291 void VtolAutoTakeoffFSM::assessAltitude(void)
 292 {
 293     float positionDown;
 294
 295     PositionStateDownGet(&positionDown);
 296     float takeOffDown = 0.0f;
 297     if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
 298         takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
 299     }
 300     float positionDownRelativeToTakeoff = positionDown - takeOffDown;
 301     if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
 302         mAutoTakeoffData->flLowAltitude = false;
 303     } else {
 304         mAutoTakeoffData->flLowAltitude = true;
 305     }
 306 }
 307
 308 // Action the required state from plans.c
 309 void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOptions controlState)
 310 {
 311     mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = controlState;
 312
 313     switch (controlState) {
 314     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
 315         setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 316         break;
 317     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
 318         setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 319         break;
 320     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
 321         setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 322         break;
 323     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
 324         setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 325         break;
 326     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
 327         setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 328         break;
 329     case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
 330         setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 331         break;
 332     }
 333 }
 334
 335
 336 // State: INACTIVE
 337 void VtolAutoTakeoffFSM::setup_inactive(void)
 338 {
 339     // Re-initialise local variables
 340     mAutoTakeoffData->flZeroStabiHorizontal = false;
 341     mAutoTakeoffData->flConstrainThrust     = false;
 342 }
 343
 344 // State: CHECKSTATE
 345 void VtolAutoTakeoffFSM::setup_checkstate(void)
 346 {
 347     // Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
 348     // 1. Already armed
 349     // 2. Not in flight. This was checked in plans.c
 350     // 3. User has placed throttle position to more than 50% to allow autotakeoff
 351
 352     // If pathplanner, we need additional checks
 353     // E.g. if inflight, this mode is just positon hol
 354     StabilizationDesiredData stabDesired;
 355
 356     StabilizationDesiredGet(&stabDesired);
 357     if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
 358         setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 359         return;
 360     }
 361
 362
 363     // Start from a enforced thrust off condition
 364     mAutoTakeoffData->flConstrainThrust = false;
 365     mAutoTakeoffData->boundThrustMin    = -0.1f;
 366     mAutoTakeoffData->boundThrustMax    = 0.0f;
 367
 368     setState(STATUSVTOLAUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
 369 }
 370
 371 // STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
 372 // PID loops may be cumulating I terms but that problem needs to be solved
 373 #define SLOWSTART_INITIAL_THRUST 0.05f // assumed to be less than vtol min
 374 void VtolAutoTakeoffFSM::setup_slowstart(void)
 375 {
 376     setStateTimeout(TIMEOUT_SLOWSTART);
 377     mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
 378     StabilizationDesiredData stabDesired;
 379     StabilizationDesiredGet(&stabDesired);
 380     float vtol_thrust_min = vtolPathFollowerSettings->ThrustLimits.Min;
 381     if (vtol_thrust_min < SLOWSTART_INITIAL_THRUST) {
 382         vtol_thrust_min = SLOWSTART_INITIAL_THRUST;
 383     }
 384     mAutoTakeoffData->sum1 = (vtol_thrust_min - SLOWSTART_INITIAL_THRUST) / (float)TIMEOUT_SLOWSTART;
 385     mAutoTakeoffData->sum2 = vtol_thrust_min;
 386     mAutoTakeoffData->boundThrustMin = SLOWSTART_INITIAL_THRUST;
 387     mAutoTakeoffData->boundThrustMax = SLOWSTART_INITIAL_THRUST;
 388     PositionStateData positionState;
 389     PositionStateGet(&positionState);
 390     mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North;
 391     mAutoTakeoffData->expectedAutoTakeoffPositionEast  = positionState.East;
 392 }
 393
 394 void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout)
 395 {
 396     // increase thrust setpoint step by step
 397     if (mAutoTakeoffData->boundThrustMin < mAutoTakeoffData->sum2) {
 398         mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
 399     }
 400     mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
 401     if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
 402         mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
 403     }
 404
 405     if (flTimeout) {
 406         setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
 407     }
 408 }
 409
 410 // STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
 411 // PID loops may be cumulating I terms but that problem needs to be solved
 412 #define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
 413 void VtolAutoTakeoffFSM::setup_thrustup(void)
 414 {
 415     setStateTimeout(TIMEOUT_THRUSTUP);
 416     mAutoTakeoffData->flZeroStabiHorizontal = false;
 417     StabilizationDesiredData stabDesired;
 418     StabilizationDesiredGet(&stabDesired);
 419     mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
 420     mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
 421     mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
 422 }
 423
 424 void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
 425 {
 426     // increase thrust setpoint step by step
 427     mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
 428     if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
 429         mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
 430     }
 431
 432     if (flTimeout) {
 433         setState(STATUSVTOLAUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
 434     }
 435 }
 436
 437
 438 // STATE: TAKEOFF
 439 void VtolAutoTakeoffFSM::setup_takeoff(void)
 440 {
 441     mAutoTakeoffData->flZeroStabiHorizontal = false;
 442 }
 443 void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
 444 {
 445     StabilizationDesiredData stabDesired;
 446
 447     StabilizationDesiredGet(&stabDesired);
 448     if (stabDesired.Thrust < 0.0f) {
 449         setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
 450         return;
 451     }
 452
 453     // detect broad sideways drift.
 454     PositionStateData positionState;
 455     PositionStateGet(&positionState);
 456     float north_error   = mAutoTakeoffData->expectedAutoTakeoffPositionNorth - positionState.North;
 457     float east_error    = mAutoTakeoffData->expectedAutoTakeoffPositionEast - positionState.East;
 458     float down_error    = pathDesired->End.Down - positionState.Down;
 459     float positionError = sqrtf(north_error * north_error + east_error * east_error);
 460     if (positionError > 3.0f) {
 461         setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
 462         return;
 463     }
 464     if (fabsf(down_error) < 0.5f) {
 465         setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
 466         return;
 467     }
 468 }
 469
 470 // STATE: HOLD
 471 void VtolAutoTakeoffFSM::setup_hold(void)
 472 {
 473     mAutoTakeoffData->flZeroStabiHorizontal = false;
 474     mAutoTakeoffData->flAltitudeHold = true;
 475 }
 476 void VtolAutoTakeoffFSM::run_hold(__attribute__((unused)) uint8_t flTimeout)
 477 {}
 478
 479 uint8_t VtolAutoTakeoffFSM::PositionHoldState(void)
 480 {
 481     return mAutoTakeoffData->flAltitudeHold;
 482 }
 483
 484 // STATE: THRUSTDOWN
 485 void VtolAutoTakeoffFSM::setup_thrustdown(void)
 486 {
 487     setStateTimeout(TIMEOUT_THRUSTDOWN);
 488     mAutoTakeoffData->flZeroStabiHorizontal = true;
 489     mAutoTakeoffData->flConstrainThrust     = true;
 490     StabilizationDesiredData stabDesired;
 491     StabilizationDesiredGet(&stabDesired);
 492     mAutoTakeoffData->thrustLimit    = stabDesired.Thrust;
 493     mAutoTakeoffData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
 494     mAutoTakeoffData->boundThrustMin = -0.1f;
 495     mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Neutral;
 496 }
 497
 498 void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
 499 {
 500     // reduce thrust setpoint step by step
 501     mAutoTakeoffData->thrustLimit -= mAutoTakeoffData->sum1;
 502
 503     StabilizationDesiredData stabDesired;
 504     StabilizationDesiredGet(&stabDesired);
 505     if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
 506         setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
 507     }
 508
 509     if (flTimeout) {
 510         setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
 511     }
 512 }
 513
 514 // STATE: THRUSTOFF
 515 void VtolAutoTakeoffFSM::setup_thrustoff(void)
 516 {
 517     mAutoTakeoffData->thrustLimit       = -1.0f;
 518     mAutoTakeoffData->flConstrainThrust = true;
 519     mAutoTakeoffData->boundThrustMin    = -0.1f;
 520     mAutoTakeoffData->boundThrustMax    = 0.0f;
 521 }
 522
 523 void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
 524 {
 525     setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 526 }
 527
 528 // STATE: DISARMED
 529 void VtolAutoTakeoffFSM::setup_disarmed(void)
 530 {
 531     mAutoTakeoffData->thrustLimit = -1.0f;
 532     mAutoTakeoffData->flConstrainThrust     = true;
 533     mAutoTakeoffData->flZeroStabiHorizontal = true;
 534     mAutoTakeoffData->observationCount = 0;
 535     mAutoTakeoffData->boundThrustMin   = -0.1f;
 536     mAutoTakeoffData->boundThrustMax   = 0.0f;
 537
 538     if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 539         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 540     }
 541 }
 542
 543 void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
 544 {
 545     if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 546         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 547     }
 548 #ifdef DEBUG_GROUNDIMPACT
 549     if (mAutoTakeoffData->observationCount++ > 100) {
 550         setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
 551     }
 552 #endif
 553 }