Add StatusText to Telemetry (#733)

Add StatusText to Telemetry

backend/src/plugins/telemetry/telemetry_service_impl.h

@@ -96,6 +96,43 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet
         return grpc::Status::OK;
     }
 
+    grpc::Status SubscribeStatusText(
+        grpc::ServerContext * /* context */,
+        const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */,
+        grpc::ServerWriter<rpc::telemetry::StatusTextResponse> *writer) override
+    {
+        _telemetry.status_text_async(
+            [this, &writer](dronecode_sdk::Telemetry::StatusText status_text) {
+                auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText();
+                rpc_status_text->set_text(status_text.text);
+                rpc_status_text->set_type(translateStatusTextType(status_text.type));
+
+                dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response;
+                rpc_status_text_response.set_allocated_status_text(rpc_status_text);
+                writer->Write(rpc_status_text_response);
+            });
+
+        _stop_future.wait();
+        return grpc::Status::OK;
+    }
+
+    dronecode_sdk::rpc::telemetry::StatusText::StatusType
+    translateStatusTextType(const dronecode_sdk::Telemetry::StatusText::StatusType type) const
+    {
+        switch (type) {
+            default:
+            case dronecode_sdk::Telemetry::StatusText::StatusType::INFO:
+                return dronecode_sdk::rpc::telemetry::StatusText::StatusType::
+                    StatusText_StatusType_INFO;
+            case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING:
+                return dronecode_sdk::rpc::telemetry::StatusText::StatusType::
+                    StatusText_StatusType_WARNING;
+            case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL:
+                return dronecode_sdk::rpc::telemetry::StatusText::StatusType::
+                    StatusText_StatusType_CRITICAL;
+        }
+    }
+
     grpc::Status
     SubscribeArmed(grpc::ServerContext * /* context */,
                    const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */,

plugins/telemetry/include/plugins/telemetry/telemetry.h

@@ -129,6 +129,25 @@ class Telemetry : public PluginBase {
                                      5: RTK float, 6: RTK fixed). */
     };
 
+    /**
+     * @brief Status Text information type.
+     */
+    struct StatusText {
+        /**
+         * @brief Status Types.
+         *
+         * @note PX4 only supports these 3 status types.
+         * If other status types are returned for some reason,
+         * they will be marked as INFO type and logged as a warning.
+         */
+        enum class StatusType {
+            INFO, /**< @brief Message type is an information or other. */
+            WARNING, /**< @brief Message type is a warning. */
+            CRITICAL /**< @brief Message type is critical. */
+        } type; /**< @brief Message type. */
+        std::string text; /**< @brief Mavlink status message. */
+    };
+
     /**
      * @brief Battery type.
      */
@@ -396,6 +415,13 @@ class Telemetry : public PluginBase {
      */
     Position home_position() const;
 
+    /**
+     * @brief Get status text (synchronous).
+     *
+     * @return Status text.
+     */
+    StatusText status_text() const;
+
     /**
      * @brief Get the in-air status (synchronous).
      *
@@ -527,13 +553,27 @@ class Telemetry : public PluginBase {
      */
     typedef std::function<void(bool in_air)> in_air_callback_t;
 
+    /**
+     * @brief Callback for mavlink status text updates.
+     *
+     * @param status text with message type and text.
+     */
+    typedef std::function<void(StatusText status_text)> status_text_callback_t;
+
     /**
      * @brief Subscribe to in-air updates (asynchronous).
      *
      * @param callback Function to call with updates.
      */
     void in_air_async(in_air_callback_t callback);
 
+    /**
+     * @brief Subscribe to status text updates (asynchronous).
+     *
+     * @param callback Function to call with updates.
+     */
+    void status_text_async(status_text_callback_t callback);
+
     /**
      * @brief Callback type for armed updates (asynchronous).
      *
@@ -853,4 +893,11 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs);
  */
 std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status);
 
+/**
+ * @brief Stream operator to print information about a `Telemetry::StatusText`.
+ *
+ * @returns A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text);
+
 } // namespace dronecode_sdk

plugins/telemetry/mocks/telemetry_mock.h

@@ -11,6 +11,7 @@ class MockTelemetry {
     MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){};
     MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){};
     MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){};
+    MOCK_CONST_METHOD1(status_text_async, void(Telemetry::status_text_callback_t)){};
     MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){};
     MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){};
     MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){};

plugins/telemetry/telemetry.cpp

@@ -131,6 +131,11 @@ bool Telemetry::in_air() const
     return _impl->in_air();
 }
 
+Telemetry::StatusText Telemetry::status_text() const
+{
+    return _impl->get_status_text();
+}
+
 bool Telemetry::armed() const
 {
     return _impl->armed();
@@ -211,6 +216,11 @@ void Telemetry::in_air_async(in_air_callback_t callback)
     return _impl->in_air_async(callback);
 }
 
+void Telemetry::status_text_async(status_text_callback_t callback)
+{
+    return _impl->status_text_async(callback);
+}
+
 void Telemetry::armed_async(armed_callback_t callback)
 {
     return _impl->armed_async(callback);
@@ -472,4 +482,14 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status
                << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]";
 }
 
+bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs)
+{
+    return lhs.text == rhs.text && lhs.type == rhs.type;
+}
+
+std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text)
+{
+    return str << "[statustext: " << status_text.text << "]";
+}
+
 } // namespace dronecode_sdk

plugins/telemetry/telemetry_impl.cpp

@@ -5,6 +5,7 @@
 #include "px4_custom_mode.h"
 #include <cmath>
 #include <functional>
+#include <string>
 
 namespace dronecode_sdk {
 
@@ -61,6 +62,9 @@ void TelemetryImpl::init()
     _parent->register_mavlink_message_handler(
         MAVLINK_MSG_ID_HEARTBEAT, std::bind(&TelemetryImpl::process_heartbeat, this, _1), this);
 
+    _parent->register_mavlink_message_handler(
+        MAVLINK_MSG_ID_STATUSTEXT, std::bind(&TelemetryImpl::process_statustext, this, _1), this);
+
     _parent->register_mavlink_message_handler(
         MAVLINK_MSG_ID_RC_CHANNELS, std::bind(&TelemetryImpl::process_rc_channels, this, _1), this);
 
@@ -489,6 +493,43 @@ void TelemetryImpl::process_heartbeat(const mavlink_message_t &message)
     }
 }
 
+void TelemetryImpl::process_statustext(const mavlink_message_t &message)
+{
+    mavlink_statustext_t statustext;
+    mavlink_msg_statustext_decode(&message, &statustext);
+
+    Telemetry::StatusText::StatusType type;
+
+    switch (statustext.severity) {
+        case MAV_SEVERITY_WARNING:
+            type = Telemetry::StatusText::StatusType::WARNING;
+            break;
+        case MAV_SEVERITY_CRITICAL:
+            type = Telemetry::StatusText::StatusType::CRITICAL;
+            break;
+        case MAV_SEVERITY_INFO:
+            type = Telemetry::StatusText::StatusType::INFO;
+            break;
+        default:
+            LogWarn() << "Unknown StatusText severity";
+            type = Telemetry::StatusText::StatusType::INFO;
+            break;
+    }
+
+    // statustext.text is not null terminated, therefore we copy it first to
+    // an array big enough that is zeroed.
+    char text_with_null[sizeof(statustext.text) + 1]{};
+    memcpy(text_with_null, statustext.text, sizeof(statustext.text));
+
+    const std::string text = text_with_null;
+
+    set_status_text({type, text});
+
+    if (_status_text_subscription) {
+        _status_text_subscription(get_status_text());
+    }
+}
+
 void TelemetryImpl::process_rc_channels(const mavlink_message_t &message)
 {
     mavlink_rc_channels_t rc_channels;
@@ -659,6 +700,18 @@ void TelemetryImpl::set_in_air(bool in_air_new)
     _in_air = in_air_new;
 }
 
+void TelemetryImpl::set_status_text(Telemetry::StatusText status_text)
+{
+    std::lock_guard<std::mutex> lock(_status_text_mutex);
+    _status_text = status_text;
+}
+
+Telemetry::StatusText TelemetryImpl::get_status_text() const
+{
+    std::lock_guard<std::mutex> lock(_status_text_mutex);
+    return _status_text;
+}
+
 void TelemetryImpl::set_armed(bool armed_new)
 {
     _armed = armed_new;
@@ -854,6 +907,11 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback)
     _in_air_subscription = callback;
 }
 
+void TelemetryImpl::status_text_async(Telemetry::status_text_callback_t &callback)
+{
+    _status_text_subscription = callback;
+}
+
 void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback)
 {
     _armed_subscription = callback;

plugins/telemetry/telemetry_impl.h

@@ -54,6 +54,7 @@ class TelemetryImpl : public PluginImplBase {
     Telemetry::Position get_home_position() const;
     bool in_air() const;
     bool armed() const;
+    Telemetry::StatusText get_status_text() const;
     Telemetry::EulerAngle get_attitude_euler_angle() const;
     Telemetry::Quaternion get_attitude_quaternion() const;
     Telemetry::EulerAngle get_camera_attitude_euler_angle() const;
@@ -70,6 +71,7 @@ class TelemetryImpl : public PluginImplBase {
     void position_async(Telemetry::position_callback_t &callback);
     void home_position_async(Telemetry::position_callback_t &callback);
     void in_air_async(Telemetry::in_air_callback_t &callback);
+    void status_text_async(Telemetry::status_text_callback_t &callback);
     void armed_async(Telemetry::armed_callback_t &callback);
     void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback);
     void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback);
@@ -91,6 +93,7 @@ class TelemetryImpl : public PluginImplBase {
     void set_position(Telemetry::Position position);
     void set_home_position(Telemetry::Position home_position);
     void set_in_air(bool in_air);
+    void set_status_text(Telemetry::StatusText status_text);
     void set_armed(bool armed);
     void set_attitude_quaternion(Telemetry::Quaternion quaternion);
     void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle);
@@ -116,6 +119,7 @@ class TelemetryImpl : public PluginImplBase {
     void process_extended_sys_state(const mavlink_message_t &message);
     void process_sys_status(const mavlink_message_t &message);
     void process_heartbeat(const mavlink_message_t &message);
+    void process_statustext(const mavlink_message_t &message);
     void process_rc_channels(const mavlink_message_t &message);
 
     void receive_param_cal_gyro(MAVLinkParameters::Result result, int value);
@@ -154,6 +158,9 @@ class TelemetryImpl : public PluginImplBase {
     std::atomic_bool _in_air{false};
     std::atomic_bool _armed{false};
 
+    mutable std::mutex _status_text_mutex{};
+    Telemetry::StatusText _status_text{Telemetry::StatusText::StatusType::INFO, ""};
+
     mutable std::mutex _attitude_quaternion_mutex{};
     Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN};
 
@@ -184,6 +191,7 @@ class TelemetryImpl : public PluginImplBase {
     Telemetry::position_callback_t _position_subscription{nullptr};
     Telemetry::position_callback_t _home_position_subscription{nullptr};
     Telemetry::in_air_callback_t _in_air_subscription{nullptr};
+    Telemetry::status_text_callback_t _status_text_subscription{nullptr};
     Telemetry::armed_callback_t _armed_subscription{nullptr};
     Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr};
     Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr};
@@ -204,5 +212,4 @@ class TelemetryImpl : public PluginImplBase {
 
     void *_timeout_cookie{nullptr};
 };
-
 } // namespace dronecode_sdk