flight/modules/PathFollower/inc/vtollandfsm.h

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup PathFollower FSM
   6  * @brief Executes landing sequence via an FSM
   7  * @{
   8  *
   9  * @file       vtollandfsm.h
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2018
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  12  * @brief      Executes FSM for landing sequence
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32 #ifndef VTOLLANDFSM_H
  33 #define VTOLLANDFSM_H
  34
  35 extern "C" {
  36 #include "statusvtolland.h"
  37 }
  38 #include "pathfollowerfsm.h"
  39
  40 // Landing state machine
  41 typedef enum {
  42     LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
  43     LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent
  44     LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate
  45     LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved
  46     LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected
  47     LAND_STATE_GROUNDEFFECT, // Ground effect detected
  48     LAND_STATE_THRUSTDOWN, // Thrust down sequence
  49     LAND_STATE_THRUSTOFF, // Thrust is now off
  50     LAND_STATE_DISARMED, // Disarmed
  51     LAND_STATE_SIZE
  52 } PathFollowerFSM_LandState_T;
  53
  54 class VtolLandFSM : public PathFollowerFSM {
  55 private:
  56     static VtolLandFSM *p_inst;
  57     VtolLandFSM();
  58
  59 public:
  60     static VtolLandFSM *instance()
  61     {
  62         if (!p_inst) {
  63             p_inst = new VtolLandFSM();
  64         }
  65         return p_inst;
  66     }
  67     int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
  68                        PathDesiredData *pathDesired,
  69                        FlightStatusData *flightStatus);
  70     void Inactive(void);
  71     void Activate(void);
  72     void Update(void);
  73     void BoundThrust(float &ulow, float &uhigh);
  74     PathFollowerFSMState_T GetCurrentState(void);
  75     void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
  76     float BoundVelocityDown(float);
  77     void CheckPidScaler(pid_scaler *scaler);
  78
  79 protected:
  80
  81     // FSM instance data type
  82     typedef struct {
  83         StatusVtolLandData  fsmLandStatus;
  84         StatusVtolLandStateOptions currentState;
  85         TakeOffLocationData takeOffLocation;
  86         uint32_t stateRunCount;
  87         uint32_t stateTimeoutCount;
  88         float    sum1;
  89         float    sum2;
  90         float    expectedLandPositionNorth;
  91         float    expectedLandPositionEast;
  92         float    thrustLimit;
  93         float    boundThrustMin;
  94         float    boundThrustMax;
  95         uint8_t  observationCount;
  96         uint8_t  observation2Count;
  97         uint8_t  flZeroStabiHorizontal;
  98         uint8_t  flConstrainThrust;
  99         uint8_t  flLowAltitude;
 100         uint8_t  flAltitudeHold;
 101     } VtolLandFSMData_T;
 102
 103     // FSM state structure
 104     typedef struct {
 105         void(VtolLandFSM::*setup) (void); // Called to initialise the state
 106         void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state
 107     } PathFollowerFSM_LandStateHandler_T;
 108
 109     // Private variables
 110     VtolLandFSMData_T *mLandData;
 111     VtolPathFollowerSettingsData *vtolPathFollowerSettings;
 112     PathDesiredData *pathDesired;
 113     FlightStatusData *flightStatus;
 114
 115     void setup_inactive(void);
 116     void setup_init_althold(void);
 117     void setup_wtg_for_descentrate(void);
 118     void setup_at_descentrate(void);
 119     void setup_wtg_for_groundeffect(void);
 120     void run_init_althold(uint8_t);
 121     void run_wtg_for_descentrate(uint8_t);
 122     void run_at_descentrate(uint8_t);
 123     void run_wtg_for_groundeffect(uint8_t);
 124     void setup_groundeffect(void);
 125     void run_groundeffect(uint8_t);
 126     void setup_thrustdown(void);
 127     void run_thrustdown(uint8_t);
 128     void setup_thrustoff(void);
 129     void run_thrustoff(uint8_t);
 130     void setup_disarmed(void);
 131     void run_disarmed(uint8_t);
 132     void initFSM(void);
 133     void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
 134     int32_t runState();
 135     int32_t runAlways();
 136     void updateVtolLandFSMStatus();
 137     float assessAltitude(void);
 138
 139     void setStateTimeout(int32_t count);
 140
 141     static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
 142 };
 143
 144 #endif // VTOLLANDFSM_H