Merge pull request #1878 from mavlink/pr-split-attitude

telemetry: split attitude into Euler + Quaternion
mavlink · Sep 6, 2022 · eaaaa51 · eaaaa51
2 parents b8b3f2f + e79f7f8
commit eaaaa51
Show file tree

Hide file tree

Showing 13 changed files with 2,198 additions and 1,068 deletions.
diff --git a/proto b/proto
diff --git a/src/integration_tests/telemetry_async.cpp b/src/integration_tests/telemetry_async.cpp
@@ -85,7 +85,11 @@ TEST_F(SitlTest, PX4TelemetryAsync)
         10.0, [](Telemetry::Result result) { return receive_result(result); });
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
-    telemetry->set_rate_attitude_async(
+    telemetry->set_rate_attitude_euler_async(
+        10.0, [](Telemetry::Result result) { return receive_result(result); });
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+    telemetry->set_rate_attitude_quaternion_async(
         10.0, [](Telemetry::Result result) { return receive_result(result); });
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
 

diff --git a/src/integration_tests/telemetry_sync.cpp b/src/integration_tests/telemetry_sync.cpp
@@ -22,7 +22,8 @@ TEST_F(SitlTest, PX4TelemetrySync)
     EXPECT_EQ(telemetry->set_rate_position(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_home(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_in_air(10.0), Telemetry::Result::Success);
-    EXPECT_EQ(telemetry->set_rate_attitude(10.0), Telemetry::Result::Success);
+    EXPECT_EQ(telemetry->set_rate_attitude_quaternion(10.0), Telemetry::Result::Success);
+    EXPECT_EQ(telemetry->set_rate_attitude_euler(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_velocity_ned(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_gps_info(10.0), Telemetry::Result::Success);
     EXPECT_EQ(telemetry->set_rate_battery(10.0), Telemetry::Result::Success);

diff --git a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h
@@ -1963,20 +1963,39 @@ class Telemetry : public PluginBase {
     Result set_rate_vtol_state(double rate_hz) const;
 
     /**
-     * @brief Set rate to 'attitude' updates.
+     * @brief Set rate to 'attitude euler angle' updates.
      *
-     * This function is non-blocking. See 'set_rate_attitude' for the blocking counterpart.
+     * This function is non-blocking. See 'set_rate_attitude_quaternion' for the blocking
+     * counterpart.
      */
-    void set_rate_attitude_async(double rate_hz, const ResultCallback callback);
+    void set_rate_attitude_quaternion_async(double rate_hz, const ResultCallback callback);
 
     /**
-     * @brief Set rate to 'attitude' updates.
+     * @brief Set rate to 'attitude euler angle' updates.
+     *
+     * This function is blocking. See 'set_rate_attitude_quaternion_async' for the non-blocking
+     * counterpart.
      *
-     * This function is blocking. See 'set_rate_attitude_async' for the non-blocking counterpart.
+     * @return Result of request.
+     */
+    Result set_rate_attitude_quaternion(double rate_hz) const;
+
+    /**
+     * @brief Set rate to 'attitude quaternion' updates.
+     *
+     * This function is non-blocking. See 'set_rate_attitude_euler' for the blocking counterpart.
+     */
+    void set_rate_attitude_euler_async(double rate_hz, const ResultCallback callback);
+
+    /**
+     * @brief Set rate to 'attitude quaternion' updates.
+     *
+     * This function is blocking. See 'set_rate_attitude_euler_async' for the non-blocking
+     * counterpart.
      *
      * @return Result of request.
      */
-    Result set_rate_attitude(double rate_hz) const;
+    Result set_rate_attitude_euler(double rate_hz) const;
 
     /**
      * @brief Set rate of camera attitude updates.

diff --git a/src/mavsdk/plugins/telemetry/mocks/telemetry_mock.h b/src/mavsdk/plugins/telemetry/mocks/telemetry_mock.h
@@ -121,7 +121,8 @@ class MockTelemetry {
     MOCK_METHOD1(set_rate_home, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_in_air, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_landed_state, Telemetry::Result(double)){};
-    MOCK_METHOD1(set_rate_attitude, Telemetry::Result(double)){};
+    MOCK_METHOD1(set_rate_attitude_quaternion, Telemetry::Result(double)){};
+    MOCK_METHOD1(set_rate_attitude_euler, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_camera_attitude, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_velocity_ned, Telemetry::Result(double)){};
     MOCK_METHOD1(set_rate_gps_info, Telemetry::Result(double)){};
@@ -146,7 +147,9 @@ class MockTelemetry {
     MOCK_CONST_METHOD2(set_rate_home_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_in_air_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_landed_state_async, void(double, Telemetry::ResultCallback)){};
-    MOCK_CONST_METHOD2(set_rate_attitude_async, void(double, Telemetry::ResultCallback)){};
+    MOCK_CONST_METHOD2(
+        set_rate_attitude_quaternion_async, void(double, Telemetry::ResultCallback)){};
+    MOCK_CONST_METHOD2(set_rate_attitude_euler_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_camera_attitude_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_velocity_ned_async, void(double, Telemetry::ResultCallback)){};
     MOCK_CONST_METHOD2(set_rate_gps_info_async, void(double, Telemetry::ResultCallback)){};

diff --git a/src/mavsdk/plugins/telemetry/telemetry.cpp b/src/mavsdk/plugins/telemetry/telemetry.cpp
@@ -606,14 +606,24 @@ Telemetry::Result Telemetry::set_rate_vtol_state(double rate_hz) const
     return _impl->set_rate_vtol_state(rate_hz);
 }
 
-void Telemetry::set_rate_attitude_async(double rate_hz, const ResultCallback callback)
+void Telemetry::set_rate_attitude_quaternion_async(double rate_hz, const ResultCallback callback)
 {
-    _impl->set_rate_attitude_async(rate_hz, callback);
+    _impl->set_rate_attitude_quaternion_async(rate_hz, callback);
 }
 
-Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) const
+Telemetry::Result Telemetry::set_rate_attitude_quaternion(double rate_hz) const
 {
-    return _impl->set_rate_attitude(rate_hz);
+    return _impl->set_rate_attitude_quaternion(rate_hz);
+}
+
+void Telemetry::set_rate_attitude_euler_async(double rate_hz, const ResultCallback callback)
+{
+    _impl->set_rate_attitude_euler_async(rate_hz, callback);
+}
+
+Telemetry::Result Telemetry::set_rate_attitude_euler(double rate_hz) const
+{
+    return _impl->set_rate_attitude_euler(rate_hz);
 }
 
 void Telemetry::set_rate_camera_attitude_async(double rate_hz, const ResultCallback callback)

diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp
@@ -283,12 +283,18 @@ Telemetry::Result TelemetryImpl::set_rate_landed_state(double rate_hz)
         _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz));
 }
 
-Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz)
+Telemetry::Result TelemetryImpl::set_rate_attitude_quaternion(double rate_hz)
 {
     return telemetry_result_from_command_result(
         _parent->set_msg_rate(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, rate_hz));
 }
 
+Telemetry::Result TelemetryImpl::set_rate_attitude_euler(double rate_hz)
+{
+    return telemetry_result_from_command_result(
+        _parent->set_msg_rate(MAVLINK_MSG_ID_ATTITUDE, rate_hz));
+}
+
 Telemetry::Result TelemetryImpl::set_rate_camera_attitude(double rate_hz)
 {
     return telemetry_result_from_command_result(
@@ -443,7 +449,8 @@ void TelemetryImpl::set_rate_landed_state_async(double rate_hz, Telemetry::Resul
         });
 }
 
-void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::ResultCallback callback)
+void TelemetryImpl::set_rate_attitude_quaternion_async(
+    double rate_hz, Telemetry::ResultCallback callback)
 {
     _parent->set_msg_rate_async(
         MAVLINK_MSG_ID_ATTITUDE_QUATERNION,
@@ -453,6 +460,17 @@ void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::ResultCal
         });
 }
 
+void TelemetryImpl::set_rate_attitude_euler_async(
+    double rate_hz, Telemetry::ResultCallback callback)
+{
+    _parent->set_msg_rate_async(
+        MAVLINK_MSG_ID_ATTITUDE,
+        rate_hz,
+        [callback](MavlinkCommandSender::Result command_result, float) {
+            command_result_callback(command_result, callback);
+        });
+}
+
 void TelemetryImpl::set_rate_camera_attitude_async(
     double rate_hz, Telemetry::ResultCallback callback)
 {
@@ -752,13 +770,6 @@ void TelemetryImpl::process_attitude(const mavlink_message_t& message)
     angular_velocity_body.yaw_rad_s = attitude.yawspeed;
     set_attitude_angular_velocity_body(angular_velocity_body);
 
-    auto quaternion = mavsdk::to_quaternion_from_euler_angle(euler_angle);
-    set_attitude_quaternion(quaternion);
-
-    std::lock_guard<std::mutex> lock(_subscription_mutex);
-    _attitude_quaternion_angle_subscriptions.queue(
-        attitude_quaternion(), [this](const auto& func) { _parent->call_user_callback(func); });
-
     _attitude_euler_angle_subscriptions.queue(
         attitude_euler(), [this](const auto& func) { _parent->call_user_callback(func); });
 
@@ -793,9 +804,6 @@ void TelemetryImpl::process_attitude_quaternion(const mavlink_message_t& message
     _attitude_quaternion_angle_subscriptions.queue(
         attitude_quaternion(), [this](const auto& func) { _parent->call_user_callback(func); });
 
-    _attitude_euler_angle_subscriptions.queue(
-        attitude_euler(), [this](const auto& func) { _parent->call_user_callback(func); });
-
     _attitude_angular_velocity_body_subscriptions.queue(
         attitude_angular_velocity_body(),
         [this](const auto& func) { _parent->call_user_callback(func); });

diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h
@@ -32,7 +32,8 @@ class TelemetryImpl : public PluginImplBase {
     Telemetry::Result set_rate_in_air(double rate_hz);
     Telemetry::Result set_rate_landed_state(double rate_hz);
     Telemetry::Result set_rate_vtol_state(double rate_hz);
-    Telemetry::Result set_rate_attitude(double rate_hz);
+    Telemetry::Result set_rate_attitude_quaternion(double rate_hz);
+    Telemetry::Result set_rate_attitude_euler(double rate_hz);
     Telemetry::Result set_rate_camera_attitude(double rate_hz);
     Telemetry::Result set_rate_velocity_ned(double rate_hz);
     Telemetry::Result set_rate_imu(double rate_hz);
@@ -56,7 +57,8 @@ class TelemetryImpl : public PluginImplBase {
     void set_rate_in_air_async(double rate_hz, Telemetry::ResultCallback callback);
     void set_rate_landed_state_async(double rate_hz, Telemetry::ResultCallback callback);
     void set_rate_vtol_state_async(double rate_hz, Telemetry::ResultCallback callback);
-    void set_rate_attitude_async(double rate_hz, Telemetry::ResultCallback callback);
+    void set_rate_attitude_quaternion_async(double rate_hz, Telemetry::ResultCallback callback);
+    void set_rate_attitude_euler_async(double rate_hz, Telemetry::ResultCallback callback);
     void set_rate_camera_attitude_async(double rate_hz, Telemetry::ResultCallback callback);
     void set_rate_velocity_ned_async(double rate_hz, Telemetry::ResultCallback callback);
     void set_rate_imu_async(double rate_hz, Telemetry::ResultCallback callback);