DUNE: Uniform Navigational Environment  2019.02.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
- 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 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

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

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

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

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

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

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

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

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

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

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

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

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.

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

void Maneuver::VehicleFormation::FormCollAvoid::Task::formationEvaluation ( void  )
inline
bool Maneuver::VehicleFormation::FormCollAvoid::Task::isControlActive ( void  )
inline
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().

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

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.

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.

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.

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

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).
void Maneuver::VehicleFormation::FormCollAvoid::Task::printState ( const std::string  what,
const Matrix  pos,
const Matrix  vel 
)
inline
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().

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

The task to be executed on each cycle.

Implements DUNE::Tasks::Periodic.

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

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

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
IMC::DesiredSpeed Maneuver::VehicleFormation::FormCollAvoid::Task::m_airspeed_cmd
uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_alias_id
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_alt_cmd_leader

Leader commanded altitude.

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
IMC::DesiredZ Maneuver::VehicleFormation::FormCollAvoid::Task::m_altitude_cmd
Arguments Maneuver::VehicleFormation::FormCollAvoid::Task::m_args

Task arguments.

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

Vehicle commands.

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

Vehicle command limits.

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

Process logic control variables.

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
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_dist_min_mean
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_err_mean
IMC::EstimatedState Maneuver::VehicleFormation::FormCollAvoid::Task::m_estate_leader

Leader's estimated state.

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

List of entities allowed to define a command.

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

List of systems allowed to define a command.

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.

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

Controller evaluation data.

unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_frame
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_pos
std::vector<std::string> Maneuver::VehicleFormation::FormCollAvoid::Task::m_formation_systems
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_frequency

Simulation process frequency.

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

Leader initial state.

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

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_last_time_verb_ctrlactiv
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.

double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_alt_max
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_alt_min
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_bank_lim

Leader command limits.

uint32_t Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_id
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_speed_max
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_leader_speed_min
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_llh_ref_pos[3]

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

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_first
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_time
double Maneuver::VehicleFormation::FormCollAvoid::Task::m_mean_time_start
UAVSimulation* Maneuver::VehicleFormation::FormCollAvoid::Task::m_model

Leader vehicle model.

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

Simulated vehicles' models.

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_param_update_first
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_position

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

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

Leader commanded true airspeed.

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
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
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ctrl

System command variables.

Systems Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_id
Systems Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_id_last
unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_ind
unsigned int Maneuver::VehicleFormation::FormCollAvoid::Task::m_uav_n

Formation configuration.

bool Maneuver::VehicleFormation::FormCollAvoid::Task::m_valid_airspeed
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_accel
Matrix Maneuver::VehicleFormation::FormCollAvoid::Task::m_vehicle_state

System state variables.

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

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

Environment variables.

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