From 6e4a2b97f903f17f2af0ca9d5490d9e8f8b0158f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 17 Jan 2019 10:54:15 -0700 Subject: [PATCH] ArduPlane:add subclass for copter tailsitters cleanup and add tailsit_motmx add constexpr and motor mask check add support for tricopter tailsitter don't call output_motor_mask unless armed fix whitespace --- ArduPlane/quadplane.cpp | 50 +++++++++++++++++++++++++++++----------- ArduPlane/quadplane.h | 1 + ArduPlane/tailsitter.cpp | 39 +++++++++++++++++++++++-------- 3 files changed, 67 insertions(+), 23 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 790894db82..bb451d0ac7 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -402,6 +402,13 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Advanced AP_GROUPINFO("TRANS_FAIL", 8, QuadPlane, transition_failure, 0), + // @Param: TAILSIT_MOTMX + // @DisplayName: Tailsiter mask + // @Description: Bitmask of motors to remain active in forward flight for a 'copter' tailsitter. Non-zero indicates airframe is a tailsitter which pitches forward 90 degrees in forward flight modes. + // @User: Standard + // @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8 + AP_GROUPINFO("TAILSIT_MOTMX", 9, QuadPlane, tailsitter.motor_mask, 0), + AP_GROUPEND }; @@ -563,21 +570,36 @@ bool QuadPlane::setup(void) break; } - switch (motor_class) { - case AP_Motors::MOTOR_FRAME_TRI: - motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed); - motors_var_info = AP_MotorsTri::var_info; - break; - case AP_Motors::MOTOR_FRAME_TAILSITTER: - motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); - motors_var_info = AP_MotorsTailsitter::var_info; + if (tailsitter.motor_mask == 0) { + // this is a normal quadplane + switch (motor_class) { + case AP_Motors::MOTOR_FRAME_TRI: + motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors_var_info = AP_MotorsTri::var_info; + break; + case AP_Motors::MOTOR_FRAME_TAILSITTER: + // this is a duo-motor tailsitter (vectored thrust if tilt.tilt_mask != 0) + motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors_var_info = AP_MotorsTailsitter::var_info; + rotation = ROTATION_PITCH_90; + break; + default: + motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors_var_info = AP_MotorsMatrix::var_info; + break; + } + } else { + // this is a copter tailsitter with motor layout specified by frame_class and frame_type + // tilting motors are not supported (tiltrotor control variables are ignored) + if (tilt.tilt_mask != 0) { + hal.console->printf("Warning tilting motors not supported, setting tilt_mask to zero\n"); + tilt.tilt_mask.set(0); + } rotation = ROTATION_PITCH_90; - break; - default: - motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed); - motors_var_info = AP_MotorsMatrix::var_info; - break; + motors = new AP_MotorsMatrixTS(plane.scheduler.get_loop_rate_hz(), rc_speed); + motors_var_info = AP_MotorsMatrixTS::var_info; } + const static char *strUnableToAllocate = "Unable to allocate"; if (!motors) { hal.console->printf("%s motors\n", strUnableToAllocate); @@ -644,7 +666,7 @@ bool QuadPlane::setup(void) setup_defaults(); AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table)); - + gcs().send_text(MAV_SEVERITY_INFO, "QuadPlane initialised"); initialised = true; return true; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c66f248501..7bf3137159 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -432,6 +432,7 @@ private: AP_Float vectored_hover_power; AP_Float throttle_scale_max; AP_Float max_roll_angle; + AP_Int16 motor_mask; } tailsitter; // the attitude view of the VTOL attitude controller diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 67e15efbe9..4bb534b872 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -14,6 +14,8 @@ */ /* control code for tailsitters. Enabled by setting Q_FRAME_CLASS=10 + or by setting Q_TAILSIT_MOTMX nonzero and Q_FRAME_CLASS and Q_FRAME_TYPE + to a configuration supported by AP_MotorsMatrix */ #include "Plane.h" @@ -23,7 +25,8 @@ */ bool QuadPlane::is_tailsitter(void) const { - return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER; + return available() && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || + (tailsitter.motor_mask != 0)); } /* @@ -55,8 +58,11 @@ void QuadPlane::tailsitter_output(void) float tilt_left = 0.0f; float tilt_right = 0.0f; + uint16_t mask = tailsitter.motor_mask; + // handle forward flight modes and transition to VTOL modes if (!tailsitter_active() || in_tailsitter_vtol_transition()) { + // in forward flight: set motor tilt servos and throttles using FW controller if (tailsitter.vectored_forward_gain > 0) { // thrust vectoring in fixed wing flight float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); @@ -66,23 +72,38 @@ void QuadPlane::tailsitter_output(void) } SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); - - if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) { + + // get FW controller throttle demand and mask of motors enabled during forward flight + float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); + if (hal.util->get_soft_armed()) { + if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) { /* - during transitions to vtol mode set the throttle to the - hover throttle, and set the altitude controller + during transitions to vtol mode set the throttle to + hover thrust, center the rudder and set the altitude controller integrator to the same throttle level */ - uint8_t throttle = motors->get_throttle_hover() * 100; - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); + throttle = motors->get_throttle_hover() * 100; SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); pos_control->get_accel_z_pid().set_integrator(throttle*10); + + if (mask == 0) { + // override AP_MotorsTailsitter throttles during back transition + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); + } + } + if (mask != 0) { + // set AP_MotorsMatrix throttles enabled for forward flight + motors->output_motor_mask(throttle * 0.01f, mask, plane.rudder_dt); + } } return; } + // handle VTOL modes + // the MultiCopter rate controller has already been run in an earlier call + // to motors_output() from quadplane.update() motors_output(false); plane.pitchController.reset_I(); plane.rollController.reset_I();