From 21cd4f19760a8698af6de25b429626a3c33455b9 Mon Sep 17 00:00:00 2001 From: ES-Alexander Date: Tue, 3 May 2022 12:05:48 +1000 Subject: [PATCH] sub: GCS_Mavlink: fix depth hold typemask bug --- ArduSub/GCS_Mavlink.cpp | 3 ++- ArduSub/defines.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 6105d73ddf..dd3df300ce 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -660,6 +660,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) break; } + bool z_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_Z_IGNORE; bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; @@ -671,7 +672,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; */ - if (!pos_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD + if (!z_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD sub.pos_control.set_pos_target_z_cm(packet.alt*100); break; } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 6abaece5b1..b3c7109f10 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -186,6 +186,7 @@ enum LoggingParameters { #define FS_TERRAIN_RECOVER_TIMEOUT_MS 10000 // for mavlink SET_POSITION_TARGET messages +#define MAVLINK_SET_POS_TYPE_MASK_Z_IGNORE (1<<2) #define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2)) #define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5)) #define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8))