PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <ObstacleAvoidance.hpp>
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... | |
Definition at line 71 of file ObstacleAvoidance.hpp.
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.
|
default |
|
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().
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.
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().
|
protected |
_param_nav_mc_alt_rad | Acceptance radius for multicopter altitude |
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.
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().
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.
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().
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.
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().
|
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().
|
protected |
current position triplet
Definition at line 128 of file ObstacleAvoidance.hpp.
Referenced by checkAvoidanceProgress(), and updateAvoidanceDesiredWaypoints().
|
protected |
desired vehicle trajectory waypoint to be sent to OA
Definition at line 122 of file ObstacleAvoidance.hpp.
Referenced by ObstacleAvoidance(), updateAvoidanceDesiredSetpoints(), and updateAvoidanceDesiredWaypoints().
|
protected |
true, if external yaw handling is active
Definition at line 137 of file ObstacleAvoidance.hpp.
Referenced by injectAvoidanceSetpoints(), and updateAvoidanceDesiredWaypoints().
|
protected |
vehicle position when entered in failsafe
Definition at line 130 of file ObstacleAvoidance.hpp.
Referenced by injectAvoidanceSetpoints(), and ObstacleAvoidance().
|
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().
|
protected |
current vehicle position
Definition at line 129 of file ObstacleAvoidance.hpp.
Referenced by checkAvoidanceProgress(), and injectAvoidanceSetpoints().
|
protected |
z distance to the goal
Definition at line 135 of file ObstacleAvoidance.hpp.
Referenced by checkAvoidanceProgress().
|
protected |
position controller status publication
Definition at line 125 of file ObstacleAvoidance.hpp.
Referenced by checkAvoidanceProgress().
|
protected |
trajectory waypoint desired publication
Definition at line 124 of file ObstacleAvoidance.hpp.
Referenced by updateAvoidanceDesiredSetpoints().
|
protected |
vehicle command do publication
Definition at line 126 of file ObstacleAvoidance.hpp.
Referenced by _publishVehicleCmdDoLoiter().
|
protected |
vehicle status subscription
Definition at line 120 of file ObstacleAvoidance.hpp.
Referenced by injectAvoidanceSetpoints().
|
protected |
vehicle trajectory waypoint subscription
Definition at line 119 of file ObstacleAvoidance.hpp.
Referenced by injectAvoidanceSetpoints().