From c061d5615b59b6f4c531487b914aaba046cbee43 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 24 Feb 2017 18:02:34 +1100
Subject: [PATCH] Plane: added manual input mask for tailsitter prop-hang

This adds new parameters Q_TAILSIT_MASK and Q_TAILSIT_MASKCH. These
parameters allow a user to use the tailsitter capabilities of a 3D
plane to teach themselves to prop-hang.

It works by allowing the user to setup a switch on their transmitter
to enable manual pass-thru of a set of the input channels to outputs
when in tailsitter hover. The user can then use that switch to allow
learning of hover control in a 3D plane one channel (or two channels)
at a time.
---
 ArduPlane/quadplane.cpp  | 12 ++++++++++++
 ArduPlane/quadplane.h    |  9 +++++++++
 ArduPlane/tailsitter.cpp | 28 ++++++++++++++++++++++++----
 3 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index ea2af4bc71..08db78cb6e 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -355,6 +355,18 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Values: 0:MultiCopterInput,1:FixedWingInput
     AP_GROUPINFO("TAILSIT_INPUT", 50, QuadPlane, tailsitter.input_type, TAILSITTER_INPUT_MULTICOPTER),
 
+    // @Param: TAILSIT_MASK
+    // @DisplayName: Tailsitter input mask
+    // @Description: This controls what channels have full manual control when hovering as a tailsitter and the Q_TAILSIT_MASKCH channel in high. This can be used to teach yourself to prop-hang a 3D plane by learning one or more channels at a time.
+    // @Bitmask: 0:Aileron,1:Elevator,2:Throttle,3:Rudder
+    AP_GROUPINFO("TAILSIT_MASK", 51, QuadPlane, tailsitter.input_mask, 0),
+
+    // @Param: TAILSIT_MASKCH
+    // @DisplayName: Tailsitter input mask channel
+    // @Description: This controls what input channel will activate the Q_TAILSIT_MASK mask. When this channel goes above 1700 then the pilot will have direct manual control of the output channels specified in Q_TAILSIT_MASK. Set to zero to disable.
+    // @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8
+    AP_GROUPINFO("TAILSIT_MASKCH", 52, QuadPlane, tailsitter.input_mask_chan, 0),
+    
     AP_GROUPEND
 };
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 69d85b3934..6ececfc1df 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -335,11 +335,20 @@ private:
         TAILSITTER_INPUT_MULTICOPTER = 0,
         TAILSITTER_INPUT_PLANE       = 1,
     };
+
+    enum tailsitter_mask {
+        TAILSITTER_MASK_AILERON  = 1,
+        TAILSITTER_MASK_ELEVATOR = 2,
+        TAILSITTER_MASK_THROTTLE = 4,
+        TAILSITTER_MASK_RUDDER   = 8,
+    };
     
     // tailsitter control variables
     struct {
         AP_Int8 transition_angle;
         AP_Int8 input_type;
+        AP_Int8 input_mask;
+        AP_Int8 input_mask_chan;
     } tailsitter;
 
     // the attitude view of the VTOL attitude controller
diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index 37564a08a5..647bf82e36 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -39,10 +39,30 @@ bool QuadPlane::tailsitter_active(void)
  */
 void QuadPlane::tailsitter_output(void)
 {
-    if (tailsitter_active()) {
-        motors_output();
-        plane.pitchController.reset_I();
-        plane.rollController.reset_I();
+    if (!tailsitter_active()) {
+        return;
+    }
+
+    motors_output();
+    plane.pitchController.reset_I();
+    plane.rollController.reset_I();
+
+    if (tailsitter.input_mask_chan > 0 &&
+        tailsitter.input_mask > 0 &&
+        hal.rcin->read(tailsitter.input_mask_chan-1) > 1700) {
+        // the user is learning to prop-hang
+        if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
+        }
+        if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
+        }
+        if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.channel_throttle->get_control_in_zero_dz());
+        }
+        if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());
+        }
     }
 }