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()); + } } }