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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 7 years ago

lic

File size: 9.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 {
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
55namespace flair { namespace meta {
56
57 /*! \class UavStateMachine
58*
59* \brief State machine for UAV
60*
61* thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
62*/
63
64class UavStateMachine : public core::Thread
65{
66 protected:
67 enum class AltitudeMode_t {
68 Manual,
69 Custom
70 };
71 const AltitudeMode_t &GetAltitudeMode(void) const {
72 return altitudeMode;
73 }
74 bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
75
76 //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
77 //available in mode AltitudeMode_t::Manual
78 //return true if goto is possible
79 bool GotoAltitude(float desiredAltitude);
80
81 enum class OrientationMode_t {
82 Manual,
83 Custom
84 };
85 const OrientationMode_t &GetOrientationMode(void) const {
86 return orientationMode;
87 }
88 bool SetOrientationMode(const OrientationMode_t &orientationMode);
89
90 enum class ThrustMode_t {
91 Default,
92 Custom
93 };
94 const ThrustMode_t &GetThrustMode() const {
95 return thrustMode;
96 }
97 bool SetThrustMode(const ThrustMode_t &thrustMode);
98
99 enum class TorqueMode_t {
100 Default,
101 Custom
102 };
103 const TorqueMode_t &GetTorqueMode(void) const {
104 return torqueMode;
105 }
106 bool SetTorqueMode(const TorqueMode_t &torqueMode);
107
108 enum class Event_t {
109 EnteringFailSafeMode,
110 EnteringControlLoop,
111 StartLanding,
112 FinishLanding,
113 Stopped,
114 TakingOff,
115 EmergencyStop,
116 Stabilized, //as soon as uav is 3cm far from the ground
117 ZTrajectoryFinished,
118 };
119
120 UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
121 ~UavStateMachine();
122
123 const core::Quaternion &GetCurrentQuaternion(void) const;
124
125 const core::Vector3D &GetCurrentAngularSpeed(void) const;
126
127 const meta::Uav *GetUav(void) const;
128
129 void Land(void);
130 void TakeOff(void);
131 void EmergencyStop(void);
132 //! Used to signal an event
133 /*!
134 \param event the event which occured
135 */
136 virtual void SignalEvent(Event_t event);
137
138 virtual const core::AhrsData *GetOrientation(void) const;
139 const core::AhrsData *GetDefaultOrientation(void) const;
140
141 virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
142 void EnterFailSafeMode(void);
143 bool ExitFailSafeMode(void);
144 void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
145
146 gui::GridLayout *GetButtonsLayout(void) const;
147 virtual void ExtraSecurityCheck(void){};
148 virtual void ExtraCheckJoystick(void){};
149 virtual void ExtraCheckPushButton(void){};
150
151 void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
152 virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
153 //float GetDefaultThrustOffset(void);
154 const core::AhrsData *GetDefaultReferenceOrientation(void) const;
155 virtual const core::AhrsData *GetReferenceOrientation(void);
156
157 /*!
158 * \brief Compute Custom Torques
159 *
160 * Reimplement this method to use TorqueMode_t::Custom. \n
161 * This method is called internally by UavStateMachine, do not call it by yourself. \n
162 * See GetTorques if you need torques values.
163 *
164 * \param torques custom torques
165 */
166 virtual void ComputeCustomTorques(core::Euler &torques);
167
168 /*!
169 * \brief Compute Default Torques
170 *
171 * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
172 * Torques are only computed once by loop, other calls to this method will use 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 yourself. \n
190 * See GetThrust if you need thrust value.
191 *
192 * \return custom Thrust
193 */
194 virtual float ComputeCustomThrust(void);
195
196 /*!
197 * \brief Compute Default Thrust
198 *
199 * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
200 * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
201 *
202 * \return default thrust
203 */
204 float ComputeDefaultThrust(void);
205
206 /*!
207 * \brief Get Thrust
208 *
209 * \return current thrust
210 */
211 //float GetThrust() const;
212
213 /*!
214 * \brief Add an IODevice to the control law logs
215 *
216 * The IODevice will be automatically logged among the Uz logs,
217 * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
218 * and FrameworkManager::AddDeviceToLog). \n
219 *
220 * \param device IODevice to log
221 */
222 void AddDeviceToControlLawLog(const core::IODevice* device);
223
224 /*!
225 * \brief Add an io_data to the control law logs
226 *
227 * The io_data will be automatically logged among the Uz logs,
228 * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
229 * and FrameworkManager::AddDeviceToLog). \n
230 *
231 * \param data io_data to log
232 */
233 void AddDataToControlLawLog(const core::io_data* data);
234
235 const sensor::TargetController *GetJoystick(void) const;
236
237 gui::Tab *setupLawTab,*graphLawTab;
238
239 private:
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 the UAV lift up */
247 Stabilized, /*!< the uav is actively maintaining its altitude */
248 StartLanding, /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
249 FinishLanding /*!< motors are gradually stopped */
250 };
251 AltitudeState_t altitudeState;
252 void ProcessAltitudeFiniteStateMachine();
253 void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
254
255
256 float groundAltitude; // effective altitude when the uav leaves the ground
257 float currentAltitude,currentVerticalSpeed;
258
259 bool failSafeMode;
260 void SecurityCheck(void);
261 void MandatorySecurityCheck(void);
262 void CheckJoystick();
263 void GenericCheckJoystick();
264 void CheckPushButton(void);
265 void GenericCheckPushButton(void);
266 void Run(void);
267 void StopMotors(void);
268
269 meta::Uav *uav;
270
271 core::Quaternion currentQuaternion;
272 core::Vector3D currentAngularSpeed;
273
274 const core::AhrsData *ComputeReferenceOrientation(void);
275
276 void ComputeOrientation(void);
277 void ComputeAltitude(void);
278
279 void ComputeTorques(void);
280 core::Euler currentTorques,savedDefaultTorques;
281 bool needToComputeDefaultTorques;
282
283 void ComputeThrust(void);
284 float currentThrust,savedDefaultThrust;
285 bool needToComputeDefaultThrust;
286
287 gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
288 gui::GridLayout *buttonslayout;
289 gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
290 AltitudeMode_t altitudeMode;
291 OrientationMode_t orientationMode;
292 ThrustMode_t thrustMode;
293 TorqueMode_t torqueMode;
294 bool flagBatteryLow;
295 bool flagConnectionLost;
296 bool flagZTrajectoryFinished;
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};
307#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.