From f0856bc15ad362e5397af72766db73271cb61c98 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Rozenblut?=
 <48156138+miodine@users.noreply.github.com>
Date: Sat, 14 Oct 2023 22:09:15 +0200
Subject: [PATCH] Expose Attitude Target for a Quadplane - Review Fixes no. 2

As per instructions:

- Implemented the early return functionality if the attitude control is  inactive

- The procedure of obtaining the attitude targets is now identical to the one implemented in Copter

- Deleted the indication for streaming of MSG_ATTITUDE_TARGET by default if in VTOL mode

Co-Authored-By: Peter Hall <33176108+IamPete1@users.noreply.github.com>
---
 ArduPlane/GCS_Mavlink.cpp | 31 ++++++++++---------------------
 1 file changed, 10 insertions(+), 21 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 766f4cb835697b..bc8cbd5c17fd1b 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -159,23 +159,15 @@ void GCS_MAVLINK_Plane::send_attitude() const
 
 void GCS_MAVLINK_Plane::send_attitude_target() {
 
-    float target_r = 0.0f;
-    float target_p = 0.0f;
-    float target_y = 0.0f;
-    float throttle = 0.0f;
-    Quaternion quat(0.0f,0.0f,0.0f,0.0f);
+    // Check if the attitude target is valid for reporting
+    if (AP_HAL::millis() - plane.quadplane.last_att_control_ms > 100) return;
+    
+    const Quaternion quat  = plane.quadplane.attitude_control->get_attitude_target_quat();
+    const Vector3f ang_vel = plane.quadplane.attitude_control->get_attitude_target_ang_vel();
+    const float throttle = plane.quadplane.attitude_control->get_throttle_in();
 
+    const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4};
 
-    if (plane.quadplane.show_vtol_view()) {
-        const Vector3f rate_targets = plane.quadplane.attitude_control->rate_bf_targets();
-        target_r = rate_targets.x;
-        target_p = rate_targets.y;
-        target_y = rate_targets.z;
-        throttle = plane.quadplane.attitude_control->get_throttle_in();
-        quat = plane.quadplane.attitude_control->get_attitude_target_quat();
-    }
-
-    const float quat_out[4] = {quat.q1, quat.q2, quat.q3, quat.q4};
     const uint16_t typemask = 0; 
 
     mavlink_msg_attitude_target_send(
@@ -183,9 +175,9 @@ void GCS_MAVLINK_Plane::send_attitude_target() {
         AP_HAL::millis(),       // time since boot (ms)
         typemask,               // Bitmask that tells the system what control dimensions should be ignored by the vehicle
         quat_out,               // Target attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length
-        target_r,               // bodyframe target roll rate (rad/s)
-        target_p,               // bodyframe target pitch rate (rad/s)
-        target_y,               // bodyframe yaw rate (rad/s)
+        ang_vel.x,              // bodyframe target roll rate (rad/s)
+        ang_vel.y,              // bodyframe target pitch rate (rad/s)
+        ang_vel.z,              // bodyframe yaw rate (rad/s)
         throttle);              // Collective thrust, normalized to 0 .. 1
 }
 
@@ -647,9 +639,6 @@ static const ap_message STREAM_RC_CHANNELS_msgs[] = {
 };
 static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_ATTITUDE,
-#if HAL_QUADPLANE_ENABLED
-    MSG_ATTITUDE_TARGET,
-#endif 
     MSG_SIMSTATE,
     MSG_AHRS2,
 #if AP_RPM_ENABLED