diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f94bcd9047..2b4008878d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1899,6 +1899,10 @@ Mavlink::task_main(int argc, char *argv[]) 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)); + /* 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); + uint64_t ack_time = 0; MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 5255b16ee9..a775dd3082 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -53,7 +53,8 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instanc _instance(instance), _fd(-1), _published(false), - _last_pub_check(0) + _last_pub_check(0), + _subscribe_from_beginning(false) { } @@ -157,15 +158,6 @@ MavlinkOrbSubscription::is_published() return true; } - // This is a workaround for this issue: - // https://github.com/PX4/Firmware/issues/5438 -#if defined(__PX4_LINUX) || defined(__PX4_QURT) - - if (_fd < 0) { - _fd = orb_subscribe_multi(_topic, _instance); - } - -#else // Telemetry can sustain an initial published check at 10 Hz hrt_abstime now = hrt_absolute_time(); @@ -176,14 +168,22 @@ MavlinkOrbSubscription::is_published() // We are checking now _last_pub_check = now; - // If it does not exist its not published - if (orb_exists(_topic, _instance)) { - return false; - - } else if (_fd < 0) { +#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) + if (_fd < 0) { _fd = orb_subscribe_multi(_topic, _instance); } +#else + // We don't want to subscribe to anything that does not exist + // in order to save memory and file descriptors. + // However, for some topics like vehicle_command_ack, we want to subscribe + // from the beginning in order not to miss the first publish respective advertise. + if (!_subscribe_from_beginning && orb_exists(_topic, _instance)) { + return false; + } + if (_fd < 0) { + _fd = orb_subscribe_multi(_topic, _instance); + } #endif bool updated; @@ -195,3 +195,9 @@ MavlinkOrbSubscription::is_published() return _published; } + +void +MavlinkOrbSubscription::subscribe_from_beginning(bool subscribe_from_beginning) +{ + _subscribe_from_beginning = subscribe_from_beginning; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 0e1f48128a..e5ab24d23b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -87,6 +87,9 @@ public: * If no data is available the buffer will be filled with zeros. */ bool is_published(); + + void subscribe_from_beginning(bool from_beginning); + orb_id_t get_topic() const; int get_instance() const; @@ -96,6 +99,7 @@ private: int _fd; ///< subscription handle bool _published; ///< topic was ever published hrt_abstime _last_pub_check; ///< when we checked last + bool _subscribe_from_beginning; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks /* do not allow copying this class */ MavlinkOrbSubscription(const MavlinkOrbSubscription &);