Changeset 15 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.h


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (5 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairMeta/src/UavStateMachine.h

    r10 r15  
    2626
    2727namespace flair {
    28     namespace core {
    29         class FrameworkManager;
    30         class AhrsData;
    31         class io_data;
    32     }
    33     namespace gui {
    34         class PushButton;
    35         class GridLayout;
    36         class Tab;
    37         class DoubleSpinBox;
    38     }
    39     namespace filter {
    40         class ControlLaw;
    41         class NestedSat;
    42         class Pid;
    43         class PidThrust;
    44         class TrajectoryGenerator1D;
    45     }
    46     namespace sensor {
    47         class TargetController;
    48     }
    49     namespace meta {
    50         class MetaDualShock3;
    51         class Uav;
    52     }
    53 }
    54 
    55 namespace flair { namespace meta {
    56 
    57  /*! \class UavStateMachine
     28namespace core {
     29class FrameworkManager;
     30class AhrsData;
     31class io_data;
     32}
     33namespace gui {
     34class PushButton;
     35class GridLayout;
     36class Tab;
     37class DoubleSpinBox;
     38}
     39namespace filter {
     40class ControlLaw;
     41class NestedSat;
     42class Pid;
     43class PidThrust;
     44class TrajectoryGenerator1D;
     45}
     46namespace sensor {
     47class TargetController;
     48}
     49namespace meta {
     50class MetaDualShock3;
     51class Uav;
     52}
     53}
     54
     55namespace flair {
     56namespace meta {
     57
     58/*! \class UavStateMachine
    5859*
    5960* \brief State machine for UAV
    6061*
    61 * thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
     62* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
     63*SetPeriodMS
    6264*/
    6365
    64 class UavStateMachine : public core::Thread
    65 {
    66     protected:
    67         enum class AltitudeMode_t {
    68             Manual,
    69             Custom
    70         };
    71         const AltitudeMode_t &GetAltitudeMode(void) const {
    72             return altitudeMode;
    73         }
    74         bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
    75 
    76         //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
    77         //available in mode AltitudeMode_t::Manual
    78         //return true if goto is possible
    79         bool GotoAltitude(float desiredAltitude);
    80 
    81         enum class OrientationMode_t {
    82             Manual,
    83             Custom
    84         };
    85         const OrientationMode_t &GetOrientationMode(void) const {
    86             return orientationMode;
    87         }
    88         bool SetOrientationMode(const OrientationMode_t &orientationMode);
    89 
    90         enum class ThrustMode_t {
    91             Default,
    92             Custom
    93         };
    94         const ThrustMode_t &GetThrustMode() const {
    95             return thrustMode;
    96         }
    97         bool SetThrustMode(const ThrustMode_t &thrustMode);
    98 
    99         enum class TorqueMode_t {
    100             Default,
    101             Custom
    102         };
    103         const TorqueMode_t &GetTorqueMode(void) const {
    104             return torqueMode;
    105         }
    106         bool SetTorqueMode(const TorqueMode_t &torqueMode);
    107 
    108         enum class Event_t {
    109             EnteringFailSafeMode,
    110             EnteringControlLoop,
    111             StartLanding,
    112             FinishLanding,
    113             Stopped,
    114             TakingOff,
    115             EmergencyStop,
    116             Stabilized, //as soon as uav is 3cm far from the ground
    117             ZTrajectoryFinished,
    118         };
    119 
    120         UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
    121         ~UavStateMachine();
    122 
    123         const core::Quaternion &GetCurrentQuaternion(void) const;
    124 
    125         const core::Vector3D &GetCurrentAngularSpeed(void) const;
    126 
    127         const meta::Uav *GetUav(void) const;
    128 
    129         void Land(void);
    130         void TakeOff(void);
    131         void EmergencyStop(void);
    132         //! Used to signal an event
    133         /*!
    134             \param event the event which occured
    135         */
    136         virtual void SignalEvent(Event_t event);
    137 
    138         virtual const core::AhrsData *GetOrientation(void) const;
    139         const core::AhrsData *GetDefaultOrientation(void) const;
    140 
    141         virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
    142         void EnterFailSafeMode(void);
    143         bool ExitFailSafeMode(void);
    144         void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
    145 
    146         gui::GridLayout *GetButtonsLayout(void) const;
    147         virtual void ExtraSecurityCheck(void){};
    148         virtual void ExtraCheckJoystick(void){};
    149         virtual void ExtraCheckPushButton(void){};
    150 
    151         void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    152         virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    153         //float GetDefaultThrustOffset(void);
    154         const core::AhrsData *GetDefaultReferenceOrientation(void) const;
    155         virtual const core::AhrsData *GetReferenceOrientation(void);
    156 
    157         /*!
    158         * \brief Compute Custom Torques
    159         *
    160         * Reimplement this method to use TorqueMode_t::Custom. \n
    161         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    162         * See GetTorques if you need torques values.
    163         *
    164         * \param torques custom torques
    165         */
    166         virtual void ComputeCustomTorques(core::Euler &torques);
    167 
    168         /*!
    169         * \brief Compute Default Torques
    170         *
    171         * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
    172         * Torques are only computed once by loop, other calls to this method will use previously computed torques.
    173         *
    174         * \param torques default torques
    175         */
    176         void ComputeDefaultTorques(core::Euler &torques);
    177 
    178         /*!
    179         * \brief Get Torques
    180         *
    181         * \return torques current torques
    182         */
    183         //const core::Euler &GetTorques() const;
    184 
    185         /*!
    186         * \brief Compute Custom Thrust
    187         *
    188         * Reimplement this method to use ThrustMode_t::Custom. \n
    189         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    190         * See GetThrust if you need thrust value.
    191         *
    192         * \return custom Thrust
    193         */
    194         virtual float ComputeCustomThrust(void);
    195 
    196         /*!
    197         * \brief Compute Default Thrust
    198         *
    199         * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
    200         * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
    201         *
    202         * \return default thrust
    203         */
    204         float ComputeDefaultThrust(void);
    205 
    206         /*!
    207         * \brief Get Thrust
    208         *
    209         * \return current thrust
    210         */
    211         //float GetThrust() const;
    212 
    213         /*!
    214         * \brief Add an IODevice to the control law logs
    215         *
    216         * The IODevice will be automatically logged among the Uz logs,
    217         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    218         * and FrameworkManager::AddDeviceToLog). \n
    219         *
    220         * \param device IODevice to log
    221         */
    222         void AddDeviceToControlLawLog(const core::IODevice* device);
    223 
    224         /*!
    225         * \brief Add an io_data to the control law logs
    226         *
    227         * The io_data will be automatically logged among the Uz logs,
    228         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    229         * and FrameworkManager::AddDeviceToLog). \n
    230         *
    231         * \param data io_data to log
    232         */
    233         void AddDataToControlLawLog(const core::io_data* data);
    234 
    235         const sensor::TargetController *GetJoystick(void) const;
    236 
    237         gui::Tab *setupLawTab,*graphLawTab;
    238 
    239     private:
    240         /*!
    241         \enum AltitudeState_t
    242         \brief States of the altitude state machine
    243         */
    244         enum class AltitudeState_t {
    245             Stopped,        /*!< the uav motors are stopped */
    246             TakingOff,      /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
    247             Stabilized,     /*!< the uav is actively maintaining its altitude */
    248             StartLanding,   /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
    249             FinishLanding   /*!< motors are gradually stopped */
    250         };
    251         AltitudeState_t altitudeState;
    252         void ProcessAltitudeFiniteStateMachine();
    253         void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    254 
    255 
    256         float groundAltitude; // effective altitude when the uav leaves the ground
    257         float currentAltitude,currentVerticalSpeed;
    258 
    259         bool failSafeMode;
    260         void SecurityCheck(void);
    261         void MandatorySecurityCheck(void);
    262         void CheckJoystick();
    263         void GenericCheckJoystick();
    264         void CheckPushButton(void);
    265         void GenericCheckPushButton(void);
    266         void Run(void);
    267         void StopMotors(void);
    268 
    269         meta::Uav *uav;
    270 
    271         core::Quaternion currentQuaternion;
    272         core::Vector3D currentAngularSpeed;
    273 
    274         const core::AhrsData *ComputeReferenceOrientation(void);
    275 
    276         void ComputeOrientation(void);
    277         void ComputeAltitude(void);
    278 
    279         void ComputeTorques(void);
    280         core::Euler currentTorques,savedDefaultTorques;
    281         bool needToComputeDefaultTorques;
    282 
    283         void ComputeThrust(void);
    284         float currentThrust,savedDefaultThrust;
    285         bool needToComputeDefaultThrust;
    286 
    287         gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
    288         gui::GridLayout *buttonslayout;
    289         gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
    290         AltitudeMode_t altitudeMode;
    291         OrientationMode_t orientationMode;
    292         ThrustMode_t thrustMode;
    293         TorqueMode_t torqueMode;
    294         bool flagBatteryLow;
    295         bool flagConnectionLost;
    296         bool flagZTrajectoryFinished;
    297         filter::NestedSat *uRoll,*uPitch;
    298         filter::Pid *uYaw;
    299         filter::PidThrust *uZ;
    300 
    301         MetaDualShock3 *joy;
    302         filter::TrajectoryGenerator1D *altitudeTrajectory;
     66class UavStateMachine : public core::Thread {
     67protected:
     68  enum class AltitudeMode_t { Manual, Custom };
     69  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
     70  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
     71
     72  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
     73  // available in mode AltitudeMode_t::Manual
     74  // return true if goto is possible
     75  bool GotoAltitude(float desiredAltitude);
     76
     77  enum class OrientationMode_t { Manual, Custom };
     78  const OrientationMode_t &GetOrientationMode(void) const {
     79    return orientationMode;
     80  }
     81  bool SetOrientationMode(const OrientationMode_t &orientationMode);
     82
     83  enum class ThrustMode_t { Default, Custom };
     84  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
     85  bool SetThrustMode(const ThrustMode_t &thrustMode);
     86
     87  enum class TorqueMode_t { Default, Custom };
     88  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
     89  bool SetTorqueMode(const TorqueMode_t &torqueMode);
     90
     91  enum class Event_t {
     92    EnteringFailSafeMode,
     93    EnteringControlLoop,
     94    StartLanding,
     95    FinishLanding,
     96    Stopped,
     97    TakingOff,
     98    EmergencyStop,
     99    Stabilized, // as soon as uav is 3cm far from the ground
     100    ZTrajectoryFinished,
     101  };
     102
     103  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
     104  ~UavStateMachine();
     105
     106  const core::Quaternion &GetCurrentQuaternion(void) const;
     107
     108  const core::Vector3D &GetCurrentAngularSpeed(void) const;
     109
     110  const meta::Uav *GetUav(void) const;
     111
     112  void Land(void);
     113  void TakeOff(void);
     114  void EmergencyStop(void);
     115  //! Used to signal an event
     116  /*!
     117      \param event the event which occured
     118  */
     119  virtual void SignalEvent(Event_t event);
     120
     121  virtual const core::AhrsData *GetOrientation(void) const;
     122  const core::AhrsData *GetDefaultOrientation(void) const;
     123
     124  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
     125  void EnterFailSafeMode(void);
     126  bool ExitFailSafeMode(void);
     127  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
     128
     129  gui::GridLayout *GetButtonsLayout(void) const;
     130  virtual void ExtraSecurityCheck(void){};
     131  virtual void ExtraCheckJoystick(void){};
     132  virtual void ExtraCheckPushButton(void){};
     133
     134  void GetDefaultReferenceAltitude(float &refAltitude,
     135                                   float &refVerticalVelocity);
     136  virtual void GetReferenceAltitude(float &refAltitude,
     137                                    float &refVerticalVelocity);
     138  // float GetDefaultThrustOffset(void);
     139  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
     140  virtual const core::AhrsData *GetReferenceOrientation(void);
     141
     142  /*!
     143  * \brief Compute Custom Torques
     144  *
     145  * Reimplement this method to use TorqueMode_t::Custom. \n
     146  * This method is called internally by UavStateMachine, do not call it by
     147  *yourself. \n
     148  * See GetTorques if you need torques values.
     149  *
     150  * \param torques custom torques
     151  */
     152  virtual void ComputeCustomTorques(core::Euler &torques);
     153
     154  /*!
     155  * \brief Compute Default Torques
     156  *
     157  * This method is called internally by UavStateMachine when using
     158  *TorqueMode_t::Default. \n
     159  * Torques are only computed once by loop, other calls to this method will use
     160  *previously computed torques.
     161  *
     162  * \param torques default torques
     163  */
     164  void ComputeDefaultTorques(core::Euler &torques);
     165
     166  /*!
     167  * \brief Get Torques
     168  *
     169  * \return torques current torques
     170  */
     171  // const core::Euler &GetTorques() const;
     172
     173  /*!
     174  * \brief Compute Custom Thrust
     175  *
     176  * Reimplement this method to use ThrustMode_t::Custom. \n
     177  * This method is called internally by UavStateMachine, do not call it by
     178  *yourself. \n
     179  * See GetThrust if you need thrust value.
     180  *
     181  * \return custom Thrust
     182  */
     183  virtual float ComputeCustomThrust(void);
     184
     185  /*!
     186  * \brief Compute Default Thrust
     187  *
     188  * This method is called internally by UavStateMachine when using
     189  *ThrustMode_t::Default. \n
     190  * Thrust is only computed once by loop, other calls to this method will use
     191  *previously computed thrust.
     192  *
     193  * \return default thrust
     194  */
     195  float ComputeDefaultThrust(void);
     196
     197  /*!
     198  * \brief Get Thrust
     199  *
     200  * \return current thrust
     201  */
     202  // float GetThrust() const;
     203
     204  /*!
     205  * \brief Add an IODevice to the control law logs
     206  *
     207  * The IODevice will be automatically logged among the Uz logs,
     208  * if logging is enabled (see IODevice::SetDataToLog,
     209  *FrameworkManager::StartLog
     210  * and FrameworkManager::AddDeviceToLog). \n
     211  *
     212  * \param device IODevice to log
     213  */
     214  void AddDeviceToControlLawLog(const core::IODevice *device);
     215
     216  /*!
     217  * \brief Add an io_data to the control law logs
     218  *
     219  * The io_data will be automatically logged among the Uz logs,
     220  * if logging is enabled (see IODevice::SetDataToLog,
     221  *FrameworkManager::StartLog
     222  * and FrameworkManager::AddDeviceToLog). \n
     223  *
     224  * \param data io_data to log
     225  */
     226  void AddDataToControlLawLog(const core::io_data *data);
     227
     228  const sensor::TargetController *GetJoystick(void) const;
     229
     230  gui::Tab *setupLawTab, *graphLawTab;
     231
     232private:
     233  /*!
     234  \enum AltitudeState_t
     235  \brief States of the altitude state machine
     236  */
     237  enum class AltitudeState_t {
     238    Stopped,      /*!< the uav motors are stopped */
     239    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
     240                     the UAV lift up */
     241    Stabilized,   /*!< the uav is actively maintaining its altitude */
     242    StartLanding, /*!< landing initiated. Altitude is required to reach the
     243                     landing altitude (0 by default) */
     244    FinishLanding /*!< motors are gradually stopped */
     245  };
     246  AltitudeState_t altitudeState;
     247  void ProcessAltitudeFiniteStateMachine();
     248  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
     249
     250  float groundAltitude; // effective altitude when the uav leaves the ground
     251  float currentAltitude, currentVerticalSpeed;
     252
     253  bool failSafeMode;
     254  void SecurityCheck(void);
     255  void MandatorySecurityCheck(void);
     256  void CheckJoystick();
     257  void GenericCheckJoystick();
     258  void CheckPushButton(void);
     259  void GenericCheckPushButton(void);
     260  void Run(void);
     261  void StopMotors(void);
     262
     263  meta::Uav *uav;
     264
     265  core::Quaternion currentQuaternion;
     266  core::Vector3D currentAngularSpeed;
     267
     268  const core::AhrsData *ComputeReferenceOrientation(void);
     269
     270  void ComputeOrientation(void);
     271  void ComputeAltitude(void);
     272
     273  void ComputeTorques(void);
     274  core::Euler currentTorques, savedDefaultTorques;
     275  bool needToComputeDefaultTorques;
     276
     277  void ComputeThrust(void);
     278  float currentThrust, savedDefaultThrust;
     279  bool needToComputeDefaultThrust;
     280
     281  gui::PushButton *button_kill, *button_take_off, *button_land,
     282      *button_start_log, *button_stop_log;
     283  gui::GridLayout *buttonslayout;
     284  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
     285  AltitudeMode_t altitudeMode;
     286  OrientationMode_t orientationMode;
     287  ThrustMode_t thrustMode;
     288  TorqueMode_t torqueMode;
     289  bool flagBatteryLow;
     290  bool flagConnectionLost;
     291  bool flagZTrajectoryFinished;
     292  filter::NestedSat *uRoll, *uPitch;
     293  filter::Pid *uYaw;
     294  filter::PidThrust *uZ;
     295
     296  MetaDualShock3 *joy;
     297  filter::TrajectoryGenerator1D *altitudeTrajectory;
    303298};
    304 
    305299};
    306300};
Note: See TracChangeset for help on using the changeset viewer.