flight/modules/PathFollower/vtollandfsm.cpp

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