Public Member Functions
	Task (const std::string &name, Tasks::Context &ctx)

void	onUpdateParameters (void)

void	onEntityResolution (void)

void	onResourceAcquisition (void)

void	onResourceRelease (void)

void	consume (const IMC::PlanControl *msg)

void	consume (const IMC::PlanDB *msg)

void	consume (const IMC::DesiredRoll *msg)

void	consume (const IMC::DesiredSpeed *msg)

void	consume (const IMC::DesiredZ *msg)

void	consume (const IMC::EstimatedStreamVelocity *msg)

void	consume (const IMC::EstimatedState *msg)

void	task (void)

void	dispatchAlias (IMC::Message *msg)

void	setLeaderState (const IMC::LeaderState *leader_state)

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	periodicUpdate (const double &d_time)

void	updateLeaderLimits (void)

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

uint16_t	getActivationTime (void) const

uint16_t	getDeactivationTime (void) const

unsigned int	resolveSystemName (const std::string &name) const

const char *	resolveSystemId (unsigned int id) const

void	loadConfig (void)

void	setPriority (unsigned int value)

unsigned int	getPriority (void) const

void	inf (const char *format,...) DUNE_PRINTF_FORMAT(2

void void	war (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void	err (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void void	cri (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void void void	debug (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void void void void	trace (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void void void void void	spew (const char *format,...) DUNE_PRINTF_FORMAT(2

void void void void void void void void	dispatch (IMC::Message *msg, unsigned int flags=0)

void	dispatch (IMC::Message &msg, unsigned int flags=0)

void	dispatchReply (const IMC::Message &original, IMC::Message &msg, unsigned int flags=0)

void	receive (const IMC::Message *msg)

void	reserveEntities (void)

void	resolveEntities (void)

void	acquireResources (void)

void	releaseResources (void)

void	initializeResources (void)

void	updateParameters (bool act_deact=true)

void	writeParamsXML (std::ostream &os) const

Public Member Functions inherited from DUNE::Tasks::AbstractTask
	AbstractTask (void)

virtual	~AbstractTask (void)

Public Member Functions inherited from DUNE::Concurrency::Thread
	Thread (void)

virtual	~Thread (void)

int	getProcessorUsage (void)

Public Member Functions inherited from DUNE::Concurrency::Runnable
	Runnable (void)

virtual	~Runnable (void)

void	start (void)

void	stop (void)

void	join (void)

void	stopAndJoin (void)

void	setPriority (Scheduler::Policy policy, unsigned priority)

State	getState (void)

bool	isCreated (void)

bool	isStopping (void)

bool	isRunning (void)

bool	isStarting (void)

bool	isDead (void)

Public Attributes
Arguments	m_args

UAVSimulation *	m_model

double	m_llh_ref_pos [3]

Matrix	m_position

Matrix	m_velocity

IMC::EstimatedState	m_estate_leader

double	m_clock_diff

double	m_last_leader_update

double	m_last_time_debug

double	m_last_time_trace

double	m_last_time_spew

Matrix	m_uav_state

Matrix	m_team_state

Matrix	m_wind

double	m_wind_speed

Matrix	m_wind_team

double	m_g

Matrix	m_uav_ctrl

IMC::DesiredRoll	m_bank_cmd

IMC::DesiredSpeed	m_airspeed_cmd

IMC::DesiredZ	m_altitude_cmd

double	m_tas_cmd_leader

double	m_alt_cmd_leader

double	m_tas_min

double	m_tas_max

double	m_bank_lim

Matrix	m_tas_min_uav

Matrix	m_tas_max_uav

Matrix	m_bank_lim_uav

unsigned int	m_uav_n

Systems	m_uav_id

Matrix	m_formation_pos

double	m_timestep

IMC::FormationPlanExecution	m_uav_formation

IMC::LeaderState	m_init_leader

bool	m_debug

IMC::PlanControl	m_plan_ctrl_last

std::map< uint32_t, Systems >	m_filtered_sys

std::map< uint32_t, Entities >	m_filtered_ent

uint32_t	m_alias_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
const char *	getEntityLabel (void) const

void	setEntityLabel (const std::string &label)

void	setEntityState (IMC::EntityState::StateEnum state, Status::Code code)

void	setEntityState (IMC::EntityState::StateEnum state, const std::string &description)

IMC::EntityState::StateEnum	getEntityState (void) const

unsigned int	reserveEntity (const std::string &label)

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 void	onEntityReservation (void)

virtual void	onReportEntityState (void)

virtual void	onResourceInitialization (void)

virtual void	onRequestActivation (void)

virtual void	onRequestDeactivation (void)

virtual void	onActivation (void)

virtual void	onDeactivation (void)

Protected Member Functions inherited from DUNE::Concurrency::Thread
void	startImpl (void)

void	stopImpl (void)

void	joinImpl (void)

void	setPriorityImpl (Scheduler::Policy policy, unsigned priority)

Protected Attributes inherited from DUNE::Tasks::Task
Context &	m_ctx

Constructor & Destructor Documentation

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

inline

Member Function Documentation

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

inline

References dispatchAlias(), DUNE::Tasks::Task::getSystemId(), m_alias_id, m_plan_ctrl_last, DUNE::Tasks::Task::resolveSystemId(), and DUNE::Tasks::Task::trace().

void Maneuver::VehicleFormation::Coordinator::Task::consume ( const IMC::PlanDB * msg )

inline

References dispatchAlias(), DUNE::Tasks::Task::err(), DUNE::Tasks::Task::getEntityId(), DUNE::Tasks::Task::getSystemId(), m_alias_id, m_init_leader, m_llh_ref_pos, m_plan_ctrl_last, m_team_state, m_uav_id, m_uav_n, m_wind, DUNE::Tasks::Task::resolveEntity(), DUNE::Tasks::Task::resolveSystemId(), setLeaderState(), and DUNE::Tasks::Task::trace().

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

inline

References DUNE::Simulation::UAVSimulation::commandBank(), DUNE::Math::Angles::degrees(), DUNE::Tasks::Task::isActive(), m_bank_lim, m_filtered_ent, m_filtered_sys, m_model, DUNE::Tasks::Task::resolveEntity(), DUNE::Tasks::Task::resolveSystemId(), DUNE::Tasks::Task::spew(), DUNE::Tasks::Task::trace(), and DUNE::Math::trimValue().

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

inline

References DUNE::Simulation::UAVSimulation::commandAirspeed(), DUNE::Tasks::Task::isActive(), m_args, m_filtered_ent, m_filtered_sys, m_model, m_tas_cmd_leader, m_uav_formation, DUNE::Tasks::Task::resolveEntity(), DUNE::Tasks::Task::resolveSystemId(), DUNE::Tasks::Task::spew(), Maneuver::VehicleFormation::Coordinator::Arguments::tas_max, Maneuver::VehicleFormation::Coordinator::Arguments::tas_min, DUNE::Tasks::Task::trace(), DUNE::Math::trimValue(), and updateLeaderLimits().

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

inline

References Maneuver::VehicleFormation::Coordinator::Arguments::alt_max, Maneuver::VehicleFormation::Coordinator::Arguments::alt_min, DUNE::Simulation::UAVSimulation::commandAlt(), DUNE::Tasks::Task::isActive(), m_alt_cmd_leader, m_args, m_filtered_ent, m_filtered_sys, m_llh_ref_pos, m_model, DUNE::Tasks::Task::resolveEntity(), DUNE::Tasks::Task::resolveSystemId(), DUNE::Tasks::Task::spew(), DUNE::Tasks::Task::trace(), and DUNE::Math::trimValue().

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

inline

References m_uav_id, m_uav_n, m_wind, m_wind_speed, m_wind_team, and DUNE::Math::Matrix::norm_2().

void Maneuver::VehicleFormation::Coordinator::Task::consume ( const IMC::EstimatedState * msg )

inline

References DUNE::Tasks::Task::getSystemId(), m_formation_pos, m_llh_ref_pos, m_team_state, m_uav_id, m_uav_n, m_uav_state, positionReframing(), and DUNE::Math::Matrix::set().

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

inline

References DUNE::Tasks::Task::dispatch(), and m_alias_id.

Referenced by consume(), onUpdateParameters(), periodicUpdate(), and updateLeaderLimits().

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

inline

void Maneuver::VehicleFormation::Coordinator::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.

void Maneuver::VehicleFormation::Coordinator::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::Coordinator::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::Coordinator::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.

Referenced by onEntityResolution().

void Maneuver::VehicleFormation::Coordinator::Task::periodicUpdate ( const double & d_time )

inline

Update leader simulated state for standard time periods.

References dispatchAlias(), DUNE::Math::Matrix::get(), DUNE::Simulation::UAVSimulation::getAirspeed(), DUNE::Simulation::UAVSimulation::getPosition(), DUNE::Simulation::UAVSimulation::getVelocity(), m_debug, m_estate_leader, m_last_leader_update, m_model, m_position, m_timestep, m_velocity, m_wind, DUNE::Simulation::UAVSimulation::m_wind, DUNE::Math::Matrix::norm_2(), DUNE::Math::Matrix::toDCM(), DUNE::Math::transpose(), and DUNE::Simulation::UAVSimulation::update().

Referenced by task().

template<typename Ta , typename Tb , typename Tc , typename Td , typename Te >

void Maneuver::VehicleFormation::Coordinator::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]	lat1	new reference latitude (rad).
[in]	lon1	new reference longitude (rad).
[in]	hei1	new reference height (m).
[in]	lat2	old reference latitude (rad).
[in]	lon2	old reference longitude (rad).
[in]	hei2	old reference height (m).
[in,out]	n	North offset (m).
[in,out]	e	East offset (m).
[in,out]	d	Down offset (m).

Referenced by consume(), and setLeaderState().

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

inline

References DUNE::Tasks::Task::debug(), DUNE::Math::Angles::degrees(), DUNE::Math::Matrix::get(), m_bank_lim, m_estate_leader, m_init_leader, m_last_leader_update, m_llh_ref_pos, m_model, m_uav_state, DUNE::Simulation::UAVSimulation::m_wind, positionReframing(), DUNE::Math::Matrix::set(), DUNE::Simulation::UAVSimulation::setPosition(), DUNE::Simulation::UAVSimulation::setVelocity(), DUNE::Math::trimValue(), and DUNE::Math::Matrix::vertCat().

Referenced by consume().

void Maneuver::VehicleFormation::Coordinator::Task::task ( void )

inlinevirtual

The task to be executed on each cycle.

Implements DUNE::Tasks::Periodic.

References DUNE::Tasks::Task::consumeMessages(), DUNE::Tasks::Task::getSystemName(), DUNE::Tasks::Task::isActive(), m_clock_diff, m_last_time_trace, periodicUpdate(), and DUNE::Tasks::Task::spew().

void Maneuver::VehicleFormation::Coordinator::Task::updateLeaderLimits ( void )

inline

Member Data Documentation

IMC::DesiredSpeed Maneuver::VehicleFormation::Coordinator::Task::m_airspeed_cmd

Referenced by onEntityResolution().

uint32_t Maneuver::VehicleFormation::Coordinator::Task::m_alias_id

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

double Maneuver::VehicleFormation::Coordinator::Task::m_alt_cmd_leader

Referenced by consume(), and onEntityResolution().

IMC::DesiredZ Maneuver::VehicleFormation::Coordinator::Task::m_altitude_cmd

Referenced by onEntityResolution().

Arguments Maneuver::VehicleFormation::Coordinator::Task::m_args

Task arguments.

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

IMC::DesiredRoll Maneuver::VehicleFormation::Coordinator::Task::m_bank_cmd

Vehicle commands.

double Maneuver::VehicleFormation::Coordinator::Task::m_bank_lim

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

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_bank_lim_uav

Referenced by onUpdateParameters(), and updateLeaderLimits().

double Maneuver::VehicleFormation::Coordinator::Task::m_clock_diff

Simulation and control time variables.

Referenced by task().

bool Maneuver::VehicleFormation::Coordinator::Task::m_debug

Referenced by onUpdateParameters(), and periodicUpdate().

IMC::EstimatedState Maneuver::VehicleFormation::Coordinator::Task::m_estate_leader

Leader's estimated state.

Referenced by periodicUpdate(), and setLeaderState().

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

List of entities allowed to define a command.

Referenced by consume(), and onEntityResolution().

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

List of systems allowed to define a command.

Referenced by consume(), and onEntityResolution().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_formation_pos

Referenced by consume(), and onUpdateParameters().

double Maneuver::VehicleFormation::Coordinator::Task::m_g

Referenced by updateLeaderLimits().

IMC::LeaderState Maneuver::VehicleFormation::Coordinator::Task::m_init_leader

Leader initial state.

Referenced by consume(), and setLeaderState().

double Maneuver::VehicleFormation::Coordinator::Task::m_last_leader_update

Referenced by onEntityResolution(), periodicUpdate(), and setLeaderState().

double Maneuver::VehicleFormation::Coordinator::Task::m_last_time_debug

Last time debug information was shown.

double Maneuver::VehicleFormation::Coordinator::Task::m_last_time_spew

double Maneuver::VehicleFormation::Coordinator::Task::m_last_time_trace

Referenced by task().

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

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

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

UAVSimulation* Maneuver::VehicleFormation::Coordinator::Task::m_model

Leader vehicle model.

Referenced by consume(), onEntityResolution(), periodicUpdate(), and setLeaderState().

IMC::PlanControl Maneuver::VehicleFormation::Coordinator::Task::m_plan_ctrl_last

Referenced by consume(), and onUpdateParameters().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_position

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

Referenced by onEntityResolution(), and periodicUpdate().

double Maneuver::VehicleFormation::Coordinator::Task::m_tas_cmd_leader

Referenced by consume(), onEntityResolution(), onUpdateParameters(), and updateLeaderLimits().

double Maneuver::VehicleFormation::Coordinator::Task::m_tas_max

Referenced by updateLeaderLimits().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_tas_max_uav

Referenced by onUpdateParameters(), and updateLeaderLimits().

double Maneuver::VehicleFormation::Coordinator::Task::m_tas_min

Constraints.

Referenced by updateLeaderLimits().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_tas_min_uav

Referenced by onUpdateParameters(), and updateLeaderLimits().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_team_state

Referenced by consume().

double Maneuver::VehicleFormation::Coordinator::Task::m_timestep

Simulation process time step.

Referenced by onUpdateParameters(), and periodicUpdate().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_uav_ctrl

System command variables.

IMC::FormationPlanExecution Maneuver::VehicleFormation::Coordinator::Task::m_uav_formation

Formation synchronization messages.

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

Systems Maneuver::VehicleFormation::Coordinator::Task::m_uav_id

Referenced by consume(), and onUpdateParameters().

unsigned int Maneuver::VehicleFormation::Coordinator::Task::m_uav_n

Number of team vehicles.

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

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_uav_state

System state variables.

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

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_velocity

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

Referenced by onEntityResolution(), and periodicUpdate().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_wind

Environment variables.

Referenced by consume(), and periodicUpdate().

double Maneuver::VehicleFormation::Coordinator::Task::m_wind_speed

Referenced by consume(), and updateLeaderLimits().

Matrix Maneuver::VehicleFormation::Coordinator::Task::m_wind_team

Referenced by consume(), and onUpdateParameters().

Collaboration diagram for Maneuver::VehicleFormation::Coordinator::Task:

Public Member Functions

Public Attributes

Additional Inherited Members

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation