DUNE: Uniform Navigational Environment  2.5.1
Maneuver::VehicleFormation::FormCollAvoid::Task Struct Reference

Public Member Functions

 Task (const std::string &name, Tasks::Context &ctx)
 
void onUpdateParameters (void)
 
void onResourceAcquisition (void)
 
void consume (const IMC::LeaderState *msg)
 
void consume (const IMC::IndicatedSpeed *msg)
 
void consume (const IMC::EstimatedStreamVelocity *msg)
 
void consume (const IMC::SimulatedState *msg)
 
void consume (const IMC::Acceleration *msg)
 
void consume (const IMC::DesiredRoll *msg)
 
void consume (const IMC::DesiredSpeed *msg)
 
void consume (const IMC::DesiredZ *msg)
 
void consume (const IMC::EstimatedState *msg)
 
void task (void)
 
void checkParameters (void)
 
void setLeaderState (const IMC::LeaderState *msg)
 
Matrix matrixRotRbody2gnd (float roll, float pitch)
 
template<typename Ta , typename Tb , typename Tc , typename Td , typename Te >
void positionReframing (const Ta lat1, const Ta lon1, const Tb hei1, const Tc lat2, const Tc lon2, const Td hei2, Te *n, Te *e, Te *d)
 
void teamPeriodicUpdate (const double &d_time)
 
void teamUnevenUpdate (const double &d_time)
 
void formationControl (const Matrix &md_uav_state, const Matrix &md_uav_accel, const int &ind_uav, const double &d_time_step, Matrix *vd_cmd, const bool b_debug)
 
- Public Member Functions inherited from DUNE::Tasks::Periodic
 Periodic (const std::string &name, Context &ctx)
 
virtual ~Periodic (void)
 
void setFrequency (double freq)
 
double getFrequency (void) const
 
double getRunTime (void) const
 
unsigned getRunCount (void) const
 
- Public Member Functions inherited from DUNE::Tasks::Task
 Task (const std::string &name, Context &context)
 
virtual ~Task (void)
 
const char * getName (void) const
 
const char * getSystemName (void) const
 
unsigned int getSystemId (void) const
 
unsigned int getEntityId (void) const
 
uint16_t getActivationTime (void) const
 
uint16_t getDeactivationTime (void) const
 
unsigned int resolveSystemName (const std::string &name) const
 
const char * resolveSystemId (unsigned int id) const
 
void loadConfig (void)
 
void setPriority (unsigned int value)
 
unsigned int getPriority (void) const
 
void inf (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void war (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void err (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void cri (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void debug (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void trace (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void void spew (const char *format,...) DUNE_PRINTF_FORMAT(2
 
void void void void void void
void void 
dispatch (IMC::Message *msg, unsigned int flags=0)
 
void dispatch (IMC::Message &msg, unsigned int flags=0)
 
void dispatchReply (const IMC::Message &original, IMC::Message &msg, unsigned int flags=0)
 
void receive (const IMC::Message *msg)
 
void reserveEntities (void)
 
void resolveEntities (void)
 
void acquireResources (void)
 
void releaseResources (void)
 
void initializeResources (void)
 
void updateParameters (bool act_deact=true)
 
void writeParamsXML (std::ostream &os) const
 
- Public Member Functions inherited from DUNE::Tasks::AbstractTask
 AbstractTask (void)
 
virtual ~AbstractTask (void)
 
- Public Member Functions inherited from DUNE::Concurrency::Thread
 Thread (void)
 
virtual ~Thread (void)
 
int getProcessorUsage (void)
 
- Public Member Functions inherited from DUNE::Concurrency::Runnable
 Runnable (void)
 
virtual ~Runnable (void)
 
void start (void)
 
void stop (void)
 
void join (void)
 
void stopAndJoin (void)
 
void setPriority (Scheduler::Policy policy, unsigned priority)
 
State getState (void)
 
bool isCreated (void)
 
bool isStopping (void)
 
bool isRunning (void)
 
bool isStarting (void)
 
bool isDead (void)
 

Public Attributes

Arguments m_args
 
std::vector< UAVSimulation * > m_models
 
UAVSimulationm_model
 
double m_llh_ref_pos [3]
 
Matrix m_position
 
Matrix m_velocity
 
IMC::SimulatedState m_sstate
 
double m_clock_diff
 
double m_last_leader_update
 
double m_last_ctrl_update
 
Matrix m_last_state_estim
 
Matrix m_last_simctrl_update
 
double m_last_time_debug
 
double m_last_time_trace
 
double m_last_time_spew
 
Matrix m_uav_state
 
Matrix m_uav_accel
 
double m_airspeed
 
Matrix m_wind
 
Matrix m_uav_ctrl
 
IMC::DesiredRoll m_bank_cmd
 
IMC::DesiredSpeed m_airspeed_cmd
 
int m_uav_n
 
bool m_team_state_init
 
bool m_valid_airspeed
 
double m_frequency
 
IMC::LeaderState m_init_leader
 
bool m_init_leader_set
 
bool m_debug
 

Additional Inherited Members

- Public Types inherited from DUNE::Concurrency::Runnable
enum  State {
  StateStarting, StateRunning, StateStopping, StateDead,
  StateUnknown
}
 
- Protected Member Functions inherited from DUNE::Tasks::Task
const char * getEntityLabel (void) const
 
void setEntityLabel (const std::string &label)
 
void setEntityState (IMC::EntityState::StateEnum state, Status::Code code)
 
void setEntityState (IMC::EntityState::StateEnum state, const std::string &description)
 
IMC::EntityState::StateEnum getEntityState (void) const
 
unsigned int reserveEntity (const std::string &label)
 
unsigned int resolveEntity (const std::string &label) const
 
std::string resolveEntity (unsigned int id) const
 
bool stopping (void)
 
bool isActive (void) const
 
void waitForMessages (double timeout)
 
void consumeMessages (void)
 
template<typename T >
Parameterparam (const std::string &name, T &var)
 
template<typename Y , typename T >
Parameterparam (const std::string &name, T &var)
 
template<typename T >
bool paramChanged (T &var)
 
void paramActive (Parameter::Scope def_scope, Parameter::Visibility def_visibility, bool def_value=false)
 
void setParamSectionEditor (const std::string &name)
 
template<typename M , typename T >
void bind (T *task_obj, void(T::*consumer)(const M *)=&T::consume)
 
template<typename T >
void bind (T *task_obj, const std::vector< uint32_t > &list)
 
template<typename T >
void bind (T *task_obj, const std::vector< std::string > &list)
 
void requestActivation (void)
 
void requestDeactivation (void)
 
void activate (void)
 
void activationFailed (const std::string &reason)
 
void deactivate (void)
 
void deactivationFailed (const std::string &reason)
 
virtual void onEntityReservation (void)
 
virtual void onEntityResolution (void)
 
virtual void onReportEntityState (void)
 
virtual void onResourceRelease (void)
 
virtual void onResourceInitialization (void)
 
virtual void onRequestActivation (void)
 
virtual void onRequestDeactivation (void)
 
virtual void onActivation (void)
 
virtual void onDeactivation (void)
 
- Protected Member Functions inherited from DUNE::Concurrency::Thread
void startImpl (void)
 
void stopImpl (void)
 
void joinImpl (void)
 
void setPriorityImpl (Scheduler::Policy policy, unsigned priority)
 
- Protected Attributes inherited from DUNE::Tasks::Task
Contextm_ctx
 

Constructor & Destructor Documentation

Maneuver::VehicleFormation::FormCollAvoid::Task::Task ( const std::string &  name,
Tasks::Context &  ctx 
)
inline

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::acc_safety_marg, Maneuver::VehicleFormation::FormCollAvoid::Arguments::accel_lim_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::bank_lim, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_bank, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug, Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, DUNE::Tasks::Parameter::defaultValue(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_frame, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, Maneuver::VehicleFormation::FormCollAvoid::Arguments::gaccel, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lat, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lon, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_roll, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_yaw, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_boundary, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_deconfliction, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_lateral, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_leader, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_longitudinal, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_bank_rate, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_lon_accel, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_vert_slope, m_args, DUNE::Math::min(), DUNE::Tasks::Task::param(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::plan, DUNE::Math::Angles::radians(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, and Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_n.

Member Function Documentation

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::LeaderState *  msg)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::IndicatedSpeed *  msg)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::EstimatedStreamVelocity *  msg)
inline

References m_wind.

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::Acceleration *  msg)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::formationControl ( const Matrix md_uav_state,
const Matrix md_uav_accel,
const int &  ind_uav,
const double &  d_time_step,
Matrix vd_cmd,
const bool  b_debug 
)
inline

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::acc_safety_marg, Maneuver::VehicleFormation::FormCollAvoid::Arguments::accel_lim_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::bank_lim, DUNE::Math::Matrix::column(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_frame, Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, Maneuver::VehicleFormation::FormCollAvoid::Arguments::gaccel, DUNE::Math::Matrix::get(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_boundary, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_deconfliction, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_lateral, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_leader, Maneuver::VehicleFormation::FormCollAvoid::Arguments::k_longitudinal, m_args, m_last_time_spew, m_uav_n, m_wind, DUNE::Math::Matrix::norm_2(), DUNE::Math::Matrix::row(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, DUNE::Math::Matrix::set(), DUNE::Tasks::Task::spew(), DUNE::Math::sum(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::tas_min, DUNE::Math::transpose(), DUNE::Math::trimValue(), and Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind.

Referenced by consume(), and teamPeriodicUpdate().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::matrixRotRbody2gnd ( float  roll,
float  pitch 
)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::onResourceAcquisition ( void  )
inlinevirtual

Called when the task is instructed to acquire resources whose configuration is defined by run-time parameters.

Reimplemented from DUNE::Tasks::Task.

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_bank, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_speed, checkParameters(), DUNE::Simulation::UAVSimulation::command(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug, DUNE::Tasks::Task::debug(), DUNE::Math::Angles::degrees(), DUNE::Tasks::Task::dispatch(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::formation_pos, DUNE::Math::Matrix::get(), DUNE::Tasks::Periodic::getFrequency(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lat, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_lon, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_roll, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::init_yaw, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_bank_rate, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_lon_accel, m_airspeed, m_airspeed_cmd, m_args, m_debug, m_frequency, m_init_leader, m_last_ctrl_update, m_last_leader_update, m_last_simctrl_update, m_last_state_estim, m_llh_ref_pos, m_model, m_models, m_position, DUNE::Simulation::UAVSimulation::m_sim_type, m_uav_ctrl, m_uav_n, m_uav_state, m_velocity, m_wind, DUNE::Math::Matrix, Maneuver::VehicleFormation::FormCollAvoid::Arguments::plan, DUNE::Math::Angles::radians(), DUNE::Simulation::UAVSimulation::setAccelLim(), DUNE::Simulation::UAVSimulation::setBankRateLim(), setLeaderState(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, DUNE::Tasks::Task::spew(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::uav_ind, DUNE::Simulation::UAVSimulation, and DUNE::Math::Matrix::vertCat().

void Maneuver::VehicleFormation::FormCollAvoid::Task::onUpdateParameters ( void  )
inlinevirtual

Called when the task is instructed to update its run-time parameters.

Derived classes that need to compute auxiliary values based on run-time parameters should override this function.

Reimplemented from DUNE::Tasks::Task.

References checkParameters().

template<typename Ta , typename Tb , typename Tc , typename Td , typename Te >
void Maneuver::VehicleFormation::FormCollAvoid::Task::positionReframing ( const Ta  lat1,
const Ta  lon1,
const Tb  hei1,
const Tc  lat2,
const Tc  lon2,
const Td  hei2,
Te *  n,
Te *  e,
Te *  d 
)
inline

Adjust offset position from a reference frame to a new reference frame.

Parameters
[in]lat1new reference latitude (rad).
[in]lon1new reference longitude (rad).
[in]hei1new reference height (m).
[in]lat2old reference latitude (rad).
[in]lon2old reference longitude (rad).
[in]hei2old reference height (m).
[in,out]nNorth offset (m).
[in,out]eEast offset (m).
[in,out]dDown offset (m).

Referenced by consume().

void Maneuver::VehicleFormation::FormCollAvoid::Task::teamUnevenUpdate ( const double &  d_time)
inline

Member Data Documentation

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed

Referenced by consume(), and onResourceAcquisition().

IMC::DesiredSpeed Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed_cmd

Referenced by consume(), and onResourceAcquisition().

Arguments Maneuver::VehicleFormation::FormCollAvoid::Task::m_args
IMC::DesiredRoll Maneuver::VehicleFormation::FormCollAvoid::Task::m_bank_cmd

Vehicle commands.

Referenced by consume().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_clock_diff

Simulation and control time variables.

Referenced by consume(), and task().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_debug

Referenced by consume(), and onResourceAcquisition().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_frequency

Simulation process frequency.

Referenced by onResourceAcquisition(), and teamPeriodicUpdate().

IMC::LeaderState Maneuver::VehicleFormation::FormCollAvoid::Task::m_init_leader

Leader initial state.

Referenced by onResourceAcquisition(), and task().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_init_leader_set
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_ctrl_update

Referenced by consume(), and onResourceAcquisition().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_leader_update
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_simctrl_update
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_estim
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_debug

Last time debug information was shown.

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_spew

Referenced by formationControl(), and task().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_trace
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_llh_ref_pos[3]

Vehicls's referencial position (Latitude, Longitude, and height).

Referenced by consume(), and onResourceAcquisition().

UAVSimulation* Maneuver::VehicleFormation::FormCollAvoid::Task::m_model
std::vector<UAVSimulation*> Maneuver::VehicleFormation::FormCollAvoid::Task::m_models

Simulated vehicles' models.

Referenced by consume(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_position

Leader's simulated position (X,Y,Z,phi,theta,psi).

Referenced by consume(), onResourceAcquisition(), setLeaderState(), and task().

IMC::SimulatedState Maneuver::VehicleFormation::FormCollAvoid::Task::m_sstate

Leader's simulated state.

Referenced by setLeaderState(), and task().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_state_init

Process logic control variables.

Referenced by consume().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_accel

Referenced by consume().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ctrl

System command variables.

Referenced by consume(), onResourceAcquisition(), and teamPeriodicUpdate().

int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_n
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_state

System state variables.

Referenced by consume(), onResourceAcquisition(), teamPeriodicUpdate(), and teamUnevenUpdate().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_valid_airspeed

Referenced by consume().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_velocity

Leader's simulated velocity (u,v,w,p,q,r).

Referenced by onResourceAcquisition(), setLeaderState(), and task().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind
Collaboration diagram for Maneuver::VehicleFormation::FormCollAvoid::Task:
Collaboration graph