Merge pull request #2212 from lamlaaaam/v2.0.1-ardupilot-fix

ArduPilot: Prevent std::future_error in takeoff, and return attitude in euler angles

src/mavsdk/plugins/action/action_impl.cpp

@@ -407,8 +407,9 @@ void ActionImpl::takeoff_async_apm(const Action::ResultCallback& callback) const
                     if (callback) {
                         callback(action_result);
                     }
+                } else {
+                    send_takeoff_command();
                 }
-                send_takeoff_command();
             });
     } else {
         send_takeoff_command();

src/mavsdk/plugins/telemetry/telemetry_impl.cpp

@@ -787,6 +787,7 @@ void TelemetryImpl::process_attitude(const mavlink_message_t& message)
     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;
+    set_attitude_euler(euler_angle);
 
     Telemetry::AngularVelocityBody angular_velocity_body;
     angular_velocity_body.roll_rad_s = attitude.rollspeed;
@@ -2094,10 +2095,8 @@ Telemetry::FixedwingMetrics TelemetryImpl::fixedwing_metrics() const
 
 Telemetry::EulerAngle TelemetryImpl::attitude_euler() const
 {
-    std::lock_guard<std::mutex> lock(_attitude_quaternion_mutex);
-    Telemetry::EulerAngle euler = to_euler_angle_from_quaternion(_attitude_quaternion);
-
-    return euler;
+    std::lock_guard<std::mutex> lock(_attitude_euler_mutex);
+    return _attitude_euler;
 }
 
 void TelemetryImpl::set_attitude_quaternion(Telemetry::Quaternion quaternion)
@@ -2106,6 +2105,12 @@ void TelemetryImpl::set_attitude_quaternion(Telemetry::Quaternion quaternion)
     _attitude_quaternion = quaternion;
 }
 
+void TelemetryImpl::set_attitude_euler(Telemetry::EulerAngle euler)
+{
+    std::lock_guard<std::mutex> lock(_attitude_euler_mutex);
+    _attitude_euler = euler;
+}
+
 void TelemetryImpl::set_attitude_angular_velocity_body(
     Telemetry::AngularVelocityBody angular_velocity_body)
 {

src/mavsdk/plugins/telemetry/telemetry_impl.h

@@ -217,6 +217,7 @@ class TelemetryImpl : public PluginImplBase {
     void set_status_text(Telemetry::StatusText status_text);
     void set_armed(bool armed);
     void set_attitude_quaternion(Telemetry::Quaternion quaternion);
+    void set_attitude_euler(Telemetry::EulerAngle euler);
     void set_attitude_angular_velocity_body(Telemetry::AngularVelocityBody angular_velocity_body);
     void set_fixedwing_metrics(Telemetry::FixedwingMetrics fixedwing_metrics);
     void set_ground_truth(Telemetry::GroundTruth ground_truth);
@@ -337,6 +338,9 @@ class TelemetryImpl : public PluginImplBase {
     mutable std::mutex _attitude_quaternion_mutex{};
     Telemetry::Quaternion _attitude_quaternion{};
 
+    mutable std::mutex _attitude_euler_mutex{};
+    Telemetry::EulerAngle _attitude_euler{};
+
     mutable std::mutex _camera_attitude_euler_angle_mutex{};
     Telemetry::EulerAngle _camera_attitude_euler_angle{};