#include <follow_target.h>

Inheritance diagram for FollowTarget:

Collaboration diagram for FollowTarget:

Public Member Functions
	FollowTarget (Navigator *navigator)

	~FollowTarget ()=default

void	on_inactive () override
	This function is called while the mode is inactive. More...

void	on_activation () override
	This function is called one time when mode becomes active, pos_sp_triplet must be initialized here. More...

void	on_active () override
	This function is called while the mode is active. More...

Public Member Functions inherited from MissionBlock
	MissionBlock (Navigator *navigator)
	Constructor. More...

virtual	~MissionBlock ()=default

	MissionBlock (const MissionBlock &)=delete

MissionBlock &	operator= (const MissionBlock &)=delete

Public Member Functions inherited from NavigatorMode
	NavigatorMode (Navigator *navigator)

virtual	~NavigatorMode ()=default

	NavigatorMode (const NavigatorMode &)=delete

NavigatorMode	operator= (const NavigatorMode &)=delete

void	run (bool active)

virtual void	on_inactivation ()
	This function is called one time when mode becomes inactive. More...

Private Types
enum	FollowTargetState { TRACK_POSITION, TRACK_VELOCITY, SET_WAIT_FOR_TARGET_POSITION, WAIT_FOR_TARGET_POSITION }

enum	{ FOLLOW_FROM_RIGHT, FOLLOW_FROM_BEHIND, FOLLOW_FROM_FRONT, FOLLOW_FROM_LEFT }

enum	{ POS = 0, VEL = 1, ACCEL = 2, ATT_RATES = 3 }

Private Member Functions
	DEFINE_PARAMETERS ((ParamFloat< px4::params::NAV_MIN_FT_HT >) _param_nav_min_ft_ht,(ParamFloat< px4::params::NAV_FT_DST >) _param_nav_ft_dst,(ParamInt< px4::params::NAV_FT_FS >) _param_nav_ft_fs,(ParamFloat< px4::params::NAV_FT_RS >) _param_nav_ft_rs) FollowTargetState _follow_target_state

void	track_target_position ()

void	track_target_velocity ()

bool	target_velocity_valid ()

bool	target_position_valid ()

void	reset_target_validity ()

void	update_position_sp (bool velocity_valid, bool position_valid, float yaw_rate)

void	update_target_motion ()

void	update_target_velocity ()

void	set_follow_target_item (struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw)
	Set follow_target item. More...

Private Attributes
int	_follow_target_position {FOLLOW_FROM_BEHIND}

uORB::Subscription	_follow_target_sub {ORB_ID(follow_target)}

float	_step_time_in_ms {0.0f}

float	_follow_offset {OFFSET_M}

uint64_t	_target_updates {0}

uint64_t	_last_update_time {0}

matrix::Vector3f	_current_vel

matrix::Vector3f	_step_vel

matrix::Vector3f	_est_target_vel

matrix::Vector3f	_target_distance

matrix::Vector3f	_target_position_offset

matrix::Vector3f	_target_position_delta

matrix::Vector3f	_filtered_target_position_delta

follow_target_s	_current_target_motion {}

follow_target_s	_previous_target_motion {}

float	_yaw_rate {0.0f}

float	_responsiveness {0.0f}

float	_yaw_angle {0.0f}

matrix::Dcmf	_rot_matrix

Static Private Attributes
static constexpr int	TARGET_TIMEOUT_MS = 2500

static constexpr int	TARGET_ACCEPTANCE_RADIUS_M = 5

static constexpr int	INTERPOLATION_PNTS = 20

static constexpr float	FF_K = .25F

static constexpr float	OFFSET_M = 8

static constexpr float	_follow_position_matricies [4][9]

Additional Inherited Members
Static Public Member Functions inherited from MissionBlock
static bool	item_contains_position (const mission_item_s &item)

Protected Member Functions inherited from MissionBlock
bool	is_mission_item_reached ()
	Check if mission item has been reached. More...

void	reset_mission_item_reached ()
	Reset all reached flags. More...

bool	mission_item_to_position_setpoint (const mission_item_s &item, position_setpoint_s *sp)
	Convert a mission item to a position setpoint. More...

void	set_loiter_item (struct mission_item_s *item, float min_clearance=-1.0f)
	Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position. More...

void	set_takeoff_item (struct mission_item_s *item, float abs_altitude, float min_pitch=0.0f)
	Set a takeoff mission item. More...

void	set_land_item (struct mission_item_s *item, bool at_current_location)
	Set a land mission item. More...

void	set_idle_item (struct mission_item_s *item)
	Set idle mission item. More...

void	set_vtol_transition_item (struct mission_item_s *item, const uint8_t new_mode)
	Set vtol transition item. More...

void	mission_apply_limitation (mission_item_s &item)
	General function used to adjust the mission item based on vehicle specific limitations. More...

void	issue_command (const mission_item_s &item)

float	get_time_inside (const mission_item_s &item) const

float	get_absolute_altitude_for_item (const mission_item_s &mission_item) const

Protected Attributes inherited from MissionBlock
mission_item_s	_mission_item {}

bool	_waypoint_position_reached {false}

bool	_waypoint_yaw_reached {false}

bool	_waypoint_position_reached_previously {false}

hrt_abstime	_time_first_inside_orbit {0}

hrt_abstime	_action_start {0}

hrt_abstime	_time_wp_reached {0}

uORB::Publication< actuator_controls_s >	_actuator_pub {ORB_ID(actuator_controls_2)}

Protected Attributes inherited from NavigatorMode
Navigator *	_navigator {nullptr}

Detailed Description

Definition at line 53 of file follow_target.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

private

Enumerator
FOLLOW_FROM_RIGHT
FOLLOW_FROM_BEHIND
FOLLOW_FROM_FRONT
FOLLOW_FROM_LEFT

Definition at line 79 of file follow_target.h.

◆ anonymous enum

anonymous enum

private

Enumerator
POS
VEL
ACCEL
ATT_RATES

Definition at line 126 of file follow_target.h.

◆ FollowTargetState

enum FollowTarget::FollowTargetState

private

Enumerator
TRACK_POSITION
TRACK_VELOCITY
SET_WAIT_FOR_TARGET_POSITION
WAIT_FOR_TARGET_POSITION

Definition at line 72 of file follow_target.h.

Constructor & Destructor Documentation

◆ FollowTarget()

FollowTarget::FollowTarget ( Navigator * navigator )

Definition at line 63 of file follow_target.cpp.

References _current_vel, _est_target_vel, _step_vel, _target_distance, _target_position_delta, _target_position_offset, and matrix::Matrix< Type, M, N >::zero().

Here is the call graph for this function:

◆ ~FollowTarget()

FollowTarget::~FollowTarget ( )

default

Member Function Documentation

◆ DEFINE_PARAMETERS()

FollowTarget::DEFINE_PARAMETERS	(	(ParamFloat< px4::params::NAV_MIN_FT_HT >)	_param_nav_min_ft_ht,
		(ParamFloat< px4::params::NAV_FT_DST >)	_param_nav_ft_dst,
		(ParamInt< px4::params::NAV_FT_FS >)	_param_nav_ft_fs,
		(ParamFloat< px4::params::NAV_FT_RS >)	_param_nav_ft_rs
	)

inlineprivate

Definition at line 93 of file follow_target.h.

References SET_WAIT_FOR_TARGET_POSITION.

◆ on_activation()

void FollowTarget::on_activation ( )

overridevirtual

This function is called one time when mode becomes active, pos_sp_triplet must be initialized here.

Reimplemented from NavigatorMode.

Definition at line 80 of file follow_target.cpp.

References _follow_offset, _follow_position_matricies, _follow_target_position, _responsiveness, _rot_matrix, math::constrain(), FOLLOW_FROM_BEHIND, FOLLOW_FROM_LEFT, and FOLLOW_FROM_RIGHT.

Here is the call graph for this function:

◆ on_active()

void FollowTarget::on_active ( )

overridevirtual

This function is called while the mode is active.

Reimplemented from NavigatorMode.

Definition at line 95 of file follow_target.cpp.

References _current_target_motion, _current_vel, _est_target_vel, _follow_offset, _follow_target_sub, _last_update_time, MissionBlock::_mission_item, NavigatorMode::_navigator, _previous_target_motion, _responsiveness, _rot_matrix, _step_time_in_ms, _step_vel, _target_distance, _target_position_delta, _target_position_offset, _target_updates, _yaw_angle, _yaw_rate, follow_target_s::alt, uORB::Subscription::copy(), f(), FF_K, get_bearing_to_next_waypoint(), Navigator::get_global_position(), hrt_absolute_time(), INTERPOLATION_PNTS, MissionBlock::is_mission_item_reached(), follow_target_s::lat, vehicle_global_position_s::lat, matrix::Vector< Type, M >::length(), vehicle_global_position_s::lon, follow_target_s::lon, map_projection_init(), map_projection_project(), map_projection_reproject(), matrix::Vector3< Type >::normalized(), math::radians(), reset_target_validity(), set_follow_target_item(), SET_WAIT_FOR_TARGET_POSITION, TARGET_ACCEPTANCE_RADIUS_M, target_position_valid(), TARGET_TIMEOUT_MS, target_velocity_valid(), follow_target_s::timestamp, TRACK_POSITION, TRACK_VELOCITY, update_position_sp(), uORB::Subscription::updated(), WAIT_FOR_TARGET_POSITION, matrix::wrap_pi(), and vehicle_global_position_s::yaw.

Here is the call graph for this function:

◆ on_inactive()

void FollowTarget::on_inactive ( )

overridevirtual

This function is called while the mode is inactive.

Reimplemented from NavigatorMode.

Definition at line 75 of file follow_target.cpp.

References reset_target_validity().

Here is the call graph for this function:

◆ reset_target_validity()

void FollowTarget::reset_target_validity ( )

private

Definition at line 340 of file follow_target.cpp.

References _current_target_motion, _current_vel, _est_target_vel, _previous_target_motion, _step_vel, _target_distance, _target_position_offset, _target_updates, _yaw_rate, MissionBlock::reset_mission_item_reached(), SET_WAIT_FOR_TARGET_POSITION, and matrix::Matrix< Type, M, N >::zero().

Referenced by on_active(), and on_inactive().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_follow_target_item()

void FollowTarget::set_follow_target_item	(	struct mission_item_s *	item,
		float	min_clearance,
		follow_target_s &	target,
		float	yaw
	)

private

Set follow_target item.

Definition at line 368 of file follow_target.cpp.

References NavigatorMode::_navigator, mission_item_s::acceptance_radius, home_position_s::alt, mission_item_s::altitude, mission_item_s::altitude_is_relative, mission_item_s::autocontinue, f(), Navigator::get_acceptance_radius(), Navigator::get_home_position(), Navigator::get_land_detected(), Navigator::get_loiter_radius(), vehicle_land_detected_s::landed, follow_target_s::lat, mission_item_s::lat, mission_item_s::loiter_radius, follow_target_s::lon, mission_item_s::lon, mission_item_s::nav_cmd, NAV_CMD_DO_FOLLOW_REPOSITION, NAV_CMD_IDLE, mission_item_s::origin, ORIGIN_ONBOARD, mission_item_s::time_inside, and mission_item_s::yaw.

Referenced by on_active().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ target_position_valid()

bool FollowTarget::target_position_valid ( )

private

Definition at line 361 of file follow_target.cpp.

References _target_updates.

Referenced by on_active().

Here is the caller graph for this function:

◆ target_velocity_valid()

bool FollowTarget::target_velocity_valid ( )

private

Definition at line 355 of file follow_target.cpp.

References _target_updates.

Referenced by on_active().

Here is the caller graph for this function:

◆ track_target_position()

void FollowTarget::track_target_position ( )

private

◆ track_target_velocity()

void FollowTarget::track_target_velocity ( )

private

◆ update_position_sp()

void FollowTarget::update_position_sp	(	bool	velocity_valid,
		bool	position_valid,
		float	yaw_rate
	)

private

Definition at line 317 of file follow_target.cpp.

Referenced by on_active().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_target_motion()

void FollowTarget::update_target_motion ( )

private

◆ update_target_velocity()

void FollowTarget::update_target_velocity ( )

private

Member Data Documentation

◆ _current_target_motion

follow_target_s FollowTarget::_current_target_motion {}

private

Definition at line 118 of file follow_target.h.

Referenced by on_active(), and reset_target_validity().

◆ _current_vel

matrix::Vector3f FollowTarget::_current_vel

private

Definition at line 110 of file follow_target.h.

Referenced by FollowTarget(), on_active(), reset_target_validity(), and update_position_sp().

◆ _est_target_vel

matrix::Vector3f FollowTarget::_est_target_vel

private

Definition at line 112 of file follow_target.h.

Referenced by FollowTarget(), on_active(), and reset_target_validity().

◆ _filtered_target_position_delta

matrix::Vector3f FollowTarget::_filtered_target_position_delta

private

Definition at line 116 of file follow_target.h.

◆ _follow_offset

float FollowTarget::_follow_offset {OFFSET_M}

private

Definition at line 105 of file follow_target.h.

Referenced by on_activation(), and on_active().

◆ _follow_position_matricies

constexpr float FollowTarget::_follow_position_matricies

staticprivate

Initial value:

= {
        { 1.0F, -1.0F, 0.0F,  1.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, 
        {-1.0F,  0.0F, 0.0F,  0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, 
        { 1.0F,  0.0F, 0.0F,  0.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, 
        { 1.0F,  1.0F, 0.0F, -1.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}  
    }

Definition at line 86 of file follow_target.h.

Referenced by on_activation().

◆ _follow_target_position

int FollowTarget::_follow_target_position {FOLLOW_FROM_BEHIND}

private

Definition at line 101 of file follow_target.h.

Referenced by on_activation().

◆ _follow_target_sub

uORB::Subscription FollowTarget::_follow_target_sub {ORB_ID(follow_target)}

private

Definition at line 103 of file follow_target.h.

Referenced by on_active().

◆ _last_update_time

uint64_t FollowTarget::_last_update_time {0}

private

Definition at line 108 of file follow_target.h.

Referenced by on_active().

◆ _previous_target_motion

follow_target_s FollowTarget::_previous_target_motion {}

private

Definition at line 119 of file follow_target.h.

Referenced by on_active(), and reset_target_validity().

◆ _responsiveness

float FollowTarget::_responsiveness {0.0f}

private

Definition at line 122 of file follow_target.h.

Referenced by on_activation(), and on_active().

◆ _rot_matrix

matrix::Dcmf FollowTarget::_rot_matrix

private

Definition at line 133 of file follow_target.h.

Referenced by on_activation(), and on_active().

◆ _step_time_in_ms

float FollowTarget::_step_time_in_ms {0.0f}

private

Definition at line 104 of file follow_target.h.

Referenced by on_active().

◆ _step_vel

matrix::Vector3f FollowTarget::_step_vel

private

Definition at line 111 of file follow_target.h.

Referenced by FollowTarget(), on_active(), and reset_target_validity().

◆ _target_distance

matrix::Vector3f FollowTarget::_target_distance

private

Definition at line 113 of file follow_target.h.

Referenced by FollowTarget(), on_active(), and reset_target_validity().

◆ _target_position_delta

matrix::Vector3f FollowTarget::_target_position_delta

private

Definition at line 115 of file follow_target.h.

Referenced by FollowTarget(), and on_active().

◆ _target_position_offset

matrix::Vector3f FollowTarget::_target_position_offset

private

Definition at line 114 of file follow_target.h.

Referenced by FollowTarget(), on_active(), and reset_target_validity().

◆ _target_updates

uint64_t FollowTarget::_target_updates {0}

private

Definition at line 107 of file follow_target.h.

Referenced by on_active(), reset_target_validity(), target_position_valid(), and target_velocity_valid().

◆ _yaw_angle

float FollowTarget::_yaw_angle {0.0f}

private

Definition at line 123 of file follow_target.h.

Referenced by on_active().

◆ _yaw_rate

float FollowTarget::_yaw_rate {0.0f}

private

Definition at line 121 of file follow_target.h.

Referenced by on_active(), and reset_target_validity().

◆ FF_K

constexpr float FollowTarget::FF_K = .25F

staticprivate

Definition at line 69 of file follow_target.h.

Referenced by on_active().

◆ INTERPOLATION_PNTS

constexpr int FollowTarget::INTERPOLATION_PNTS = 20

staticprivate

Definition at line 68 of file follow_target.h.

Referenced by on_active().

◆ OFFSET_M

constexpr float FollowTarget::OFFSET_M = 8

staticprivate

Definition at line 70 of file follow_target.h.

◆ TARGET_ACCEPTANCE_RADIUS_M

constexpr int FollowTarget::TARGET_ACCEPTANCE_RADIUS_M = 5

staticprivate

Definition at line 67 of file follow_target.h.

Referenced by on_active().

◆ TARGET_TIMEOUT_MS

constexpr int FollowTarget::TARGET_TIMEOUT_MS = 2500

staticprivate

Definition at line 66 of file follow_target.h.

Referenced by on_active().

The documentation for this class was generated from the following files:

src/modules/navigator/follow_target.h
src/modules/navigator/follow_target.cpp

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ anonymous enum

◆ anonymous enum

◆ FollowTargetState

Constructor & Destructor Documentation

◆ FollowTarget()

◆ ~FollowTarget()

Member Function Documentation

◆ DEFINE_PARAMETERS()

◆ on_activation()

◆ on_active()

◆ on_inactive()

◆ reset_target_validity()

◆ set_follow_target_item()

◆ target_position_valid()

◆ target_velocity_valid()

◆ track_target_position()

◆ track_target_velocity()

◆ update_position_sp()

◆ update_target_motion()

◆ update_target_velocity()

Member Data Documentation

◆ _current_target_motion

◆ _current_vel

◆ _est_target_vel

◆ _filtered_target_position_delta

◆ _follow_offset

◆ _follow_position_matricies

◆ _follow_target_position

◆ _follow_target_sub

◆ _last_update_time

◆ _previous_target_motion

◆ _responsiveness

◆ _rot_matrix

◆ _step_time_in_ms

◆ _step_vel

◆ _target_distance

◆ _target_position_delta

◆ _target_position_offset

◆ _target_updates

◆ _yaw_angle

◆ _yaw_rate

◆ FF_K

◆ INTERPOLATION_PNTS

◆ OFFSET_M

◆ TARGET_ACCEPTANCE_RADIUS_M

◆ TARGET_TIMEOUT_MS