fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 03:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

Browse Source
This commit is contained in:
Daniel Agar
2019-08-04 10:08:09 -04:00
parent 839787568c
commit e69398c09f
62 changed files with 421 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+3 -42
View File
@@ -63,19 +63,6 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
ObstacleAvoidance::~ObstacleAvoidance()
{
//unadvertise publishers
if (_pub_traj_wp_avoidance_desired != nullptr) {
orb_unadvertise(_pub_traj_wp_avoidance_desired);
}
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)) {
@@ -170,7 +157,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp,
vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
_publishAvoidanceDesiredWaypoint();
_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
_desired_waypoint = empty_trajectory_waypoint;
}
@@ -208,27 +195,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
// 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::_publishAvoidanceDesiredWaypoint()
{
// publish 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 {
_pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
&_desired_waypoint);
}
_pub_pos_control_status.publish(pos_control_status);
}
void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
@@ -248,11 +215,5 @@ void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
_pub_vehicle_command.publish(command);
}
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+8 -10
View File
@@ -46,6 +46,8 @@
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
@@ -62,7 +64,7 @@ class ObstacleAvoidance : public ModuleParams
{
public:
ObstacleAvoidance(ModuleParams *parent);
~ObstacleAvoidance();
~ObstacleAvoidance() = default;
bool initializeSubscriptions(SubscriptionArray &subscription_array);
@@ -113,10 +115,11 @@ private:
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */
);
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 */
vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */
uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */
uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
matrix::Vector3f _curr_wp = {}; /**< current position triplet */
matrix::Vector3f _position = {}; /**< current vehicle position */
@@ -126,11 +129,6 @@ private:
bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
/**
* Publishes vehicle trajectory waypoint desired.
*/
void _publishAvoidanceDesiredWaypoint();
/**
* Publishes vehicle command.
*/