flight/modules/PathFollower/vtollandfsm.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtollandfsm.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      This landing state machine is a helper state machine to the
   7  *             VtolLandController.
   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 <alarms.h>
  33 #include <CoordinateConversions.h>
  34 #include <sin_lookup.h>
  35 #include <pathdesired.h>
  36 #include <paths.h>
  37 #include "plans.h"
  38 #include <sanitycheck.h>
  39
  40 #include <homelocation.h>
  41 #include <accelstate.h>
  42 #include <vtolpathfollowersettings.h>
  43 #include <flightstatus.h>
  44 #include <flightmodesettings.h>
  45 #include <pathstatus.h>
  46 #include <positionstate.h>
  47 #include <velocitystate.h>
  48 #include <velocitydesired.h>
  49 #include <stabilizationdesired.h>
  50 #include <airspeedstate.h>
  51 #include <attitudestate.h>
  52 #include <takeofflocation.h>
  53 #include <poilocation.h>
  54 #include <manualcontrolcommand.h>
  55 #include <systemsettings.h>
  56 #include <stabilizationbank.h>
  57 #include <stabilizationdesired.h>
  58 #include <vtolselftuningstats.h>
  59 #include <statusvtolland.h>
  60 #include <pathsummary.h>
  61 }
  62
  63 // C++ includes
  64 #include <vtollandfsm.h>
  65
  66
  67 // Private constants
  68 #define TIMER_COUNT_PER_SECOND            (1000 / vtolPathFollowerSettings->UpdatePeriod)
  69 #define MIN_LANDRATE                      0.1f
  70 #define MAX_LANDRATE                      0.6f
  71 #define LOW_ALT_DESCENT_REDUCTION_FACTOR  0.7f  // TODO Need to make the transition smooth
  72 #define LANDRATE_LOWLIMIT_FACTOR          0.5f
  73 #define LANDRATE_HILIMIT_FACTOR           1.5f
  74 #define TIMEOUT_INIT_ALTHOLD              (3 * TIMER_COUNT_PER_SECOND)
  75 #define TIMEOUT_WTG_FOR_DESCENTRATE       (10 * TIMER_COUNT_PER_SECOND)
  76 #define WTG_FOR_DESCENTRATE_COUNT_LIMIT   10
  77 #define TIMEOUT_AT_DESCENTRATE            10
  78 #define TIMEOUT_GROUNDEFFECT              (1 * TIMER_COUNT_PER_SECOND)
  79 #define TIMEOUT_THRUSTDOWN                (2 * TIMER_COUNT_PER_SECOND)
  80 #define LANDING_PID_SCALAR_P              2.0f
  81 #define LANDING_PID_SCALAR_I              10.0f
  82 #define LANDING_SLOWDOWN_HEIGHT           -5.0f
  83 #define BOUNCE_VELOCITY_TRIGGER_LIMIT     -0.3f
  84 #define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
  85 #define BOUNCE_TRIGGER_COUNT              4
  86 #define GROUNDEFFECT_SLOWDOWN_FACTOR      0.3f
  87 #define GROUNDEFFECT_SLOWDOWN_COUNT       4
  88
  89 VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
  90     [LAND_STATE_INACTIVE]       =       { .setup = &VtolLandFSM::setup_inactive,             .run = 0                                      },
  91     [LAND_STATE_INIT_ALTHOLD]   =       { .setup = &VtolLandFSM::setup_init_althold,         .run = &VtolLandFSM::run_init_althold         },
  92     [LAND_STATE_WTG_FOR_DESCENTRATE] =  { .setup = &VtolLandFSM::setup_wtg_for_descentrate,  .run = &VtolLandFSM::run_wtg_for_descentrate  },
  93     [LAND_STATE_AT_DESCENTRATE] =       { .setup = &VtolLandFSM::setup_at_descentrate,       .run = &VtolLandFSM::run_at_descentrate       },
  94     [LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
  95     [LAND_STATE_GROUNDEFFECT]   =       { .setup = &VtolLandFSM::setup_groundeffect,         .run = &VtolLandFSM::run_groundeffect         },
  96     [LAND_STATE_THRUSTDOWN]     =       { .setup = &VtolLandFSM::setup_thrustdown,           .run = &VtolLandFSM::run_thrustdown           },
  97     [LAND_STATE_THRUSTOFF]      =       { .setup = &VtolLandFSM::setup_thrustoff,            .run = &VtolLandFSM::run_thrustoff            },
  98     [LAND_STATE_DISARMED]       =       { .setup = &VtolLandFSM::setup_disarmed,             .run = &VtolLandFSM::run_disarmed             }
  99 };
 100
 101 // pointer to a singleton instance
 102 VtolLandFSM *VtolLandFSM::p_inst = 0;
 103
 104
 105 VtolLandFSM::VtolLandFSM()
 106     : mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
 107 {}
 108
 109 // Private types
 110
 111 // Private functions
 112 // Public API methods
 113 /**
 114  * Initialise the module, called on startup
 115  * \returns 0 on success or -1 if initialisation failed
 116  */
 117 int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
 118                                 PathDesiredData *ptr_pathDesired,
 119                                 FlightStatusData *ptr_flightStatus)
 120 {
 121     PIOS_Assert(ptr_vtolPathFollowerSettings);
 122     PIOS_Assert(ptr_pathDesired);
 123     PIOS_Assert(ptr_flightStatus);
 124
 125     if (mLandData == 0) {
 126         mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
 127         PIOS_Assert(mLandData);
 128     }
 129     memset(mLandData, 0, sizeof(VtolLandFSMData_T));
 130     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 131     pathDesired  = ptr_pathDesired;
 132     flightStatus = ptr_flightStatus;
 133     initFSM();
 134
 135     return 0;
 136 }
 137
 138 void VtolLandFSM::Inactive(void)
 139 {
 140     memset(mLandData, 0, sizeof(VtolLandFSMData_T));
 141     initFSM();
 142 }
 143
 144 // Initialise the FSM
 145 void VtolLandFSM::initFSM(void)
 146 {
 147     if (vtolPathFollowerSettings != 0) {
 148         setState(STATUSVTOLLAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
 149     } else {
 150         mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
 151     }
 152 }
 153
 154 void VtolLandFSM::Activate()
 155 {
 156     memset(mLandData, 0, sizeof(VtolLandFSMData_T));
 157     mLandData->currentState   = STATUSVTOLLAND_STATE_INACTIVE;
 158     mLandData->flLowAltitude  = false;
 159     mLandData->flAltitudeHold = false;
 160     mLandData->fsmLandStatus.averageDescentRate      = MIN_LANDRATE;
 161     mLandData->fsmLandStatus.averageDescentThrust    = vtolPathFollowerSettings->ThrustLimits.Neutral;
 162     mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
 163     mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
 164     mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
 165     TakeOffLocationGet(&(mLandData->takeOffLocation));
 166     mLandData->fsmLandStatus.AltitudeAtState[STATUSVTOLLAND_STATE_INACTIVE] = 0.0f;
 167     assessAltitude();
 168
 169     if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
 170 #ifndef DEBUG_GROUNDIMPACT
 171         setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
 172 #else
 173         setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
 174 #endif
 175     } else {
 176         // move to error state and callback to position hold
 177         setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
 178     }
 179 }
 180
 181 PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
 182 {
 183     switch (mLandData->currentState) {
 184     case STATUSVTOLLAND_STATE_INACTIVE:
 185         return PFFSM_STATE_INACTIVE;
 186
 187         break;
 188     case STATUSVTOLLAND_STATE_DISARMED:
 189         return PFFSM_STATE_DISARMED;
 190
 191         break;
 192     default:
 193         return PFFSM_STATE_ACTIVE;
 194
 195         break;
 196     }
 197 }
 198
 199 void VtolLandFSM::Update()
 200 {
 201     runState();
 202     if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
 203         runAlways();
 204     }
 205 }
 206
 207 int32_t VtolLandFSM::runState(void)
 208 {
 209     uint8_t flTimeout = false;
 210
 211     mLandData->stateRunCount++;
 212
 213     if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
 214         flTimeout = true;
 215     }
 216
 217     // If the current state has a static function, call it
 218     if (sLandStateTable[mLandData->currentState].run) {
 219         (this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
 220     }
 221     return 0;
 222 }
 223
 224 int32_t VtolLandFSM::runAlways(void)
 225 {
 226     void assessAltitude(void);
 227
 228     return 0;
 229 }
 230
 231 // PathFollower implements the PID scheme and has a objective
 232 // set by a PathDesired object.  Based on the mode, pathfollower
 233 // uses FSM's as helper functions that manage state and event detection.
 234 // PathFollower calls into FSM methods to alter its commands.
 235
 236 void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
 237 {
 238     ulow  = mLandData->boundThrustMin;
 239     uhigh = mLandData->boundThrustMax;
 240
 241
 242     if (mLandData->flConstrainThrust) {
 243         uhigh = mLandData->thrustLimit;
 244     }
 245 }
 246
 247 void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
 248 {
 249     if (mLandData->flZeroStabiHorizontal && stabDesired) {
 250         stabDesired->Pitch = 0.0f;
 251         stabDesired->Roll  = 0.0f;
 252         stabDesired->Yaw   = 0.0f;
 253     }
 254 }
 255
 256 void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
 257 {
 258     if (mLandData->flLowAltitude) {
 259         local_scaler->p = LANDING_PID_SCALAR_P;
 260         local_scaler->i = LANDING_PID_SCALAR_I;
 261     }
 262 }
 263
 264
 265 // Set the new state and perform setup for subsequent state run calls
 266 // This is called by state run functions on event detection that drive
 267 // state transitions.
 268 void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason)
 269 {
 270     mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
 271
 272     if (mLandData->currentState == newState) {
 273         return;
 274     }
 275     mLandData->currentState = newState;
 276
 277     if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
 278         PositionStateData positionState;
 279         PositionStateGet(&positionState);
 280         float takeOffDown = 0.0f;
 281         if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
 282             takeOffDown = mLandData->takeOffLocation.Down;
 283         }
 284         mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
 285         assessAltitude();
 286     }
 287
 288     // Restart state timer counter
 289     mLandData->stateRunCount     = 0;
 290
 291     // Reset state timeout to disabled/zero
 292     mLandData->stateTimeoutCount = 0;
 293
 294     if (sLandStateTable[mLandData->currentState].setup) {
 295         (this->*sLandStateTable[mLandData->currentState].setup)();
 296     }
 297
 298     updateVtolLandFSMStatus();
 299 }
 300
 301
 302 // Timeout utility function for use by state init implementations
 303 void VtolLandFSM::setStateTimeout(int32_t count)
 304 {
 305     mLandData->stateTimeoutCount = count;
 306 }
 307
 308 void VtolLandFSM::updateVtolLandFSMStatus()
 309 {
 310     mLandData->fsmLandStatus.State = mLandData->currentState;
 311     if (mLandData->flLowAltitude) {
 312         mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
 313     } else {
 314         mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
 315     }
 316     StatusVtolLandSet(&mLandData->fsmLandStatus);
 317 }
 318
 319
 320 float VtolLandFSM::BoundVelocityDown(float velocity_down)
 321 {
 322     velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
 323     if (mLandData->flLowAltitude) {
 324         velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
 325     }
 326     mLandData->fsmLandStatus.targetDescentRate = velocity_down;
 327
 328     if (mLandData->flAltitudeHold) {
 329         return 0.0f;
 330     } else {
 331         return velocity_down;
 332     }
 333 }
 334
 335 void VtolLandFSM::assessAltitude(void)
 336 {
 337     float positionDown;
 338
 339     PositionStateDownGet(&positionDown);
 340     float takeOffDown = 0.0f;
 341     if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
 342         takeOffDown = mLandData->takeOffLocation.Down;
 343     }
 344     float positionDownRelativeToTakeoff = positionDown - takeOffDown;
 345     if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
 346         mLandData->flLowAltitude = false;
 347     } else {
 348         mLandData->flLowAltitude = true;
 349     }
 350 }
 351
 352
 353 // FSM Setup and Run method implementation
 354
 355 // State: INACTIVE
 356 void VtolLandFSM::setup_inactive(void)
 357 {
 358     // Re-initialise local variables
 359     mLandData->flZeroStabiHorizontal = false;
 360     mLandData->flConstrainThrust     = false;
 361 }
 362
 363 // State: INIT ALTHOLD
 364 void VtolLandFSM::setup_init_althold(void)
 365 {
 366     setStateTimeout(TIMEOUT_INIT_ALTHOLD);
 367     // get target descent velocity
 368     mLandData->flZeroStabiHorizontal = false;
 369     mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
 370     mLandData->flConstrainThrust     = false;
 371     mLandData->flAltitudeHold = true;
 372     mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
 373     mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
 374 }
 375
 376 void VtolLandFSM::run_init_althold(uint8_t flTimeout)
 377 {
 378     if (flTimeout) {
 379         mLandData->flAltitudeHold = false;
 380         setState(STATUSVTOLLAND_STATE_WTGFORDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
 381     }
 382 }
 383
 384
 385 // State: WAITING FOR DESCENT RATE
 386 void VtolLandFSM::setup_wtg_for_descentrate(void)
 387 {
 388     setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
 389     // get target descent velocity
 390     mLandData->flZeroStabiHorizontal = false;
 391     mLandData->observationCount = 0;
 392     mLandData->observation2Count     = 0;
 393     mLandData->flConstrainThrust     = false;
 394     mLandData->flAltitudeHold = false;
 395     mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
 396     mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
 397 }
 398
 399 void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
 400 {
 401     // Look at current actual thrust...are we already shutdown??
 402     VelocityStateData velocityState;
 403
 404     VelocityStateGet(&velocityState);
 405     StabilizationDesiredData stabDesired;
 406     StabilizationDesiredGet(&stabDesired);
 407
 408     // We don't expect PID to get exactly the target descent rate, so have a lower
 409     // water mark but need to see 5 observations to be confident that we have semi-stable
 410     // descent achieved
 411
 412     // we need to see velocity down within a range of control before we proceed, without which we
 413     // really don't have confidence to allow later states to run.
 414     if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
 415         velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
 416         if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
 417             setState(STATUSVTOLLAND_STATE_ATDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
 418             return;
 419         }
 420     }
 421
 422     if (flTimeout) {
 423         setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
 424     }
 425 }
 426
 427
 428 // State: AT DESCENT RATE
 429 void VtolLandFSM::setup_at_descentrate(void)
 430 {
 431     setStateTimeout(TIMEOUT_AT_DESCENTRATE);
 432     mLandData->flZeroStabiHorizontal = false;
 433     mLandData->observationCount  = 0;
 434     mLandData->sum1 = 0.0f;
 435     mLandData->sum2 = 0.0f;
 436     mLandData->flConstrainThrust = false;
 437     mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
 438     mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
 439     mLandData->boundThrustMin    = vtolPathFollowerSettings->ThrustLimits.Min;
 440     mLandData->boundThrustMax    = vtolPathFollowerSettings->ThrustLimits.Max;
 441 }
 442
 443 void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
 444 {
 445     VelocityStateData velocityState;
 446
 447     VelocityStateGet(&velocityState);
 448
 449     StabilizationDesiredData stabDesired;
 450     StabilizationDesiredGet(&stabDesired);
 451
 452     mLandData->sum1 += velocityState.Down;
 453     mLandData->sum2 += stabDesired.Thrust;
 454     mLandData->observationCount++;
 455     if (flTimeout) {
 456         mLandData->fsmLandStatus.averageDescentRate   = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
 457         mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
 458
 459         // We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
 460         // As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
 461         // detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
 462         mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
 463         mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
 464
 465
 466         setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
 467     }
 468 }
 469
 470
 471 // State: WAITING FOR GROUND EFFECT
 472 void VtolLandFSM::setup_wtg_for_groundeffect(void)
 473 {
 474     // No timeout
 475     mLandData->flZeroStabiHorizontal = false;
 476     mLandData->observationCount = 0;
 477     mLandData->observation2Count     = 0;
 478     mLandData->sum1 = 0.0f;
 479     mLandData->sum2 = 0.0f;
 480     mLandData->flConstrainThrust     = false;
 481     mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
 482     mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel    = 0.0f;
 483     mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
 484     mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
 485 }
 486
 487 void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
 488 {
 489     // detect material downrating in thrust for 1 second.
 490     VelocityStateData velocityState;
 491
 492     VelocityStateGet(&velocityState);
 493     AccelStateData accelState;
 494     AccelStateGet(&accelState);
 495
 496     // +ve 9.8 expected
 497     float g_e;
 498     HomeLocationg_eGet(&g_e);
 499
 500     StabilizationDesiredData stabDesired;
 501     StabilizationDesiredGet(&stabDesired);
 502
 503     // detect bounce
 504     uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
 505     if (flBounce) {
 506         mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
 507     } else {
 508         mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
 509     }
 510
 511     // invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
 512     // a relative acceleration term.
 513     float bounceAccel     = -accelState.z - g_e;
 514     uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
 515     if (flBounceAccel) {
 516         mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
 517     } else {
 518         mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
 519     }
 520
 521     if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
 522         mLandData->observation2Count++;
 523         if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
 524             setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
 525             return;
 526         }
 527     } else {
 528         mLandData->observation2Count = 0;
 529     }
 530
 531     // detect low descent rate
 532     uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
 533     if (flDescentRateLow) {
 534         mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
 535         mLandData->observationCount++;
 536         if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
 537 #ifndef DEBUG_GROUNDIMPACT
 538             setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
 539 #endif
 540             return;
 541         }
 542     } else {
 543         mLandData->observationCount = 0;
 544     }
 545
 546     updateVtolLandFSMStatus();
 547 }
 548
 549 // STATE: GROUNDEFFET
 550 void VtolLandFSM::setup_groundeffect(void)
 551 {
 552     setStateTimeout(TIMEOUT_GROUNDEFFECT);
 553     mLandData->flZeroStabiHorizontal     = false;
 554     PositionStateData positionState;
 555     PositionStateGet(&positionState);
 556     mLandData->expectedLandPositionNorth = positionState.North;
 557     mLandData->expectedLandPositionEast  = positionState.East;
 558     mLandData->flConstrainThrust = false;
 559
 560     // now that we have ground effect limit max thrust to neutral
 561     mLandData->boundThrustMin    = -0.1f;
 562     mLandData->boundThrustMax    = mLandData->fsmLandStatus.calculatedNeutralThrust;
 563 }
 564 void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
 565 {
 566     StabilizationDesiredData stabDesired;
 567
 568     StabilizationDesiredGet(&stabDesired);
 569     if (stabDesired.Thrust < 0.0f) {
 570         setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
 571         return;
 572     }
 573
 574     // Stay in this state until we get a low altitude flag.
 575     if (mLandData->flLowAltitude == false) {
 576         // worst case scenario is that we land and the pid brings thrust down to zero.
 577         return;
 578     }
 579
 580     // detect broad sideways drift.  If for some reason we have a hard landing that the bounce detection misses, this will kick in
 581     PositionStateData positionState;
 582     PositionStateGet(&positionState);
 583     float north_error   = mLandData->expectedLandPositionNorth - positionState.North;
 584     float east_error    = mLandData->expectedLandPositionEast - positionState.East;
 585     float positionError = sqrtf(north_error * north_error + east_error * east_error);
 586     if (positionError > 1.5f) {
 587         setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
 588         return;
 589     }
 590
 591     if (flTimeout) {
 592         setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
 593     }
 594 }
 595
 596 // STATE: THRUSTDOWN
 597 void VtolLandFSM::setup_thrustdown(void)
 598 {
 599     setStateTimeout(TIMEOUT_THRUSTDOWN);
 600     mLandData->flZeroStabiHorizontal = true;
 601     mLandData->flConstrainThrust     = true;
 602     StabilizationDesiredData stabDesired;
 603     StabilizationDesiredGet(&stabDesired);
 604     mLandData->thrustLimit    = stabDesired.Thrust;
 605     mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
 606     mLandData->boundThrustMin = -0.1f;
 607     mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
 608 }
 609
 610 void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
 611 {
 612     // reduce thrust setpoint step by step
 613     mLandData->thrustLimit -= mLandData->sum1;
 614
 615     StabilizationDesiredData stabDesired;
 616     StabilizationDesiredGet(&stabDesired);
 617     if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
 618         setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
 619     }
 620
 621     if (flTimeout) {
 622         setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
 623     }
 624 }
 625
 626 // STATE: THRUSTOFF
 627 void VtolLandFSM::setup_thrustoff(void)
 628 {
 629     mLandData->thrustLimit           = -1.0f;
 630     mLandData->flZeroStabiHorizontal = true;
 631     mLandData->flConstrainThrust     = true;
 632     mLandData->boundThrustMin        = -0.1f;
 633     mLandData->boundThrustMax        = 0.0f;
 634 }
 635
 636 void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
 637 {
 638     setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
 639 }
 640
 641 // STATE: DISARMED
 642 void VtolLandFSM::setup_disarmed(void)
 643 {
 644     // nothing to do
 645     mLandData->flConstrainThrust     = false;
 646     mLandData->flZeroStabiHorizontal = true;
 647     mLandData->observationCount = 0;
 648     mLandData->boundThrustMin   = -0.1f;
 649     mLandData->boundThrustMax   = 0.0f;
 650
 651     // force disarm unless in pathplanner mode
 652     // to clear, a new pathfollower mode must be selected that is not land,
 653     // and also a non-pathfollower mode selection will set this to uninitialised.
 654     if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 655         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 656     }
 657 }
 658
 659 void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
 660 {
 661     if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 662         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 663     }
 664
 665 #ifdef DEBUG_GROUNDIMPACT
 666     if (mLandData->observationCount++ > 100) {
 667         setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
 668     }
 669 #endif
 670 }