fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 18:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move all obstacle avoidance interfaces to the ObstacleAvoidance library

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-04-04 16:41:26 +02:00
committed by bresch
parent 4e806d79fe
commit 34b0f33098
5 changed files with 127 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+103
View File
@@ -52,6 +52,19 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
ObstacleAvoidance::~ObstacleAvoidance()
{
//unadvertise publishers
if (_traj_wp_avoidance_desired_pub != nullptr) {
orb_unadvertise(_traj_wp_avoidance_desired_pub);
}
if (_pub_pos_control_status != nullptr) {
orb_unadvertise(_pub_pos_control_status);
}
}
bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
@@ -81,3 +94,93 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
}
}
void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const float curr_yaw,
const float curr_yawspeed,
const Vector3f &next_wp, const float next_yaw, const float next_yawspeed)
{
_desired_waypoint.timestamp = hrt_absolute_time();
_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
}
void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp)
{
pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
_publish_avoidance_desired_waypoint();
// TODO: reset all fields to NaN
}
void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
float target_acceptance_radius,
const Vector2f &closest_pt)
{
position_controller_status_s pos_control_status = {};
pos_control_status.timestamp = hrt_absolute_time();
Vector3f curr_wp = _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position;
// vector from previous triplet to current target
Vector2f prev_to_target = Vector2f(curr_wp - prev_wp);
// vector from previous triplet to the vehicle projected position on the line previous-target triplet
Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
// fraction of the previous-tagerget line that has been flown
const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
Vector2f pos_to_target = Vector2f(curr_wp - pos);
if (prev_curr_travelled > 1.0f) {
// if the vehicle projected position on the line previous-target is past the target waypoint,
// increase the target acceptance radius such that navigator will update the triplets
pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
}
const float pos_to_target_z = fabsf(curr_wp(2) - pos(2));
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
// do not check for waypoints yaw acceptance in navigator
pos_control_status.yaw_acceptance = NAN;
if (_pub_pos_control_status == nullptr) {
_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
} else {
orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
}
}
void
ObstacleAvoidance::_publish_avoidance_desired_waypoint()
{
// publish desired waypoint
if (_traj_wp_avoidance_desired_pub != nullptr) {
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_desired_waypoint);
} else {
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
&_desired_waypoint);
}
}