source: flair-src/tags/latest/lib/FlairMeta/src/UavStateMachine.h@ 456

Last change on this file since 456 was 313, checked in by Sanahuja Guillaume, 3 years ago

maj thread

File size: 8.7 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* 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
65*/
66
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);
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
104 UavStateMachine(sensor::TargetController* controller);
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);
115 //! Used to signal an event
116 /*!
117 \param event the event which occured
118 */
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
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);
153
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);
165
166 /*!
167 * \brief Get Torques
168 *
169 * \return torques current torques
170 */
171 // const core::Euler &GetTorques() const;
172
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);
184
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);
196
197 /*!
198 * \brief Get Thrust
199 *
200 * \return current thrust
201 */
202 // float GetThrust() const;
203
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);
215
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);
227
228 const sensor::TargetController *GetTargetController(void) const;
229 MetaDualShock3 *GetJoystick(void) const;
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
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);
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 Uav *uav;
272 MetaDualShock3 *joy;
273 sensor::TargetController *controller;
274
275 core::Quaternion currentQuaternion;
276 core::Vector3Df currentAngularSpeed;
277
278 const core::AhrsData *ComputeReferenceOrientation(void);
279
280 void ComputeOrientation(void);
281 void ComputeAltitude(void);
282
283 void ComputeTorques(void);
284 core::Euler currentTorques, savedDefaultTorques;
285 bool needToComputeDefaultTorques;
286
287 void ComputeThrust(void);
288 float currentThrust, savedDefaultThrust;
289 bool needToComputeDefaultThrust;
290
291 gui::PushButton *button_kill, *button_take_off, *button_land,
292 *button_start_log, *button_stop_log;
293 gui::GridLayout *buttonslayout;
294 gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
295 AltitudeMode_t altitudeMode;
296 OrientationMode_t orientationMode;
297 ThrustMode_t thrustMode;
298 TorqueMode_t torqueMode;
299 bool flagBatteryLow;
300 bool flagConnectionLost;
301 bool flagCriticalSensorLost;
302 bool flagZTrajectoryFinished;
303 bool safeToFly;
304 filter::NestedSat *uRoll, *uPitch;
305 filter::Pid *uYaw;
306 filter::PidThrust *uZ;
307 filter::TrajectoryGenerator1D *altitudeTrajectory;
308};
309};
310};
311#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.