flight/modules/PathFollower/vtolautotakeofffsm.cpp

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