From e0765747e70cc149ffdf9a6cdcede6ab199adeb7 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 16 Mar 2019 06:51:05 -0600 Subject: [PATCH] Plane: add new tailsitter bodyframe roll option --- ArduPlane/quadplane.cpp | 14 +++++++++----- ArduPlane/quadplane.h | 3 ++- ArduPlane/tailsitter.cpp | 3 ++- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 45462c1d21..adb25bc56b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -278,7 +278,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: TAILSIT_INPUT // @DisplayName: Tailsitter input type // @Description: This controls whether stick input when hovering as a tailsitter follows the conventions for fixed wing hovering or multicopter hovering. When multicopter input is selected the roll stick will roll the aircraft in earth frame and yaw stick will yaw in earth frame. When using fixed wing input the roll and yaw sticks are swapped so that the roll stick controls earth-frame yaw and rudder controls earth-frame roll. When body-frame roll is selected, the yaw stick controls earth-frame yaw rate and the roll stick controls roll in the tailsitter's body frame. - // @Values: 0:MultiCopterInput,1:FixedWingInput,2:BodyFrameRoll + // @Values: 0:MultiCopterInput,1:FixedWingInput,2:BodyFrameRoll_M,3:BodyFrameRoll_P AP_GROUPINFO("TAILSIT_INPUT", 50, QuadPlane, tailsitter.input_type, TAILSITTER_INPUT_MULTICOPTER), // @Param: TAILSIT_MASK @@ -738,15 +738,19 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) check_attitude_relax(); if (in_vtol_mode() || is_tailsitter()) { - if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL) { + if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M) { // Angle mode attitude control for pitch and body-frame roll, rate control for yaw. // this version interprets the first argument as yaw rate and the third as roll angle // because it is intended to be used with Q_TAILSIT_INPUT=1 where the roll and yaw sticks // act in the tailsitter's body frame (i.e. roll is MC/earth frame yaw and // yaw is MC/earth frame roll) - attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(plane.nav_roll_cd, - plane.nav_pitch_cd, - yaw_rate_cds); + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_cds); + } else if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P) { + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_cds); } else { // use euler angle attitude control attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 891c27b182..9d5d3d7300 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -414,7 +414,8 @@ private: enum tailsitter_input { TAILSITTER_INPUT_MULTICOPTER = 0, TAILSITTER_INPUT_PLANE = 1, - TAILSITTER_INPUT_BF_ROLL = 2, + TAILSITTER_INPUT_BF_ROLL_M = 2, + TAILSITTER_INPUT_BF_ROLL_P = 3, }; enum tailsitter_mask { diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index d058d61a56..5cac733c74 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -210,7 +210,8 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const void QuadPlane::tailsitter_check_input(void) { if (tailsitter_active() && - (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL || + (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P || + tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M || tailsitter.input_type == TAILSITTER_INPUT_PLANE)) { // the user has asked for body frame controls when tailsitter // is active. We switch around the control_in value for the