Changeset 13 in flair-dev for trunk/include/FlairMeta/UavStateMachine.h

Timestamp:

04/08/16 15:39:24 (8 years ago)

Author:

Bayard Gildas

Message:

formatting script + include reformatted

File:

• trunk/include/FlairMeta/UavStateMachine.h (modified) (1 diff)

trunk/include/FlairMeta/UavStateMachine.h

@@ -26 +26 @@
 
 namespace flair {
-    namespace core {
-        class FrameworkManager;
-        class AhrsData;
-        class io_data;
-    }
-    namespace gui {
-        class PushButton;
-        class GridLayout;
-        class Tab;
-        class DoubleSpinBox;
-    }
-    namespace filter {
-        class ControlLaw;
-        class NestedSat;
-        class Pid;
-        class PidThrust;
-        class TrajectoryGenerator1D;
-    }
-    namespace sensor {
-        class TargetController;
-    }
-    namespace meta {
-        class MetaDualShock3;
-        class Uav;
-    }
-}
-
-namespace flair { namespace meta {
-
- /*! \class UavStateMachine
+namespace core {
+class FrameworkManager;
+class AhrsData;
+class io_data;
+}
+namespace gui {
+class PushButton;
+class GridLayout;
+class Tab;
+class DoubleSpinBox;
+}
+namespace filter {
+class ControlLaw;
+class NestedSat;
+class Pid;
+class PidThrust;
+class TrajectoryGenerator1D;
+}
+namespace sensor {
+class TargetController;
+}
+namespace meta {
+class MetaDualShock3;
+class Uav;
+}
+}
+
+namespace flair {
+namespace meta {
+
+/*! \class UavStateMachine
 *
 * \brief State machine for UAV
 *
-* thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
+* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
+*SetPeriodMS
 */
 
-class UavStateMachine : public core::Thread
-{
-    protected:
-        enum class AltitudeMode_t {
-            Manual,
-            Custom
-        };
-        const AltitudeMode_t &GetAltitudeMode(void) const {
-            return altitudeMode;
-        }
-        bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
-
-        //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
-        //available in mode AltitudeMode_t::Manual
-        //return true if goto is possible
-        bool GotoAltitude(float desiredAltitude);
-
-        enum class OrientationMode_t {
-            Manual,
-            Custom
-        };
-        const OrientationMode_t &GetOrientationMode(void) const {
-            return orientationMode;
-        }
-        bool SetOrientationMode(const OrientationMode_t &orientationMode);
-
-        enum class ThrustMode_t {
-            Default,
-            Custom
-        };
-        const ThrustMode_t &GetThrustMode() const {
-            return thrustMode;
-        }
-        bool SetThrustMode(const ThrustMode_t &thrustMode);
-
-        enum class TorqueMode_t {
-            Default,
-            Custom
-        };
-        const TorqueMode_t &GetTorqueMode(void) const {
-            return torqueMode;
-        }
-        bool SetTorqueMode(const TorqueMode_t &torqueMode);
-
-        enum class Event_t {
-            EnteringFailSafeMode,
-            EnteringControlLoop,
-            StartLanding,
-            FinishLanding,
-            Stopped,
-            TakingOff,
-            EmergencyStop,
-            Stabilized, //as soon as uav is 3cm far from the ground
-            ZTrajectoryFinished,
-        };
-
-        UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
-        ~UavStateMachine();
-
-        const core::Quaternion &GetCurrentQuaternion(void) const;
-
-        const core::Vector3D &GetCurrentAngularSpeed(void) const;
-
-        const meta::Uav *GetUav(void) const;
-
-        void Land(void);
-        void TakeOff(void);
-        void EmergencyStop(void);
-        //! Used to signal an event
-        /*!
-            \param event the event which occured
-        */
-        virtual void SignalEvent(Event_t event);
-
-        virtual const core::AhrsData *GetOrientation(void) const;
-        const core::AhrsData *GetDefaultOrientation(void) const;
-
-        virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
-        void EnterFailSafeMode(void);
-        bool ExitFailSafeMode(void);
-        void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
-
-        gui::GridLayout *GetButtonsLayout(void) const;
-        virtual void ExtraSecurityCheck(void){};
-        virtual void ExtraCheckJoystick(void){};
-        virtual void ExtraCheckPushButton(void){};
-
-        void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-        virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-        //float GetDefaultThrustOffset(void);
-        const core::AhrsData *GetDefaultReferenceOrientation(void) const;
-        virtual const core::AhrsData *GetReferenceOrientation(void);
-
-        /*!
-        * \brief Compute Custom Torques
-        *
-        * Reimplement this method to use TorqueMode_t::Custom. \n
-        * This method is called internally by UavStateMachine, do not call it by yourself. \n
-        * See GetTorques if you need torques values.
-        *
-        * \param torques custom torques
-        */
-        virtual void ComputeCustomTorques(core::Euler &torques);
-
-        /*!
-        * \brief Compute Default Torques
-        *
-        * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
-        * Torques are only computed once by loop, other calls to this method will use previously computed torques.
-        *
-        * \param torques default torques
-        */
-        void ComputeDefaultTorques(core::Euler &torques);
-
-        /*!
-        * \brief Get Torques
-        *
-        * \return torques current torques
-        */
-        //const core::Euler &GetTorques() const;
-
-        /*!
-        * \brief Compute Custom Thrust
-        *
-        * Reimplement this method to use ThrustMode_t::Custom. \n
-        * This method is called internally by UavStateMachine, do not call it by yourself. \n
-        * See GetThrust if you need thrust value.
-        *
-        * \return custom Thrust
-        */
-        virtual float ComputeCustomThrust(void);
-
-        /*!
-        * \brief Compute Default Thrust
-        *
-        * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
-        * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
-        *
-        * \return default thrust
-        */
-        float ComputeDefaultThrust(void);
-
-        /*!
-        * \brief Get Thrust
-        *
-        * \return current thrust
-        */
-        //float GetThrust() const;
-
-        /*!
-        * \brief Add an IODevice to the control law logs
-        *
-        * The IODevice will be automatically logged among the Uz logs,
-        * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
-        * and FrameworkManager::AddDeviceToLog). \n
-        *
-        * \param device IODevice to log
-        */
-        void AddDeviceToControlLawLog(const core::IODevice* device);
-
-        /*!
-        * \brief Add an io_data to the control law logs
-        *
-        * The io_data will be automatically logged among the Uz logs,
-        * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
-        * and FrameworkManager::AddDeviceToLog). \n
-        *
-        * \param data io_data to log
-        */
-        void AddDataToControlLawLog(const core::io_data* data);
-
-        const sensor::TargetController *GetJoystick(void) const;
-
-        gui::Tab *setupLawTab,*graphLawTab;
-
-    private:
-        /*!
-        \enum AltitudeState_t
-        \brief States of the altitude state machine
-        */
-        enum class AltitudeState_t {
-            Stopped,        /*!< the uav motors are stopped */
-            TakingOff,      /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
-            Stabilized,     /*!< the uav is actively maintaining its altitude */
-            StartLanding,   /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
-            FinishLanding   /*!< motors are gradually stopped */
-        };
-        AltitudeState_t altitudeState;
-        void ProcessAltitudeFiniteStateMachine();
-        void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
-
-
-        float groundAltitude; // effective altitude when the uav leaves the ground
-        float currentAltitude,currentVerticalSpeed;
-
-        bool failSafeMode;
-        void SecurityCheck(void);
-        void MandatorySecurityCheck(void);
-        void CheckJoystick();
-        void GenericCheckJoystick();
-        void CheckPushButton(void);
-        void GenericCheckPushButton(void);
-        void Run(void);
-        void StopMotors(void);
-
-        meta::Uav *uav;
-
-        core::Quaternion currentQuaternion;
-        core::Vector3D currentAngularSpeed;
-
-        const core::AhrsData *ComputeReferenceOrientation(void);
-
-        void ComputeOrientation(void);
-        void ComputeAltitude(void);
-
-        void ComputeTorques(void);
-        core::Euler currentTorques,savedDefaultTorques;
-        bool needToComputeDefaultTorques;
-
-        void ComputeThrust(void);
-        float currentThrust,savedDefaultThrust;
-        bool needToComputeDefaultThrust;
-
-        gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
-        gui::GridLayout *buttonslayout;
-        gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
-        AltitudeMode_t altitudeMode;
-        OrientationMode_t orientationMode;
-        ThrustMode_t thrustMode;
-        TorqueMode_t torqueMode;
-        bool flagBatteryLow;
-        bool flagConnectionLost;
-        bool flagZTrajectoryFinished;
-        filter::NestedSat *uRoll,*uPitch;
-        filter::Pid *uYaw;
-        filter::PidThrust *uZ;
-
-        MetaDualShock3 *joy;
-        filter::TrajectoryGenerator1D *altitudeTrajectory;
+class UavStateMachine : public core::Thread {
+protected:
+  enum class AltitudeMode_t { Manual, Custom };
+  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
+  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
+
+  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
+  // available in mode AltitudeMode_t::Manual
+  // return true if goto is possible
+  bool GotoAltitude(float desiredAltitude);
+
+  enum class OrientationMode_t { Manual, Custom };
+  const OrientationMode_t &GetOrientationMode(void) const {
+    return orientationMode;
+  }
+  bool SetOrientationMode(const OrientationMode_t &orientationMode);
+
+  enum class ThrustMode_t { Default, Custom };
+  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
+  bool SetThrustMode(const ThrustMode_t &thrustMode);
+
+  enum class TorqueMode_t { Default, Custom };
+  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
+  bool SetTorqueMode(const TorqueMode_t &torqueMode);
+
+  enum class Event_t {
+    EnteringFailSafeMode,
+    EnteringControlLoop,
+    StartLanding,
+    FinishLanding,
+    Stopped,
+    TakingOff,
+    EmergencyStop,
+    Stabilized, // as soon as uav is 3cm far from the ground
+    ZTrajectoryFinished,
+  };
+
+  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
+  ~UavStateMachine();
+
+  const core::Quaternion &GetCurrentQuaternion(void) const;
+
+  const core::Vector3D &GetCurrentAngularSpeed(void) const;
+
+  const meta::Uav *GetUav(void) const;
+
+  void Land(void);
+  void TakeOff(void);
+  void EmergencyStop(void);
+  //! Used to signal an event
+  /*!
+      \param event the event which occured
+  */
+  virtual void SignalEvent(Event_t event);
+
+  virtual const core::AhrsData *GetOrientation(void) const;
+  const core::AhrsData *GetDefaultOrientation(void) const;
+
+  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
+  void EnterFailSafeMode(void);
+  bool ExitFailSafeMode(void);
+  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
+
+  gui::GridLayout *GetButtonsLayout(void) const;
+  virtual void ExtraSecurityCheck(void){};
+  virtual void ExtraCheckJoystick(void){};
+  virtual void ExtraCheckPushButton(void){};
+
+  void GetDefaultReferenceAltitude(float &refAltitude,
+                                   float &refVerticalVelocity);
+  virtual void GetReferenceAltitude(float &refAltitude,
+                                    float &refVerticalVelocity);
+  // float GetDefaultThrustOffset(void);
+  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
+  virtual const core::AhrsData *GetReferenceOrientation(void);
+
+  /*!
+  * \brief Compute Custom Torques
+  *
+  * Reimplement this method to use TorqueMode_t::Custom. \n
+  * This method is called internally by UavStateMachine, do not call it by
+  *yourself. \n
+  * See GetTorques if you need torques values.
+  *
+  * \param torques custom torques
+  */
+  virtual void ComputeCustomTorques(core::Euler &torques);
+
+  /*!
+  * \brief Compute Default Torques
+  *
+  * This method is called internally by UavStateMachine when using
+  *TorqueMode_t::Default. \n
+  * Torques are only computed once by loop, other calls to this method will use
+  *previously computed torques.
+  *
+  * \param torques default torques
+  */
+  void ComputeDefaultTorques(core::Euler &torques);
+
+  /*!
+  * \brief Get Torques
+  *
+  * \return torques current torques
+  */
+  // const core::Euler &GetTorques() const;
+
+  /*!
+  * \brief Compute Custom Thrust
+  *
+  * Reimplement this method to use ThrustMode_t::Custom. \n
+  * This method is called internally by UavStateMachine, do not call it by
+  *yourself. \n
+  * See GetThrust if you need thrust value.
+  *
+  * \return custom Thrust
+  */
+  virtual float ComputeCustomThrust(void);
+
+  /*!
+  * \brief Compute Default Thrust
+  *
+  * This method is called internally by UavStateMachine when using
+  *ThrustMode_t::Default. \n
+  * Thrust is only computed once by loop, other calls to this method will use
+  *previously computed thrust.
+  *
+  * \return default thrust
+  */
+  float ComputeDefaultThrust(void);
+
+  /*!
+  * \brief Get Thrust
+  *
+  * \return current thrust
+  */
+  // float GetThrust() const;
+
+  /*!
+  * \brief Add an IODevice to the control law logs
+  *
+  * The IODevice will be automatically logged among the Uz logs,
+  * if logging is enabled (see IODevice::SetDataToLog,
+  *FrameworkManager::StartLog
+  * and FrameworkManager::AddDeviceToLog). \n
+  *
+  * \param device IODevice to log
+  */
+  void AddDeviceToControlLawLog(const core::IODevice *device);
+
+  /*!
+  * \brief Add an io_data to the control law logs
+  *
+  * The io_data will be automatically logged among the Uz logs,
+  * if logging is enabled (see IODevice::SetDataToLog,
+  *FrameworkManager::StartLog
+  * and FrameworkManager::AddDeviceToLog). \n
+  *
+  * \param data io_data to log
+  */
+  void AddDataToControlLawLog(const core::io_data *data);
+
+  const sensor::TargetController *GetJoystick(void) const;
+
+  gui::Tab *setupLawTab, *graphLawTab;
+
+private:
+  /*!
+  \enum AltitudeState_t
+  \brief States of the altitude state machine
+  */
+  enum class AltitudeState_t {
+    Stopped,      /*!< the uav motors are stopped */
+    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
+                     the UAV lift up */
+    Stabilized,   /*!< the uav is actively maintaining its altitude */
+    StartLanding, /*!< landing initiated. Altitude is required to reach the
+                     landing altitude (0 by default) */
+    FinishLanding /*!< motors are gradually stopped */
+  };
+  AltitudeState_t altitudeState;
+  void ProcessAltitudeFiniteStateMachine();
+  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
+
+  float groundAltitude; // effective altitude when the uav leaves the ground
+  float currentAltitude, currentVerticalSpeed;
+
+  bool failSafeMode;
+  void SecurityCheck(void);
+  void MandatorySecurityCheck(void);
+  void CheckJoystick();
+  void GenericCheckJoystick();
+  void CheckPushButton(void);
+  void GenericCheckPushButton(void);
+  void Run(void);
+  void StopMotors(void);
+
+  meta::Uav *uav;
+
+  core::Quaternion currentQuaternion;
+  core::Vector3D currentAngularSpeed;
+
+  const core::AhrsData *ComputeReferenceOrientation(void);
+
+  void ComputeOrientation(void);
+  void ComputeAltitude(void);
+
+  void ComputeTorques(void);
+  core::Euler currentTorques, savedDefaultTorques;
+  bool needToComputeDefaultTorques;
+
+  void ComputeThrust(void);
+  float currentThrust, savedDefaultThrust;
+  bool needToComputeDefaultThrust;
+
+  gui::PushButton *button_kill, *button_take_off, *button_land,
+      *button_start_log, *button_stop_log;
+  gui::GridLayout *buttonslayout;
+  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
+  AltitudeMode_t altitudeMode;
+  OrientationMode_t orientationMode;
+  ThrustMode_t thrustMode;
+  TorqueMode_t torqueMode;
+  bool flagBatteryLow;
+  bool flagConnectionLost;
+  bool flagZTrajectoryFinished;
+  filter::NestedSat *uRoll, *uPitch;
+  filter::Pid *uYaw;
+  filter::PidThrust *uZ;
+
+  MetaDualShock3 *joy;
+  filter::TrajectoryGenerator1D *altitudeTrajectory;
 };
-
 };
 };