Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

ObstacleAvoidance: add comments

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-03-04 08:56:40 +01:00
committed by bresch
parent aa1b46f85a
commit b165c41737
3 changed files with 61 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
-1
View File
@@ -248,7 +248,6 @@ bool FlightTaskAuto::_evaluateTriplets()
_sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN);
_obstacle_avoidance.updateAvoidanceSetpoints(_position_setpoint, _velocity_setpoint);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
// _checkAvoidanceProgress();
}
return true;
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+6 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,9 +36,6 @@
*/
#include "ObstacleAvoidance.hpp"
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
using namespace matrix;
using namespace time_literals;
@@ -64,8 +61,8 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
ObstacleAvoidance::~ObstacleAvoidance()
{
//unadvertise publishers
if (_traj_wp_avoidance_desired_pub != nullptr) {
orb_unadvertise(_traj_wp_avoidance_desired_pub);
if (_pub_traj_wp_avoidance_desired != nullptr) {
orb_unadvertise(_pub_traj_wp_avoidance_desired);
}
if (_pub_pos_control_status != nullptr) {
@@ -194,11 +191,11 @@ 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);
if (_pub_traj_wp_avoidance_desired != nullptr) {
orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _pub_traj_wp_avoidance_desired, &_desired_waypoint);
} else {
_traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
_pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
&_desired_waypoint);
}
}
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+55 -15
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,15 +41,19 @@
#pragma once
#include <px4_defines.h>
#include <px4_module_params.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
#include <commander/px4_custom_mode.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <matrix/matrix/math.hpp>
#include <px4_defines.h>
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
class ObstacleAvoidance : public ModuleParams
{
@@ -59,32 +63,68 @@ public:
bool initializeSubscriptions(SubscriptionArray &subscription_array);
/**
* Inject setpoints from obstacle avoidance system into FLightTasks.
* @param pos_sp, position setpoint
* @param vel_sp, velocity setpoint
* @param yaw_sp, yaw setpoint
* @param yaw_speed_sp, yaw speed setpoint
*/
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
/**
* Updates the desired waypoints to send to the obstacle avoidance system.
* @param curr_wp, current position triplet
* @param curr_yaw, current yaw triplet
* @param curr_yawspeed, current yaw speed triplet
* @param next_wp, next position triplet
* @param next_yaw, next yaw triplet
* @param next_yawspeed, next yaw speed triplet
*/
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);
/**
* Updates the desired setpoints to send to the obstacle avoidance system.
* @param pos_sp, desired position setpoint computed by the active FlightTask
* @param vel_sp, desired velocity setpoint computed by the active FlightTask
*/
void updateAvoidanceSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
/**
* Checks the vehicle progress between previous and current position triplet.
* @param pos, vehicle position
* @param prev_wp, previous position triplet
* @param target_acceptance_radius, current position triplet xy acceptance radius
* @param closest_pt, closest point to the vehicle on the line previous-current position triplet
*/
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
float target_acceptance_radius,
const matrix::Vector2f &closest_pt);
float target_acceptance_radius, const matrix::Vector2f &closest_pt);
private:
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr};
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< obstacle avoidance enabled */
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD /**< Acceptance radius for multicopter altitude */
);
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};
orb_advert_t _pub_vehicle_command{nullptr};
vehicle_trajectory_waypoint_s _desired_waypoint = {}; /**< desired vehicle trajectory waypoint to be sent to OA */
orb_advert_t _pub_traj_wp_avoidance_desired{nullptr}; /**< trajectory waypoint desired publication */
orb_advert_t _pub_pos_control_status{nullptr}; /**< position controller status publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
matrix::Vector3f _curr_wp = {};
matrix::Vector3f _curr_wp = {}; /**< current position triplet */
/**
* Publishes vehicle trajectory waypoint desired.
*/
void _publish_avoidance_desired_waypoint();
/**
* Publishes vehicle vehicle command.
*/
void _publish_vehicle_cmd_do_loiter();
};