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