source: flair-dev/trunk/include/FlairMeta/UavStateMachine.h @ 27

Last change on this file since 27 was 27, checked in by Sanahuja Guillaume, 4 years ago

m

File size: 8.4 KB
RevLine 
[9]1// %flair:license{
[13]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[9]4// %flair:license}
[7]5//  created:    2014/04/29
6//  filename:   UavStateMachine.h
7//
8//  author:     Gildas Bayard, Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    state machine for UAV
14//
15//
16/*********************************************************************/
17
18#ifndef UAVSTATEMACHINE_H
19#define UAVSTATEMACHINE_H
20
21#include <Thread.h>
22#include <Vector2D.h>
23#include <Vector3D.h>
24#include <Euler.h>
25#include <Quaternion.h>
26
27namespace flair {
[13]28namespace core {
29class FrameworkManager;
30class AhrsData;
31class io_data;
[7]32}
[13]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}
[7]54
[13]55namespace flair {
56namespace meta {
[7]57
[13]58/*! \class UavStateMachine
[7]59*
60* \brief State machine for UAV
61*
[13]62* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
63*SetPeriodMS
[7]64*/
65
[13]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);
[7]71
[13]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);
[7]76
[13]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);
[7]82
[13]83  enum class ThrustMode_t { Default, Custom };
84  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
85  bool SetThrustMode(const ThrustMode_t &thrustMode);
[7]86
[13]87  enum class TorqueMode_t { Default, Custom };
88  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
89  bool SetTorqueMode(const TorqueMode_t &torqueMode);
[7]90
[13]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  };
[7]102
[25]103        UavStateMachine(meta::Uav* uav, sensor::TargetController* controller);
104        ~UavStateMachine();
[7]105
[13]106  const core::Quaternion &GetCurrentQuaternion(void) const;
[7]107
[13]108  const core::Vector3D &GetCurrentAngularSpeed(void) const;
[7]109
[13]110  const meta::Uav *GetUav(void) const;
[7]111
[25]112        void Land(void);
113        void EmergencyLand(void);
114        void TakeOff(void);
115        void EmergencyStop(void);
116        //! Used to signal an event
117        /*!
118            \param event the event which occured
119        */
120        virtual void SignalEvent(Event_t event);
[7]121
[13]122  virtual const core::AhrsData *GetOrientation(void) const;
123  const core::AhrsData *GetDefaultOrientation(void) const;
[7]124
[13]125  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
126  void EnterFailSafeMode(void);
127  bool ExitFailSafeMode(void);
128  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
[7]129
[13]130  gui::GridLayout *GetButtonsLayout(void) const;
131  virtual void ExtraSecurityCheck(void){};
132  virtual void ExtraCheckJoystick(void){};
133  virtual void ExtraCheckPushButton(void){};
[7]134
[13]135  void GetDefaultReferenceAltitude(float &refAltitude,
136                                   float &refVerticalVelocity);
137  virtual void GetReferenceAltitude(float &refAltitude,
138                                    float &refVerticalVelocity);
139  // float GetDefaultThrustOffset(void);
140  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
141  virtual const core::AhrsData *GetReferenceOrientation(void);
[7]142
[13]143  /*!
144  * \brief Compute Custom Torques
145  *
146  * Reimplement this method to use TorqueMode_t::Custom. \n
147  * This method is called internally by UavStateMachine, do not call it by
148  *yourself. \n
149  * See GetTorques if you need torques values.
150  *
151  * \param torques custom torques
152  */
153  virtual void ComputeCustomTorques(core::Euler &torques);
[7]154
[13]155  /*!
156  * \brief Compute Default Torques
157  *
158  * This method is called internally by UavStateMachine when using
159  *TorqueMode_t::Default. \n
160  * Torques are only computed once by loop, other calls to this method will use
161  *previously computed torques.
162  *
163  * \param torques default torques
164  */
165  void ComputeDefaultTorques(core::Euler &torques);
[7]166
[13]167  /*!
168  * \brief Get Torques
169  *
170  * \return torques current torques
171  */
172  // const core::Euler &GetTorques() const;
[7]173
[13]174  /*!
175  * \brief Compute Custom Thrust
176  *
177  * Reimplement this method to use ThrustMode_t::Custom. \n
178  * This method is called internally by UavStateMachine, do not call it by
179  *yourself. \n
180  * See GetThrust if you need thrust value.
181  *
182  * \return custom Thrust
183  */
184  virtual float ComputeCustomThrust(void);
[7]185
[13]186  /*!
187  * \brief Compute Default Thrust
188  *
189  * This method is called internally by UavStateMachine when using
190  *ThrustMode_t::Default. \n
191  * Thrust is only computed once by loop, other calls to this method will use
192  *previously computed thrust.
193  *
194  * \return default thrust
195  */
196  float ComputeDefaultThrust(void);
[7]197
[13]198  /*!
199  * \brief Get Thrust
200  *
201  * \return current thrust
202  */
203  // float GetThrust() const;
[7]204
[13]205  /*!
206  * \brief Add an IODevice to the control law logs
207  *
208  * The IODevice will be automatically logged among the Uz logs,
209  * if logging is enabled (see IODevice::SetDataToLog,
210  *FrameworkManager::StartLog
211  * and FrameworkManager::AddDeviceToLog). \n
212  *
213  * \param device IODevice to log
214  */
215  void AddDeviceToControlLawLog(const core::IODevice *device);
[7]216
[13]217  /*!
218  * \brief Add an io_data to the control law logs
219  *
220  * The io_data will be automatically logged among the Uz logs,
221  * if logging is enabled (see IODevice::SetDataToLog,
222  *FrameworkManager::StartLog
223  * and FrameworkManager::AddDeviceToLog). \n
224  *
225  * \param data io_data to log
226  */
227  void AddDataToControlLawLog(const core::io_data *data);
[7]228
[13]229  const sensor::TargetController *GetJoystick(void) const;
[7]230
[13]231  gui::Tab *setupLawTab, *graphLawTab;
[7]232
[13]233private:
234  /*!
235  \enum AltitudeState_t
236  \brief States of the altitude state machine
237  */
238  enum class AltitudeState_t {
239    Stopped,      /*!< the uav motors are stopped */
240    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
241                     the UAV lift up */
242    Stabilized,   /*!< the uav is actively maintaining its altitude */
243    StartLanding, /*!< landing initiated. Altitude is required to reach the
244                     landing altitude (0 by default) */
245    FinishLanding /*!< motors are gradually stopped */
246  };
247  AltitudeState_t altitudeState;
248  void ProcessAltitudeFiniteStateMachine();
249  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
[7]250
[13]251  float groundAltitude; // effective altitude when the uav leaves the ground
252  float currentAltitude, currentVerticalSpeed;
[7]253
[13]254  bool failSafeMode;
255  void SecurityCheck(void);
256  void MandatorySecurityCheck(void);
257  void CheckJoystick();
258  void GenericCheckJoystick();
259  void CheckPushButton(void);
260  void GenericCheckPushButton(void);
261  void Run(void);
262  void StopMotors(void);
[27]263  bool IsValuePossible(float value,std::string desc);
[7]264
[13]265  meta::Uav *uav;
[25]266  sensor::TargetController *controller;
[7]267
[13]268  core::Quaternion currentQuaternion;
269  core::Vector3D currentAngularSpeed;
[7]270
[13]271  const core::AhrsData *ComputeReferenceOrientation(void);
[7]272
[13]273  void ComputeOrientation(void);
274  void ComputeAltitude(void);
[7]275
[13]276  void ComputeTorques(void);
277  core::Euler currentTorques, savedDefaultTorques;
278  bool needToComputeDefaultTorques;
[7]279
[13]280  void ComputeThrust(void);
281  float currentThrust, savedDefaultThrust;
282  bool needToComputeDefaultThrust;
[7]283
[13]284  gui::PushButton *button_kill, *button_take_off, *button_land,
285      *button_start_log, *button_stop_log;
286  gui::GridLayout *buttonslayout;
287  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
288  AltitudeMode_t altitudeMode;
289  OrientationMode_t orientationMode;
290  ThrustMode_t thrustMode;
291  TorqueMode_t torqueMode;
292  bool flagBatteryLow;
293  bool flagConnectionLost;
[25]294  bool flagCriticalSensorLost;
[13]295  bool flagZTrajectoryFinished;
[25]296  bool safeToFly;
[13]297  filter::NestedSat *uRoll, *uPitch;
298  filter::Pid *uYaw;
299  filter::PidThrust *uZ;
[7]300
[13]301  MetaDualShock3 *joy;
302  filter::TrajectoryGenerator1D *altitudeTrajectory;
[7]303};
304};
305};
306#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.