From 72f40b486240773255a233e06d57599a16bc30dd Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 12 Mar 2024 19:42:05 +0000 Subject: [PATCH] Plane: update type mask comparisons in set_position_global handler Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 01ec6266222538..afcc3063e64df7 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1369,18 +1369,28 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess mavlink_set_position_target_global_int_t pos_target; mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target); + + const uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; + // Unexpectedly, the mask is expecting "ones" for dimensions that should // be IGNORNED rather than INCLUDED. See mavlink documentation of the // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field. - const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3) - - // bit mask for path following: ignore force_set, yaw, yaw_rate - const uint16_t path_mask = 0b1111111000000000; + const uint16_t alt_mask = ( + POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | + 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) ^ 0xFFFF; + + // bit mask for path following + const uint16_t path_mask = ( + POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | + POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF; bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; - if (pos_target.type_mask & alt_mask) + if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == alt_mask) { cmd.content.location.alt = pos_target.alt * 100; cmd.content.location.relative_alt = false; @@ -1411,7 +1421,7 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess } // end if alt_mask // guided path following - if (pos_target.type_mask & path_mask) { + if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == path_mask) { cmd.content.location.lat = pos_target.lat_int; cmd.content.location.lng = pos_target.lon_int;