source: flair-src/branches/mavlink/lib/FlairMeta/src/UavStateMachine.h@ 46

Last change on this file since 46 was 42, checked in by Sanahuja Guillaume, 6 years ago

m

File size: 8.4 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 {
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
59*
60* \brief State machine for UAV
61*
62* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
63*SetPeriodMS
64*/
65
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, sensor::TargetController* controller);
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 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 gui::GridLayout *GetButtonsLayout(void) const;
131 virtual void ExtraSecurityCheck(void){};
132 virtual void ExtraCheckJoystick(void){};
133 virtual void ExtraCheckPushButton(void){};
134
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);
142
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);
154
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);
166
167 /*!
168 * \brief Get Torques
169 *
170 * \return torques current torques
171 */
172 // const core::Euler &GetTorques() const;
173
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);
185
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);
197
198 /*!
199 * \brief Get Thrust
200 *
201 * \return current thrust
202 */
203 // float GetThrust() const;
204
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);
216
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);
228
229 const sensor::TargetController *GetJoystick(void) const;
230
231 gui::Tab *setupLawTab, *graphLawTab;
232
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);
250
251 float groundAltitude; // effective altitude when the uav leaves the ground
252 float currentAltitude, currentVerticalSpeed;
253
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);
263 bool CheckIsNan(float value,std::string desc);
264
265 meta::Uav *uav;
266 sensor::TargetController *controller;
267
268 core::Quaternion currentQuaternion;
269 core::Vector3D currentAngularSpeed;
270
271 const core::AhrsData *ComputeReferenceOrientation(void);
272
273 void ComputeOrientation(void);
274 void ComputeAltitude(void);
275
276 void ComputeTorques(void);
277 core::Euler currentTorques, savedDefaultTorques;
278 bool needToComputeDefaultTorques;
279
280 void ComputeThrust(void);
281 float currentThrust, savedDefaultThrust;
282 bool needToComputeDefaultThrust;
283
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;
294 bool flagCriticalSensorLost;
295 bool flagZTrajectoryFinished;
296 bool safeToFly;
297 filter::NestedSat *uRoll, *uPitch;
298 filter::Pid *uYaw;
299 filter::PidThrust *uZ;
300
301 MetaDualShock3 *joy;
302 filter::TrajectoryGenerator1D *altitudeTrajectory;
303};
304};
305};
306#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.