Merge pull request #1312 from mavlink/pr-attitude-timestamps

telemetry: add timestamp to attitude

src/mavsdk_server/src/plugins/telemetry/telemetry_service_impl.h

@@ -302,6 +302,8 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
 
         rpc_obj->set_z(quaternion.z);
 
+        rpc_obj->set_timestamp_us(quaternion.timestamp_us);
+
         return rpc_obj;
     }
 
@@ -318,6 +320,8 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
 
         obj.z = quaternion.z();
 
+        obj.timestamp_us = quaternion.timestamp_us();
+
         return obj;
     }
 
@@ -332,6 +336,8 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
 
         rpc_obj->set_yaw_deg(euler_angle.yaw_deg);
 
+        rpc_obj->set_timestamp_us(euler_angle.timestamp_us);
+
         return rpc_obj;
     }
 
@@ -346,6 +352,8 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
 
         obj.yaw_deg = euler_angle.yaw_deg();
 
+        obj.timestamp_us = euler_angle.timestamp_us();
+
         return obj;
     }
 

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

@@ -189,6 +189,7 @@ class Telemetry : public PluginBase {
         float x{float(NAN)}; /**< @brief Quaternion entry 1, also denoted as b */
         float y{float(NAN)}; /**< @brief Quaternion entry 2, also denoted as c */
         float z{float(NAN)}; /**< @brief Quaternion entry 3, also denoted as d */
+        uint64_t timestamp_us{}; /**< @brief Timestamp in microseconds */
     };
 
     /**
@@ -220,6 +221,7 @@ class Telemetry : public PluginBase {
             float(NAN)}; /**< @brief Pitch angle in degrees, positive is pitching nose up */
         float yaw_deg{
             float(NAN)}; /**< @brief Yaw angle in degrees, positive is clock-wise seen from above */
+        uint64_t timestamp_us{}; /**< @brief Timestamp in microseconds */
     };
 
     /**

src/plugins/telemetry/math_conversions.cpp

@@ -15,6 +15,8 @@ Telemetry::EulerAngle to_euler_angle_from_quaternion(Telemetry::Quaternion quate
     euler_angle.yaw_deg = to_deg_from_rad(
         atan2f(2.0f * (q.w * q.z + q.x * q.y), 1.0f - 2.0f * (q.y * q.y + q.z * q.z)));
 
+    euler_angle.timestamp_us = quaternion.timestamp_us;
+
     return euler_angle;
 }
 
@@ -33,6 +35,8 @@ Telemetry::Quaternion to_quaternion_from_euler_angle(Telemetry::EulerAngle euler
     quaternion.y = float(cos_phi_2 * sin_theta_2 * cos_psi_2 + sin_phi_2 * cos_theta_2 * sin_psi_2);
     quaternion.z = float(cos_phi_2 * cos_theta_2 * sin_psi_2 - sin_phi_2 * sin_theta_2 * cos_psi_2);
 
+    quaternion.timestamp_us = euler_angle.timestamp_us;
+
     return quaternion;
 }
 

src/plugins/telemetry/math_conversions_test.cpp

@@ -12,11 +12,13 @@ TEST(MathConversions, QuaternionToEulerAnglesAndBackBaseCase)
     q1.x = 0.0f;
     q1.y = 0.0f;
     q1.z = 0.0f;
+    q1.timestamp_us = 4242;
     Telemetry::EulerAngle e = to_euler_angle_from_quaternion(q1);
 
     EXPECT_FLOAT_EQ(e.roll_deg, 0.0f);
     EXPECT_FLOAT_EQ(e.pitch_deg, 0.0f);
     EXPECT_FLOAT_EQ(e.yaw_deg, 0.0f);
+    EXPECT_EQ(e.timestamp_us, q1.timestamp_us);
 
     Telemetry::Quaternion q2 = to_quaternion_from_euler_angle(e);
 
@@ -63,18 +65,21 @@ TEST(MathConversions, QuaternionToEulerAndBackSomeCase)
     q1.x = 0.280f;
     q1.y = 0.595f;
     q1.z = 0.739f;
+    q1.timestamp_us = 9999;
     Telemetry::EulerAngle e = to_euler_angle_from_quaternion(q1);
 
     EXPECT_NEAR(e.roll_deg, 82.086f, 0.2f);
     EXPECT_NEAR(e.pitch_deg, -14.142f, 0.2f);
     EXPECT_NEAR(e.yaw_deg, 145.774f, 0.2f);
+    EXPECT_EQ(e.timestamp_us, q1.timestamp_us);
 
     Telemetry::Quaternion q2 = to_quaternion_from_euler_angle(e);
 
     EXPECT_NEAR(q1.w, q2.w, 0.01f);
     EXPECT_NEAR(q1.x, q2.x, 0.01f);
     EXPECT_NEAR(q1.y, q2.y, 0.01f);
     EXPECT_NEAR(q1.z, q2.z, 0.01f);
+    EXPECT_EQ(q1.timestamp_us, q2.timestamp_us);
 
     // We also compare against the MAVLink implementation and test
     // the MAVLink functions while we're at it.

src/plugins/telemetry/telemetry.cpp

@@ -546,7 +546,8 @@ bool operator==(const Telemetry::Quaternion& lhs, const Telemetry::Quaternion& r
     return ((std::isnan(rhs.w) && std::isnan(lhs.w)) || rhs.w == lhs.w) &&
            ((std::isnan(rhs.x) && std::isnan(lhs.x)) || rhs.x == lhs.x) &&
            ((std::isnan(rhs.y) && std::isnan(lhs.y)) || rhs.y == lhs.y) &&
-           ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z);
+           ((std::isnan(rhs.z) && std::isnan(lhs.z)) || rhs.z == lhs.z) &&
+           (rhs.timestamp_us == lhs.timestamp_us);
 }
 
 std::ostream& operator<<(std::ostream& str, Telemetry::Quaternion const& quaternion)
@@ -557,6 +558,7 @@ std::ostream& operator<<(std::ostream& str, Telemetry::Quaternion const& quatern
     str << "    x: " << quaternion.x << '\n';
     str << "    y: " << quaternion.y << '\n';
     str << "    z: " << quaternion.z << '\n';
+    str << "    timestamp_us: " << quaternion.timestamp_us << '\n';
     str << '}';
     return str;
 }
@@ -567,7 +569,8 @@ bool operator==(const Telemetry::EulerAngle& lhs, const Telemetry::EulerAngle& r
             rhs.roll_deg == lhs.roll_deg) &&
            ((std::isnan(rhs.pitch_deg) && std::isnan(lhs.pitch_deg)) ||
             rhs.pitch_deg == lhs.pitch_deg) &&
-           ((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) || rhs.yaw_deg == lhs.yaw_deg);
+           ((std::isnan(rhs.yaw_deg) && std::isnan(lhs.yaw_deg)) || rhs.yaw_deg == lhs.yaw_deg) &&
+           (rhs.timestamp_us == lhs.timestamp_us);
 }
 
 std::ostream& operator<<(std::ostream& str, Telemetry::EulerAngle const& euler_angle)
@@ -577,6 +580,7 @@ std::ostream& operator<<(std::ostream& str, Telemetry::EulerAngle const& euler_a
     str << "    roll_deg: " << euler_angle.roll_deg << '\n';
     str << "    pitch_deg: " << euler_angle.pitch_deg << '\n';
     str << "    yaw_deg: " << euler_angle.yaw_deg << '\n';
+    str << "    timestamp_us: " << euler_angle.timestamp_us << '\n';
     str << '}';
     return str;
 }

src/plugins/telemetry/telemetry_impl.cpp

@@ -608,6 +608,7 @@ void TelemetryImpl::process_attitude(const mavlink_message_t& message)
     euler_angle.roll_deg = to_deg_from_rad(attitude.roll);
     euler_angle.pitch_deg = to_deg_from_rad(attitude.pitch);
     euler_angle.yaw_deg = to_deg_from_rad(attitude.yaw);
+    euler_angle.timestamp_us = static_cast<uint64_t>(attitude.time_boot_ms) * 1000;
 
     Telemetry::AngularVelocityBody angular_velocity_body;
     angular_velocity_body.roll_rad_s = attitude.rollspeed;
@@ -648,6 +649,8 @@ void TelemetryImpl::process_attitude_quaternion(const mavlink_message_t& message
     quaternion.x = mavlink_attitude_quaternion.q2;
     quaternion.y = mavlink_attitude_quaternion.q3;
     quaternion.z = mavlink_attitude_quaternion.q4;
+    quaternion.timestamp_us =
+        static_cast<uint64_t>(mavlink_attitude_quaternion.time_boot_ms) * 1000;
 
     Telemetry::AngularVelocityBody angular_velocity_body;
     angular_velocity_body.roll_rad_s = mavlink_attitude_quaternion.rollspeed;