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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 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.