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

Public Member Functions

 Task (const std::string &name, Tasks::Context &ctx)
 
void onUpdateParameters (void)
 
void onEntityResolution (void)
 
void onResourceAcquisition (void)
 
void onResourceRelease (void)
 
void onRequestActivation (void)
 
void onRequestDeactivation (void)
 
void consume (const IMC::LeaderState *msg)
 
void consume (const IMC::Formation *msg)
 
void consume (const IMC::PlanControl *msg)
 
void consume (const IMC::IndicatedSpeed *msg)
 
void consume (const IMC::EstimatedStreamVelocity *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 dispatchAlias (IMC::Message *msg)
 
void dispatchLeader (IMC::Message *msg)
 
bool isControlActive (void)
 
void setLeaderState (const IMC::LeaderState *leader_state)
 
void leaderOutput (void)
 
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_vehicle_accel, const unsigned int &ind_uav, const double &d_time_step, Matrix *vd_cmd, const bool b_debug, FormMonitor *form_monitor)
 
void dispatchFormationParameters (void)
 
void formationEvaluation (void)
 
void printState (const std::string what, const Matrix pos, const Matrix vel)
 
- 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
 
unsigned int resolveEntity (const std::string &label) const
 
std::string resolveEntity (unsigned int id) const
 
DebugLevel getDebugLevel (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
 
const char * getEntityLabel (void) const
 
void setEntityLabel (const std::string &label)
 
- 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)
 
unsigned getPriority (void)
 
State getState (void)
 
bool isCreated (void)
 
bool isStopping (void)
 
bool isRunning (void)
 
bool isStarting (void)
 
bool isDead (void)
 

Public Attributes

Arguments m_args
 
bool m_param_update_first
 
std::vector< UAVSimulation * > m_models
 
UAVSimulationm_model
 
double m_llh_ref_pos [3]
 
Matrix m_position
 
Matrix m_velocity
 
IMC::EstimatedState m_estate_leader
 
Matrix m_last_state_update
 
Matrix m_last_state_estim
 
Matrix m_last_simctrl_update
 
double m_last_leader_output
 
double m_last_time_verb_task
 
double m_last_time_verb_ctrlactiv
 
double m_last_time_verb_leadertrace
 
double m_last_time_verb_leaderspew
 
double m_last_time_verb_formctrl
 
double m_timestep_trace
 
double m_timestep_spew
 
Matrix m_vehicle_state
 
Matrix m_vehicle_accel
 
double m_airspeed
 
Matrix m_wind
 
Math::MovingAverage< fp64_t > * m_wind_avg_x
 
Math::MovingAverage< fp64_t > * m_wind_avg_y
 
Math::MovingAverage< fp64_t > * m_wind_avg_z
 
double m_g
 
Matrix m_uav_ctrl
 
IMC::DesiredRoll m_bank_cmd
 
IMC::DesiredSpeed m_airspeed_cmd
 
IMC::DesiredZ m_altitude_cmd
 
double m_speed_cmd_leader
 
uint8_t m_speed_units_leader
 
double m_alt_cmd_leader
 
uint8_t m_alt_units_leader
 
double m_bank_lim
 
double m_speed_min
 
double m_speed_max
 
double m_alt_min
 
double m_alt_max
 
double m_leader_bank_lim
 
double m_leader_speed_min
 
double m_leader_speed_max
 
double m_leader_alt_min
 
double m_leader_alt_max
 
IMC::FormationControlParams m_formation_ctrl_params
 
double m_k_longitudinal
 
double m_k_lateral
 
double m_k_boundary
 
double m_k_leader
 
double m_k_deconfliction
 
double m_flow_accel_max
 
double m_safe_dist
 
double m_deconfliction_offset
 
double m_acc_safety_marg
 
double m_accel_lim_x
 
IMC::FormationEvaluation m_formation_eval
 
double m_dist_min_abs
 
double m_dist_min_mean
 
double m_err_mean
 
double m_mean_time
 
double m_mean_time_start
 
bool m_mean_first
 
unsigned int m_uav_n
 
unsigned int m_uav_ind
 
Systems m_uav_id
 
Systems m_uav_id_last
 
std::vector< std::string > m_formation_systems
 
unsigned int m_formation_frame
 
Matrix m_formation_pos
 
IMC::PlanControl m_current_plan
 
bool m_ctrl_active
 
bool m_team_plan_init
 
bool m_team_leader_init
 
bool m_team_state_init
 
bool m_valid_airspeed
 
std::vector< bool > m_vehicle_state_flag
 
double m_frequency
 
double m_timestep_sim
 
double m_timestep_ctrl
 
double m_timestep_leader
 
IMC::LeaderState m_init_leader
 
bool m_debug
 
FormMonitorm_form_monitor
 
std::map< uint32_t, Systemsm_filtered_sys
 
std::map< uint32_t, Entitiesm_filtered_ent
 
uint32_t m_alias_id
 
uint32_t m_leader_id
 

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
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)
 
template<typename E >
E * reserveEntity (const std::string &label)
 
bool stopping (void)
 
bool isActive (void) const
 
bool isActivating (void) const
 
bool isDeactivating (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 , typename M >
void bind (T *task_obj, const std::vector< uint32_t > &list, void(T::*consumer)(const M *)=&T::consume)
 
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 bool onWriteParamsXML (std::ostream &os) const
 
virtual void onEntityReservation (void)
 
virtual void onReportEntityState (void)
 
virtual void onResourceInitialization (void)
 
virtual void onActivation (void)
 
virtual void onDeactivation (void)
 
virtual void onQueryEntityParameters (const IMC::QueryEntityParameters *msg)
 
virtual void onSetEntityParameters (const IMC::SetEntityParameters *msg)
 
virtual void onPushEntityParameters (const IMC::PushEntityParameters *msg)
 
virtual void onPopEntityParameters (const IMC::PopEntityParameters *msg)
 
- Protected Member Functions inherited from DUNE::Concurrency::Thread
void startImpl (void)
 
void stopImpl (void)
 
void joinImpl (void)
 
void setPriorityImpl (Scheduler::Policy policy, unsigned priority)
 
unsigned getPriorityImpl (void)
 
- Protected Attributes inherited from DUNE::Tasks::Task
Contextm_ctx
 
std::vector
< Entities::BasicEntity * > 
m_entities
 

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::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::cmd_src, Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug, Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_speed, DUNE::Tasks::Parameter::defaultValue(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, 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_accel_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_bank_rate, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_vert_slope, Maneuver::VehicleFormation::FormCollAvoid::Arguments::leader_alias, Maneuver::VehicleFormation::FormCollAvoid::Arguments::leader_frequency, m_args, DUNE::Math::min(), DUNE::Tasks::Task::param(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, Maneuver::VehicleFormation::FormCollAvoid::Arguments::speed_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::speed_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::src_alias, and Maneuver::VehicleFormation::FormCollAvoid::Arguments::wind_average_window.

Member Function Documentation

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

References Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ax_cmd, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ax_des, Maneuver::VehicleFormation::FormCollAvoid::RelState::ctrl_imp, dispatchAlias(), Maneuver::VehicleFormation::FormCollAvoid::RelState::dist, Maneuver::VehicleFormation::FormCollAvoid::RelState::err, Maneuver::VehicleFormation::FormCollAvoid::RelState::err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::err_y, Maneuver::VehicleFormation::FormCollAvoid::RelState::err_z, formationControl(), DUNE::Math::Matrix::get(), DUNE::Tasks::Task::getSystemId(), DUNE::Tasks::Task::isActive(), isControlActive(), m_airspeed_cmd, m_alias_id, m_alt_units_leader, m_altitude_cmd, m_bank_cmd, m_debug, m_dist_min_abs, m_dist_min_mean, m_err_mean, m_form_monitor, m_formation_pos, m_last_simctrl_update, m_last_state_estim, m_last_state_update, m_llh_ref_pos, m_mean_first, m_mean_time, m_mean_time_start, m_models, m_timestep_ctrl, m_uav_ctrl, m_uav_id, m_uav_ind, m_uav_n, m_vehicle_accel, m_vehicle_state, m_vehicle_state_flag, m_wind, DUNE::Math::Matrix::norm_2(), positionReframing(), Maneuver::VehicleFormation::FormCollAvoid::RelState::rel_dir_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::rel_dir_y, Maneuver::VehicleFormation::FormCollAvoid::RelState::rel_dir_z, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::rel_state, DUNE::Tasks::Task::resolveSystemId(), Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_vx, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_vy, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_vz, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_y, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_z, Maneuver::VehicleFormation::FormCollAvoid::RelState::s_id, DUNE::Math::Matrix::set(), DUNE::Tasks::Task::spew(), Maneuver::VehicleFormation::FormCollAvoid::RelState::ss_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ss_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::ss_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ss_y, Maneuver::VehicleFormation::FormCollAvoid::RelState::ss_z, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_fdbk_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_fdbk_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_unkn_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_unkn_y, teamPeriodicUpdate(), teamUnevenUpdate(), Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::virt_err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::virt_err_y, Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_z, and DUNE::Tasks::Task::war().

void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchAlias ( IMC::Message *  msg)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchFormationParameters ( void  )
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchLeader ( IMC::Message *  msg)
inline
void Maneuver::VehicleFormation::FormCollAvoid::Task::formationControl ( const Matrix md_uav_state,
const Matrix md_vehicle_accel,
const unsigned int &  ind_uav,
const double &  d_time_step,
Matrix vd_cmd,
const bool  b_debug,
FormMonitor form_monitor 
)
inline

References Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ax_cmd, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ax_des, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ay_cmd, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ay_des, DUNE::Math::Matrix::column(), Maneuver::VehicleFormation::FormCollAvoid::RelState::dist, Maneuver::VehicleFormation::FormCollAvoid::RelState::err, Maneuver::VehicleFormation::FormCollAvoid::RelState::err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::err_y, DUNE::Math::Matrix::get(), m_acc_safety_marg, m_accel_lim_x, m_alt_max, m_alt_min, m_bank_lim, m_deconfliction_offset, m_flow_accel_max, m_formation_frame, m_formation_pos, m_formation_systems, m_g, m_k_boundary, m_k_deconfliction, m_k_lateral, m_k_leader, m_k_longitudinal, m_safe_dist, m_speed_cmd_leader, m_speed_max, m_speed_min, m_uav_n, m_wind, DUNE::Math::Matrix::norm_2(), Maneuver::VehicleFormation::FormCollAvoid::RelState::rel_dir_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::rel_dir_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::rel_state, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_vx, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_vy, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::rf_err_y, DUNE::Math::Matrix::row(), Maneuver::VehicleFormation::FormCollAvoid::RelState::s_id, DUNE::Math::Matrix::set(), Maneuver::VehicleFormation::FormCollAvoid::RelState::ss_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ss_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::ss_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::ss_y, DUNE::Math::sum(), Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_fdbk_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_fdbk_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_unkn_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::surf_unkn_y, DUNE::Math::transpose(), DUNE::Math::trimValue(), Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_x, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::virt_err_x, Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_y, Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::virt_err_y, and DUNE::Tasks::Task::war().

Referenced by consume(), and teamPeriodicUpdate().

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

Called when the task is instructed to resolve all the entity identifiers it needs for normal execution.

See resolveEntity(). Derived classes that need to resolve entity identifiers should override this function.

Reimplemented from DUNE::Tasks::Task.

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::cmd_src, DUNE::Tasks::Task::debug(), m_args, m_current_plan, m_filtered_ent, m_filtered_sys, DUNE::Tasks::Task::resolveEntity(), DUNE::Tasks::Task::resolveSystemName(), DUNE::Tasks::Task::spew(), and DUNE::Tasks::Task::war().

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

Called when an external activation request is received.

Derived classes that need to perform extra steps to prepare normal execution should replace the default behaviour of immediate activation with calls to activate() when the request is completed or activationFailed() if the request cannot be honoured.

Reimplemented from DUNE::Tasks::Task.

References dispatchFormationParameters(), dispatchLeader(), DUNE::Tasks::Task::inf(), isControlActive(), m_current_plan, m_debug, m_team_leader_init, and DUNE::Tasks::Task::spew().

Referenced by setLeaderState().

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

Called when an external deactivation request is received.

Derived classes that need to perform extra steps to prepare normal execution should replace the default behaviour of immediate deactivation with calls to deactivate() when the request is completed or deactivationFailed() if the request cannot be honoured.

Reimplemented from DUNE::Tasks::Task.

References formationEvaluation(), isControlActive(), m_debug, m_mean_first, m_team_leader_init, and DUNE::Tasks::Task::spew().

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 DUNE::Tasks::Task::setEntityState().

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

Called when the task is instructed to release resources.

Derived classes that override this function must not assume that any resource was previously acquired. This function must be implemented in such a way that it can be called at any time.

Reimplemented from DUNE::Tasks::Task.

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 Maneuver::VehicleFormation::FormCollAvoid::Arguments::acc_safety_marg, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::alt_min, Maneuver::VehicleFormation::FormCollAvoid::Arguments::bank_lim, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_bank, Maneuver::VehicleFormation::FormCollAvoid::Arguments::c_speed, DUNE::Simulation::UAVSimulation::command(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::ctrl_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug, DUNE::Tasks::Task::debug(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_speed, dispatchFormationParameters(), DUNE::Tasks::Task::err(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, formationEvaluation(), DUNE::Math::Matrix::get(), DUNE::Tasks::Periodic::getFrequency(), DUNE::Tasks::Task::getSystemId(), DUNE::Tasks::Task::inf(), 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_accel_x, Maneuver::VehicleFormation::FormCollAvoid::Arguments::l_bank_rate, Maneuver::VehicleFormation::FormCollAvoid::Arguments::leader_alias, Maneuver::VehicleFormation::FormCollAvoid::Arguments::leader_frequency, m_acc_safety_marg, m_accel_lim_x, m_airspeed, m_airspeed_cmd, m_alias_id, m_alt_cmd_leader, m_alt_max, m_alt_min, m_altitude_cmd, m_args, m_bank_lim, m_debug, m_deconfliction_offset, m_flow_accel_max, m_form_monitor, m_formation_pos, m_frequency, m_k_boundary, m_k_deconfliction, m_k_lateral, m_k_leader, m_k_longitudinal, m_last_simctrl_update, m_last_state_estim, m_last_state_update, m_leader_alt_max, m_leader_alt_min, m_leader_bank_lim, m_leader_id, m_leader_speed_max, m_leader_speed_min, m_llh_ref_pos, m_mean_first, m_model, m_models, m_param_update_first, m_position, m_safe_dist, DUNE::Simulation::UAVSimulation::m_sim_type, m_speed_cmd_leader, m_speed_max, m_speed_min, m_team_state_init, m_timestep_ctrl, m_timestep_leader, m_timestep_sim, m_uav_ctrl, m_uav_id, m_uav_id_last, m_uav_ind, m_uav_n, m_vehicle_accel, m_vehicle_state, m_vehicle_state_flag, m_velocity, m_wind_avg_x, m_wind_avg_y, m_wind_avg_z, DUNE::Math::Matrix, DUNE::Tasks::Task::paramChanged(), DUNE::Math::Angles::radians(), Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::rel_state, DUNE::Math::Matrix::resizeAndKeep(), DUNE::Tasks::Task::resolveSystemId(), DUNE::Tasks::Task::resolveSystemName(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::safe_dist, DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::setAccelLim(), DUNE::Simulation::UAVSimulation::setBankRateLim(), DUNE::Simulation::UAVSimulation::setCtrl(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_frequency, Maneuver::VehicleFormation::FormCollAvoid::Arguments::sim_type, Maneuver::VehicleFormation::FormCollAvoid::Arguments::speed_max, Maneuver::VehicleFormation::FormCollAvoid::Arguments::speed_min, DUNE::Tasks::Task::spew(), Maneuver::VehicleFormation::FormCollAvoid::Arguments::src_alias, DUNE::Simulation::UAVSimulation, DUNE::Math::Matrix::vertCat(), DUNE::Tasks::Task::war(), and Maneuver::VehicleFormation::FormCollAvoid::Arguments::wind_average_window.

Referenced by consume().

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(), and setLeaderState().

void Maneuver::VehicleFormation::FormCollAvoid::Task::printState ( const std::string  what,
const Matrix  pos,
const Matrix  vel 
)
inline

Member Data Documentation

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_acc_safety_marg
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_accel_lim_x
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed

Referenced by onUpdateParameters().

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

Referenced by consume(), and onUpdateParameters().

uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_alias_id
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_cmd_leader

Leader commanded altitude.

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_max
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_min
uint8_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_units_leader

Referenced by consume().

IMC::DesiredZ Maneuver::VehicleFormation::FormCollAvoid::Task::m_altitude_cmd

Referenced by consume(), and onUpdateParameters().

Arguments Maneuver::VehicleFormation::FormCollAvoid::Task::m_args

Task arguments.

Referenced by onEntityResolution(), onUpdateParameters(), and Task().

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

Vehicle commands.

Referenced by consume().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_bank_lim

Vehicle command limits.

Referenced by formationControl(), and onUpdateParameters().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_ctrl_active

Process logic control variables.

Referenced by isControlActive().

IMC::PlanControl Maneuver::VehicleFormation::FormCollAvoid::Task::m_current_plan
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_debug
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_deconfliction_offset
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_dist_min_abs

Referenced by consume(), and formationEvaluation().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_dist_min_mean

Referenced by consume(), and formationEvaluation().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_err_mean

Referenced by consume(), and formationEvaluation().

IMC::EstimatedState Maneuver::VehicleFormation::FormCollAvoid::Task::m_estate_leader

Leader's estimated state.

Referenced by leaderOutput(), and setLeaderState().

std::map<uint32_t, Entities> Maneuver::VehicleFormation::FormCollAvoid::Task::m_filtered_ent

List of entities allowed to define a command.

Referenced by consume(), and onEntityResolution().

std::map<uint32_t, Systems> Maneuver::VehicleFormation::FormCollAvoid::Task::m_filtered_sys

List of systems allowed to define a command.

Referenced by consume(), and onEntityResolution().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_flow_accel_max
FormMonitor* Maneuver::VehicleFormation::FormCollAvoid::Task::m_form_monitor
IMC::FormationControlParams Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_ctrl_params

Controller parameters.

Referenced by dispatchFormationParameters().

IMC::FormationEvaluation Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_eval

Controller evaluation data.

Referenced by formationEvaluation().

unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_frame

Referenced by consume(), and formationControl().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_pos
std::vector<std::string> Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_systems

Referenced by consume(), and formationControl().

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

Simulation process frequency.

Referenced by onUpdateParameters().

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

Leader initial state.

Referenced by setLeaderState().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_boundary
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_deconfliction
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_lateral
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_leader
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_longitudinal
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_leader_output

Referenced by leaderOutput(), and teamPeriodicUpdate().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_simctrl_update
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_estim
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_update

Synchronization, simulation and control time variables.

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_ctrlactiv

Referenced by isControlActive().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_formctrl
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_leaderspew
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_leadertrace
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_task

Last time debug information was shown.

Referenced by task().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_alt_max

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_alt_min

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_bank_lim

Leader command limits.

Referenced by consume(), onUpdateParameters(), and setLeaderState().

uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_id
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_speed_max

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_speed_min

Referenced by consume(), and onUpdateParameters().

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

Vehicle's referential position (Latitude, Longitude, and height).

Referenced by consume(), onUpdateParameters(), and setLeaderState().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_first
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_time

Referenced by consume(), and formationEvaluation().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_time_start

Referenced by consume().

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

Simulated vehicles' models.

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

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_param_update_first

Referenced by onUpdateParameters().

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

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

Referenced by leaderOutput(), onUpdateParameters(), setLeaderState(), and teamPeriodicUpdate().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_safe_dist
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_cmd_leader

Leader commanded true airspeed.

Referenced by consume(), formationControl(), and onUpdateParameters().

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_max
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_min
uint8_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_units_leader

Referenced by consume().

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_leader_init
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_plan_init
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_state_init
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_ctrl
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_leader
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_sim
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_spew
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_trace

Referenced by isControlActive(), and task().

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

System command variables.

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

Systems Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_id

Referenced by consume(), and onUpdateParameters().

Systems Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_id_last

Referenced by onUpdateParameters().

unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ind
unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_n
bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_valid_airspeed

Referenced by consume(), and isControlActive().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_accel
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_state
std::vector<bool> Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_state_flag
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_velocity

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

Referenced by leaderOutput(), onUpdateParameters(), setLeaderState(), and teamPeriodicUpdate().

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind

Environment variables.

Referenced by consume(), formationControl(), and leaderOutput().

Math::MovingAverage<fp64_t>* Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind_avg_x

Referenced by consume(), and onUpdateParameters().

Math::MovingAverage<fp64_t>* Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind_avg_y

Referenced by consume(), and onUpdateParameters().

Math::MovingAverage<fp64_t>* Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind_avg_z

Referenced by consume(), and onUpdateParameters().

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