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

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

add AltitudeSensor class
failsafe altitude sensor in changeable

File size: 9.2 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 class AltitudeSensor;
49 }
50 namespace meta {
51 class MetaDualShock3;
52 class Uav;
53 }
54}
55
56namespace flair {
57namespace meta {
58
59/*! \class UavStateMachine
60*
61* \brief State machine for UAV
62* The Thread is created with
63* the FrameworkManager as parent. FrameworkManager must be created before.
64* The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or
65* SetPeriodMS
66*/
67
68class UavStateMachine : public core::Thread {
69protected:
70 enum class AltitudeMode_t { Manual, Custom };
71 const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
72 bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
73
74 // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
75 // available in mode AltitudeMode_t::Manual
76 // return true if goto is possible
77 bool GotoAltitude(float desiredAltitude);
78
79 enum class OrientationMode_t { Manual, Custom };
80 const OrientationMode_t &GetOrientationMode(void) const {
81 return orientationMode;
82 }
83 bool SetOrientationMode(const OrientationMode_t &orientationMode);
84
85 enum class ThrustMode_t { Default, Custom };
86 const ThrustMode_t &GetThrustMode() const { return thrustMode; }
87 bool SetThrustMode(const ThrustMode_t &thrustMode);
88
89 enum class TorqueMode_t { Default, Custom };
90 const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
91 bool SetTorqueMode(const TorqueMode_t &torqueMode);
92
93 enum class Event_t {
94 EnteringFailSafeMode,
95 EnteringControlLoop,
96 StartLanding,
97 FinishLanding,
98 Stopped,
99 TakingOff,
100 EmergencyStop,
101 Stabilized, // as soon as uav is 3cm far from the ground
102 ZTrajectoryFinished,
103 };
104
105 UavStateMachine(sensor::TargetController* controller);
106 ~UavStateMachine();
107
108 const core::Quaternion &GetCurrentQuaternion(void) const;
109
110 const core::Vector3Df &GetCurrentAngularSpeed(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 /*!
131 * \brief Set the FailSafe AltitudeSensor
132 *
133 * by default, the failsafe AltitudeSensor is the MetaUsRangeFinder
134 * you can change it (by a vrpn one) using this method in the case us range finder is not working well
135 * use it carefully, trusting only vrpn can be risquee
136 *
137 * \param altitudeSensor altitudeSensor to use when is failsafe
138 */
139 void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);
140
141 gui::GridLayout *GetButtonsLayout(void) const;
142 virtual void ExtraSecurityCheck(void){};
143 virtual void ExtraCheckJoystick(void){};
144 virtual void ExtraCheckPushButton(void){};
145
146 void GetDefaultReferenceAltitude(float &refAltitude,
147 float &refVerticalVelocity);
148 virtual void GetReferenceAltitude(float &refAltitude,
149 float &refVerticalVelocity);
150 // float GetDefaultThrustOffset(void);
151 const core::AhrsData *GetDefaultReferenceOrientation(void) const;
152 virtual const core::AhrsData *GetReferenceOrientation(void);
153
154 /*!
155 * \brief Compute Custom Torques
156 *
157 * Reimplement this method to use TorqueMode_t::Custom. \n
158 * This method is called internally by UavStateMachine, do not call it by
159 *yourself. \n
160 * See GetTorques if you need torques values.
161 *
162 * \param torques custom torques
163 */
164 virtual void ComputeCustomTorques(core::Euler &torques);
165
166 /*!
167 * \brief Compute Default Torques
168 *
169 * This method is called internally by UavStateMachine when using
170 *TorqueMode_t::Default. \n
171 * Torques are only computed once by loop, other calls to this method will use
172 *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
190 *yourself. \n
191 * See GetThrust if you need thrust value.
192 *
193 * \return custom Thrust
194 */
195 virtual float ComputeCustomThrust(void);
196
197 /*!
198 * \brief Compute Default Thrust
199 *
200 * This method is called internally by UavStateMachine when using
201 *ThrustMode_t::Default. \n
202 * Thrust is only computed once by loop, other calls to this method will use
203 *previously computed thrust.
204 *
205 * \return default thrust
206 */
207 float ComputeDefaultThrust(void);
208
209 /*!
210 * \brief Get Thrust
211 *
212 * \return current thrust
213 */
214 // float GetThrust() const;
215
216 /*!
217 * \brief Add an IODevice to the control law logs
218 *
219 * The IODevice 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 device IODevice to log
225 */
226 void AddDeviceToControlLawLog(const core::IODevice *device);
227
228 /*!
229 * \brief Add an io_data to the control law logs
230 *
231 * The io_data will be automatically logged among the Uz logs,
232 * if logging is enabled (see IODevice::SetDataToLog,
233 *FrameworkManager::StartLog
234 * and FrameworkManager::AddDeviceToLog). \n
235 *
236 * \param data io_data to log
237 */
238 void AddDataToControlLawLog(const core::io_data *data);
239
240 const sensor::TargetController *GetTargetController(void) const;
241 MetaDualShock3 *GetJoystick(void) const;
242
243 filter::NestedSat *GetURoll(void);
244 filter::NestedSat *GetUPitch(void);
245 filter::Pid *GetUYaw(void);
246 filter::PidThrust *GetUZ(void);
247 filter::TrajectoryGenerator1D *GetAltitudeTrajectory(void);
248
249 gui::Tab *setupLawTab, *graphLawTab;
250
251private:
252 /*!
253 \enum AltitudeState_t
254 \brief States of the altitude state machine
255 */
256 enum class AltitudeState_t {
257 Stopped, /*!< the uav motors are stopped */
258 TakingOff, /*!< take off initiated. Motors accelerate progressively until
259 the UAV lift up */
260 Stabilized, /*!< the uav is actively maintaining its altitude */
261 StartLanding, /*!< landing initiated. Altitude is required to reach the
262 landing altitude (0 by default) */
263 FinishLanding /*!< motors are gradually stopped */
264 };
265 AltitudeState_t altitudeState;
266 void ProcessAltitudeFiniteStateMachine();
267 void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
268
269 float groundAltitude; // effective altitude when the uav leaves the ground
270 float currentAltitude, currentVerticalSpeed;
271
272 bool failSafeMode;
273 void SecurityCheck(void);
274 void MandatorySecurityCheck(void);
275 void CheckJoystick();
276 void GenericCheckJoystick();
277 void CheckPushButton(void);
278 void GenericCheckPushButton(void);
279 void Run(void);
280 void StopMotors(void);
281 bool IsValuePossible(float value,std::string desc);
282
283 Uav *uav;
284 MetaDualShock3 *joy;
285 sensor::TargetController *controller;
286
287 core::Quaternion currentQuaternion;
288 core::Vector3Df currentAngularSpeed;
289
290 const core::AhrsData *ComputeReferenceOrientation(void);
291
292 void ComputeOrientation(void);
293 void ComputeAltitude(void);
294
295 void ComputeTorques(void);
296 core::Euler currentTorques, savedDefaultTorques;
297 bool needToComputeDefaultTorques;
298
299 void ComputeThrust(void);
300 float currentThrust, savedDefaultThrust;
301 bool needToComputeDefaultThrust;
302
303 gui::PushButton *button_kill, *button_take_off, *button_land,
304 *button_start_log, *button_stop_log;
305 gui::GridLayout *buttonslayout;
306 gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
307 AltitudeMode_t altitudeMode;
308 OrientationMode_t orientationMode;
309 ThrustMode_t thrustMode;
310 TorqueMode_t torqueMode;
311 bool flagBatteryLow;
312 bool flagConnectionLost;
313 bool flagCriticalSensorLost;
314 bool flagZTrajectoryFinished;
315 bool safeToFly;
316 filter::NestedSat *uRoll, *uPitch;
317 filter::Pid *uYaw;
318 filter::PidThrust *uZ;
319 filter::TrajectoryGenerator1D *altitudeTrajectory;
320 sensor::AltitudeSensor *failSafeAltitudeSensor;
321};
322}; // end namespace meta
323}; // end namespace flair
324#endif // UAVSTATEMACHINE_H
Note: See TracBrowser for help on using the repository browser.