Flair
Framework Libre Air
flair::meta::UavStateMachine Class Reference

State machine for UAV The Thread is created with the FrameworkManager as parent. FrameworkManager must be created before. The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or SetPeriodMS. More...

#include <UavStateMachine.h>

+ Inheritance diagram for flair::meta::UavStateMachine:

Protected Types

enum  AltitudeMode_t { Manual, Custom }
 
enum  OrientationMode_t { Manual, Custom }
 
enum  ThrustMode_t { Default, Custom }
 
enum  TorqueMode_t { Default, Custom }
 
enum  Event_t {
  EnteringFailSafeMode, EnteringControlLoop, StartLanding, FinishLanding,
  Stopped, TakingOff, EmergencyStop, Stabilized,
  ZTrajectoryFinished
}
 

Protected Member Functions

const AltitudeMode_t & GetAltitudeMode (void) const
 
bool SetAltitudeMode (const AltitudeMode_t &altitudeMode)
 
bool GotoAltitude (float desiredAltitude)
 
const OrientationMode_t & GetOrientationMode (void) const
 
bool SetOrientationMode (const OrientationMode_t &orientationMode)
 
const ThrustMode_t & GetThrustMode () const
 
bool SetThrustMode (const ThrustMode_t &thrustMode)
 
const TorqueMode_t & GetTorqueMode (void) const
 
bool SetTorqueMode (const TorqueMode_t &torqueMode)
 
 UavStateMachine (sensor::TargetController *controller)
 
const core::QuaternionGetCurrentQuaternion (void) const
 
const core::Vector3DfGetCurrentAngularSpeed (void) const
 
void Land (void)
 
void EmergencyLand (void)
 
void TakeOff (void)
 
void EmergencyStop (void)
 
virtual void SignalEvent (Event_t event)
 Used to signal an event. More...
 
virtual const core::AhrsDataGetOrientation (void) const
 
const core::AhrsDataGetDefaultOrientation (void) const
 
virtual void AltitudeValues (float &z, float &dz) const
 
void EnterFailSafeMode (void)
 
bool ExitFailSafeMode (void)
 
void FailSafeAltitudeValues (float &z, float &dz) const
 
gui::GridLayoutGetButtonsLayout (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)
 
const core::AhrsDataGetDefaultReferenceOrientation (void) const
 
virtual const core::AhrsDataGetReferenceOrientation (void)
 
virtual void ComputeCustomTorques (core::Euler &torques)
 Compute Custom Torques. More...
 
void ComputeDefaultTorques (core::Euler &torques)
 Compute Default Torques. More...
 
virtual float ComputeCustomThrust (void)
 Get Torques. More...
 
float ComputeDefaultThrust (void)
 Compute Default Thrust. More...
 
void AddDeviceToControlLawLog (const core::IODevice *device)
 Get Thrust. More...
 
void AddDataToControlLawLog (const core::io_data *data)
 Add an io_data to the control law logs. More...
 
const sensor::TargetControllerGetJoystick (void) const
 
filter::NestedSatGetURoll (void)
 
filter::NestedSatGetUPitch (void)
 
filter::PidGetUYaw (void)
 
filter::PidThrustGetUZ (void)
 
filter::TrajectoryGenerator1DGetAltitudeTrajectory (void)
 

Protected Attributes

MetaDualShock3joy
 
gui::TabsetupLawTab
 
gui::TabgraphLawTab
 

Additional Inherited Members

- Public Types inherited from flair::core::Object
enum  color_t { Auto = 0, Red = 31, Green = 32, Orange = 33 }
 
- Public Member Functions inherited from flair::core::Thread
 Thread (const Object *parent, std::string name, uint8_t priority, uint32_t stackSize=1024 *128)
 Constructor. More...
 
virtual ~Thread ()
 Destructor. More...
 
void Start (void)
 Start the thread. More...
 
void SafeStop (void)
 Set a stop flag. More...
 
bool ToBeStopped (void) const
 Set a stop flag. More...
 
void Join (void)
 Join the thread. More...
 
void SetPeriodUS (uint32_t period_us)
 Set the period in micro second. More...
 
uint32_t GetPeriodUS () const
 
void SetPeriodMS (uint32_t period_ms)
 Set the period in milli second. More...
 
uint32_t GetPeriodMS () const
 
bool IsPeriodSet (void)
 Returns if period was set. More...
 
void WaitPeriod (void) const
 Wait the period. More...
 
int WaitUpdate (const IODevice *device)
 Wait update of an IODevice. More...
 
void Suspend (void)
 Suspend the thread. More...
 
bool SuspendUntil (Time date)
 Suspend the thread with timeout. More...
 
void Resume (void)
 Resume the thread. More...
 
bool IsSuspended (void) const
 Is the thread suspended? More...
 
bool IsRunning (void) const
 Is the thread running? More...
 
void SleepUntil (Time time) const
 Sleep until absolute time. More...
 
void SleepUS (uint32_t time_us) const
 Sleep for a certain time in micro second. More...
 
void SleepMS (uint32_t time_ms) const
 Sleep for a cartain time in milli second. More...
 
- Public Member Functions inherited from flair::core::Object
 Object (const Object *parent=NULL, std::string name="", std::string type="")
 Constructor. More...
 
virtual ~Object ()
 Destructor. More...
 
std::string ObjectName (void) const
 Name. More...
 
std::string ObjectType (void) const
 Type. More...
 
const ObjectParent (void) const
 Parent. More...
 
std::vector< const Object * > * TypeChilds (void) const
 Childs of the same type. More...
 
std::vector< const Object * > * Childs (void) const
 Childs. More...
 
void Information (const char *function, int line, const char *format,...) const
 Formatted information. More...
 
void Warning (const char *function, const char *format,...) const
 Formatted warning. More...
 
void Error (const char *function, const char *format,...) const
 Formatted error. More...
 
bool ErrorOccured (bool recursive=true) const
 Has an errror occured? More...
 
- Static Public Member Functions inherited from flair::core::Thread
static void WarnUponSwitches (bool enable)
 Warn if real time / non real time switches occur. More...
 

Detailed Description

State machine for UAV The Thread is created with the FrameworkManager as parent. FrameworkManager must be created before. The Thread is synchronized with Ahrs, unless a period is set with SetPeriodUS or SetPeriodMS.

Member Function Documentation

virtual void flair::meta::UavStateMachine::SignalEvent ( Event_t  event)
protectedvirtual

Used to signal an event.

Parameters
eventthe event which occured
virtual void flair::meta::UavStateMachine::ComputeCustomTorques ( core::Euler torques)
protectedvirtual

Compute Custom Torques.

Reimplement this method to use TorqueMode_t::Custom.
This method is called internally by UavStateMachine, do not call it by yourself.
See GetTorques if you need torques values.

Parameters
torquescustom torques
void flair::meta::UavStateMachine::ComputeDefaultTorques ( core::Euler torques)
protected

Compute Default Torques.

This method is called internally by UavStateMachine when using TorqueMode_t::Default.
Torques are only computed once by loop, other calls to this method will use previously computed torques.

Parameters
torquesdefault torques
virtual float flair::meta::UavStateMachine::ComputeCustomThrust ( void  )
protectedvirtual

Get Torques.

Returns
torques current torques

Compute Custom Thrust

Reimplement this method to use ThrustMode_t::Custom.
This method is called internally by UavStateMachine, do not call it by yourself.
See GetThrust if you need thrust value.

Returns
custom Thrust
float flair::meta::UavStateMachine::ComputeDefaultThrust ( void  )
protected

Compute Default Thrust.

This method is called internally by UavStateMachine when using ThrustMode_t::Default.
Thrust is only computed once by loop, other calls to this method will use previously computed thrust.

Returns
default thrust
void flair::meta::UavStateMachine::AddDeviceToControlLawLog ( const core::IODevice device)
protected

Get Thrust.

Returns
current thrust

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).

Parameters
deviceIODevice to log
void flair::meta::UavStateMachine::AddDataToControlLawLog ( const core::io_data data)
protected

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).

Parameters
dataio_data to log