From ba415c99fed8a7cbbffbdff4bae8d07179f2ef25 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 20 Feb 2020 15:40:01 -0300 Subject: [PATCH] Sub: send Roll/Pitch Toggle flag --- ArduSub/GCS_Mavlink.cpp | 3 +++ ArduSub/Sub.h | 3 +++ ArduSub/joystick.cpp | 1 - 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 203296e634..777cc8739d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -140,6 +140,9 @@ bool GCS_MAVLINK_Sub::send_info() CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("InputHold", sub.input_hold_engaged); + CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); + send_named_float("RollPitch", sub.roll_pitch_flag); + return true; } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4f83861257..9cc72bddfc 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -308,6 +308,9 @@ private: // Flag indicating if we are currently using input hold bool input_hold_engaged; + // Flag indicating if we are currently controlling Pitch and Roll instead of forward/lateral + bool roll_pitch_flag = false; + // 3D Location vectors // Current location of the Sub (altitude is relative to home) Location current_loc; diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index d9c72d3ac0..89f6bf2f6f 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -24,7 +24,6 @@ const uint8_t SERVO_CHAN_1 = 9; // Pixhawk Aux1 const uint8_t SERVO_CHAN_2 = 10; // Pixhawk Aux2 const uint8_t SERVO_CHAN_3 = 11; // Pixhawk Aux3 -uint8_t roll_pitch_flag = false; // Flag to adjust roll/pitch instead of forward/lateral bool controls_reset_since_input_hold = true; }