flight/modules/PathFollower/inc/vtolautotakeofffsm.h

   1 /**
   2  ******************************************************************************
   3  * @addtogroup LibrePilotModules LibrePilot 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) 2016.
  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 VTOLAUTOTAKEOFFFSM_H
  33 #define VTOLAUTOTAKEOFFFSM_H
  34
  35 extern "C" {
  36 #include "statusvtolautotakeoff.h"
  37 }
  38 #include "pathfollowerfsm.h"
  39
  40 // AutoTakeoffing state machine
  41 typedef enum {
  42     AUTOTAKEOFF_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
  43     AUTOTAKEOFF_STATE_CHECKSTATE, // Initial condition checks
  44     AUTOTAKEOFF_STATE_SLOWSTART, // Slow start motors
  45     AUTOTAKEOFF_STATE_THRUSTUP, // Ramp motors up to neutral thrust
  46     AUTOTAKEOFF_STATE_TAKEOFF, // Ascend to target velocity
  47     AUTOTAKEOFF_STATE_HOLD, // Hold position as completion of the sequence
  48     AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
  49     AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
  50     AUTOTAKEOFF_STATE_DISARMED, // Disarmed
  51     AUTOTAKEOFF_STATE_ABORT, // Abort on error triggers fallback to land
  52     AUTOTAKEOFF_STATE_SIZE
  53 } PathFollowerFSM_AutoTakeoffState_T;
  54
  55 class VtolAutoTakeoffFSM : public PathFollowerFSM {
  56 private:
  57     static VtolAutoTakeoffFSM *p_inst;
  58     VtolAutoTakeoffFSM();
  59
  60 public:
  61     static VtolAutoTakeoffFSM *instance()
  62     {
  63         if (!p_inst) {
  64             p_inst = new VtolAutoTakeoffFSM();
  65         }
  66         return p_inst;
  67     }
  68     int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
  69                        PathDesiredData *pathDesired,
  70                        FlightStatusData *flightStatus);
  71     void Inactive(void);
  72     void Activate(void);
  73     void Update(void);
  74     void BoundThrust(float &ulow, float &uhigh);
  75     PathFollowerFSMState_T GetCurrentState(void);
  76     void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
  77     uint8_t PositionHoldState(void);
  78     void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
  79
  80 protected:
  81
  82     // FSM instance data type
  83     typedef struct {
  84         StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
  85         StatusVtolAutoTakeoffStateOptions currentState;
  86         TakeOffLocationData takeOffLocation;
  87         uint32_t stateRunCount;
  88         uint32_t stateTimeoutCount;
  89         float    sum1;
  90         float    sum2;
  91         float    expectedAutoTakeoffPositionNorth;
  92         float    expectedAutoTakeoffPositionEast;
  93         float    thrustLimit;
  94         float    boundThrustMin;
  95         float    boundThrustMax;
  96         uint8_t  observationCount;
  97         uint8_t  observation2Count;
  98         uint8_t  flZeroStabiHorizontal;
  99         uint8_t  flConstrainThrust;
 100         uint8_t  flLowAltitude;
 101         uint8_t  flAltitudeHold;
 102     } VtolAutoTakeoffFSMData_T;
 103
 104     // FSM state structure
 105     typedef struct {
 106         void(VtolAutoTakeoffFSM::*setup) (void); // Called to initialise the state
 107         void(VtolAutoTakeoffFSM::*run) (uint8_t); // Run the event detection code for a state
 108     } PathFollowerFSM_AutoTakeoffStateHandler_T;
 109
 110     // Private variables
 111     VtolAutoTakeoffFSMData_T *mAutoTakeoffData;
 112     VtolPathFollowerSettingsData *vtolPathFollowerSettings;
 113     PathDesiredData *pathDesired;
 114     FlightStatusData *flightStatus;
 115
 116     void setup_inactive(void);
 117
 118     void setup_checkstate(void);
 119
 120     void setup_slowstart(void);
 121     void run_slowstart(uint8_t);
 122
 123     void setup_takeoff(void);
 124     void run_takeoff(uint8_t);
 125
 126     void setup_hold(void);
 127     void run_hold(uint8_t);
 128
 129     void setup_thrustup(void);
 130     void run_thrustup(uint8_t);
 131
 132     void setup_thrustdown(void);
 133     void run_thrustdown(uint8_t);
 134
 135     void setup_thrustoff(void);
 136     void run_thrustoff(uint8_t);
 137
 138     void setup_disarmed(void);
 139     void run_disarmed(uint8_t);
 140
 141     void initFSM(void);
 142     void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
 143     int32_t runState();
 144     int32_t runAlways();
 145
 146     void updateVtolAutoTakeoffFSMStatus();
 147     void assessAltitude(void);
 148
 149     void setStateTimeout(int32_t count);
 150
 151     static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
 152 };
 153
 154 #endif // VTOLAUTOTAKEOFFFSM_H