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