telemetry: add local position subscription

integration_tests/telemetry_async.cpp

@@ -23,6 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned);
 static void print_gps_info(Telemetry::GPSInfo gps_info);
 static void print_battery(Telemetry::Battery battery);
 static void print_rc_status(Telemetry::RCStatus rc_status);
+static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned);
 
 static bool _set_rate_error = false;
 static bool _received_position = false;
@@ -39,6 +40,7 @@ static bool _received_ground_speed = false;
 static bool _received_gps_info = false;
 static bool _received_battery = false;
 static bool _received_rc_status = false;
+static bool _received_position_velocity_ned = false;
 
 TEST_F(SitlTest, TelemetryAsync)
 {
@@ -111,6 +113,8 @@ TEST_F(SitlTest, TelemetryAsync)
 
     telemetry->rc_status_async(std::bind(&print_rc_status, _1));
 
+    telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1));
+
     std::this_thread::sleep_for(std::chrono::seconds(10));
 
     EXPECT_FALSE(_set_rate_error);
@@ -128,6 +132,7 @@ TEST_F(SitlTest, TelemetryAsync)
     EXPECT_TRUE(_received_gps_info);
     EXPECT_TRUE(_received_battery);
     EXPECT_TRUE(_received_rc_status);
+    EXPECT_TRUE(_received_position_velocity_ned);
 }
 
 void receive_result(Telemetry::Result result)
@@ -232,3 +237,16 @@ void print_rc_status(Telemetry::RCStatus rc_status)
               << std::endl;
     _received_rc_status = true;
 }
+
+void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned)
+{
+    std::cout << "Got position north:  " << position_velocity_ned.position.north_m << " m, "
+              << "east: " << position_velocity_ned.position.east_m << " m, "
+              << "down: " << position_velocity_ned.position.down_m << " m" << std::endl
+              << "velocity north: " << position_velocity_ned.velocity.north_m_s << " m/s, "
+              << "velocity east: " << position_velocity_ned.velocity.east_m_s << " m/s, "
+              << "velocity down: " << position_velocity_ned.velocity.down_m_s << " m/s"
+              << std::endl;
+
+    _received_position_velocity_ned = true;
+}

plugins/telemetry/telemetry.cpp

@@ -10,6 +10,11 @@ Telemetry::Telemetry(System &system) : PluginBase(), _impl{new TelemetryImpl(sys
 
 Telemetry::~Telemetry() {}
 
+Telemetry::Result Telemetry::set_rate_position_velocity_ned(double rate_hz)
+{
+    return _impl->set_rate_position_velocity_ned(rate_hz);
+}
+
 Telemetry::Result Telemetry::set_rate_position(double rate_hz)
 {
     return _impl->set_rate_position(rate_hz);
@@ -55,10 +60,16 @@ Telemetry::Result Telemetry::set_rate_rc_status(double rate_hz)
     return _impl->set_rate_rc_status(rate_hz);
 }
 
+void Telemetry::set_rate_position_velocity_ned_async(double rate_hz, result_callback_t callback)
+{
+    _impl->set_rate_position_velocity_ned_async(rate_hz, callback);
+}
+
 void Telemetry::set_rate_position_async(double rate_hz, result_callback_t callback)
 {
     _impl->set_rate_position_async(rate_hz, callback);
 }
+
 void Telemetry::set_rate_home_position_async(double rate_hz, result_callback_t callback)
 {
     _impl->set_rate_home_position_async(rate_hz, callback);
@@ -99,6 +110,11 @@ void Telemetry::set_rate_rc_status_async(double rate_hz, result_callback_t callb
     _impl->set_rate_rc_status_async(rate_hz, callback);
 }
 
+Telemetry::PositionVelocityNED Telemetry::position_velocity_ned() const
+{
+    return _impl->get_position_velocity_ned();
+}
+
 Telemetry::Position Telemetry::position() const
 {
     return _impl->get_position();
@@ -174,6 +190,11 @@ Telemetry::RCStatus Telemetry::rc_status() const
     return _impl->get_rc_status();
 }
 
+void Telemetry::position_velocity_ned_async(position_velocity_ned_callback_t callback)
+{
+    return _impl->position_velocity_ned_async(callback);
+}
+
 void Telemetry::position_async(position_callback_t callback)
 {
     return _impl->position_async(callback);
@@ -295,6 +316,23 @@ const char *Telemetry::result_str(Result result)
     }
 }
 
+bool operator==(const Telemetry::PositionVelocityNED &lhs,
+                const Telemetry::PositionVelocityNED &rhs)
+{
+    return abs(lhs.position.north_m - rhs.position.north_m) <=
+               std::numeric_limits<double>::epsilon() &&
+           abs(lhs.position.east_m - rhs.position.east_m) <=
+               std::numeric_limits<double>::epsilon() &&
+           abs(lhs.position.down_m - rhs.position.down_m) <=
+               std::numeric_limits<float>::epsilon() &&
+           abs(lhs.velocity.north_m_s - rhs.velocity.north_m_s) <=
+               std::numeric_limits<float>::epsilon() &&
+           abs(lhs.velocity.east_m_s - rhs.velocity.east_m_s) <=
+               std::numeric_limits<float>::epsilon() &&
+           abs(lhs.velocity.down_m_s - rhs.velocity.down_m_s) <=
+               std::numeric_limits<float>::epsilon();
+}
+
 bool operator==(const Telemetry::Position &lhs, const Telemetry::Position &rhs)
 {
     return abs(lhs.latitude_deg - rhs.latitude_deg) <= std::numeric_limits<double>::epsilon() &&

plugins/telemetry/telemetry.h

@@ -34,7 +34,7 @@ class Telemetry : public PluginBase {
     ~Telemetry();
 
     /**
-     * @brief Position type.
+     * @brief Position type in global coordinates.
      */
     struct Position {
         double latitude_deg; /**< @brief Latitude in degrees (range: -90 to +90) */
@@ -43,6 +43,39 @@ class Telemetry : public PluginBase {
         float relative_altitude_m; /**< @brief Altitude relative to takeoff altitude in metres */
     };
 
+    /**
+     * @brief Position type in local coordinates.
+     *
+     * Position is represented in the NED (North East Down) frame in local coordinate system.
+     */
+    struct PositionNED {
+        float north_m; /**< @brief Position along north-direction in meters. */
+        float east_m; /**< @brief Position along east-direction  in meters. */
+        float down_m; /**< @brief Position along down-direction  in meters. */
+    };
+
+    /**
+     * @brief Velocity type in local coordinates.
+     *
+     * Velocity is represented in NED (North East Down) frame in local coordinate system.
+     */
+    struct VelocityNED {
+        float north_m_s; /**< @brief Velocity along north-direction in meters per seconds. */
+        float east_m_s; /**< @brief Velocity along east-direction in meters per seconds. */
+        float down_m_s; /**< @brief Velocity along down-direction in meters per seconds. */
+    };
+
+    /**
+     * @brief Kinematic type in local coordinates.
+     *
+     * Position and Velocity are represented in NED (North East Down) frame in local coordinate
+     * system.
+     */
+    struct PositionVelocityNED {
+        PositionNED position; /**< @see PositionNED */
+        VelocityNED velocity; /**< @see VelocityNED */
+    };
+
     /**
      * @brief Quaternion type.
      *
@@ -178,6 +211,15 @@ class Telemetry : public PluginBase {
      */
     typedef std::function<void(Result)> result_callback_t;
 
+    /**
+     * @brief Set rate of kinematic (position and velocity) updates (synchronous).
+     *
+     * @see PositionVelocityNED
+     * @param rate_hz Rate in Hz.
+     * @return Result of request.
+     */
+    Result set_rate_position_velocity_ned(double rate_hz);
+
     /**
      * @brief Set rate of position updates (synchronous).
      *
@@ -250,6 +292,15 @@ class Telemetry : public PluginBase {
      */
     Result set_rate_rc_status(double rate_hz);
 
+    /**
+     * @brief Set rate of kinematic (position and velocity) updates (asynchronous).
+     *
+     * @see PositionVelocityNED
+     * @param rate_hz Rate in Hz.
+     * @param callback Callback to receive request result.
+     */
+    void set_rate_position_velocity_ned_async(double rate_hz, result_callback_t callback);
+
     /**
      * @brief Set rate of position updates (asynchronous).
      *
@@ -322,6 +373,13 @@ class Telemetry : public PluginBase {
      */
     void set_rate_rc_status_async(double rate_hz, result_callback_t callback);
 
+    /**
+     * @brief Get the current kinematic (position and velocity) in NED frame (synchronous).
+     *
+     * @return PositionVelocityNED.
+     */
+    PositionVelocityNED position_velocity_ned() const;
+
     /**
      * @brief Get the current position (synchronous).
      *
@@ -429,6 +487,18 @@ class Telemetry : public PluginBase {
      */
     RCStatus rc_status() const;
 
+    /**
+     * @brief Callback type for kinematic (position and velocity) updates.
+     */
+    typedef std::function<void(PositionVelocityNED)> position_velocity_ned_callback_t;
+
+    /**
+     * @brief Subscribe to kinematic (position and velocity) updates (asynchronous).
+     *
+     * @param callback Function to call with updates.
+     */
+    void position_velocity_ned_async(position_velocity_ned_callback_t callback);
+
     /**
      * @brief Callback type for position updates.
      */
@@ -639,6 +709,14 @@ class Telemetry : public PluginBase {
     std::unique_ptr<TelemetryImpl> _impl;
 };
 
+/**
+ * @brief Equal operator to compare two `Telemetry::PositionVelocityNED` objects.
+ *
+ * @return `true` if items are equal.
+ */
+bool operator==(const Telemetry::PositionVelocityNED &lhs,
+                const Telemetry::PositionVelocityNED &rhs);
+
 /**
  * @brief Equal operator to compare two `Telemetry::Position` objects.
  *

plugins/telemetry/telemetry_impl.cpp

@@ -62,6 +62,11 @@ void TelemetryImpl::init()
 {
     using namespace std::placeholders; // for `_1`
 
+    _parent->register_mavlink_message_handler(
+        MAVLINK_MSG_ID_LOCAL_POSITION_NED,
+        std::bind(&TelemetryImpl::process_position_velocity_ned, this, _1),
+        this);
+
     _parent->register_mavlink_message_handler(
         MAVLINK_MSG_ID_GLOBAL_POSITION_INT,
         std::bind(&TelemetryImpl::process_global_position_int, this, _1),
@@ -148,6 +153,12 @@ void TelemetryImpl::disable()
     _parent->unregister_timeout_handler(_timeout_cookie);
 }
 
+Telemetry::Result TelemetryImpl::set_rate_position_velocity_ned(double rate_hz)
+{
+    return telemetry_result_from_command_result(
+        _parent->set_msg_rate(MAVLINK_MSG_ID_LOCAL_POSITION_NED, rate_hz));
+}
+
 Telemetry::Result TelemetryImpl::set_rate_position(double rate_hz)
 {
     _position_rate_hz = rate_hz;
@@ -208,6 +219,15 @@ Telemetry::Result TelemetryImpl::set_rate_rc_status(double rate_hz)
         _parent->set_msg_rate(MAVLINK_MSG_ID_RC_CHANNELS, rate_hz));
 }
 
+void TelemetryImpl::set_rate_position_velocity_ned_async(double rate_hz,
+                                                         Telemetry::result_callback_t callback)
+{
+    _parent->set_msg_rate_async(
+        MAVLINK_MSG_ID_LOCAL_POSITION_NED,
+        rate_hz,
+        std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback));
+}
+
 void TelemetryImpl::set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback)
 {
     _position_rate_hz = rate_hz;
@@ -318,6 +338,22 @@ void TelemetryImpl::command_result_callback(MAVLinkCommands::Result command_resu
     callback(action_result);
 }
 
+void TelemetryImpl::process_position_velocity_ned(const mavlink_message_t &message)
+{
+    mavlink_local_position_ned_t local_position;
+    mavlink_msg_local_position_ned_decode(&message, &local_position);
+    set_position_velocity_ned(Telemetry::PositionVelocityNED({local_position.x,
+                                                              local_position.y,
+                                                              local_position.z,
+                                                              local_position.vx,
+                                                              local_position.vy,
+                                                              local_position.vz}));
+
+    if (_position_velocity_ned_subscription) {
+        _position_velocity_ned_subscription(get_position_velocity_ned());
+    }
+}
+
 void TelemetryImpl::process_global_position_int(const mavlink_message_t &message)
 {
     mavlink_global_position_int_t global_position_int;
@@ -574,6 +610,18 @@ void TelemetryImpl::receive_rc_channels_timeout()
     set_rc_status(rc_ok, 0.0f);
 }
 
+Telemetry::PositionVelocityNED TelemetryImpl::get_position_velocity_ned() const
+{
+    std::lock_guard<std::mutex> lock(_position_velocity_ned_mutex);
+    return _position_velocity_ned;
+}
+
+void TelemetryImpl::set_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned)
+{
+    std::lock_guard<std::mutex> lock(_position_velocity_ned_mutex);
+    _position_velocity_ned = position_velocity_ned;
+}
+
 Telemetry::Position TelemetryImpl::get_position() const
 {
     std::lock_guard<std::mutex> lock(_position_mutex);
@@ -787,6 +835,12 @@ void TelemetryImpl::set_rc_status(bool available, float signal_strength_percent)
     _rc_status.available = available;
 }
 
+void TelemetryImpl::position_velocity_ned_async(
+    Telemetry::position_velocity_ned_callback_t &callback)
+{
+    _position_velocity_ned_subscription = callback;
+}
+
 void TelemetryImpl::position_async(Telemetry::position_callback_t &callback)
 {
     _position_subscription = callback;