fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:43:33 +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/FlightTasks.cpp
+6 -18
View File
@@ -137,22 +137,16 @@ void FlightTasks::reActivate()
void FlightTasks::_updateCommand()
{
// lazy subscription to command topic
if (_sub_vehicle_command < 0) {
_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
}
// check if there's any new command
bool updated = false;
orb_check(_sub_vehicle_command, &updated);
bool updated = _sub_vehicle_command.updated();
if (!updated) {
return;
}
// get command
struct vehicle_command_s command;
orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
vehicle_command_s command{};
_sub_vehicle_command.copy(&command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
@@ -183,19 +177,13 @@ void FlightTasks::_updateCommand()
}
// send back acknowledgment
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = switch_result;
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
if (_pub_vehicle_command_ack == nullptr) {
_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
}
_pub_vehicle_command_ack.publish(command_ack);
}
src/lib/FlightTasks/FlightTasks.hpp
+8 -2
View File
@@ -45,6 +45,10 @@
#include "SubscriptionArray.hpp"
#include "FlightTasks_generated.hpp"
#include <lib/WeatherVane/WeatherVane.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_command.h>
#include <new>
@@ -180,8 +184,10 @@ private:
*/
void _updateCommand();
FlightTaskIndex switchVehicleCommand(const int command);
int _sub_vehicle_command = -1; /**< topic handle on which commands are received */
orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */
uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */
uORB::PublicationQueued<vehicle_command_ack_s> _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
int _initTask(FlightTaskIndex task_index);
};
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.
*/
src/lib/systemlib/CMakeLists.txt
+1 -1
View File
@@ -35,7 +35,7 @@ set(SRCS
conversions.c
cpuload.c
crc.c
mavlink_log.c
mavlink_log.cpp
otp.c
)
src/lib/systemlib/mavlink_log.c → src/lib/systemlib/mavlink_log.cpp
+4 -15
View File
@@ -48,9 +48,6 @@
#include <uORB/topics/mavlink_log.h>
#include "mavlink_log.h"
#define MAVLINK_LOG_QUEUE_SIZE 5
__EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...)
{
// TODO: add compile check for maxlen
@@ -59,31 +56,23 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
return;
}
if (mavlink_log_pub == NULL) {
if (mavlink_log_pub == nullptr) {
return;
}
struct mavlink_log_s log_msg;
mavlink_log_s log_msg;
log_msg.severity = severity;
log_msg.timestamp = hrt_absolute_time();
va_list ap;
va_start(ap, fmt);
vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap);
va_end(ap);
if (*mavlink_log_pub != NULL) {
if (*mavlink_log_pub != nullptr) {
orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
} else {
*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log),
&log_msg,
MAVLINK_LOG_QUEUE_SIZE);
*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH);
}
}