DUNE: Uniform Navigational Environment  2022.04.1
List of all members | Public Member Functions | Public Attributes
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
 
UAVSimulation * m_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
 
FormMonitor * m_form_monitor
 
std::map< uint32_t, Systems > m_filtered_sys
 
std::map< uint32_t, Entities > m_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)
 
Entities::BasicEntity * getLocalEntity (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 >
Parameter & param (const std::string &name, T &var)
 
template<typename Y , typename T >
Parameter & param (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 bind (unsigned int message_id, AbstractConsumer *consumer)
 
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
Context & m_ctx
 
std::vector< Entities::BasicEntity * > m_entities
 

Constructor & Destructor Documentation

◆ Task()

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, 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, DUNE::Math::min(), 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

◆ consume() [1/10]

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

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug.

◆ consume() [2/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::Formation *  msg)
inline

References Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug, DUNE::Math::Matrix::resizeAndKeep(), and DUNE::Math::Matrix::~Matrix().

◆ consume() [3/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::PlanControl *  msg)
inline

◆ consume() [4/10]

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

◆ consume() [5/10]

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

◆ consume() [6/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::Acceleration *  msg)
inline

References DUNE::Math::Matrix::set().

◆ consume() [7/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::DesiredRoll *  msg)
inline

References DUNE::Simulation::UAVSimulation::commandBank(), DUNE::Math::Angles::degrees(), DUNE::Simulation::UAVSimulation::getBankCmd(), and DUNE::Math::trimValue().

◆ consume() [8/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::DesiredSpeed *  msg)
inline

References DUNE::Simulation::UAVSimulation::commandAirspeed(), and DUNE::Math::trimValue().

◆ consume() [9/10]

void Maneuver::VehicleFormation::FormCollAvoid::Task::consume ( const IMC::DesiredZ *  msg)
inline

References DUNE::Simulation::UAVSimulation::commandAlt(), and DUNE::Math::trimValue().

◆ consume() [10/10]

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, 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, DUNE::Math::Matrix::get(), DUNE::Math::Matrix::norm_2(), 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, 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(), 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, 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 Maneuver::VehicleFormation::FormCollAvoid::RelState::virt_err_z.

◆ dispatchAlias()

void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchAlias ( IMC::Message *  msg)
inline

◆ dispatchFormationParameters()

void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchFormationParameters ( void  )
inline

◆ dispatchLeader()

void Maneuver::VehicleFormation::FormCollAvoid::Task::dispatchLeader ( IMC::Message *  msg)
inline

◆ formationControl()

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(), 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, and Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::virt_err_y.

◆ formationEvaluation()

void Maneuver::VehicleFormation::FormCollAvoid::Task::formationEvaluation ( void  )
inline

◆ isControlActive()

bool Maneuver::VehicleFormation::FormCollAvoid::Task::isControlActive ( void  )
inline

◆ leaderOutput()

void Maneuver::VehicleFormation::FormCollAvoid::Task::leaderOutput ( void  )
inline

References DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getAirspeed(), DUNE::Simulation::UAVSimulation::m_wind, DUNE::Math::Matrix::norm_2(), DUNE::Math::Matrix::toDCM(), and DUNE::Math::transpose().

◆ matrixRotRbody2gnd()

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

◆ onEntityResolution()

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, and Maneuver::VehicleFormation::FormCollAvoid::Arguments::debug.

◆ onRequestActivation()

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.

◆ onRequestDeactivation()

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.

◆ onResourceAcquisition()

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.

◆ onResourceRelease()

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.

◆ onUpdateParameters()

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, Maneuver::VehicleFormation::FormCollAvoid::Arguments::deconfliction_offset, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_alt, Maneuver::VehicleFormation::FormCollAvoid::Arguments::default_speed, Maneuver::VehicleFormation::FormCollAvoid::Arguments::flow_accel_max, 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, 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, DUNE::Simulation::UAVSimulation::m_sim_type, DUNE::Math::Matrix, DUNE::Math::Angles::radians(), Maneuver::VehicleFormation::FormCollAvoid::FormMonitor::rel_state, DUNE::Math::Matrix::resizeAndKeep(), 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, Maneuver::VehicleFormation::FormCollAvoid::Arguments::src_alias, DUNE::Simulation::UAVSimulation, DUNE::Math::Matrix::vertCat(), and Maneuver::VehicleFormation::FormCollAvoid::Arguments::wind_average_window.

◆ positionReframing()

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

◆ printState()

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

◆ setLeaderState()

void Maneuver::VehicleFormation::FormCollAvoid::Task::setLeaderState ( const IMC::LeaderState *  leader_state)
inline

References DUNE::Math::Angles::degrees(), DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::m_wind, DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::setPosition(), DUNE::Simulation::UAVSimulation::setVelocity(), DUNE::Math::trimValue(), and DUNE::Math::Matrix::vertCat().

◆ task()

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

The task to be executed on each cycle.

Implements DUNE::Tasks::Periodic.

◆ teamPeriodicUpdate()

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

Update team simulated state for standard time periods.

References DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Simulation::UAVSimulation::getVelocity(), DUNE::Math::Matrix::set(), and DUNE::Simulation::UAVSimulation::update().

◆ teamUnevenUpdate()

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

Update team simulated state for uneven time periods.

References DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Simulation::UAVSimulation::getVelocity(), DUNE::Math::Matrix::set(), and DUNE::Simulation::UAVSimulation::update().

Member Data Documentation

◆ m_acc_safety_marg

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_acc_safety_marg

◆ m_accel_lim_x

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_accel_lim_x

◆ m_airspeed

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

◆ m_airspeed_cmd

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

◆ m_alias_id

uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_alias_id

◆ m_alt_cmd_leader

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_cmd_leader

Leader commanded altitude.

◆ m_alt_max

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_max

◆ m_alt_min

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_min

◆ m_alt_units_leader

uint8_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_units_leader

◆ m_altitude_cmd

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

◆ m_args

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

Task arguments.

◆ m_bank_cmd

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

Vehicle commands.

◆ m_bank_lim

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

Vehicle command limits.

◆ m_ctrl_active

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

Process logic control variables.

◆ m_current_plan

IMC::PlanControl Maneuver::VehicleFormation::FormCollAvoid::Task::m_current_plan

◆ m_debug

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

◆ m_deconfliction_offset

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_deconfliction_offset

◆ m_dist_min_abs

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_dist_min_abs

◆ m_dist_min_mean

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

◆ m_err_mean

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

◆ m_estate_leader

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

Leader's estimated state.

◆ m_filtered_ent

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

List of entities allowed to define a command.

◆ m_filtered_sys

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

List of systems allowed to define a command.

◆ m_flow_accel_max

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_flow_accel_max

◆ m_form_monitor

FormMonitor* Maneuver::VehicleFormation::FormCollAvoid::Task::m_form_monitor

◆ m_formation_ctrl_params

IMC::FormationControlParams Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_ctrl_params

Controller parameters.

◆ m_formation_eval

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

Controller evaluation data.

◆ m_formation_frame

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

◆ m_formation_pos

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_pos

◆ m_formation_systems

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

◆ m_frequency

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

Simulation process frequency.

◆ m_g

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_g

◆ m_init_leader

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

Leader initial state.

◆ m_k_boundary

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_boundary

◆ m_k_deconfliction

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_deconfliction

◆ m_k_lateral

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_lateral

◆ m_k_leader

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_leader

◆ m_k_longitudinal

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_k_longitudinal

◆ m_last_leader_output

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_leader_output

◆ m_last_simctrl_update

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_simctrl_update

◆ m_last_state_estim

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_estim

◆ m_last_state_update

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_state_update

Synchronization, simulation and control time variables.

◆ m_last_time_verb_ctrlactiv

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

◆ m_last_time_verb_formctrl

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_formctrl

◆ m_last_time_verb_leaderspew

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_leaderspew

◆ m_last_time_verb_leadertrace

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_leadertrace

◆ m_last_time_verb_task

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_task

Last time debug information was shown.

◆ m_leader_alt_max

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

◆ m_leader_alt_min

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

◆ m_leader_bank_lim

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

Leader command limits.

◆ m_leader_id

uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_id

◆ m_leader_speed_max

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_speed_max

◆ m_leader_speed_min

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

◆ m_llh_ref_pos

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

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

◆ m_mean_first

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_first

◆ m_mean_time

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_time

◆ m_mean_time_start

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

◆ m_model

UAVSimulation* Maneuver::VehicleFormation::FormCollAvoid::Task::m_model

Leader vehicle model.

◆ m_models

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

Simulated vehicles' models.

◆ m_param_update_first

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

◆ m_position

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

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

◆ m_safe_dist

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

◆ m_speed_cmd_leader

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

Leader commanded true airspeed.

◆ m_speed_max

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_max

◆ m_speed_min

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_min

◆ m_speed_units_leader

uint8_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_speed_units_leader

◆ m_team_leader_init

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_leader_init

◆ m_team_plan_init

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_team_plan_init

◆ m_team_state_init

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

◆ m_timestep_ctrl

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_ctrl

◆ m_timestep_leader

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_leader

◆ m_timestep_sim

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_sim

◆ m_timestep_spew

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_spew

◆ m_timestep_trace

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_timestep_trace

◆ m_uav_ctrl

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

System command variables.

◆ m_uav_id

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

◆ m_uav_id_last

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

◆ m_uav_ind

unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ind

◆ m_uav_n

unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_n

Formation configuration.

◆ m_valid_airspeed

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

◆ m_vehicle_accel

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_accel

◆ m_vehicle_state

Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_state

System state variables.

◆ m_vehicle_state_flag

std::vector<bool> Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_state_flag

◆ m_velocity

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

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

◆ m_wind

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

Environment variables.

◆ m_wind_avg_x

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

◆ m_wind_avg_y

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

◆ m_wind_avg_z

Math::MovingAverage<fp64_t>* Maneuver::VehicleFormation::FormCollAvoid::Task::m_wind_avg_z
Collaboration diagram for Maneuver::VehicleFormation::FormCollAvoid::Task:
Collaboration graph