source: flair-dev/trunk/include/FlairMeta/UavStateMachine.h @ 7

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

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.