From d914e4e63bdc1f5307ed2549bd0743e5bb63428b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 6 Feb 2022 11:07:05 +1100 Subject: [PATCH] Plane: convert next WP alt to global frame in POSITION_TARGET_GLOBAL_INT --- ArduPlane/GCS_Mavlink.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 158c623f7c..58a1c89937 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -214,6 +214,11 @@ void GCS_MAVLINK_Plane::send_position_target_global_int() static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE; + int32_t alt = 0; + if (!next_WP_loc.is_zero()) { + UNUSED_RESULT(next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt)); + } + mavlink_msg_position_target_global_int_send( chan, AP_HAL::millis(), // time_boot_ms @@ -221,7 +226,7 @@ void GCS_MAVLINK_Plane::send_position_target_global_int() TYPE_MASK, // ignore everything except the x/y/z components next_WP_loc.lat, // latitude as 1e7 next_WP_loc.lng, // longitude as 1e7 - next_WP_loc.alt * 0.01f, // altitude is sent as a float + alt * 0.01, // altitude is sent as a float 0.0f, // vx 0.0f, // vy 0.0f, // vz