From d23ad705e1c9d29888a115f1f2a7d148bbeed3e7 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 7 Aug 2019 13:02:23 -0300 Subject: [PATCH] Sub: Accept position_target_global_int for depth control in ALT_HOLD --- ArduSub/GCS_Mavlink.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 22d0e35478..613977d624 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -687,8 +687,10 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) mavlink_set_position_target_global_int_t packet; mavlink_msg_set_position_target_global_int_decode(msg, &packet); - // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { + // exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes + if ((sub.control_mode != GUIDED) + && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided) + && !(sub.control_mode == ALT_HOLD)) { break; } @@ -703,6 +705,11 @@ void GCS_MAVLINK_Sub::handleMessage(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 + sub.pos_control.set_alt_target(packet.alt*100); + break; + } + Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters if (!pos_ignore) {