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

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

m

File size: 8.6 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
[44]61*  The Thread is created with
62*  the FrameworkManager as parent. FrameworkManager must be created before.
63* The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or
64* SetPeriodMS
[7]65*/
66
[13]67class UavStateMachine : public core::Thread {
68protected:
69  enum class AltitudeMode_t { Manual, Custom };
70  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
71  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
[7]72
[13]73  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
74  // available in mode AltitudeMode_t::Manual
75  // return true if goto is possible
76  bool GotoAltitude(float desiredAltitude);
[7]77
[13]78  enum class OrientationMode_t { Manual, Custom };
79  const OrientationMode_t &GetOrientationMode(void) const {
80    return orientationMode;
81  }
82  bool SetOrientationMode(const OrientationMode_t &orientationMode);
[7]83
[13]84  enum class ThrustMode_t { Default, Custom };
85  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
86  bool SetThrustMode(const ThrustMode_t &thrustMode);
[7]87
[13]88  enum class TorqueMode_t { Default, Custom };
89  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
90  bool SetTorqueMode(const TorqueMode_t &torqueMode);
[7]91
[13]92  enum class Event_t {
93    EnteringFailSafeMode,
94    EnteringControlLoop,
95    StartLanding,
96    FinishLanding,
97    Stopped,
98    TakingOff,
99    EmergencyStop,
100    Stabilized, // as soon as uav is 3cm far from the ground
101    ZTrajectoryFinished,
102  };
[7]103
[44]104        UavStateMachine(sensor::TargetController* controller);
[25]105        ~UavStateMachine();
[7]106
[13]107  const core::Quaternion &GetCurrentQuaternion(void) const;
[7]108
[50]109  const core::Vector3Df &GetCurrentAngularSpeed(void) const;
[7]110
[25]111        void Land(void);
112        void EmergencyLand(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);
[7]120
[13]121  virtual const core::AhrsData *GetOrientation(void) const;
122  const core::AhrsData *GetDefaultOrientation(void) const;
[7]123
[13]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!
[7]128
[13]129  gui::GridLayout *GetButtonsLayout(void) const;
130  virtual void ExtraSecurityCheck(void){};
131  virtual void ExtraCheckJoystick(void){};
132  virtual void ExtraCheckPushButton(void){};
[7]133
[13]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);
[7]141
[13]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);
[7]153
[13]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);
[7]165
[13]166  /*!
167  * \brief Get Torques
168  *
169  * \return torques current torques
170  */
171  // const core::Euler &GetTorques() const;
[7]172
[13]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);
[7]184
[13]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);
[7]196
[13]197  /*!
198  * \brief Get Thrust
199  *
200  * \return current thrust
201  */
202  // float GetThrust() const;
[7]203
[13]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);
[7]215
[13]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);
[7]227
[13]228  const sensor::TargetController *GetJoystick(void) const;
[50]229  MetaDualShock3 *joy;
[62]230 
231  filter::NestedSat *GetURoll(void);
232  filter::NestedSat *GetUPitch(void);
233  filter::Pid *GetUYaw(void);
234  filter::PidThrust *GetUZ(void);
235  filter::TrajectoryGenerator1D *GetAltitudeTrajectory(void);
[7]236
[13]237  gui::Tab *setupLawTab, *graphLawTab;
[7]238
[13]239private:
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
247                     the UAV lift up */
248    Stabilized,   /*!< the uav is actively maintaining its altitude */
249    StartLanding, /*!< landing initiated. Altitude is required to reach the
250                     landing altitude (0 by default) */
251    FinishLanding /*!< motors are gradually stopped */
252  };
253  AltitudeState_t altitudeState;
254  void ProcessAltitudeFiniteStateMachine();
255  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
[7]256
[13]257  float groundAltitude; // effective altitude when the uav leaves the ground
258  float currentAltitude, currentVerticalSpeed;
[7]259
[13]260  bool failSafeMode;
261  void SecurityCheck(void);
262  void MandatorySecurityCheck(void);
263  void CheckJoystick();
264  void GenericCheckJoystick();
265  void CheckPushButton(void);
266  void GenericCheckPushButton(void);
267  void Run(void);
268  void StopMotors(void);
[27]269  bool IsValuePossible(float value,std::string desc);
[7]270
[13]271  meta::Uav *uav;
[25]272  sensor::TargetController *controller;
[7]273
[13]274  core::Quaternion currentQuaternion;
[50]275  core::Vector3Df currentAngularSpeed;
[7]276
[13]277  const core::AhrsData *ComputeReferenceOrientation(void);
[7]278
[13]279  void ComputeOrientation(void);
280  void ComputeAltitude(void);
[7]281
[13]282  void ComputeTorques(void);
283  core::Euler currentTorques, savedDefaultTorques;
284  bool needToComputeDefaultTorques;
[7]285
[13]286  void ComputeThrust(void);
287  float currentThrust, savedDefaultThrust;
288  bool needToComputeDefaultThrust;
[7]289
[13]290  gui::PushButton *button_kill, *button_take_off, *button_land,
291      *button_start_log, *button_stop_log;
292  gui::GridLayout *buttonslayout;
293  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
294  AltitudeMode_t altitudeMode;
295  OrientationMode_t orientationMode;
296  ThrustMode_t thrustMode;
297  TorqueMode_t torqueMode;
298  bool flagBatteryLow;
299  bool flagConnectionLost;
[25]300  bool flagCriticalSensorLost;
[13]301  bool flagZTrajectoryFinished;
[25]302  bool safeToFly;
[13]303  filter::NestedSat *uRoll, *uPitch;
304  filter::Pid *uYaw;
305  filter::PidThrust *uZ;
306  filter::TrajectoryGenerator1D *altitudeTrajectory;
[7]307};
308};
309};
310#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.