source: flair-src/trunk/lib/FlairMeta/src/UavStateMachine.h @ 432

Last change on this file since 432 was 432, checked in by Sanahuja Guillaume, 5 months ago

add AltitudeSensor? class
failsafe altitude sensor in changeable

File size: 9.2 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
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 {
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    class AltitudeSensor;
49  }
50  namespace meta {
51    class MetaDualShock3;
52    class Uav;
53  }
54}
55
56namespace flair {
57namespace meta {
58
59/*! \class UavStateMachine
60*
61* \brief State machine for UAV
62*  The Thread is created with
63*  the FrameworkManager as parent. FrameworkManager must be created before.
64* The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or
65* SetPeriodMS
66*/
67
68class UavStateMachine : public core::Thread {
69protected:
70  enum class AltitudeMode_t { Manual, Custom };
71  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
72  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
73
74  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
75  // available in mode AltitudeMode_t::Manual
76  // return true if goto is possible
77  bool GotoAltitude(float desiredAltitude);
78
79  enum class OrientationMode_t { Manual, Custom };
80  const OrientationMode_t &GetOrientationMode(void) const {
81    return orientationMode;
82  }
83  bool SetOrientationMode(const OrientationMode_t &orientationMode);
84
85  enum class ThrustMode_t { Default, Custom };
86  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
87  bool SetThrustMode(const ThrustMode_t &thrustMode);
88
89  enum class TorqueMode_t { Default, Custom };
90  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
91  bool SetTorqueMode(const TorqueMode_t &torqueMode);
92
93  enum class Event_t {
94    EnteringFailSafeMode,
95    EnteringControlLoop,
96    StartLanding,
97    FinishLanding,
98    Stopped,
99    TakingOff,
100    EmergencyStop,
101    Stabilized, // as soon as uav is 3cm far from the ground
102    ZTrajectoryFinished,
103  };
104
105  UavStateMachine(sensor::TargetController* controller);
106  ~UavStateMachine();
107
108  const core::Quaternion &GetCurrentQuaternion(void) const;
109
110  const core::Vector3Df &GetCurrentAngularSpeed(void) const;
111
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);
121
122  virtual const core::AhrsData *GetOrientation(void) const;
123  const core::AhrsData *GetDefaultOrientation(void) const;
124
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!
129 
130  /*!
131  * \brief Set the FailSafe AltitudeSensor
132  *
133  * by default, the failsafe AltitudeSensor is the MetaUsRangeFinder
134  * you can change it (by a vrpn one) using this method in the case us range finder is not working well
135  * use it carefully, trusting only vrpn can be risquee
136  *
137  * \param altitudeSensor altitudeSensor to use when is failsafe
138  */
139  void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);
140
141  gui::GridLayout *GetButtonsLayout(void) const;
142  virtual void ExtraSecurityCheck(void){};
143  virtual void ExtraCheckJoystick(void){};
144  virtual void ExtraCheckPushButton(void){};
145
146  void GetDefaultReferenceAltitude(float &refAltitude,
147                                   float &refVerticalVelocity);
148  virtual void GetReferenceAltitude(float &refAltitude,
149                                    float &refVerticalVelocity);
150  // float GetDefaultThrustOffset(void);
151  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
152  virtual const core::AhrsData *GetReferenceOrientation(void);
153
154  /*!
155  * \brief Compute Custom Torques
156  *
157  * Reimplement this method to use TorqueMode_t::Custom. \n
158  * This method is called internally by UavStateMachine, do not call it by
159  *yourself. \n
160  * See GetTorques if you need torques values.
161  *
162  * \param torques custom torques
163  */
164  virtual void ComputeCustomTorques(core::Euler &torques);
165
166  /*!
167  * \brief Compute Default Torques
168  *
169  * This method is called internally by UavStateMachine when using
170  *TorqueMode_t::Default. \n
171  * Torques are only computed once by loop, other calls to this method will use
172  *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
190  *yourself. \n
191  * See GetThrust if you need thrust value.
192  *
193  * \return custom Thrust
194  */
195  virtual float ComputeCustomThrust(void);
196
197  /*!
198  * \brief Compute Default Thrust
199  *
200  * This method is called internally by UavStateMachine when using
201  *ThrustMode_t::Default. \n
202  * Thrust is only computed once by loop, other calls to this method will use
203  *previously computed thrust.
204  *
205  * \return default thrust
206  */
207  float ComputeDefaultThrust(void);
208
209  /*!
210  * \brief Get Thrust
211  *
212  * \return current thrust
213  */
214  // float GetThrust() const;
215
216  /*!
217  * \brief Add an IODevice to the control law logs
218  *
219  * The IODevice 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 device IODevice to log
225  */
226  void AddDeviceToControlLawLog(const core::IODevice *device);
227
228  /*!
229  * \brief Add an io_data to the control law logs
230  *
231  * The io_data will be automatically logged among the Uz logs,
232  * if logging is enabled (see IODevice::SetDataToLog,
233  *FrameworkManager::StartLog
234  * and FrameworkManager::AddDeviceToLog). \n
235  *
236  * \param data io_data to log
237  */
238  void AddDataToControlLawLog(const core::io_data *data);
239
240  const sensor::TargetController *GetTargetController(void) const;
241  MetaDualShock3 *GetJoystick(void) const;
242 
243  filter::NestedSat *GetURoll(void);
244  filter::NestedSat *GetUPitch(void);
245  filter::Pid *GetUYaw(void);
246  filter::PidThrust *GetUZ(void);
247  filter::TrajectoryGenerator1D *GetAltitudeTrajectory(void);
248
249  gui::Tab *setupLawTab, *graphLawTab;
250
251private:
252  /*!
253  \enum AltitudeState_t
254  \brief States of the altitude state machine
255  */
256  enum class AltitudeState_t {
257    Stopped,      /*!< the uav motors are stopped */
258    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
259                     the UAV lift up */
260    Stabilized,   /*!< the uav is actively maintaining its altitude */
261    StartLanding, /*!< landing initiated. Altitude is required to reach the
262                     landing altitude (0 by default) */
263    FinishLanding /*!< motors are gradually stopped */
264  };
265  AltitudeState_t altitudeState;
266  void ProcessAltitudeFiniteStateMachine();
267  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
268
269  float groundAltitude; // effective altitude when the uav leaves the ground
270  float currentAltitude, currentVerticalSpeed;
271
272  bool failSafeMode;
273  void SecurityCheck(void);
274  void MandatorySecurityCheck(void);
275  void CheckJoystick();
276  void GenericCheckJoystick();
277  void CheckPushButton(void);
278  void GenericCheckPushButton(void);
279  void Run(void);
280  void StopMotors(void);
281  bool IsValuePossible(float value,std::string desc);
282
283  Uav *uav;
284  MetaDualShock3 *joy;
285  sensor::TargetController *controller;
286
287  core::Quaternion currentQuaternion;
288  core::Vector3Df currentAngularSpeed;
289
290  const core::AhrsData *ComputeReferenceOrientation(void);
291
292  void ComputeOrientation(void);
293  void ComputeAltitude(void);
294
295  void ComputeTorques(void);
296  core::Euler currentTorques, savedDefaultTorques;
297  bool needToComputeDefaultTorques;
298
299  void ComputeThrust(void);
300  float currentThrust, savedDefaultThrust;
301  bool needToComputeDefaultThrust;
302
303  gui::PushButton *button_kill, *button_take_off, *button_land,
304      *button_start_log, *button_stop_log;
305  gui::GridLayout *buttonslayout;
306  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
307  AltitudeMode_t altitudeMode;
308  OrientationMode_t orientationMode;
309  ThrustMode_t thrustMode;
310  TorqueMode_t torqueMode;
311  bool flagBatteryLow;
312  bool flagConnectionLost;
313  bool flagCriticalSensorLost;
314  bool flagZTrajectoryFinished;
315  bool safeToFly;
316  filter::NestedSat *uRoll, *uPitch;
317  filter::Pid *uYaw;
318  filter::PidThrust *uZ;
319  filter::TrajectoryGenerator1D *altitudeTrajectory;
320  sensor::AltitudeSensor *failSafeAltitudeSensor;
321};
322}; // end namespace meta
323}; // end namespace flair
324#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.