#include <ObstacleAvoidance.hpp>

Inheritance diagram for ObstacleAvoidance:

Collaboration diagram for ObstacleAvoidance:

Public Member Functions
	ObstacleAvoidance (ModuleParams *parent)

	~ObstacleAvoidance ()=default

void	injectAvoidanceSetpoints (matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp)
	Inject setpoints from obstacle avoidance system into FlightTasks. More...

void	updateAvoidanceDesiredWaypoints (const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, const int wp_type)
	Updates the desired waypoints to send to the obstacle avoidance system. More...

void	updateAvoidanceDesiredSetpoints (const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type)
	Updates the desired setpoints to send to the obstacle avoidance system. More...

void	checkAvoidanceProgress (const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, float target_acceptance_radius, const matrix::Vector2f &closest_pt)
	Checks the vehicle progress between previous and current position waypoint of the triplet. More...

Protected Member Functions
void	_publishVehicleCmdDoLoiter ()
	Publishes vehicle command. More...

	DEFINE_PARAMETERS ((ParamFloat< px4::params::NAV_MC_ALT_RAD >) _param_nav_mc_alt_rad)

Protected Attributes
uORB::SubscriptionData< vehicle_trajectory_waypoint_s >	_sub_vehicle_trajectory_waypoint {ORB_ID(vehicle_trajectory_waypoint)}
	vehicle trajectory waypoint subscription More...

uORB::SubscriptionData< vehicle_status_s >	_sub_vehicle_status {ORB_ID(vehicle_status)}
	vehicle status subscription More...

vehicle_trajectory_waypoint_s	_desired_waypoint {}
	desired vehicle trajectory waypoint to be sent to OA More...

uORB::Publication< vehicle_trajectory_waypoint_s >	_pub_traj_wp_avoidance_desired {ORB_ID(vehicle_trajectory_waypoint_desired)}
	trajectory waypoint desired publication More...

uORB::Publication< position_controller_status_s >	_pub_pos_control_status {ORB_ID(position_controller_status)}
	position controller status publication More...

uORB::PublicationQueued< vehicle_command_s >	_pub_vehicle_command {ORB_ID(vehicle_command)}
	vehicle command do publication More...

matrix::Vector3f	_curr_wp = {}
	current position triplet More...

matrix::Vector3f	_position = {}
	current vehicle position More...

matrix::Vector3f	_failsafe_position = {}
	vehicle position when entered in failsafe More...

systemlib::Hysteresis	_avoidance_point_not_valid_hysteresis {false}
	becomes true if the companion doesn't start sending valid setpoints More...

systemlib::Hysteresis	_no_progress_z_hysteresis {false}
	becomes true if the vehicle is not making progress towards the z component of the goal More...

float	_prev_pos_to_target_z = -1.f
	z distance to the goal More...

bool	_ext_yaw_active = false
	true, if external yaw handling is active More...

Detailed Description

Definition at line 71 of file ObstacleAvoidance.hpp.

Constructor & Destructor Documentation

◆ ObstacleAvoidance()

ObstacleAvoidance::ObstacleAvoidance ( ModuleParams * parent )

Definition at line 49 of file ObstacleAvoidance.cpp.

References _avoidance_point_not_valid_hysteresis, _desired_waypoint, _failsafe_position, _no_progress_z_hysteresis, empty_trajectory_waypoint, systemlib::Hysteresis::set_hysteresis_time_from(), matrix::Matrix< Type, M, N >::setNaN(), TIME_BEFORE_FAILSAFE, and Z_PROGRESS_TIMEOUT_US.

Here is the call graph for this function:

◆ ~ObstacleAvoidance()

ObstacleAvoidance::~ObstacleAvoidance ( )

default

Member Function Documentation

◆ _publishVehicleCmdDoLoiter()

void ObstacleAvoidance::_publishVehicleCmdDoLoiter ( )

protected

Publishes vehicle command.

Definition at line 190 of file ObstacleAvoidance.cpp.

References _pub_vehicle_command, command, hrt_absolute_time(), uORB::PublicationQueued< T >::publish(), PX4_CUSTOM_MAIN_MODE_AUTO, and PX4_CUSTOM_SUB_MODE_AUTO_LOITER.

Referenced by injectAvoidanceSetpoints().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkAvoidanceProgress()

void ObstacleAvoidance::checkAvoidanceProgress	(	const matrix::Vector3f &	pos,
		const matrix::Vector3f &	prev_wp,
		float	target_acceptance_radius,
		const matrix::Vector2f &	closest_pt
	)

Checks the vehicle progress between previous and current position waypoint of the triplet.

Parameters

pos,vehicle	position
prev_wp,previous	position triplet
target_acceptance_radius,current	position triplet xy acceptance radius
closest_pt,closest	point to the vehicle on the line previous-current position triplet

Definition at line 149 of file ObstacleAvoidance.cpp.

References _curr_wp, _no_progress_z_hysteresis, _position, _prev_pos_to_target_z, _pub_pos_control_status, position_controller_status_s::acceptance_radius, position_controller_status_s::altitude_acceptance, f(), systemlib::Hysteresis::get_state(), hrt_absolute_time(), matrix::Vector< Type, M >::length(), uORB::Publication< T >::publish(), systemlib::Hysteresis::set_state_and_update(), position_controller_status_s::timestamp, and position_controller_status_s::yaw_acceptance.

Referenced by FlightTaskAuto::_evaluateTriplets().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ DEFINE_PARAMETERS()

ObstacleAvoidance::DEFINE_PARAMETERS ( (ParamFloat< px4::params::NAV_MC_ALT_RAD >) _param_nav_mc_alt_rad )

protected

Parameters

_param_nav_mc_alt_rad Acceptance radius for multicopter altitude

◆ injectAvoidanceSetpoints()

void ObstacleAvoidance::injectAvoidanceSetpoints	(	matrix::Vector3f &	pos_sp,
		matrix::Vector3f &	vel_sp,
		float &	yaw_sp,
		float &	yaw_speed_sp
	)

Inject setpoints from obstacle avoidance system into FlightTasks.

Parameters

pos_sp,position	setpoint
vel_sp,velocity	setpoint
yaw_sp,yaw	setpoint
yaw_speed_sp,yaw	speed setpoint

Definition at line 59 of file ObstacleAvoidance.cpp.

References _avoidance_point_not_valid_hysteresis, _ext_yaw_active, _failsafe_position, _position, _publishVehicleCmdDoLoiter(), _sub_vehicle_status, _sub_vehicle_trajectory_waypoint, uORB::SubscriptionData< T >::get(), systemlib::Hysteresis::get_state(), hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), vehicle_status_s::nav_state, trajectory_waypoint_s::point_valid, trajectory_waypoint_s::position, systemlib::Hysteresis::set_state_and_update(), matrix::Matrix< Type, M, N >::setNaN(), vehicle_trajectory_waypoint_s::timestamp, TRAJECTORY_STREAM_TIMEOUT_US, uORB::SubscriptionData< T >::update(), trajectory_waypoint_s::velocity, vehicle_trajectory_waypoint_s::waypoints, trajectory_waypoint_s::yaw, and trajectory_waypoint_s::yaw_speed.

Referenced by TEST_F().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateAvoidanceDesiredSetpoints()

void ObstacleAvoidance::updateAvoidanceDesiredSetpoints	(	const matrix::Vector3f &	pos_sp,
		const matrix::Vector3f &	vel_sp,
		const int	type
	)

Updates the desired setpoints to send to the obstacle avoidance system.

Parameters

pos_sp,desired	position setpoint computed by the active FlightTask
vel_sp,desired	velocity setpoint computed by the active FlightTask
type,current	triplet type

Definition at line 137 of file ObstacleAvoidance.cpp.

References _desired_waypoint, _pub_traj_wp_avoidance_desired, matrix::Matrix< Type, M, N >::copyTo(), empty_trajectory_waypoint, trajectory_waypoint_s::point_valid, trajectory_waypoint_s::position, uORB::Publication< T >::publish(), trajectory_waypoint_s::type, trajectory_waypoint_s::velocity, and vehicle_trajectory_waypoint_s::waypoints.

Referenced by TEST_F().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateAvoidanceDesiredWaypoints()

void ObstacleAvoidance::updateAvoidanceDesiredWaypoints	(	const matrix::Vector3f &	curr_wp,
		const float	curr_yaw,
		const float	curr_yawspeed,
		const matrix::Vector3f &	next_wp,
		const float	next_yaw,
		const float	next_yawspeed,
		const bool	ext_yaw_active,
		const int	wp_type
	)

Updates the desired waypoints to send to the obstacle avoidance system.

These messages don't have any direct impact on the flight.

Parameters

curr_wp,current	position triplet
curr_yaw,current	yaw triplet
curr_yawspeed,current	yaw speed triplet
next_wp,next	position triplet
next_yaw,next	yaw triplet
next_yawspeed,next	yaw speed triplet
wp_type,current	triplet type

Definition at line 110 of file ObstacleAvoidance.cpp.

References _curr_wp, _desired_waypoint, _ext_yaw_active, trajectory_waypoint_s::acceleration, matrix::Matrix< Type, M, N >::copyTo(), hrt_absolute_time(), trajectory_waypoint_s::point_valid, trajectory_waypoint_s::position, vehicle_trajectory_waypoint_s::timestamp, trajectory_waypoint_s::type, vehicle_trajectory_waypoint_s::type, trajectory_waypoint_s::velocity, vehicle_trajectory_waypoint_s::waypoints, trajectory_waypoint_s::yaw, and trajectory_waypoint_s::yaw_speed.

Referenced by FlightTaskAuto::_evaluateTriplets(), and TEST_F().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _avoidance_point_not_valid_hysteresis

systemlib::Hysteresis ObstacleAvoidance::_avoidance_point_not_valid_hysteresis {false}

protected

becomes true if the companion doesn't start sending valid setpoints

Definition at line 132 of file ObstacleAvoidance.hpp.

Referenced by injectAvoidanceSetpoints(), and ObstacleAvoidance().

◆ _curr_wp

matrix::Vector3f ObstacleAvoidance::_curr_wp = {}

protected

current position triplet

Definition at line 128 of file ObstacleAvoidance.hpp.

Referenced by checkAvoidanceProgress(), and updateAvoidanceDesiredWaypoints().

◆ _desired_waypoint

vehicle_trajectory_waypoint_s ObstacleAvoidance::_desired_waypoint {}

protected

desired vehicle trajectory waypoint to be sent to OA

Definition at line 122 of file ObstacleAvoidance.hpp.

Referenced by ObstacleAvoidance(), updateAvoidanceDesiredSetpoints(), and updateAvoidanceDesiredWaypoints().

◆ _ext_yaw_active

bool ObstacleAvoidance::_ext_yaw_active = false

protected

true, if external yaw handling is active

Definition at line 137 of file ObstacleAvoidance.hpp.

Referenced by injectAvoidanceSetpoints(), and updateAvoidanceDesiredWaypoints().

◆ _failsafe_position

matrix::Vector3f ObstacleAvoidance::_failsafe_position = {}

protected

vehicle position when entered in failsafe

Definition at line 130 of file ObstacleAvoidance.hpp.

Referenced by injectAvoidanceSetpoints(), and ObstacleAvoidance().

◆ _no_progress_z_hysteresis

systemlib::Hysteresis ObstacleAvoidance::_no_progress_z_hysteresis {false}

protected

becomes true if the vehicle is not making progress towards the z component of the goal

Definition at line 133 of file ObstacleAvoidance.hpp.

Referenced by checkAvoidanceProgress(), and ObstacleAvoidance().

◆ _position

matrix::Vector3f ObstacleAvoidance::_position = {}

protected

current vehicle position

Definition at line 129 of file ObstacleAvoidance.hpp.

Referenced by checkAvoidanceProgress(), and injectAvoidanceSetpoints().

◆ _prev_pos_to_target_z

float ObstacleAvoidance::_prev_pos_to_target_z = -1.f

protected

z distance to the goal

Definition at line 135 of file ObstacleAvoidance.hpp.

Referenced by checkAvoidanceProgress().

◆ _pub_pos_control_status

uORB::Publication<position_controller_status_s> ObstacleAvoidance::_pub_pos_control_status {ORB_ID(position_controller_status)}

protected

position controller status publication

Definition at line 125 of file ObstacleAvoidance.hpp.

Referenced by checkAvoidanceProgress().

◆ _pub_traj_wp_avoidance_desired

uORB::Publication<vehicle_trajectory_waypoint_s> ObstacleAvoidance::_pub_traj_wp_avoidance_desired {ORB_ID(vehicle_trajectory_waypoint_desired)}

protected

trajectory waypoint desired publication

Definition at line 124 of file ObstacleAvoidance.hpp.

Referenced by updateAvoidanceDesiredSetpoints().

◆ _pub_vehicle_command

uORB::PublicationQueued<vehicle_command_s> ObstacleAvoidance::_pub_vehicle_command {ORB_ID(vehicle_command)}

protected

vehicle command do publication

Definition at line 126 of file ObstacleAvoidance.hpp.

Referenced by _publishVehicleCmdDoLoiter().

◆ _sub_vehicle_status

uORB::SubscriptionData<vehicle_status_s> ObstacleAvoidance::_sub_vehicle_status {ORB_ID(vehicle_status)}

protected

vehicle status subscription

Definition at line 120 of file ObstacleAvoidance.hpp.

Referenced by injectAvoidanceSetpoints().

◆ _sub_vehicle_trajectory_waypoint

uORB::SubscriptionData<vehicle_trajectory_waypoint_s> ObstacleAvoidance::_sub_vehicle_trajectory_waypoint {ORB_ID(vehicle_trajectory_waypoint)}

protected

vehicle trajectory waypoint subscription

Definition at line 119 of file ObstacleAvoidance.hpp.

Referenced by injectAvoidanceSetpoints().

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

src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.hpp
src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ObstacleAvoidance()

◆ ~ObstacleAvoidance()

Member Function Documentation

◆ _publishVehicleCmdDoLoiter()

◆ checkAvoidanceProgress()

◆ DEFINE_PARAMETERS()

◆ injectAvoidanceSetpoints()

◆ updateAvoidanceDesiredSetpoints()

◆ updateAvoidanceDesiredWaypoints()

Member Data Documentation

◆ _avoidance_point_not_valid_hysteresis

◆ _curr_wp

◆ _desired_waypoint

◆ _ext_yaw_active

◆ _failsafe_position

◆ _no_progress_z_hysteresis

◆ _position

◆ _prev_pos_to_target_z

◆ _pub_pos_control_status

◆ _pub_traj_wp_avoidance_desired

◆ _pub_vehicle_command

◆ _sub_vehicle_status

◆ _sub_vehicle_trajectory_waypoint