PX4 Firmware
PX4 Autopilot Software http://px4.io
ObstacleAvoidance Class Reference

#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,vehicleposition
prev_wp,previousposition triplet
target_acceptance_radius,currentposition triplet xy acceptance radius
closest_pt,closestpoint 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_radAcceptance 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,positionsetpoint
vel_sp,velocitysetpoint
yaw_sp,yawsetpoint
yaw_speed_sp,yawspeed 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,desiredposition setpoint computed by the active FlightTask
vel_sp,desiredvelocity setpoint computed by the active FlightTask
type,currenttriplet 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,currentposition triplet
curr_yaw,currentyaw triplet
curr_yawspeed,currentyaw speed triplet
next_wp,nextposition triplet
next_yaw,nextyaw triplet
next_yawspeed,nextyaw speed triplet
wp_type,currenttriplet 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: