info: add flight information subscription

That's better than having to call get once a second.

Signed-off-by: Julian Oes <julian@oes.ch>

src/integration_tests/info.cpp

@@ -74,7 +74,7 @@ TEST(SitlTest, PX4Info)
         if (flight_info_result.first == Info::Result::Success) {
             std::cout << flight_info_result.second << '\n';
         } else {
-            LogWarn() << "Product request result: " << flight_info_result.first;
+            LogWarn() << "Flight info request result: " << flight_info_result.first;
         }
 
         std::this_thread::sleep_for(std::chrono::seconds(2));

src/mavsdk/plugins/info/include/plugins/info/info.h

@@ -254,6 +254,26 @@ class Info : public PluginBase {
      */
     std::pair<Result, double> get_speed_factor() const;
 
+    /**
+     * @brief Callback type for subscribe_flight_information.
+     */
+    using FlightInformationCallback = std::function<void(FlightInfo)>;
+
+    /**
+     * @brief Handle type for subscribe_flight_information.
+     */
+    using FlightInformationHandle = Handle<FlightInfo>;
+
+    /**
+     * @brief Subscribe to 'flight information' updates.
+     */
+    FlightInformationHandle subscribe_flight_information(const FlightInformationCallback& callback);
+
+    /**
+     * @brief Unsubscribe from subscribe_flight_information
+     */
+    void unsubscribe_flight_information(FlightInformationHandle handle);
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/info/info.cpp

@@ -46,6 +46,17 @@ std::pair<Info::Result, double> Info::get_speed_factor() const
     return _impl->get_speed_factor();
 }
 
+Info::FlightInformationHandle
+Info::subscribe_flight_information(const FlightInformationCallback& callback)
+{
+    return _impl->subscribe_flight_information(callback);
+}
+
+void Info::unsubscribe_flight_information(FlightInformationHandle handle)
+{
+    _impl->unsubscribe_flight_information(handle);
+}
+
 bool operator==(const Info::FlightInfo& lhs, const Info::FlightInfo& rhs)
 {
     return (rhs.time_boot_ms == lhs.time_boot_ms) && (rhs.flight_uid == lhs.flight_uid) &&

src/mavsdk/plugins/info/info_impl.cpp

@@ -1,11 +1,15 @@
-#include <functional>
 #include <cstring>
+#include <functional>
+#include <future>
 #include <numeric>
 #include "info_impl.h"
 #include "system.h"
+#include "callback_list.tpp"
 
 namespace mavsdk {
 
+template class CallbackList<Info::FlightInfo>;
+
 InfoImpl::InfoImpl(System& system) : PluginImplBase(system)
 {
     _system_impl->register_plugin(this);
@@ -50,16 +54,17 @@ void InfoImpl::enable()
     _system_impl->add_call_every(
         [this]() { _system_impl->send_autopilot_version_request(); }, 1.0f, &_call_every_cookie);
 
-    // We're hoping to get flight information regularly to update flight time.
-    _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0);
+    if (!_flight_info_subscriptions.empty()) {
+        // We're hoping to get flight information regularly to update flight time.
+        _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0);
+    }
 }
 
 void InfoImpl::disable()
 {
     _system_impl->remove_call_every(_call_every_cookie);
 
     std::lock_guard<std::mutex> lock(_mutex);
-    _flight_information_received = false;
     _identification_received = false;
 }
 
@@ -158,7 +163,8 @@ void InfoImpl::process_flight_information(const mavlink_message_t& message)
         _flight_info.duration_since_takeoff_ms = 0;
     }
 
-    _flight_information_received = true;
+    _flight_info_subscriptions.queue(
+        _flight_info, [this](const auto& func) { _system_impl->call_user_callback(func); });
 }
 
 std::string InfoImpl::swap_and_translate_binary_to_str(uint8_t* binary, unsigned binary_len)
@@ -220,15 +226,23 @@ std::pair<Info::Result, Info::Product> InfoImpl::get_product() const
         _product);
 }
 
-std::pair<Info::Result, Info::FlightInfo> InfoImpl::get_flight_information() const
+std::pair<Info::Result, Info::FlightInfo> InfoImpl::get_flight_information()
 {
-    wait_for_information();
-    std::lock_guard<std::mutex> lock(_mutex);
-
-    return std::make_pair<>(
-        (_flight_information_received ? Info::Result::Success :
-                                        Info::Result::InformationNotReceivedYet),
-        _flight_info);
+    std::promise<std::pair<Info::Result, Info::FlightInfo>> prom;
+    auto fut = prom.get_future();
+    _system_impl->request_message().request(
+        MAVLINK_MSG_ID_FLIGHT_INFORMATION,
+        MAV_COMP_ID_AUTOPILOT1,
+        [&](MavlinkCommandSender::Result result, const mavlink_message_t& message) {
+            // This call might happen twice but that's ok.
+            process_flight_information(message);
+            if (result == MavlinkCommandSender::Result::Success) {
+                prom.set_value({Info::Result::Success, _flight_info});
+            } else {
+                prom.set_value({Info::Result::InformationNotReceivedYet, Info::FlightInfo{}});
+            }
+        });
+    return fut.get();
 }
 
 const std::string InfoImpl::vendor_id_str(uint16_t vendor_id)
@@ -292,8 +306,8 @@ std::pair<Info::Result, double> InfoImpl::get_speed_factor() const
 
 void InfoImpl::wait_for_identification() const
 {
-    // Wait 1.5 seconds max
-    for (unsigned i = 0; i < 150; ++i) {
+    // Wait 0.5 seconds max
+    for (unsigned i = 0; i < 50; ++i) {
         {
             std::lock_guard<std::mutex> lock(_mutex);
             if (_identification_received) {
@@ -304,18 +318,25 @@ void InfoImpl::wait_for_identification() const
     }
 }
 
-void InfoImpl::wait_for_information() const
+Info::FlightInformationHandle
+InfoImpl::subscribe_flight_information(const Info::FlightInformationCallback& callback)
 {
-    // Wait 1.5 seconds max
-    for (unsigned i = 0; i < 150; ++i) {
-        {
-            std::lock_guard<std::mutex> lock(_mutex);
-            if (_flight_information_received) {
-                break;
-            }
-        }
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    }
+    std::lock_guard<std::mutex> lock(_mutex);
+
+    // Make sure we get the message regularly.
+    _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0);
+
+    return _flight_info_subscriptions.subscribe(callback);
+}
+
+void InfoImpl::unsubscribe_flight_information(Info::FlightInformationHandle handle)
+{
+    std::lock_guard<std::mutex> lock(_mutex);
+
+    // Reset message to default
+    _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 0.0);
+
+    _flight_info_subscriptions.unsubscribe(handle);
 }
 
 } // namespace mavsdk

src/mavsdk/plugins/info/info_impl.h

@@ -6,6 +6,7 @@
 #include "plugins/info/info.h"
 #include "plugin_impl_base.h"
 #include "ringbuffer.h"
+#include "callback_list.h"
 
 namespace mavsdk {
 
@@ -24,9 +25,13 @@ class InfoImpl : public PluginImplBase {
     std::pair<Info::Result, Info::Identification> get_identification() const;
     std::pair<Info::Result, Info::Version> get_version() const;
     std::pair<Info::Result, Info::Product> get_product() const;
-    std::pair<Info::Result, Info::FlightInfo> get_flight_information() const;
+    std::pair<Info::Result, Info::FlightInfo> get_flight_information();
     std::pair<Info::Result, double> get_speed_factor() const;
 
+    Info::FlightInformationHandle
+    subscribe_flight_information(const Info::FlightInformationCallback& callback);
+    void unsubscribe_flight_information(Info::FlightInformationHandle handle);
+
     InfoImpl(const InfoImpl&) = delete;
     InfoImpl& operator=(const InfoImpl&) = delete;
 
@@ -40,7 +45,6 @@ class InfoImpl : public PluginImplBase {
         get_flight_software_version_type(FIRMWARE_VERSION_TYPE);
 
     void wait_for_identification() const;
-    void wait_for_information() const;
 
     mutable std::mutex _mutex{};
 
@@ -50,7 +54,6 @@ class InfoImpl : public PluginImplBase {
     bool _identification_received{false};
 
     Info::FlightInfo _flight_info{};
-    bool _flight_information_received{false};
 
     void* _call_every_cookie{nullptr};
 
@@ -71,6 +74,8 @@ class InfoImpl : public PluginImplBase {
     SteadyTimePoint _last_time_attitude_arrived{};
     uint32_t _last_time_boot_ms{0};
 
+    CallbackList<Info::FlightInfo> _flight_info_subscriptions{};
+
     static const std::string vendor_id_str(uint16_t vendor_id);
     static const std::string product_id_str(uint16_t product_id);
 

src/mavsdk/plugins/info/mocks/info_mock.h

@@ -12,6 +12,10 @@ class MockInfo {
     MOCK_CONST_METHOD0(get_product, std::pair<Info::Result, Info::Product>()){};
     MOCK_CONST_METHOD0(get_version, std::pair<Info::Result, Info::Version>()){};
     MOCK_CONST_METHOD0(get_speed_factor, std::pair<Info::Result, double>()){};
+    MOCK_CONST_METHOD1(
+        subscribe_flight_information,
+        Info::FlightInformationHandle(Info::FlightInformationCallback)){};
+    MOCK_CONST_METHOD1(unsubscribe_flight_information, void(Info::FlightInformationHandle)){};
 };
 
 } // namespace testing