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 <[email protected]>
ArduPilot · Oct 14, 2023 · f0856bc · f0856bc
1 parent 7f13f54
commit f0856bc
Showing 1 changed file with 10 additions and 21 deletions.
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -159,33 +159,25 @@ 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(
         chan,
         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