From 14d351c2fd6eb4441e0af541e917d43df7c2813c Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 16 Mar 2021 18:28:38 +0100 Subject: [PATCH] Rover: use MAVLink POSITION_TARGET_TYPEMASK --- Rover/GCS_Mavlink.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index de3d20ef2f..f6b8b14d44 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -79,11 +79,15 @@ void GCS_MAVLINK_Rover::send_position_target_global_int() if (!rover.control_mode->get_desired_location(target)) { return; } + static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; + 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_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE; mavlink_msg_position_target_global_int_send( chan, AP_HAL::millis(), // time_boot_ms MAV_FRAME_GLOBAL, // targets are always global altitude - 0xFFF8, // ignore everything except the x/y/z components + TYPE_MASK, // ignore everything except the x/y/z components target.lat, // latitude as 1e7 target.lng, // longitude as 1e7 target.alt * 0.01f, // altitude is sent as a float