From 3bb25eb194b427878f12c7ce57a2fe7e0c8a5ff8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Feb 2017 16:47:09 +1100 Subject: [PATCH] Plane: added Q_TAILSIT_INPUT this allows the user to control tailsitters either in body frame (like a plane) or in earth frame (like a multicopter). This is useful for people wanting to learn to fly prop-hang on 3D planes --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/quadplane.cpp | 11 +++++++++-- ArduPlane/quadplane.h | 9 +++++++++ ArduPlane/radio.cpp | 3 +++ ArduPlane/tailsitter.cpp | 21 +++++++++++++++++++-- 5 files changed, 41 insertions(+), 5 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index df07bc27e8..1e6979d251 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -763,7 +763,7 @@ void Plane::update_flight_mode(void) case QRTL: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); - nav_roll_cd = channel_roll->norm_input() * roll_limit; + nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0d3b34dbdc..ea2af4bc71 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -334,7 +334,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Values: 0:Continuous,1:Binary AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), - // @Param: Q_TAILSIT_ANGLE + // @Param: TAILSIT_ANGLE // @DisplayName: Tailsitter transition angle // @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control. // @Range: 5 80 @@ -349,6 +349,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("TILT_RATE_DN", 49, QuadPlane, tilt.max_rate_down_dps, 0), + // @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 will control the aircraft in body frame. + // @Values: 0:MultiCopterInput,1:FixedWingInput + AP_GROUPINFO("TAILSIT_INPUT", 50, QuadPlane, tailsitter.input_type, TAILSITTER_INPUT_MULTICOPTER), + AP_GROUPEND }; @@ -851,7 +857,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) } // add in rudder input - return plane.channel_rudder->norm_input() * 100 * yaw_rate_max; + return plane.channel_rudder->get_control_in() * yaw_rate_max / 45; } /* @@ -1191,6 +1197,7 @@ void QuadPlane::update(void) transition_state = TRANSITION_DONE; } else if (is_tailsitter()) { transition_state = TRANSITION_ANGLE_WAIT; + transition_start_ms = AP_HAL::millis(); } else { transition_state = TRANSITION_AIRSPEED_WAIT; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index b291d0b38f..69d85b3934 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -84,6 +84,9 @@ public: // create outputs for tailsitters void tailsitter_output(void); + // handle different tailsitter input types + void tailsitter_check_input(void); + // check if we have completed transition bool tailsitter_transition_complete(void); @@ -328,9 +331,15 @@ private: bool motors_active:1; } tilt; + enum tailsitter_input { + TAILSITTER_INPUT_MULTICOPTER = 0, + TAILSITTER_INPUT_PLANE = 1, + }; + // tailsitter control variables struct { AP_Int8 transition_angle; + AP_Int8 input_type; } tailsitter; // the attitude view of the VTOL attitude controller diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 557eac5f22..b4e3bb07d9 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -240,6 +240,9 @@ void Plane::read_radio() rudder_input = 0; } + // potentially swap inputs for tailsitters + quadplane.tailsitter_check_input(); + // check for transmitter tuning changes tuning.check_input(control_mode); } diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 6ad5638285..37564a08a5 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -56,10 +56,27 @@ bool QuadPlane::tailsitter_transition_complete(void) // transition immediately return true; } - if (ahrs_view->pitch_sensor < -tailsitter.transition_angle*100 || - ahrs_view->roll_sensor < -tailsitter.transition_angle*100) { + if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle*100 || + labs(ahrs_view->roll_sensor) > tailsitter.transition_angle*100 || + AP_HAL::millis() - transition_start_ms > 2000) { return true; } // still waiting return false; } + +// handle different tailsitter input types +void QuadPlane::tailsitter_check_input(void) +{ + if (tailsitter_active() && + 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 + // channels to do this, as that ensures the value is + // consistent throughout the code + int16_t roll_in = plane.channel_roll->get_control_in(); + int16_t yaw_in = plane.channel_rudder->get_control_in(); + plane.channel_roll->set_control_in(yaw_in); + plane.channel_rudder->set_control_in(-roll_in); + } +}