fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 05:11:36 +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);
}
}
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+16 -2
View File
@@ -43,6 +43,7 @@
#include <px4_module_params.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/position_controller_status.h>
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_defines.h>
@@ -51,18 +52,31 @@ class ObstacleAvoidance : public ModuleParams
{
public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance() = default;
~ObstacleAvoidance();
bool initializeSubscriptions(SubscriptionArray &subscription_array);
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
void updateAvoidanceWaypoints(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);
void updateAvoidanceSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
float target_acceptance_radius,
const matrix::Vector2f &closest_pt);
private:
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< obstacle avoidance enabled */
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD
);
vehicle_trajectory_waypoint_s _desired_waypoint = {};
orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
orb_advert_t _pub_pos_control_status{nullptr};
void _publish_avoidance_desired_waypoint();
};