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

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

filter and meta

File size: 9.3 KB
Line 
1// created: 2014/04/29
2// filename: UavStateMachine.h
3//
4// author: Gildas Bayard, Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: state machine for UAV
10//
11//
12/*********************************************************************/
13
14#ifndef UAVSTATEMACHINE_H
15#define UAVSTATEMACHINE_H
16
17#include <Thread.h>
18#include <Vector2D.h>
19#include <Vector3D.h>
20#include <Euler.h>
21#include <Quaternion.h>
22
23namespace flair {
24 namespace core {
25 class FrameworkManager;
26 class AhrsData;
27 class io_data;
28 }
29 namespace gui {
30 class PushButton;
31 class GridLayout;
32 class Tab;
33 class DoubleSpinBox;
34 }
35 namespace filter {
36 class ControlLaw;
37 class NestedSat;
38 class Pid;
39 class PidThrust;
40 class TrajectoryGenerator1D;
41 }
42 namespace sensor {
43 class TargetController;
44 }
45 namespace meta {
46 class MetaDualShock3;
47 class Uav;
48 }
49}
50
51namespace flair { namespace meta {
52
53 /*! \class UavStateMachine
54*
55* \brief State machine for UAV
56*
57* thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
58*/
59
60class UavStateMachine : public core::Thread
61{
62 protected:
63 enum class AltitudeMode_t {
64 Manual,
65 Custom
66 };
67 const AltitudeMode_t &GetAltitudeMode(void) const {
68 return altitudeMode;
69 }
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 {
78 Manual,
79 Custom
80 };
81 const OrientationMode_t &GetOrientationMode(void) const {
82 return orientationMode;
83 }
84 bool SetOrientationMode(const OrientationMode_t &orientationMode);
85
86 enum class ThrustMode_t {
87 Default,
88 Custom
89 };
90 const ThrustMode_t &GetThrustMode() const {
91 return thrustMode;
92 }
93 bool SetThrustMode(const ThrustMode_t &thrustMode);
94
95 enum class TorqueMode_t {
96 Default,
97 Custom
98 };
99 const TorqueMode_t &GetTorqueMode(void) const {
100 return torqueMode;
101 }
102 bool SetTorqueMode(const TorqueMode_t &torqueMode);
103
104 enum class Event_t {
105 EnteringFailSafeMode,
106 EnteringControlLoop,
107 StartLanding,
108 FinishLanding,
109 Stopped,
110 TakingOff,
111 EmergencyStop,
112 Stabilized, //as soon as uav is 3cm far from the ground
113 ZTrajectoryFinished,
114 };
115
116 UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
117 ~UavStateMachine();
118
119 const core::Quaternion &GetCurrentQuaternion(void) const;
120
121 const core::Vector3D &GetCurrentAngularSpeed(void) const;
122
123 const meta::Uav *GetUav(void) const;
124
125 void Land(void);
126 void TakeOff(void);
127 void EmergencyStop(void);
128 //! Used to signal an event
129 /*!
130 \param event the event which occured
131 */
132 virtual void SignalEvent(Event_t event);
133
134 virtual const core::AhrsData *GetOrientation(void) const;
135 const core::AhrsData *GetDefaultOrientation(void) const;
136
137 virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
138 void EnterFailSafeMode(void);
139 bool ExitFailSafeMode(void);
140 void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
141
142 gui::GridLayout *GetButtonsLayout(void) const;
143 virtual void ExtraSecurityCheck(void){};
144 virtual void ExtraCheckJoystick(void){};
145 virtual void ExtraCheckPushButton(void){};
146
147 void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
148 virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
149 //float GetDefaultThrustOffset(void);
150 const core::AhrsData *GetDefaultReferenceOrientation(void) const;
151 virtual const core::AhrsData *GetReferenceOrientation(void);
152
153 /*!
154 * \brief Compute Custom Torques
155 *
156 * Reimplement this method to use TorqueMode_t::Custom. \n
157 * This method is called internally by UavStateMachine, do not call it by yourself. \n
158 * See GetTorques if you need torques values.
159 *
160 * \param torques custom torques
161 */
162 virtual void ComputeCustomTorques(core::Euler &torques);
163
164 /*!
165 * \brief Compute Default Torques
166 *
167 * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
168 * Torques are only computed once by loop, other calls to this method will use previously computed torques.
169 *
170 * \param torques default torques
171 */
172 void ComputeDefaultTorques(core::Euler &torques);
173
174 /*!
175 * \brief Get Torques
176 *
177 * \return torques current torques
178 */
179 //const core::Euler &GetTorques() const;
180
181 /*!
182 * \brief Compute Custom Thrust
183 *
184 * Reimplement this method to use ThrustMode_t::Custom. \n
185 * This method is called internally by UavStateMachine, do not call it by yourself. \n
186 * See GetThrust if you need thrust value.
187 *
188 * \return custom Thrust
189 */
190 virtual float ComputeCustomThrust(void);
191
192 /*!
193 * \brief Compute Default Thrust
194 *
195 * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
196 * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
197 *
198 * \return default thrust
199 */
200 float ComputeDefaultThrust(void);
201
202 /*!
203 * \brief Get Thrust
204 *
205 * \return current thrust
206 */
207 //float GetThrust() const;
208
209 /*!
210 * \brief Add an IODevice to the control law logs
211 *
212 * The IODevice will be automatically logged among the Uz logs,
213 * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
214 * and FrameworkManager::AddDeviceToLog). \n
215 *
216 * \param device IODevice to log
217 */
218 void AddDeviceToControlLawLog(const core::IODevice* device);
219
220 /*!
221 * \brief Add an io_data to the control law logs
222 *
223 * The io_data will be automatically logged among the Uz logs,
224 * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
225 * and FrameworkManager::AddDeviceToLog). \n
226 *
227 * \param data io_data to log
228 */
229 void AddDataToControlLawLog(const core::io_data* data);
230
231 const sensor::TargetController *GetJoystick(void) const;
232
233 gui::Tab *setupLawTab,*graphLawTab;
234
235 private:
236 /*!
237 \enum AltitudeState_t
238 \brief States of the altitude state machine
239 */
240 enum class AltitudeState_t {
241 Stopped, /*!< the uav motors are stopped */
242 TakingOff, /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
243 Stabilized, /*!< the uav is actively maintaining its altitude */
244 StartLanding, /*!< landing initiated. Altitude is required to reach the 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
252 float groundAltitude; // effective altitude when the uav leaves the ground
253 float currentAltitude,currentVerticalSpeed;
254
255 bool failSafeMode;
256 void SecurityCheck(void);
257 void MandatorySecurityCheck(void);
258 void CheckJoystick();
259 void GenericCheckJoystick();
260 void CheckPushButton(void);
261 void GenericCheckPushButton(void);
262 void Run(void);
263 void StopMotors(void);
264
265 meta::Uav *uav;
266
267 core::Quaternion currentQuaternion;
268 core::Vector3D currentAngularSpeed;
269
270 const core::AhrsData *ComputeReferenceOrientation(void);
271
272 void ComputeOrientation(void);
273 void ComputeAltitude(void);
274
275 void ComputeTorques(void);
276 core::Euler currentTorques,savedDefaultTorques;
277 bool needToComputeDefaultTorques;
278
279 void ComputeThrust(void);
280 float currentThrust,savedDefaultThrust;
281 bool needToComputeDefaultThrust;
282
283 gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
284 gui::GridLayout *buttonslayout;
285 gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
286 AltitudeMode_t altitudeMode;
287 OrientationMode_t orientationMode;
288 ThrustMode_t thrustMode;
289 TorqueMode_t torqueMode;
290 bool flagBatteryLow;
291 bool flagConnectionLost;
292 bool flagZTrajectoryFinished;
293 filter::NestedSat *uRoll,*uPitch;
294 filter::Pid *uYaw;
295 filter::PidThrust *uZ;
296
297 MetaDualShock3 *joy;
298 filter::TrajectoryGenerator1D *altitudeTrajectory;
299};
300
301};
302};
303#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.