Merge pull request #1920 from CY-1992/add-altitude-to-telemetry

Added altitude to telemetry plugin

src/integration_tests/telemetry_async.cpp

@@ -32,6 +32,7 @@ static void print_position_velocity_ned(Telemetry::PositionVelocityNed position_
 static void print_unix_epoch_time_us(uint64_t time_us);
 static void print_actuator_control_target(Telemetry::ActuatorControlTarget actuator_control_target);
 static void print_actuator_output_status(Telemetry::ActuatorOutputStatus actuator_output_status);
+static void print_altitude(Telemetry::Altitude altitude);
 
 static bool _set_rate_error = false;
 static bool _received_position = false;
@@ -55,6 +56,7 @@ static bool _received_rc_status = false;
 static bool _received_position_velocity_ned = false;
 static bool _received_actuator_control_target = false;
 static bool _received_actuator_output_status = false;
+static bool _received_altitude = false;
 
 TEST_F(SitlTest, PX4TelemetryAsync)
 {
@@ -119,6 +121,10 @@ TEST_F(SitlTest, PX4TelemetryAsync)
 
     telemetry->set_rate_ground_truth_async(
         10.0, [](Telemetry::Result result) { return receive_result(result); });
+
+    telemetry->set_rate_altitude_async(
+        10.0, [](Telemetry::Result result) { return receive_result(result); });
+
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
     telemetry->subscribe_position([](Telemetry::Position position) { print_position(position); });
@@ -186,6 +192,8 @@ TEST_F(SitlTest, PX4TelemetryAsync)
             print_actuator_output_status(actuator_output_status);
         });
 
+    telemetry->subscribe_altitude([](Telemetry::Altitude altitude) { print_altitude(altitude); });
+
     std::this_thread::sleep_for(std::chrono::seconds(10));
 
     EXPECT_FALSE(_set_rate_error);
@@ -206,6 +214,7 @@ TEST_F(SitlTest, PX4TelemetryAsync)
     EXPECT_TRUE(_received_imu);
     EXPECT_TRUE(_received_gps_info);
     EXPECT_TRUE(_received_battery);
+    EXPECT_TRUE(_received_altitude);
     // EXPECT_TRUE(_received_rc_status); // No RC is sent in SITL.
     EXPECT_TRUE(_received_position_velocity_ned);
     // EXPECT_TRUE(_received_actuator_control_target); TODO check is that sent in SITL.
@@ -355,6 +364,17 @@ void print_position_velocity_ned(Telemetry::PositionVelocityNed position_velocit
     _received_position_velocity_ned = true;
 }
 
+void print_altitude(Telemetry::Altitude altitude)
+{
+    std::cout << "altitude_monotonic: " << altitude.altitude_monotonic_m << "m, "
+              << "altitude_local" << altitude.altitude_local_m << "m, "
+              << "altitude_amsl" << altitude.altitude_amsl_m << "m, "
+              << "altitude_relative" << altitude.altitude_relative_m << "m, "
+              << "altitude_terrain" << altitude.altitude_terrain_m << "m" << std::endl;
+
+    _received_altitude = true;
+}
+
 void print_unix_epoch_time_us(uint64_t time_us)
 {
     std::time_t time = time_us / 1000000;

src/integration_tests/telemetry_sync.cpp

@@ -30,6 +30,7 @@ TEST_F(SitlTest, PX4TelemetrySync)
     EXPECT_EQ(telemetry->set_rate_actuator_control_target(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_fixedwing_metrics(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_ground_truth(10.0), Telemetry::Result::Success);
+    EXPECT_EQ(telemetry->set_rate_altitude(10.0), Telemetry::Result::Success);
 
     for (unsigned i = 0; i < 10; ++i) {
         std::cout << "Position: " << telemetry->position() << '\n';
@@ -46,6 +47,7 @@ TEST_F(SitlTest, PX4TelemetrySync)
         std::cout << "Flight mode: " << telemetry->flight_mode() << '\n';
         std::cout << "Landed state: " << telemetry->landed_state()
                   << "(in air: " << telemetry->in_air() << ")" << '\n';
+        std::cout << "Altitude: " << telemetry->altitude() << '\n';
 
         std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }

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

@@ -957,6 +957,33 @@ class Telemetry : public PluginBase {
     friend std::ostream&
     operator<<(std::ostream& str, Telemetry::GpsGlobalOrigin const& gps_global_origin);
 
+    /**
+     * @brief Altitude message type
+     */
+    struct Altitude {
+        float altitude_monotonic_m{float(
+            NAN)}; /**< @brief Altitude in meters is initialized on system boot and monotonic */
+        float altitude_amsl_m{
+            float(NAN)}; /**< @brief  Altitude AMSL (above mean sea level) in meters */
+        float altitude_local_m{float(NAN)}; /**< @brief Local altitude in meters */
+        float altitude_relative_m{float(NAN)}; /**< @brief Altitude above home position in meters */
+        float altitude_terrain_m{float(NAN)}; /**< @brief Altitude above terrain in meters */
+    };
+
+    /**
+     * @brief Equal operator to compare two `Telemetry::Altitude` objects.
+     *
+     * @return `true` if items are equal.
+     */
+    friend bool operator==(const Telemetry::Altitude& lhs, const Telemetry::Altitude& rhs);
+
+    /**
+     * @brief Stream operator to print information about a `Telemetry::Altitude`.
+     *
+     * @return A reference to the stream.
+     */
+    friend std::ostream& operator<<(std::ostream& str, Telemetry::Altitude const& altitude);
+
     /**
      * @brief Possible results returned for telemetry requests.
      */
@@ -1881,6 +1908,33 @@ class Telemetry : public PluginBase {
      */
     Heading heading() const;
 
+    /**
+     * @brief Callback type for subscribe_altitude.
+     */
+    using AltitudeCallback = std::function<void(Altitude)>;
+
+    /**
+     * @brief Handle type for subscribe_altitude.
+     */
+    using AltitudeHandle = Handle<Altitude>;
+
+    /**
+     * @brief Subscribe to 'Altitude' updates.
+     */
+    AltitudeHandle subscribe_altitude(const AltitudeCallback& callback);
+
+    /**
+     * @brief Unsubscribe from subscribe_altitude
+     */
+    void unsubscribe_altitude(AltitudeHandle handle);
+
+    /**
+     * @brief Poll for 'Altitude' (blocking).
+     *
+     * @return One Altitude update.
+     */
+    Altitude altitude() const;
+
     /**
      * @brief Set rate to 'position' updates.
      *
@@ -2265,6 +2319,22 @@ class Telemetry : public PluginBase {
      */
     Result set_rate_distance_sensor(double rate_hz) const;
 
+    /**
+     * @brief Set rate to 'Altitude' updates.
+     *
+     * This function is non-blocking. See 'set_rate_altitude' for the blocking counterpart.
+     */
+    void set_rate_altitude_async(double rate_hz, const ResultCallback callback);
+
+    /**
+     * @brief Set rate to 'Altitude' updates.
+     *
+     * This function is blocking. See 'set_rate_altitude_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    Result set_rate_altitude(double rate_hz) const;
+
     /**
      * @brief Callback type for get_gps_global_origin_async.
      */

src/mavsdk/plugins/telemetry/mocks/telemetry_mock.h

@@ -116,6 +116,8 @@ class MockTelemetry {
     MOCK_CONST_METHOD1(
         subscribe_vtol_state, Telemetry::VtolStateHandle(Telemetry::VtolStateCallback)){};
     MOCK_CONST_METHOD1(unsubscribe_vtol_state, void(Telemetry::VtolStateHandle)){};
+    MOCK_CONST_METHOD1(subscribe_altitude, Telemetry::AltitudeHandle(Telemetry::AltitudeCallback));
+    MOCK_CONST_METHOD1(unsubscribe_altitude, void(Telemetry::AltitudeHandle));
 
     MOCK_METHOD1(set_rate_position, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_home, Telemetry::Result(double)){};
@@ -142,6 +144,7 @@ class MockTelemetry {
     MOCK_METHOD1(set_rate_raw_imu, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_unix_epoch_time, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_vtol_state, Telemetry::Result(double)){};
+    MOCK_METHOD1(set_rate_altitude, Telemetry::Result(double)){};
 
     MOCK_CONST_METHOD2(set_rate_position_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_home_async, void(double, Telemetry::ResultCallback)){};
@@ -171,7 +174,7 @@ class MockTelemetry {
     MOCK_CONST_METHOD2(set_rate_scaled_imu_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_raw_imu_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_unix_epoch_time_async, void(double, Telemetry::ResultCallback)){};
-
+    MOCK_CONST_METHOD2(set_rate_altitude_async, void(double, Telemetry::ResultCallback)){};
     MOCK_METHOD0(
         get_gps_global_origin, std::pair<Telemetry::Result, Telemetry::GpsGlobalOrigin>()){};
     MOCK_METHOD1(get_gps_global_origin_async, void(Telemetry::GetGpsGlobalOriginCallback)){};

src/mavsdk/plugins/telemetry/telemetry.cpp

@@ -38,6 +38,7 @@ using AngularVelocityFrd = Telemetry::AngularVelocityFrd;
 using MagneticFieldFrd = Telemetry::MagneticFieldFrd;
 using Imu = Telemetry::Imu;
 using GpsGlobalOrigin = Telemetry::GpsGlobalOrigin;
+using Altitude = Telemetry::Altitude;
 
 Telemetry::Telemetry(System& system) : PluginBase(), _impl{std::make_unique<TelemetryImpl>(system)}
 {}
@@ -556,6 +557,21 @@ Telemetry::Heading Telemetry::heading() const
     return _impl->heading();
 }
 
+Telemetry::AltitudeHandle Telemetry::subscribe_altitude(const AltitudeCallback& callback)
+{
+    return _impl->subscribe_altitude(callback);
+}
+
+void Telemetry::unsubscribe_altitude(AltitudeHandle handle)
+{
+    _impl->unsubscribe_altitude(handle);
+}
+
+Telemetry::Altitude Telemetry::altitude() const
+{
+    return _impl->altitude();
+}
+
 void Telemetry::set_rate_position_async(double rate_hz, const ResultCallback callback)
 {
     _impl->set_rate_position_async(rate_hz, callback);
@@ -787,6 +803,16 @@ Telemetry::Result Telemetry::set_rate_distance_sensor(double rate_hz) const
     return _impl->set_rate_distance_sensor(rate_hz);
 }
 
+void Telemetry::set_rate_altitude_async(double rate_hz, const ResultCallback callback)
+{
+    _impl->set_rate_altitude_async(rate_hz, callback);
+}
+
+Telemetry::Result Telemetry::set_rate_altitude(double rate_hz) const
+{
+    return _impl->set_rate_altitude(rate_hz);
+}
+
 void Telemetry::get_gps_global_origin_async(const GetGpsGlobalOriginCallback callback)
 {
     _impl->get_gps_global_origin_async(callback);
@@ -1449,6 +1475,33 @@ std::ostream& operator<<(std::ostream& str, Telemetry::GpsGlobalOrigin const& gp
     return str;
 }
 
+bool operator==(const Telemetry::Altitude& lhs, const Telemetry::Altitude& rhs)
+{
+    return ((std::isnan(rhs.altitude_monotonic_m) && std::isnan(lhs.altitude_monotonic_m)) ||
+            rhs.altitude_monotonic_m == lhs.altitude_monotonic_m) &&
+           ((std::isnan(rhs.altitude_amsl_m) && std::isnan(lhs.altitude_amsl_m)) ||
+            rhs.altitude_amsl_m == lhs.altitude_amsl_m) &&
+           ((std::isnan(rhs.altitude_local_m) && std::isnan(lhs.altitude_local_m)) ||
+            rhs.altitude_local_m == lhs.altitude_local_m) &&
+           ((std::isnan(rhs.altitude_relative_m) && std::isnan(lhs.altitude_relative_m)) ||
+            rhs.altitude_relative_m == lhs.altitude_relative_m) &&
+           ((std::isnan(rhs.altitude_terrain_m) && std::isnan(lhs.altitude_terrain_m)) ||
+            rhs.altitude_terrain_m == lhs.altitude_terrain_m);
+}
+
+std::ostream& operator<<(std::ostream& str, Telemetry::Altitude const& altitude)
+{
+    str << std::setprecision(15);
+    str << "altitude:" << '\n' << "{\n";
+    str << "    altitude_monotonic_m: " << altitude.altitude_monotonic_m << '\n';
+    str << "    altitude_amsl_m: " << altitude.altitude_amsl_m << '\n';
+    str << "    altitude_local_m: " << altitude.altitude_local_m << '\n';
+    str << "    altitude_relative_m: " << altitude.altitude_relative_m << '\n';
+    str << "    altitude_terrain_m: " << altitude.altitude_terrain_m << '\n';
+    str << '}';
+    return str;
+}
+
 std::ostream& operator<<(std::ostream& str, Telemetry::Result const& result)
 {
     switch (result) {

src/mavsdk/plugins/telemetry/telemetry_impl.cpp

@@ -39,6 +39,7 @@ template class CallbackList<Telemetry::Odometry>;
 template class CallbackList<Telemetry::DistanceSensor>;
 template class CallbackList<Telemetry::ScaledPressure>;
 template class CallbackList<Telemetry::Heading>;
+template class CallbackList<Telemetry::Altitude>;
 
 TelemetryImpl::TelemetryImpl(System& system) : PluginImplBase(system)
 {
@@ -179,6 +180,11 @@ void TelemetryImpl::init()
         [this](const mavlink_message_t& message) { process_ground_truth(message); },
         this);
 
+    _parent->register_mavlink_message_handler(
+        MAVLINK_MSG_ID_ALTITUDE,
+        [this](const mavlink_message_t& message) { process_altitude(message); },
+        this);
+
     _parent->register_param_changed_handler(
         [this](const std::string& name) { process_parameter_update(name); }, this);
 
@@ -395,6 +401,12 @@ Telemetry::Result TelemetryImpl::set_rate_unix_epoch_time(double rate_hz)
         _parent->set_msg_rate(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, rate_hz));
 }
 
+Telemetry::Result TelemetryImpl::set_rate_altitude(double rate_hz)
+{
+    return telemetry_result_from_command_result(
+        _parent->set_msg_rate(MAVLINK_MSG_ID_ALTITUDE, rate_hz));
+}
+
 void TelemetryImpl::set_rate_position_velocity_ned_async(
     double rate_hz, Telemetry::ResultCallback callback)
 {
@@ -449,6 +461,16 @@ void TelemetryImpl::set_rate_landed_state_async(double rate_hz, Telemetry::Resul
         });
 }
 
+void TelemetryImpl::set_rate_altitude_async(double rate_hz, Telemetry::ResultCallback callback)
+{
+    _parent->set_msg_rate_async(
+        MAVLINK_MSG_ID_ALTITUDE,
+        rate_hz,
+        [callback](MavlinkCommandSender::Result command_result, float) {
+            command_result_callback(command_result, callback);
+        });
+}
+
 void TelemetryImpl::set_rate_attitude_quaternion_async(
     double rate_hz, Telemetry::ResultCallback callback)
 {
@@ -809,6 +831,25 @@ void TelemetryImpl::process_attitude_quaternion(const mavlink_message_t& message
         [this](const auto& func) { _parent->call_user_callback(func); });
 }
 
+void TelemetryImpl::process_altitude(const mavlink_message_t& message)
+{
+    mavlink_altitude_t mavlink_altitude;
+    mavlink_msg_altitude_decode(&message, &mavlink_altitude);
+
+    Telemetry::Altitude new_altitude;
+    new_altitude.altitude_monotonic_m = mavlink_altitude.altitude_monotonic;
+    new_altitude.altitude_amsl_m = mavlink_altitude.altitude_amsl;
+    new_altitude.altitude_local_m = mavlink_altitude.altitude_local;
+    new_altitude.altitude_relative_m = mavlink_altitude.altitude_relative;
+    new_altitude.altitude_terrain_m = mavlink_altitude.altitude_terrain;
+
+    set_altitude(new_altitude);
+
+    std::lock_guard<std::mutex> lock(_subscription_mutex);
+    _altitude_subscriptions.queue(
+        altitude(), [this](const auto& func) { _parent->call_user_callback(func); });
+}
+
 void TelemetryImpl::process_mount_orientation(const mavlink_message_t& message)
 {
     // TODO: remove this one once we move all the way to gimbal v2 protocol
@@ -1719,6 +1760,18 @@ void TelemetryImpl::set_heading(Telemetry::Heading heading)
     _heading = heading;
 }
 
+Telemetry::Altitude TelemetryImpl::altitude() const
+{
+    std::lock_guard<std::mutex> lock(_altitude_mutex);
+    return _altitude;
+}
+
+void TelemetryImpl::set_altitude(Telemetry::Altitude altitude)
+{
+    std::lock_guard<std::mutex> lock(_altitude_mutex);
+    _altitude = altitude;
+}
+
 Telemetry::Position TelemetryImpl::home() const
 {
     std::lock_guard<std::mutex> lock(_home_position_mutex);
@@ -2543,6 +2596,19 @@ void TelemetryImpl::unsubscribe_heading(Telemetry::HeadingHandle handle)
     _heading_subscriptions.unsubscribe(handle);
 }
 
+Telemetry::AltitudeHandle
+TelemetryImpl::subscribe_altitude(const Telemetry::AltitudeCallback& callback)
+{
+    std::lock_guard<std::mutex> lock(_subscription_mutex);
+    return _altitude_subscriptions.subscribe(callback);
+}
+
+void TelemetryImpl::unsubscribe_altitude(Telemetry::AltitudeHandle handle)
+{
+    std::lock_guard<std::mutex> lock(_subscription_mutex);
+    _altitude_subscriptions.unsubscribe(handle);
+}
+
 void TelemetryImpl::request_home_position_async()
 {
     MavlinkCommandSender::CommandLong command_request_message{};