flight/modules/PathFollower/inc/vtolautotakeofffsm.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 OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  11  * @brief      Executes FSM for landing sequence
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31 #ifndef VTOLAUTOTAKEOFFFSM_H
  32 #define VTOLAUTOTAKEOFFFSM_H
  33
  34 extern "C" {
  35 #include "statusvtolautotakeoff.h"
  36 }
  37 #include "pathfollowerfsm.h"
  38
  39 // AutoTakeoffing state machine
  40 typedef enum {
  41     AUTOTAKEOFF_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
  42     AUTOTAKEOFF_STATE_CHECKSTATE, // Initial condition checks
  43     AUTOTAKEOFF_STATE_SLOWSTART, // Slow start motors
  44     AUTOTAKEOFF_STATE_THRUSTUP, // Ramp motors up to neutral thrust
  45     AUTOTAKEOFF_STATE_TAKEOFF, // Ascend to target velocity
  46     AUTOTAKEOFF_STATE_HOLD, // Hold position as completion of the sequence
  47     AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
  48     AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
  49     AUTOTAKEOFF_STATE_DISARMED, // Disarmed
  50     AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to land
  51     AUTOTAKEOFF_STATE_SIZE
  52 } PathFollowerFSM_AutoTakeoffState_T;
  53
  54 class VtolAutoTakeoffFSM : public PathFollowerFSM {
  55 private:
  56     static VtolAutoTakeoffFSM *p_inst;
  57     VtolAutoTakeoffFSM();
  58
  59 public:
  60     static VtolAutoTakeoffFSM *instance()
  61     {
  62         if (!p_inst) {
  63             p_inst = new VtolAutoTakeoffFSM();
  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     uint8_t PositionHoldState(void);
  77     void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
  78
  79 protected:
  80
  81     // FSM instance data type
  82     typedef struct {
  83         StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
  84         StatusVtolAutoTakeoffStateOptions currentState;
  85         TakeOffLocationData takeOffLocation;
  86         uint32_t stateRunCount;
  87         uint32_t stateTimeoutCount;
  88         float    sum1;
  89         float    sum2;
  90         float    expectedAutoTakeoffPositionNorth;
  91         float    expectedAutoTakeoffPositionEast;
  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     } VtolAutoTakeoffFSMData_T;
 102
 103     // FSM state structure
 104     typedef struct {
 105         void(VtolAutoTakeoffFSM::*setup) (void); // Called to initialise the state
 106         void(VtolAutoTakeoffFSM::*run) (uint8_t); // Run the event detection code for a state
 107     } PathFollowerFSM_AutoTakeoffStateHandler_T;
 108
 109     // Private variables
 110     VtolAutoTakeoffFSMData_T *mAutoTakeoffData;
 111     VtolPathFollowerSettingsData *vtolPathFollowerSettings;
 112     PathDesiredData *pathDesired;
 113     FlightStatusData *flightStatus;
 114
 115     void setup_inactive(void);
 116
 117     void setup_checkstate(void);
 118
 119     void setup_slowstart(void);
 120     void run_slowstart(uint8_t);
 121
 122     void setup_takeoff(void);
 123     void run_takeoff(uint8_t);
 124
 125     void setup_hold(void);
 126     void run_hold(uint8_t);
 127
 128     void setup_thrustup(void);
 129     void run_thrustup(uint8_t);
 130
 131     void setup_thrustdown(void);
 132     void run_thrustdown(uint8_t);
 133
 134     void setup_thrustoff(void);
 135     void run_thrustoff(uint8_t);
 136
 137     void setup_disarmed(void);
 138     void run_disarmed(uint8_t);
 139
 140     void initFSM(void);
 141     void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
 142     int32_t runState();
 143     int32_t runAlways();
 144
 145     void updateVtolAutoTakeoffFSMStatus();
 146     void assessAltitude(void);
 147
 148     void setStateTimeout(int32_t count);
 149
 150     static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
 151 };
 152
 153 #endif // VTOLAUTOTAKEOFFFSM_H