Flair
Framework Libre Air
UavStateMachine.h
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 
27 namespace 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 {
56 namespace meta {
57 
67 class UavStateMachine : public core::Thread {
68 protected:
69  enum class AltitudeMode_t { Manual, Custom };
70  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
71  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
72 
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);
77 
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);
83 
84  enum class ThrustMode_t { Default, Custom };
85  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
86  bool SetThrustMode(const ThrustMode_t &thrustMode);
87 
88  enum class TorqueMode_t { Default, Custom };
89  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
90  bool SetTorqueMode(const TorqueMode_t &torqueMode);
91 
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  };
103 
105  ~UavStateMachine();
106 
107  const core::Quaternion &GetCurrentQuaternion(void) const;
108 
109  const core::Vector3Df &GetCurrentAngularSpeed(void) const;
110 
111  void Land(void);
112  void EmergencyLand(void);
113  void TakeOff(void);
114  void EmergencyStop(void);
116 
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 
152  virtual void ComputeCustomTorques(core::Euler &torques);
153 
164  void ComputeDefaultTorques(core::Euler &torques);
165 
171  // const core::Euler &GetTorques() const;
172 
183  virtual float ComputeCustomThrust(void);
184 
195  float ComputeDefaultThrust(void);
196 
202  // float GetThrust() const;
203 
214  void AddDeviceToControlLawLog(const core::IODevice *device);
215 
226  void AddDataToControlLawLog(const core::io_data *data);
227 
228  const sensor::TargetController *GetJoystick(void) const;
229  MetaDualShock3 *joy;
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);
236 
237  gui::Tab *setupLawTab, *graphLawTab;
238 
239 private:
244  enum class AltitudeState_t {
245  Stopped,
246  TakingOff,
248  Stabilized,
249  StartLanding,
252  };
253  AltitudeState_t altitudeState;
254  void ProcessAltitudeFiniteStateMachine();
255  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
256 
257  float groundAltitude; // effective altitude when the uav leaves the ground
258  float currentAltitude, currentVerticalSpeed;
259 
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);
269  bool IsValuePossible(float value,std::string desc);
270 
271  meta::Uav *uav;
272  sensor::TargetController *controller;
273 
274  core::Quaternion currentQuaternion;
275  core::Vector3Df currentAngularSpeed;
276 
277  const core::AhrsData *ComputeReferenceOrientation(void);
278 
279  void ComputeOrientation(void);
280  void ComputeAltitude(void);
281 
282  void ComputeTorques(void);
283  core::Euler currentTorques, savedDefaultTorques;
284  bool needToComputeDefaultTorques;
285 
286  void ComputeThrust(void);
287  float currentThrust, savedDefaultThrust;
288  bool needToComputeDefaultThrust;
289 
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;
300  bool flagCriticalSensorLost;
301  bool flagZTrajectoryFinished;
302  bool safeToFly;
303  filter::NestedSat *uRoll, *uPitch;
304  filter::Pid *uYaw;
305  filter::PidThrust *uZ;
306  filter::TrajectoryGenerator1D *altitudeTrajectory;
307 };
308 };
309 };
310 #endif // UAVSTATEMACHINE_H
Abstract class for data types.
Definition: io_data.h:94
virtual void SignalEvent(Event_t event)
Used to signal an event.
Classe intégrant la manette MetaDualShock3.
Definition: MetaDualShock3.h:38
Abstract class for input/ouput system.
Definition: IODevice.h:45
namespace of the flair Framework
Definition: Ahrs.h:19
Class defining euler angles.
Definition: Euler.h:27
float ComputeDefaultThrust(void)
Compute Default Thrust.
Class defining a 2D vector.
Class displaying a QGridLayout on the ground station.
Definition: GridLayout.h:27
Class defining a Pid for Thrust. This Pid as an extra offset for compensating gravity.
Definition: PidThrust.h:33
Class defining AHRS datas.
Definition: AhrsData.h:30
Class defining a 3D vector.
State machine for UAV The Thread is created with the FrameworkManager as parent. FrameworkManager mus...
Definition: UavStateMachine.h:67
Base Class for target side remote controls.
Definition: TargetController.h:49
Class generating a trajectory in 1D.
Definition: TrajectoryGenerator1D.h:47
virtual float ComputeCustomThrust(void)
Get Torques.
void AddDeviceToControlLawLog(const core::IODevice *device)
Get Thrust.
Class displaying a QPushButton on the ground station.
Definition: PushButton.h:28
virtual void ComputeCustomTorques(core::Euler &torques)
Compute Custom Torques.
Class defining a PID.
Definition: Pid.h:32
Abstract class for a thread.
Class defining a quaternion.
Class defining a PID with saturations.
Definition: NestedSat.h:43
Abstract class for a thread.
Definition: Thread.h:38
Class displaying a QDoubleSpinBox on the ground station.
Definition: DoubleSpinBox.h:28
Base class to construct sensors/actuators depending on uav type. The Object is created with the Frame...
Definition: Uav.h:48
void AddDataToControlLawLog(const core::io_data *data)
Add an io_data to the control law logs.
void ComputeDefaultTorques(core::Euler &torques)
Compute Default Torques.
Class defining a quaternion.
Definition: Quaternion.h:26
Class displaying a QTab on the ground station.
Definition: Tab.h:29
Class defining euler angles.