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

mavlink: replace MavlinkOrbSubscription with uORB::Subscription

Browse Source 
* uORB orb_stat() and update(uint64_t *time, void *dst) are now obsolete and have been deleted
 * mavlink messages add more advertised checks in streams get_size() check to improve data rate calculation across different scenarios
This commit is contained in:
Daniel Agar
2020-03-14 12:52:46 -04:00
committed by GitHub
parent 4f2d39cb15
commit 5fcd7932e9
19 changed files with 718 additions and 1320 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+16 -46
View File
@@ -1155,12 +1155,11 @@ Mavlink::send_statustext_emergency(const char *string)
void
Mavlink::send_autopilot_capabilites()
{
struct vehicle_status_s status;
uORB::Subscription status_sub{ORB_ID(vehicle_status)};
vehicle_status_s status;
MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
if (status_sub->update(&status)) {
mavlink_autopilot_version_t msg = {};
if (status_sub.copy(&status)) {
mavlink_autopilot_version_t msg{};
msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
@@ -1242,27 +1241,6 @@ Mavlink::send_protocol_version()
set_proto_version(curr_proto_ver);
}
MavlinkOrbSubscription *
Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing)
{
if (!disable_sharing) {
/* check if already subscribed to this topic */
for (MavlinkOrbSubscription *sub : _subscriptions) {
if (sub->get_topic() == topic && sub->get_instance() == instance) {
/* already subscribed */
return sub;
}
}
}
/* add new subscription */
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
_subscriptions.add(sub_new);
return sub_new;
}
int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
@@ -2138,22 +2116,17 @@ Mavlink::task_main(int argc, char *argv[])
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true);
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
uint64_t status_time = 0;
MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true);
/* We don't want to miss the first advertise of an ACK, so we subscribe from the
* beginning and not just when the topic exists. */
ack_sub->subscribe_from_beginning(true);
cmd_sub->subscribe_from_beginning(true);
uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription status_sub{ORB_ID(vehicle_status)};
uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)};
/* command ack */
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));
uORB::Subscription mavlink_log_sub{ORB_ID(mavlink_log)};
vehicle_status_s status{};
status_sub->update(&status_time, &status);
status_sub.copy(&status);
/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
_transmitting_enabled = true;
@@ -2268,7 +2241,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_sik_radio();
if (status_sub->update(&status_time, &status)) {
if (status_sub.update(&status)) {
/* switch HIL mode if required */
set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
@@ -2291,9 +2264,9 @@ Mavlink::task_main(int argc, char *argv[])
}
}
vehicle_command_s vehicle_cmd{};
vehicle_command_s vehicle_cmd;
if (cmd_sub->update_if_changed(&vehicle_cmd)) {
if (cmd_sub.update(&vehicle_cmd)) {
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
(_mode == MAVLINK_MODE_IRIDIUM)) {
if (vehicle_cmd.param1 > 0.5f) {
@@ -2330,9 +2303,9 @@ Mavlink::task_main(int argc, char *argv[])
/* send command ACK */
uint16_t current_command_ack = 0;
vehicle_command_ack_s command_ack{};
vehicle_command_ack_s command_ack;
if (ack_sub->update_if_changed(&command_ack)) {
if (ack_sub.update(&command_ack)) {
if (!command_ack.from_external) {
mavlink_command_ack_t msg;
msg.result = command_ack.result;
@@ -2351,9 +2324,9 @@ Mavlink::task_main(int argc, char *argv[])
}
}
mavlink_log_s mavlink_log{};
mavlink_log_s mavlink_log;
if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
if (mavlink_log_sub.update(&mavlink_log)) {
_logbuffer.put(&mavlink_log);
}
@@ -2497,9 +2470,6 @@ Mavlink::task_main(int argc, char *argv[])
/* delete streams */
_streams.clear();
/* delete subscriptions */
_subscriptions.clear();
if (_uart_fd >= 0 && !_is_usb_uart) {
/* close UART */
::close(_uart_fd);