Telemetry and TelemetryServer improvements

src/integration_tests/telemetry_async.cpp

@@ -264,7 +264,10 @@ void print_angular_velocity_body(Telemetry::AngularVelocityBody angular_velocity
 void print_fixedwing_metrics(Telemetry::FixedwingMetrics fixedwing_metrics)
 {
     std::cout << "async Airspeed: " << fixedwing_metrics.airspeed_m_s << " m/s, "
+              << "Groundspeed: " << fixedwing_metrics.groundspeed_m_s << " m/s, "
+              << "Heading: " << fixedwing_metrics.heading_deg << " deg, "
               << "Throttle: " << fixedwing_metrics.throttle_percentage << " %, "
+              << "Altitude: " << fixedwing_metrics.absolute_altitude_m << " m (MSL), "
               << "Climb: " << fixedwing_metrics.climb_rate_m_s << " m/s" << '\n';
     _received_fixedwing_metrics = true;
 }

src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h

@@ -813,6 +813,10 @@ class Telemetry : public PluginBase {
             float(NAN)}; /**< @brief Current indicated airspeed (IAS) in metres per second */
         float throttle_percentage{float(NAN)}; /**< @brief Current throttle setting (0 to 100) */
         float climb_rate_m_s{float(NAN)}; /**< @brief Current climb rate in metres per second */
+        float groundspeed_m_s{float(NAN)}; /**< @brief Current groundspeed metres per second */
+        float heading_deg{
+            float(NAN)}; /**< @brief Current heading in compass units (0-360, 0=north) */
+        float absolute_altitude_m{float(NAN)}; /**< @brief Current altitude in metres (MSL) */
     };
 
     /**

src/mavsdk/plugins/telemetry/telemetry.cpp

@@ -1322,7 +1322,13 @@ bool operator==(const Telemetry::FixedwingMetrics& lhs, const Telemetry::Fixedwi
            ((std::isnan(rhs.throttle_percentage) && std::isnan(lhs.throttle_percentage)) ||
             rhs.throttle_percentage == lhs.throttle_percentage) &&
            ((std::isnan(rhs.climb_rate_m_s) && std::isnan(lhs.climb_rate_m_s)) ||
-            rhs.climb_rate_m_s == lhs.climb_rate_m_s);
+            rhs.climb_rate_m_s == lhs.climb_rate_m_s) &&
+           ((std::isnan(rhs.groundspeed_m_s) && std::isnan(lhs.groundspeed_m_s)) ||
+            rhs.groundspeed_m_s == lhs.groundspeed_m_s) &&
+           ((std::isnan(rhs.heading_deg) && std::isnan(lhs.heading_deg)) ||
+            rhs.heading_deg == lhs.heading_deg) &&
+           ((std::isnan(rhs.absolute_altitude_m) && std::isnan(lhs.absolute_altitude_m)) ||
+            rhs.absolute_altitude_m == lhs.absolute_altitude_m);
 }
 
 std::ostream& operator<<(std::ostream& str, Telemetry::FixedwingMetrics const& fixedwing_metrics)
@@ -1332,6 +1338,9 @@ std::ostream& operator<<(std::ostream& str, Telemetry::FixedwingMetrics const& f
     str << "    airspeed_m_s: " << fixedwing_metrics.airspeed_m_s << '\n';
     str << "    throttle_percentage: " << fixedwing_metrics.throttle_percentage << '\n';
     str << "    climb_rate_m_s: " << fixedwing_metrics.climb_rate_m_s << '\n';
+    str << "    groundspeed_m_s: " << fixedwing_metrics.groundspeed_m_s << '\n';
+    str << "    heading_deg: " << fixedwing_metrics.heading_deg << '\n';
+    str << "    absolute_altitude_m: " << fixedwing_metrics.absolute_altitude_m << '\n';
     str << '}';
     return str;
 }

src/mavsdk/plugins/telemetry/telemetry_impl.cpp

@@ -994,7 +994,10 @@ void TelemetryImpl::process_fixedwing_metrics(const mavlink_message_t& message)
 
     Telemetry::FixedwingMetrics new_fixedwing_metrics;
     new_fixedwing_metrics.airspeed_m_s = vfr_hud.airspeed;
+    new_fixedwing_metrics.groundspeed_m_s = vfr_hud.groundspeed;
+    new_fixedwing_metrics.heading_deg = vfr_hud.heading;
     new_fixedwing_metrics.throttle_percentage = vfr_hud.throttle * 1e-2f;
+    new_fixedwing_metrics.absolute_altitude_m = vfr_hud.alt;
     new_fixedwing_metrics.climb_rate_m_s = vfr_hud.climb;
 
     set_fixedwing_metrics(new_fixedwing_metrics);

src/mavsdk/plugins/telemetry_server/include/plugins/telemetry_server/telemetry_server.h

@@ -757,6 +757,10 @@ class TelemetryServer : public ServerPluginBase {
             float(NAN)}; /**< @brief Current indicated airspeed (IAS) in metres per second */
         float throttle_percentage{float(NAN)}; /**< @brief Current throttle setting (0 to 100) */
         float climb_rate_m_s{float(NAN)}; /**< @brief Current climb rate in metres per second */
+        float groundspeed_m_s{float(NAN)}; /**< @brief Current groundspeed metres per second */
+        float heading_deg{
+            float(NAN)}; /**< @brief Current heading in compass units (0-360, 0=north) */
+        float absolute_altitude_m{float(NAN)}; /**< @brief Current altitude in metres (MSL) */
     };
 
     /**
@@ -1052,6 +1056,24 @@ class TelemetryServer : public ServerPluginBase {
      */
     Result publish_distance_sensor(DistanceSensor distance_sensor) const;
 
+    /**
+     * @brief Publish to "attitude" updates.
+     *
+     * This function is blocking.
+     *
+     * @return Result of request.
+     */
+    Result publish_attitude(EulerAngle angle, AngularVelocityBody angular_velocity) const;
+
+    /**
+     * @brief Publish to "Visual Flight Rules HUD" updates.
+     *
+     * This function is blocking.
+     *
+     * @return Result of request.
+     */
+    Result publish_visual_flight_rules_hud(FixedwingMetrics fixed_wing_metrics) const;
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/telemetry_server/telemetry_server.cpp

@@ -131,6 +131,18 @@ TelemetryServer::publish_distance_sensor(DistanceSensor distance_sensor) const
     return _impl->publish_distance_sensor(distance_sensor);
 }
 
+TelemetryServer::Result
+TelemetryServer::publish_attitude(EulerAngle angle, AngularVelocityBody angular_velocity) const
+{
+    return _impl->publish_attitude(angle, angular_velocity);
+}
+
+TelemetryServer::Result
+TelemetryServer::publish_visual_flight_rules_hud(FixedwingMetrics fixed_wing_metrics) const
+{
+    return _impl->publish_visual_flight_rules_hud(fixed_wing_metrics);
+}
+
 bool operator==(const TelemetryServer::Position& lhs, const TelemetryServer::Position& rhs)
 {
     return ((std::isnan(rhs.latitude_deg) && std::isnan(lhs.latitude_deg)) ||
@@ -639,7 +651,13 @@ bool operator==(
            ((std::isnan(rhs.throttle_percentage) && std::isnan(lhs.throttle_percentage)) ||
             rhs.throttle_percentage == lhs.throttle_percentage) &&
            ((std::isnan(rhs.climb_rate_m_s) && std::isnan(lhs.climb_rate_m_s)) ||
-            rhs.climb_rate_m_s == lhs.climb_rate_m_s);
+            rhs.climb_rate_m_s == lhs.climb_rate_m_s) &&
+           ((std::isnan(rhs.groundspeed_m_s) && std::isnan(lhs.groundspeed_m_s)) ||
+            rhs.groundspeed_m_s == lhs.groundspeed_m_s) &&
+           ((std::isnan(rhs.heading_deg) && std::isnan(lhs.heading_deg)) ||
+            rhs.heading_deg == lhs.heading_deg) &&
+           ((std::isnan(rhs.absolute_altitude_m) && std::isnan(lhs.absolute_altitude_m)) ||
+            rhs.absolute_altitude_m == lhs.absolute_altitude_m);
 }
 
 std::ostream&
@@ -650,6 +668,9 @@ operator<<(std::ostream& str, TelemetryServer::FixedwingMetrics const& fixedwing
     str << "    airspeed_m_s: " << fixedwing_metrics.airspeed_m_s << '\n';
     str << "    throttle_percentage: " << fixedwing_metrics.throttle_percentage << '\n';
     str << "    climb_rate_m_s: " << fixedwing_metrics.climb_rate_m_s << '\n';
+    str << "    groundspeed_m_s: " << fixedwing_metrics.groundspeed_m_s << '\n';
+    str << "    heading_deg: " << fixedwing_metrics.heading_deg << '\n';
+    str << "    absolute_altitude_m: " << fixedwing_metrics.absolute_altitude_m << '\n';
     str << '}';
     return str;
 }

src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp

@@ -524,4 +524,51 @@ TelemetryServer::Result TelemetryServerImpl::publish_extended_sys_state(
                TelemetryServer::Result::Unsupported;
 }
 
+TelemetryServer::Result TelemetryServerImpl::publish_attitude(
+    TelemetryServer::EulerAngle attitude, TelemetryServer::AngularVelocityBody angular_velocity)
+{
+    return _server_component_impl->queue_message(
+               [&](MavlinkAddress mavlink_address, uint8_t channel) {
+                   mavlink_message_t message;
+                   mavlink_msg_attitude_pack_chan(
+                       mavlink_address.system_id,
+                       mavlink_address.component_id,
+                       channel,
+                       &message,
+                       static_cast<uint32_t>(attitude.timestamp_us / 1000.F),
+                       attitude.roll_deg * M_PI / 180.F,
+                       attitude.pitch_deg * M_PI / 180.F,
+                       attitude.yaw_deg * M_PI / 180.F,
+                       angular_velocity.roll_rad_s,
+                       angular_velocity.pitch_rad_s,
+                       angular_velocity.yaw_rad_s);
+                   return message;
+               }) ?
+               TelemetryServer::Result::Success :
+               TelemetryServer::Result::Unsupported;
+}
+
+TelemetryServer::Result TelemetryServerImpl::publish_visual_flight_rules_hud(
+    TelemetryServer::FixedwingMetrics fixed_wing_metrics)
+{
+    return _server_component_impl->queue_message(
+               [&](MavlinkAddress mavlink_address, uint8_t channel) {
+                   mavlink_message_t message;
+                   mavlink_msg_vfr_hud_pack_chan(
+                       mavlink_address.system_id,
+                       mavlink_address.component_id,
+                       channel,
+                       &message,
+                       fixed_wing_metrics.airspeed_m_s,
+                       fixed_wing_metrics.groundspeed_m_s,
+                       static_cast<uint16_t>(std::round(fixed_wing_metrics.heading_deg)),
+                       static_cast<uint16_t>(std::round(fixed_wing_metrics.throttle_percentage)),
+                       fixed_wing_metrics.absolute_altitude_m,
+                       fixed_wing_metrics.climb_rate_m_s);
+                   return message;
+               }) ?
+               TelemetryServer::Result::Success :
+               TelemetryServer::Result::Unsupported;
+}
+
 } // namespace mavsdk

src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h

@@ -76,6 +76,13 @@ class TelemetryServerImpl : public ServerPluginImplBase {
     TelemetryServer::Result publish_extended_sys_state(
         TelemetryServer::VtolState vtol_state, TelemetryServer::LandedState landed_state);
 
+    TelemetryServer::Result publish_attitude(
+        TelemetryServer::EulerAngle attitude,
+        TelemetryServer::AngularVelocityBody angular_velocity);
+
+    TelemetryServer::Result
+    publish_visual_flight_rules_hud(TelemetryServer::FixedwingMetrics fixed_wing_metrics);
+
 private:
     bool _send_home();