fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 04:43:34 +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/modules/mavlink/mavlink_main.cpp
+7 -17
View File
@@ -49,8 +49,9 @@
#endif
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <version/version.h>
#include <lib/mathlib/mathlib.h>
#include <lib/version/version.h>
#include <uORB/PublicationQueued.hpp>
#include "mavlink_receiver.h"
#include "mavlink_main.h"
@@ -2126,7 +2127,7 @@ Mavlink::task_main(int argc, char *argv[])
cmd_sub->subscribe_from_beginning(true);
/* command ack */
orb_advert_t command_ack_pub = nullptr;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
@@ -2263,7 +2264,7 @@ Mavlink::task_main(int argc, char *argv[])
}
// send positive command ack
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = vehicle_cmd.timestamp;
command_ack.command = vehicle_cmd.command;
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
@@ -2271,13 +2272,7 @@ Mavlink::task_main(int argc, char *argv[])
command_ack.target_system = vehicle_cmd.source_system;
command_ack.target_component = vehicle_cmd.source_component;
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
command_ack_pub.publish(command_ack);
}
}
@@ -2542,12 +2537,7 @@ void Mavlink::publish_telemetry_status()
_tstatus.timestamp = hrt_absolute_time();
if (_telem_status_pub == nullptr) {
_telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3);
} else {
orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus);
}
_telem_status_pub.publish(_tstatus);
}
void Mavlink::check_radio_config()
src/modules/mavlink/mavlink_main.h
+3 -2
View File
@@ -71,11 +71,11 @@
#include <px4_posix.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/uthash/utlist.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/uORB.h>
#include "mavlink_command_sender.h"
#include "mavlink_messages.h"
@@ -540,7 +540,8 @@ private:
bool _first_heartbeat_sent{false};
orb_advert_t _mavlink_log_pub{nullptr};
orb_advert_t _telem_status_pub{nullptr};
uORB::PublicationQueued<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
bool _task_running{false};
static bool _boot_complete;
src/modules/mavlink/mavlink_mission.cpp
-1
View File
@@ -270,7 +270,6 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
}
void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
src/modules/mavlink/mavlink_parameters.cpp
+3 -25
View File
@@ -50,13 +50,6 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
{
}
MavlinkParametersManager::~MavlinkParametersManager()
{
if (_uavcan_parameter_request_pub) {
orb_unadvertise(_uavcan_parameter_request_pub);
}
}
unsigned
MavlinkParametersManager::get_size()
{
@@ -91,12 +84,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.node_id = req_list.target_component;
req.param_index = 0;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
}
break;
@@ -163,12 +151,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
req.int_value = val;
}
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
}
break;
@@ -577,12 +560,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter()
if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) {
uavcan_parameter_request_s req = _uavcan_open_request_list->req;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
_uavcan_parameter_request_pub.publish(req);
_uavcan_waiting_for_request_response = true;
}
src/modules/mavlink/mavlink_parameters.h
+4 -3
View File
@@ -45,7 +45,7 @@
#include <parameters/param.h>
#include "mavlink_bridge_header.h"
#include <uORB/uORB.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/uavcan_parameter_request.h>
@@ -59,7 +59,7 @@ class MavlinkParametersManager
{
public:
explicit MavlinkParametersManager(Mavlink *mavlink);
~MavlinkParametersManager();
~MavlinkParametersManager() = default;
/**
* Handle sending of messages. Call this regularly at a fixed frequency.
@@ -128,7 +128,8 @@ protected:
orb_advert_t _rc_param_map_pub{nullptr};
rc_parameter_map_s _rc_param_map{};
orb_advert_t _uavcan_parameter_request_pub{nullptr};
uORB::PublicationQueued<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)};
src/modules/mavlink/mavlink_receiver.cpp
+9 -51
View File
@@ -98,20 +98,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
void
MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
{
vehicle_command_ack_s command_ack = {};
vehicle_command_ack_s command_ack{};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = command;
command_ack.result = result;
command_ack.target_system = sysid;
command_ack.target_component = compid;
if (_command_ack_pub == nullptr) {
_command_ack_pub = 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), _command_ack_pub, &command_ack);
}
_cmd_ack_pub.publish(command_ack);
}
void
@@ -515,12 +509,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
if (!send_ack) {
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command);
}
_cmd_pub.publish(vehicle_command);
}
}
@@ -547,13 +536,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
command_ack.target_system = ack.target_system;
command_ack.target_component = ack.target_component;
if (_command_ack_pub == nullptr) {
_command_ack_pub = 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), _command_ack_pub, &command_ack);
}
_cmd_ack_pub.publish(command_ack);
// TODO: move it to the same place that sent the command
if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
@@ -687,7 +670,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
vehicle_command_s vcmd = {};
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
@@ -703,12 +686,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
vcmd.confirmation = true;
vcmd.from_external = true;
if (_cmd_pub == nullptr) {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
}
_cmd_pub.publish(vcmd);
}
void
@@ -2274,12 +2252,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
}
_transponder_report_pub.publish(t);
}
void
@@ -2343,13 +2316,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
// provide these.
if (_transponder_report_pub == nullptr) {
_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
}
_transponder_report_pub.publish(t);
_last_utm_global_pos_com = t.timestamp;
}
@@ -2392,16 +2359,7 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len));
orb_advert_t &pub = _gps_inject_data_pub;
if (pub == nullptr) {
pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic,
_gps_inject_data_queue_size);
} else {
orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
}
_gps_inject_data_pub.publish(gps_inject_data_topic);
}
void
src/modules/mavlink/mavlink_receiver.h
+7 -6
View File
@@ -43,6 +43,7 @@
#include <px4_module_params.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
@@ -71,6 +72,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -212,6 +214,11 @@ private:
vehicle_land_detected_s _hil_land_detector {};
vehicle_control_mode_s _control_mode {};
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
orb_advert_t _accel_pub{nullptr};
orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr};
orb_advert_t _airspeed_pub{nullptr};
@@ -219,9 +226,7 @@ private:
orb_advert_t _attitude_pub{nullptr};
orb_advert_t _baro_pub{nullptr};
orb_advert_t _battery_pub{nullptr};
orb_advert_t _cmd_pub{nullptr};
orb_advert_t _collision_report_pub{nullptr};
orb_advert_t _command_ack_pub{nullptr};
orb_advert_t _debug_array_pub{nullptr};
orb_advert_t _debug_key_value_pub{nullptr};
orb_advert_t _debug_value_pub{nullptr};
@@ -231,7 +236,6 @@ private:
orb_advert_t _flow_pub{nullptr};
orb_advert_t _follow_target_pub{nullptr};
orb_advert_t _global_pos_pub{nullptr};
orb_advert_t _gps_inject_data_pub{nullptr};
orb_advert_t _gps_pub{nullptr};
orb_advert_t _gyro_pub{nullptr};
orb_advert_t _hil_distance_sensor_pub{nullptr};
@@ -249,15 +253,12 @@ private:
orb_advert_t _rates_sp_pub{nullptr};
orb_advert_t _rc_pub{nullptr};
orb_advert_t _trajectory_waypoint_pub{nullptr};
orb_advert_t _transponder_report_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr};
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
static constexpr int _gps_inject_data_queue_size{6};
static constexpr unsigned int MOM_SWITCH_COUNT{8};
int _orb_class_instance{-1};