Merge pull request #779 from krady21/add-angularveloc-and-acc

Telemetry: add angular velocity and acceleration

src/integration_tests/telemetry_async.cpp

@@ -20,6 +20,7 @@ static void print_camera_quaternion(Telemetry::Quaternion quaternion);
 static void print_camera_euler_angle(Telemetry::EulerAngle euler_angle);
 #endif
 static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned);
+static void print_imu_reading_ned(Telemetry::IMUReadingNED imu_reading_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);
@@ -37,6 +38,7 @@ static bool _received_camera_quaternion = false;
 static bool _received_camera_euler_angle = false;
 #endif
 static bool _received_ground_speed = false;
+static bool _received_imu_reading_ned = false;
 static bool _received_gps_info = false;
 static bool _received_battery = false;
 static bool _received_rc_status = false;
@@ -81,6 +83,9 @@ TEST_F(SitlTest, TelemetryAsync)
     telemetry->set_rate_ground_speed_ned_async(10.0, std::bind(&receive_result, _1));
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
+    telemetry->set_rate_imu_reading_ned_async(10.0, std::bind(&receive_result, _1));
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
     telemetry->set_rate_gps_info_async(10.0, std::bind(&receive_result, _1));
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
@@ -107,6 +112,8 @@ TEST_F(SitlTest, TelemetryAsync)
 
     telemetry->ground_speed_ned_async(std::bind(&print_ground_speed_ned, _1));
 
+    telemetry->imu_reading_ned_async(std::bind(&print_imu_reading_ned, _1));
+
     telemetry->gps_info_async(std::bind(&print_gps_info, _1));
 
     telemetry->battery_async(std::bind(&print_battery, _1));
@@ -129,6 +136,7 @@ TEST_F(SitlTest, TelemetryAsync)
     EXPECT_TRUE(_received_camera_euler_angle);
 #endif
     EXPECT_TRUE(_received_ground_speed);
+    EXPECT_TRUE(_received_imu_reading_ned);
     EXPECT_TRUE(_received_gps_info);
     EXPECT_TRUE(_received_battery);
     // EXPECT_TRUE(_received_rc_status); // No RC is sent in SITL.
@@ -215,6 +223,23 @@ void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned)
     _received_ground_speed = true;
 }
 
+void print_imu_reading_ned(Telemetry::IMUReadingNED imu_reading_ned)
+{
+    std::cout << "Acceleration north: " << imu_reading_ned.acceleration.north_m_s2 << " m/s^2, "
+              << "east: " << imu_reading_ned.acceleration.east_m_s2 << " m/s^2, "
+              << "down: " << imu_reading_ned.acceleration.down_m_s2 << " m/s^2, "
+              << "Angular velocity north: " << imu_reading_ned.angular_velocity.north_rad_s
+              << " rad/s, "
+              << "east: " << imu_reading_ned.angular_velocity.east_rad_s << " rad/s, "
+              << "down: " << imu_reading_ned.angular_velocity.down_rad_s << " rad/s, "
+              << "Magnetic field north: " << imu_reading_ned.magnetic_field.north_gauss << " G, "
+              << "east: " << imu_reading_ned.magnetic_field.east_gauss << " G, "
+              << "down: " << imu_reading_ned.magnetic_field.down_gauss << " G, "
+              << "Temperature: " << imu_reading_ned.temperature_degC << " C" << std::endl;
+
+    _received_imu_reading_ned = true;
+}
+
 void print_gps_info(Telemetry::GPSInfo gps_info)
 {
     std::cout << "GPS, num satellites: " << gps_info.num_satellites << ", "

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

@@ -120,6 +120,52 @@ class Telemetry : public PluginBase {
         float velocity_down_m_s; /**< @brief Velocity in Down direction in metres/second. */
     };
 
+    /**
+     * @brief Acceleration type in local coordinates.
+     *
+     * The acceleration is represented in the NED (North East Down) frame and in metres/second^2.
+     */
+    struct AccelerationNED {
+        float north_m_s2; /**< @brief Acceleration in North direction in metres/second^2 */
+        float east_m_s2; /**< @brief Acceleration in East direction in metres/second^2 */
+        float down_m_s2; /**< @brief Acceleration in Down direction in metres/second^2 */
+    };
+
+    /**
+     * @brief Angular velocity type in local coordinates.
+     *
+     * The angular velocity is represented in NED (North East Down) frame and in radians/second.
+     */
+    struct AngularVelocityNED {
+        float north_rad_s; /**< @brief Angular velocity in North direction in radians/second */
+        float east_rad_s; /**< @brief Angular velocity in East direction in radians/second */
+        float down_rad_s; /**< @brief Angular velocity in Down direction in radians/second */
+    };
+
+    /**
+     * @brief Magnetic field type in local coordinates.
+     *
+     * The magnetic field is represented in NED (North East Down) frame and is measured in Gauss.
+     */
+    struct MagneticFieldNED {
+        float north_gauss; /**< @brief Magnetic field in North direction measured in Gauss. */
+        float east_gauss; /**< @brief Magnetic field in East direction measured in Gauss. */
+        float down_gauss; /**< @brief Magnetic field in Down direction measured in Gauss. */
+    };
+
+    /**
+     * @brief Inertial measurement unit type in local coordinates.
+     *
+     * Acceleration, angular velocity and magnetic field are represented in NED (North East Down)
+     * frame in local coordinate system. Temperature is measured in degrees Celsius.
+     */
+    struct IMUReadingNED {
+        AccelerationNED acceleration; /**< @see AccelerationNED */
+        AngularVelocityNED angular_velocity; /**< @see AngularVelocityNED */
+        MagneticFieldNED magnetic_field; /**< @see MagneticFieldNED */
+        float temperature_degC; /**< @brief Temperature measured in degrees Celsius. */
+    };
+
     /**
      * @brief GPS information type.
      */
@@ -289,6 +335,14 @@ class Telemetry : public PluginBase {
      */
     Result set_rate_ground_speed_ned(double rate_hz);
 
+    /**
+     * @brief Set rate of IMU reading (NED) updates (synchronous).
+     *
+     * @param rate_hz Rate in Hz.
+     * @return Result of request.
+     */
+    Result set_rate_imu_reading_ned(double rate_hz);
+
     /**
      * @brief Set rate of GPS information updates (synchronous).
      *
@@ -370,6 +424,13 @@ class Telemetry : public PluginBase {
      */
     void set_rate_ground_speed_ned_async(double rate_hz, result_callback_t callback);
 
+    /**
+     * @brief Set rate of IMU reading (NED) updates (asynchronous).
+     *
+     * @param rate_hz Rate in Hz.
+     * @param callback Cabllback to receive request result.
+     */
+    void set_rate_imu_reading_ned_async(double rate_hz, result_callback_t callback);
     /**
      * @brief Set rate of GPS information updates (asynchronous).
      *
@@ -475,6 +536,13 @@ class Telemetry : public PluginBase {
      */
     GroundSpeedNED ground_speed_ned() const;
 
+    /**
+     * @brief Get the current IMU reading (NED) (synchronous).
+     *
+     * @return IMU reading in NED.
+     */
+    IMUReadingNED imu_reading_ned() const;
+
     /**
      * @brief Get the current GPS information (synchronous).
      *
@@ -646,6 +714,20 @@ class Telemetry : public PluginBase {
      */
     void ground_speed_ned_async(ground_speed_ned_callback_t callback);
 
+    /**
+     * @brief Callback type for IMU (NED) updates.
+     *
+     * @param imu_reading_ned IMU reading (NED).
+     */
+    typedef std::function<void(IMUReadingNED imu_reading_ned)> imu_reading_ned_callback_t;
+
+    /**
+     * @brief Subscribe to IMU reading (NED) updates (asynchronous).
+     *
+     * @param callback function to call with updates.
+     */
+    void imu_reading_ned_async(imu_reading_ned_callback_t callback);
+
     /**
      * @brief Callback type for GPS information updates.
      *
@@ -809,6 +891,42 @@ bool operator==(const Telemetry::Health &lhs, const Telemetry::Health &rhs);
  */
 std::ostream &operator<<(std::ostream &str, Telemetry::Health const &health);
 
+/**
+ * @brief Equal operator to compare two `Telemetry::IMUReadingNED` objects.
+ *
+ * @return `true` if items are equal.
+ */
+bool operator==(const Telemetry::IMUReadingNED &lhs, const Telemetry::IMUReadingNED &rhs);
+
+/**
+ * @brief Stream operator to print information about a `Telemetry::AccelerationNED`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str, Telemetry::AccelerationNED const &acceleration_ned);
+
+/**
+ * @brief Stream operator to print information about a `Telemetry::AngularVelocityNED`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str,
+                         Telemetry::AngularVelocityNED const &angular_velocity_ned);
+
+/**
+ * @brief Stream operator to print information about a `Telemetry::MagneticFieldNED`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str, Telemetry::MagneticFieldNED const &magnetic_field);
+
+/**
+ * @brief Stream operator to print information about a `Telemetry::IMUReadingNED`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream &operator<<(std::ostream &str, Telemetry::IMUReadingNED const &imu_reading_ned);
+
 /**
  * @brief Equal operator to compare two `Telemetry::GPSInfo` objects.
  *

src/plugins/telemetry/telemetry.cpp

@@ -45,6 +45,11 @@ Telemetry::Result Telemetry::set_rate_ground_speed_ned(double rate_hz)
     return _impl->set_rate_ground_speed_ned(rate_hz);
 }
 
+Telemetry::Result Telemetry::set_rate_imu_reading_ned(double rate_hz)
+{
+    return _impl->set_rate_imu_reading_ned(rate_hz);
+}
+
 Telemetry::Result Telemetry::set_rate_gps_info(double rate_hz)
 {
     return _impl->set_rate_gps_info(rate_hz);
@@ -95,6 +100,11 @@ void Telemetry::set_rate_ground_speed_ned_async(double rate_hz, result_callback_
     _impl->set_rate_ground_speed_ned_async(rate_hz, callback);
 }
 
+void Telemetry::set_rate_imu_reading_ned_async(double rate_hz, result_callback_t callback)
+{
+    _impl->set_rate_imu_reading_ned_async(rate_hz, callback);
+}
+
 void Telemetry::set_rate_gps_info_async(double rate_hz, result_callback_t callback)
 {
     _impl->set_rate_gps_info_async(rate_hz, callback);
@@ -165,6 +175,11 @@ Telemetry::GroundSpeedNED Telemetry::ground_speed_ned() const
     return _impl->get_ground_speed_ned();
 }
 
+Telemetry::IMUReadingNED Telemetry::imu_reading_ned() const
+{
+    return _impl->get_imu_reading_ned();
+}
+
 Telemetry::GPSInfo Telemetry::gps_info() const
 {
     return _impl->get_gps_info();
@@ -250,6 +265,11 @@ void Telemetry::ground_speed_ned_async(ground_speed_ned_callback_t callback)
     return _impl->ground_speed_ned_async(callback);
 }
 
+void Telemetry::imu_reading_ned_async(imu_reading_ned_callback_t callback)
+{
+    return _impl->imu_reading_ned_async(callback);
+}
+
 void Telemetry::gps_info_async(gps_info_callback_t callback)
 {
     return _impl->gps_info_async(callback);
@@ -409,6 +429,68 @@ std::ostream &operator<<(std::ostream &str, Telemetry::Health const &health)
                << ", home_position_ok: " << health.home_position_ok << "]";
 }
 
+bool operator==(const Telemetry::IMUReadingNED &lhs, const Telemetry::IMUReadingNED &rhs)
+{
+    return std::fabs(lhs.acceleration.north_m_s2 - rhs.acceleration.north_m_s2) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.acceleration.east_m_s2 - rhs.acceleration.east_m_s2) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.acceleration.down_m_s2 - rhs.acceleration.down_m_s2) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.angular_velocity.north_rad_s - rhs.angular_velocity.north_rad_s) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.angular_velocity.east_rad_s - rhs.angular_velocity.east_rad_s) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.angular_velocity.down_rad_s - rhs.angular_velocity.down_rad_s) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.magnetic_field.north_gauss - rhs.magnetic_field.north_gauss) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.magnetic_field.east_gauss - rhs.magnetic_field.east_gauss) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.magnetic_field.down_gauss - rhs.magnetic_field.down_gauss) <=
+               std::numeric_limits<float>::epsilon() &&
+           std::fabs(lhs.temperature_degC - rhs.temperature_degC) <=
+               std::numeric_limits<float>::epsilon();
+}
+
+std::ostream &operator<<(std::ostream &str, Telemetry::AccelerationNED const &acceleration_ned)
+{
+    return str << "[acceleration_north_m_s2: " << acceleration_ned.north_m_s2
+               << ", acceleration_east_m_s2: " << acceleration_ned.east_m_s2
+               << ", acceleration_down_m_s2: " << acceleration_ned.down_m_s2 << "]";
+}
+
+std::ostream &operator<<(std::ostream &str,
+                         Telemetry::AngularVelocityNED const &angular_velocity_ned)
+{
+    return str << "[angular_velocity_north_rad_s: " << angular_velocity_ned.north_rad_s
+               << ", angular_velocity_east_rad_s: " << angular_velocity_ned.east_rad_s
+               << ", angular_velocity_down_rad_s: " << angular_velocity_ned.down_rad_s << "]";
+}
+
+std::ostream &operator<<(std::ostream &str, Telemetry::MagneticFieldNED const &magnetic_field_ned)
+{
+    return str << "[magnetic_field_north_gauss: " << magnetic_field_ned.north_gauss
+               << ", magnetic_field_east_gauss: " << magnetic_field_ned.east_gauss
+               << ", magnetic_field_down_gauss: " << magnetic_field_ned.down_gauss << "]";
+}
+
+std::ostream &operator<<(std::ostream &str, Telemetry::IMUReadingNED const &imu_reading_ned)
+{
+    return str << "[acceleration_north_m_s2: " << imu_reading_ned.acceleration.north_m_s2
+               << ", acceleration_east_m_s2: " << imu_reading_ned.acceleration.east_m_s2
+               << ", acceleration_down_m_s2: " << imu_reading_ned.acceleration.down_m_s2 << "] "
+               << "[angular_velocity_north_rad_s: " << imu_reading_ned.angular_velocity.north_rad_s
+               << ", angular_velocity_east_rad_s: " << imu_reading_ned.angular_velocity.east_rad_s
+               << ", angular_velocity_down_rad_s: " << imu_reading_ned.angular_velocity.down_rad_s
+               << "] "
+               << "[magnetic_field_north_gauss: " << imu_reading_ned.magnetic_field.north_gauss
+               << ", magnetic_field_east_gauss: " << imu_reading_ned.magnetic_field.east_gauss
+               << ", magnetic_field_down_gauss: " << imu_reading_ned.magnetic_field.down_gauss
+               << "] "
+               << "[temperature_degC: " << imu_reading_ned.temperature_degC << "]";
+}
+
 bool operator==(const Telemetry::GPSInfo &lhs, const Telemetry::GPSInfo &rhs)
 {
     return lhs.num_satellites == rhs.num_satellites && lhs.fix_type == rhs.fix_type;