From 32524fbd56bbab6970140ed660256c0c918aa1cf Mon Sep 17 00:00:00 2001 From: IamPete1 <33176108+IamPete1@users.noreply.github.com> Date: Thu, 25 Apr 2019 13:39:27 -0600 Subject: [PATCH 1/5] Plane: add tailsitter gain scaling interpolation --- ArduPlane/quadplane.cpp | 30 +++++++ ArduPlane/tailsitter.cpp | 176 +++++++++++++++++++++++++-------------- 2 files changed, 143 insertions(+), 63 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1e719727ffa50..807ba3b187b96 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -461,6 +461,36 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Advanced AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed, 0), + // @Param: TAILSIT_SPDMIN + // @DisplayName: Tailsitter minimum airspeed scaling + // @Description: bellow this airspeed tailsitter is controlled by copter gains, gains scaled linearly between TAILSIT_SPDMIN and TAILSIT_SPDMAX, this apply s in Q modes and during Q assist if enabled + // @Units: m/s + // @Range: 0 50 + // @User: Standard + AP_GROUPINFO("TAILSIT_SPDMIN", 14, QuadPlane, tailsitter.scaling_speed_min, 10), + + // @Param: TAILSIT_SPDMAX + // @DisplayName: Tailsitter maximum airspeed scaling + // @Description: above this airspeed tailsitter is controlled by plane gains, gains scaled linearly between TAILSIT_SPDMIN and TAILSIT_SPDMAX, this apply s in Q modes and during Q assist if enabled + // @Units: m/s + // @Range: 0 50 + // @User: Standard + AP_GROUPINFO("TAILSIT_SPDMAX", 15, QuadPlane, tailsitter.scaling_speed_max, 20), + + // @Param: TAILSIT_GSCMSK + // @DisplayName: Tailsitter gain scaling mask + // @Description: Bitmask of gain scaling methods to be applied: BOOST: boost gain at low throttle, ATT_THR: reduce gain at high throttle/tilt, INTERP: interpolate between fixed-wing and copter controls + // @User: Standard + // @Bitmask: 1:BOOST,2:ATT_THR,4:INTERP + AP_GROUPINFO("TAILSIT_GSCMSK", 16, QuadPlane, tailsitter.gain_scaling_mask, 0), + + // @Param: TAILSIT_GSCMIN + // @DisplayName: Minimum gain scaling based on throttle and attitude + // @Description: Minimum gain scaling at high throttle/tilt angle + // @Range: 0.1 1 + // @User: Standard + AP_GROUPINFO("TAILSIT_GSCMIN", 17, QuadPlane, tailsitter.gain_scaling_min, 0.4), + AP_GROUPEND }; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 9f54cabfc5a06..524ca79ea87de 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -48,6 +48,51 @@ bool QuadPlane::tailsitter_active(void) return false; } +void QuadPlane::scale_control_surfaces(float& fw_aileron, float& fw_elevator, float& fw_rudder, float& aileron, float& elevator, float& rudder){ + float VTOL_ratio = 1.0f; + if (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_INTERP) { + float aspeed; + bool have_airspeed = ahrs.airspeed_estimate(&aspeed); + const float scaling_range = tailsitter.scaling_speed_max - tailsitter.scaling_speed_min; + if (aspeed > tailsitter.scaling_speed_min && have_airspeed && !is_zero(scaling_range)) { + // apply surface scaling to interpolate between fixed wing and VTOL outputs based on airspeed + // kd0aij note: that the airspeed estimate based only on GPS and (estimated) wind is + // not sufficiently accurate for tailsitters. (based on tests in RealFlight 8 with 10kph wind) + + if (!assisted_flight) { + // match the Q rates with plane controller + // no fixed wing yaw controller so cannot stabilize VTOL roll + const float pitch_rate = attitude_control->get_rate_pitch_pid().get_pid_info().desired * 100; + const float yaw_rate = attitude_control->get_rate_yaw_pid().get_pid_info().desired * 100; + const float speed_scaler = plane.get_speed_scaler(); + + // due to reference frame change roll and yaw are swapped, use roll as rudder input and output direct as with plane + fw_aileron = plane.rollController.get_rate_out(-yaw_rate, speed_scaler); + fw_elevator = plane.pitchController.get_rate_out(pitch_rate, speed_scaler); + fw_rudder = plane.channel_roll->get_control_in(); + } + + // calculate ratio of gains + float fw_ratio = (aspeed - tailsitter.scaling_speed_min) / scaling_range; + fw_ratio = constrain_float(fw_ratio, 0.0f, 1.0f); + VTOL_ratio = 1.0f - fw_ratio; + + // calculate interpolated outputs + aileron = aileron * VTOL_ratio + fw_aileron * fw_ratio; + elevator = elevator * VTOL_ratio + fw_elevator * fw_ratio; + rudder = rudder * VTOL_ratio + fw_rudder * fw_ratio; + } + } + + // scale surface throws using throttle and attitude + float scaling = get_thr_att_gain_scaling(); + rudder = constrain_float(rudder * scaling, -SERVO_MAX, SERVO_MAX); + aileron = constrain_float(aileron * scaling, -SERVO_MAX, SERVO_MAX); + elevator = constrain_float(elevator * scaling, -SERVO_MAX, SERVO_MAX); + + log_CTHP(scaling, VTOL_ratio, ahrs_view->get_gyro_latest()); +} + /* run output for tailsitters */ @@ -57,79 +102,75 @@ void QuadPlane::tailsitter_output(void) return; } - 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); - float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); - tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain; - tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain; - } - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); + // record plane outputs in case they are needed for interpolation + float fw_aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); + float fw_elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); + float fw_rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder); + float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - // 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 - hover thrust, center the rudder and set the altitude controller - integrator to the same throttle level - */ - 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); - } + // thrust vectoring in fixed wing flight + float fw_tilt_left = 0; + float fw_tilt_right = 0; + if (tailsitter.vectored_forward_gain > 0) { + fw_tilt_left = (fw_elevator + fw_aileron) * tailsitter.vectored_forward_gain; + fw_tilt_right = (fw_elevator - fw_aileron) * tailsitter.vectored_forward_gain; + } + + if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) { + // output tilts for forward flight + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, fw_tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, fw_tilt_right); + + // get FW controller throttle demand and mask of motors enabled during forward flight + if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) { + /* + 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 + */ + 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); + + // 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 (tailsitter.motor_mask != 0) { + // set AP_MotorsMatrix throttles enabled for forward flight + motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_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); + if (assisted_flight && tailsitter_transition_fw_complete()) { + hold_stabilize(throttle * 0.01f); + motors_output(true); + } else { + motors_output(false); + } + + // if in Q assist still a good idea to use copter I term and zero plane I to prevent windup plane.pitchController.reset_I(); plane.rollController.reset_I(); // pull in copter control outputs - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, (motors->get_throttle()) * 100); - - if (hal.util->get_soft_armed()) { - // scale surfaces for throttle - tailsitter_speed_scaling(); - } + float aileron = motors->get_yaw()*-SERVO_MAX; + float elevator = motors->get_pitch()*SERVO_MAX; + float rudder = motors->get_roll()*SERVO_MAX; + throttle = motors->get_throttle() * 100; + float tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft); + float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight); if (tailsitter.vectored_hover_gain > 0) { - // thrust vectoring VTOL modes - tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft); - tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight); /* apply extra elevator when at high pitch errors, using a power law. This allows the motors to point straight up for takeoff without integrator windup */ - int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5; + float des_pitch_cd = attitude_control->get_att_target_euler_cd().y; + int32_t pitch_error_cd = (des_pitch_cd - ahrs_view->pitch_sensor) * 0.5; float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX; float extra_sign = extra_pitch > 0?1:-1; float extra_elevator = 0; @@ -144,28 +185,37 @@ void QuadPlane::tailsitter_output(void) motors->limit.pitch = 1; motors->limit.yaw = 1; } - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); - SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); } + scale_control_surfaces(fw_aileron, fw_elevator, fw_rudder, aileron, elevator, rudder); if (tailsitter.input_mask_chan > 0 && tailsitter.input_mask > 0 && RC_Channels::get_radio_in(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()); + 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()); + 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.get_throttle_input(true)); + throttle = plane.get_throttle_input(true); } if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) { - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz()); + rudder = plane.channel_rudder->get_control_in_zero_dz(); } } + + if (hal.util->get_soft_armed()) { + // set outputs + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + } } From 5616bfdcedc2f5fd8fdf9230295c5dcc0c6a1f78 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 25 Apr 2019 13:42:36 -0600 Subject: [PATCH 2/5] Plane: add tailsitter gain scaling options and logging default tailsitter gain scaling to boost: retains original behavior for tailsitters tailsitter qassist bugfix; thanks, @iampete apply roll limit in tailsitter bodyframe roll control add define for future exclusion of tailsitter gainscaling debug code/logging tailsitter bodyframe roll and qacro input scaling bugfixes: respect Q_TAILSIT_RLL_MX and roll/yaw scale parameters in bodyframe roll modes fix unintended swap of Q_ACRO_RLL/YAW_RATE params in QACRO mode --- ArduPlane/quadplane.cpp | 56 ++++++++---- ArduPlane/quadplane.h | 22 ++++- ArduPlane/tailsitter.cpp | 181 ++++++++++++++++++++++----------------- 3 files changed, 157 insertions(+), 102 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 807ba3b187b96..0902c9d5c1d03 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -355,10 +355,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: TAILSIT_THSCMX // @DisplayName: Maximum control throttle scaling value - // @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low thorottle D ossilaitons + // @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low throttle D oscillations // @Range: 1 5 // @User: Standard - AP_GROUPINFO("TAILSIT_THSCMX", 3, QuadPlane, tailsitter.throttle_scale_max, 5), + AP_GROUPINFO("TAILSIT_THSCMX", 3, QuadPlane, tailsitter.throttle_scale_max, 2), // @Param: TRIM_PITCH // @DisplayName: Quadplane AHRS trim pitch @@ -467,7 +467,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Units: m/s // @Range: 0 50 // @User: Standard - AP_GROUPINFO("TAILSIT_SPDMIN", 14, QuadPlane, tailsitter.scaling_speed_min, 10), + AP_GROUPINFO("TAILSIT_SPDMIN", 16, QuadPlane, tailsitter.scaling_speed_min, 10), // @Param: TAILSIT_SPDMAX // @DisplayName: Tailsitter maximum airspeed scaling @@ -475,21 +475,21 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Units: m/s // @Range: 0 50 // @User: Standard - AP_GROUPINFO("TAILSIT_SPDMAX", 15, QuadPlane, tailsitter.scaling_speed_max, 20), + AP_GROUPINFO("TAILSIT_SPDMAX", 17, QuadPlane, tailsitter.scaling_speed_max, 20), // @Param: TAILSIT_GSCMSK // @DisplayName: Tailsitter gain scaling mask // @Description: Bitmask of gain scaling methods to be applied: BOOST: boost gain at low throttle, ATT_THR: reduce gain at high throttle/tilt, INTERP: interpolate between fixed-wing and copter controls // @User: Standard // @Bitmask: 1:BOOST,2:ATT_THR,4:INTERP - AP_GROUPINFO("TAILSIT_GSCMSK", 16, QuadPlane, tailsitter.gain_scaling_mask, 0), + AP_GROUPINFO("TAILSIT_GSCMSK", 18, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_BOOST), // @Param: TAILSIT_GSCMIN // @DisplayName: Minimum gain scaling based on throttle and attitude // @Description: Minimum gain scaling at high throttle/tilt angle // @Range: 0.1 1 // @User: Standard - AP_GROUPINFO("TAILSIT_GSCMIN", 17, QuadPlane, tailsitter.gain_scaling_min, 0.4), + AP_GROUPINFO("TAILSIT_GSCMIN", 19, QuadPlane, tailsitter.gain_scaling_min, 0.4), AP_GROUPEND }; @@ -832,21 +832,42 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) check_attitude_relax(); // tailsitter-only bodyframe roll control options + // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw. if (is_tailsitter()) { + const float euler_pitch = plane.nav_pitch_cd * .01f; + + int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); + // separate limit for tailsitter roll, if set + if (plane.quadplane.tailsitter.max_roll_angle > 0) { + roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f; + } + float roll_rate_limit_cds = plane.quadplane.yaw_rate_max * 100.0f; + + float bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds); + float bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit); 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_m(plane.nav_roll_cd, + // If pitch is small (nose vertical) use roll rate limit for MC yaw rate and roll limit for MC roll angle + if (fabsf(euler_pitch) < 30.0f) { + float yaw_input_scale = roll_rate_limit_cds / roll_limit; + bf_yaw_cds = constrain_float(yaw_input_scale * plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds); + bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit); + } + // multicopter style: rudder stick controls bodyframe roll when hovering + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(bf_yaw_cds, plane.nav_pitch_cd, - yaw_rate_cds); + bf_roll_cd); return; + } 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, + // If pitch is small (nose vertical) use roll rate limit for roll rate and roll limit for bf yaw angle + if (fabsf(euler_pitch) < 30.0f) { + bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_limit, roll_limit); + bf_roll_cd = constrain_float(yaw_rate_cds, -roll_rate_limit_cds, roll_rate_limit_cds); + } + // plane style: rudder stick controls bodyframe yaw when hovering + attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(bf_yaw_cds, plane.nav_pitch_cd, - yaw_rate_cds); + bf_roll_cd); return; } } @@ -1044,9 +1065,8 @@ void QuadPlane::control_qacro(void) float target_yaw = 0; if (is_tailsitter()) { // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw - // acro_roll_rate param applies to yaw in copter frame - target_roll = plane.channel_rudder->norm_input() * acro_roll_rate * 100.0f; - target_yaw = -plane.channel_roll->norm_input() * acro_yaw_rate * 100.0f; + target_roll = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0f; + target_yaw = -plane.channel_roll->norm_input() * acro_roll_rate * 100.0f; } else { target_roll = plane.channel_roll->norm_input() * acro_roll_rate * 100.0f; target_yaw = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 337fa43f0dbfb..597c32d744655 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -114,6 +114,7 @@ class QuadPlane // create outputs for tailsitters void tailsitter_output(void); + void scale_control_surfaces(float& fw_aileron, float& fw_elevator, float& fw_rudder, float& aileron, float& elevator, float& rudder); // handle different tailsitter input types void tailsitter_check_input(void); @@ -124,9 +125,9 @@ class QuadPlane // check if we have completed transition to vtol bool tailsitter_transition_vtol_complete(void) const; - // account for surface speed scaling in hover - void tailsitter_speed_scaling(void); - + // calculate speed scaler of control surfaces in VTOL modes + float get_thr_att_gain_scaling(void); + // user initiated takeoff for guided mode bool do_user_takeoff(float takeoff_altitude); @@ -450,7 +451,13 @@ class QuadPlane TAILSITTER_MASK_THROTTLE = 4, TAILSITTER_MASK_RUDDER = 8, }; - + + enum tailsitter_gscl_mask { + TAILSITTER_GSCL_BOOST = 1, + TAILSITTER_GSCL_ATT_THR = 2, + TAILSITTER_GSCL_INTERP = 4, + }; + // tailsitter control variables struct { AP_Int8 transition_angle; @@ -461,10 +468,17 @@ class QuadPlane AP_Float vectored_hover_gain; AP_Float vectored_hover_power; AP_Float throttle_scale_max; + AP_Float gain_scaling_min; AP_Float max_roll_angle; AP_Int16 motor_mask; + AP_Float scaling_speed_min; + AP_Float scaling_speed_max; + AP_Int8 gain_scaling_mask; } tailsitter; + // tailsitter speed scaler + float last_spd_scaler = 1.0f; + // the attitude view of the VTOL attitude controller AP_AHRS_View *ahrs_view; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 524ca79ea87de..8996bfda2c501 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -18,8 +18,63 @@ to a configuration supported by AP_MotorsMatrix */ +#include #include "Plane.h" +#define GSC_DEBUG 1 +#if GSC_DEBUG +// first order IIR high pass filter +// coefficients b0 = b, b1 = -b +// equation: y_n = b0 x_n + b1 x_{n-1} - a y_{n-1} +// = b (x_n - x_{n-1} - a y_{n-1} +// x1 is the new input value, y is the filtered result +// x0 is used to save the previous input +inline void high_pass(float x1, float &x0, float &y) +{ + // coefficients for nyquist/30 cutoff: + const float b = 0.974482; + const float a = -0.948964; + y = b * (x1 - x0) - a * y; + x0 = x1; +} + +// log speed scaler and highpass filtered gyro rates for validation +void log_CTHP(float spd_scaler, float VTOL_ratio, Vector3f rates) +{ + + // detect oscillation by monitoring gyros + static float in0[3] = {0}; + static float hpOut[3] = {0}; + + // highpass filter each gyro output + high_pass(rates.x, in0[0], hpOut[0]); + high_pass(rates.y, in0[1], hpOut[1]); + high_pass(rates.z, in0[2], hpOut[2]); + + // square then lowpass filter the highpass outputs + constexpr float lpcoef = 0.02f; + static float msq[3] = {0}; + + for (int i=0; i<3; i++) { + msq[i] = (1 - lpcoef) * msq[i] +lpcoef * sq(hpOut[i]); + } + + static uint32_t last_log_time = 0; + uint32_t now = AP_HAL::millis(); + if ((now - last_log_time) >= (1000 / 100)) { + last_log_time = now; + + AP::logger().Write("CTHP", "TimeUS,MSQr,MSQp,MSQy,HPr,HPp,HPy,Scl,SclV", "Qffffffff", + AP_HAL::micros64(), + msq[0], msq[1], msq[2], + hpOut[0], hpOut[1], hpOut[2], + spd_scaler, VTOL_ratio); + } +} +#else +#define log_CTHP(spd_scaler, VTOL_ratio, rates) +#endif // GSC_DEBUG + /* return true when flying a tailsitter */ @@ -174,7 +229,7 @@ void QuadPlane::tailsitter_output(void) float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX; float extra_sign = extra_pitch > 0?1:-1; float extra_elevator = 0; - if (!is_zero(extra_pitch)) { + if (!is_zero(extra_pitch) && in_vtol_mode()) { extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX; } tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain; @@ -265,8 +320,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_P || - tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M || + (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M || + tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P || 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 @@ -288,101 +343,67 @@ bool QuadPlane::in_tailsitter_vtol_transition(void) const } /* - account for speed scaling of control surfaces in VTOL modes + calculate speed scaler of control surfaces in VTOL modes */ -void QuadPlane::tailsitter_speed_scaling(void) +float QuadPlane::get_thr_att_gain_scaling(void) { const float hover_throttle = motors->get_throttle_hover(); const float throttle = motors->get_throttle(); - float spd_scaler = 1; + float spd_scaler = 1.0f; - // If throttle_scale_max is > 1, boost gains at low throttle - if (tailsitter.throttle_scale_max > 1) { - if (is_zero(throttle)) { - spd_scaler = tailsitter.throttle_scale_max; - } else { - spd_scaler = constrain_float(hover_throttle / throttle, 0, tailsitter.throttle_scale_max); - } - } else { + if (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) { // reduce gains when flying at high speed in Q modes: // critical parameter: violent oscillations if too high // sudden loss of attitude control if too low - constexpr float max_atten = 0.2f; + const float min_scale = tailsitter.gain_scaling_min; float tthr = 1.25f * hover_throttle; - float aspeed; - bool airspeed_enabled = ahrs.airspeed_sensor_enabled(); - - // If there is an airspeed sensor use the measured airspeed - // The airspeed estimate based only on GPS and (estimated) wind is - // not sufficiently accurate for tailsitters. - // (based on tests in RealFlight 8 with 10kph wind) - if (airspeed_enabled && ahrs.airspeed_estimate(&aspeed)) { - // plane.get_speed_scaler() doesn't work well for copter tailsitters - // ramp down from 1 to max_atten as speed increases to airspeed_max - spd_scaler = constrain_float(1 - (aspeed / plane.aparm.airspeed_max), max_atten, 1.0f); - } else { - // if no airspeed sensor reduce control surface throws at large tilt - // angles (assuming high airspeed) - // ramp down from 1 to max_atten at tilt angles over trans_angle - // (angles here are represented by their cosines) - - // Note that the cosf call will be necessary if trans_angle becomes a parameter - // but the C language spec does not guarantee that trig functions can be used - // in constant expressions, even though gcc currently allows it. - constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI) - - // alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90))); - constexpr float alpha = (1 - max_atten) / c_trans_angle; - constexpr float beta = 1 - alpha * c_trans_angle; - - const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z; - if (c_tilt < c_trans_angle) { - spd_scaler = constrain_float(beta + alpha * c_tilt, max_atten, 1.0f); - // reduce throttle attenuation threshold too - tthr = 0.5f * hover_throttle; - } + + // reduce control surface throws at large tilt + // angles (assuming high airspeed) + // ramp down from 1 to max_atten at tilt angles over trans_angle + // (angles here are represented by their cosines) + + // Note that the cosf call will be necessary if trans_angle becomes a parameter + // but the C language spec does not guarantee that trig functions can be used + // in constant expressions, even though gcc currently allows it. + constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI) + + // alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90))); + const float alpha = (1 - min_scale) / c_trans_angle; + const float beta = 1 - alpha * c_trans_angle; + + const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z; + if (c_tilt < c_trans_angle) { + spd_scaler = constrain_float(beta + alpha * c_tilt, min_scale, 1.0f); + // reduce throttle attenuation threshold too + tthr = 0.5f * hover_throttle; } // if throttle is above hover thrust, apply additional attenuation if (throttle > tthr) { const float throttle_atten = 1 - (throttle - tthr) / (1 - tthr); spd_scaler *= throttle_atten; - spd_scaler = constrain_float(spd_scaler, max_atten, 1.0f); + spd_scaler = constrain_float(spd_scaler, min_scale, 1.0f); } + + // limit positive and negative slew rates of applied speed scaling + constexpr float posTC = 2.0f; // seconds + constexpr float negTC = 1.0f; // seconds + const float posdelta = plane.G_Dt / posTC; + const float negdelta = plane.G_Dt / negTC; + spd_scaler = constrain_float(spd_scaler, last_spd_scaler - negdelta, last_spd_scaler + posdelta); + last_spd_scaler = spd_scaler; } - // limit positive and negative slew rates of applied speed scaling - constexpr float posTC = 5.0f; // seconds - constexpr float negTC = 2.0f; // seconds - const float posdelta = plane.G_Dt / posTC; - const float negdelta = plane.G_Dt / negTC; - static float last_scale = 0; - static float scale = 0; - if ((spd_scaler - last_scale) > 0) { - if ((spd_scaler - last_scale) > posdelta) { - scale += posdelta; - } else { - scale = spd_scaler; - } - } else { - if ((spd_scaler - last_scale) < -negdelta) { - scale -= negdelta; + + // if gain attenuation isn't active and boost is enabled + if ((spd_scaler >= 1.0f) && (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_BOOST)) { + // boost gains at low throttle + if (is_zero(throttle)) { + spd_scaler = tailsitter.throttle_scale_max; } else { - scale = spd_scaler; + spd_scaler = constrain_float(hover_throttle / throttle, 1.0f, tailsitter.throttle_scale_max); } } - last_scale = scale; - - const SRV_Channel::Aux_servo_function_t functions[5] = { - SRV_Channel::Aux_servo_function_t::k_aileron, - SRV_Channel::Aux_servo_function_t::k_elevator, - SRV_Channel::Aux_servo_function_t::k_rudder, - SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft, - SRV_Channel::Aux_servo_function_t::k_tiltMotorRight}; - for (uint8_t i=0; i Date: Tue, 21 May 2019 17:13:41 -0600 Subject: [PATCH 3/5] AC_AttitudeControl: fix argument order in tailsitter bodyframe roll input methods increase allowed yaw error in tailsitter bodyframe roll modes --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index d975bf2f1fa84..fcd03c564881e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -362,7 +362,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float error_quat.to_axis_angle(att_error); // limit yaw error - if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) { + if (fabsf(att_error.z) < 2*AC_ATTITUDE_THRUST_ERROR_ANGLE) { // update heading _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt); } @@ -423,7 +423,7 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float error_quat.to_axis_angle(att_error); // limit yaw error - if (fabsf(att_error.z) < AC_ATTITUDE_THRUST_ERROR_ANGLE) { + if (fabsf(att_error.z) < 2*AC_ATTITUDE_THRUST_ERROR_ANGLE) { // update heading float yaw_rate = euler_yaw_rate * spitch + body_roll * cpitch; _attitude_target_euler_angle.z = wrap_PI(_attitude_target_euler_angle.z + yaw_rate * _dt); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f02393e74c7bf..7a06b1389e503 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -137,11 +137,11 @@ class AC_AttitudeControl { // Command euler yaw rate and pitch angle with roll angle specified in body frame with multicopter style controls // (used only by tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float euler_yaw_rate_cds, float euler_pitch_angle_cd, float euler_roll_angle_cd); // Command euler yaw rate and pitch angle with roll angle specified in body frame with plane style controls // (used only by tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float euler_yaw_rate_cds, float euler_pitch_angle_cd, float euler_roll_angle_cd); // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); From 7fcb4442180202650a055266d3a32734e1326995 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 25 Jul 2019 08:01:00 -0600 Subject: [PATCH 4/5] Plane: change PID_Info.desired to .target --- ArduPlane/tailsitter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 8996bfda2c501..25fb17170ece4 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -117,8 +117,8 @@ void QuadPlane::scale_control_surfaces(float& fw_aileron, float& fw_elevator, fl if (!assisted_flight) { // match the Q rates with plane controller // no fixed wing yaw controller so cannot stabilize VTOL roll - const float pitch_rate = attitude_control->get_rate_pitch_pid().get_pid_info().desired * 100; - const float yaw_rate = attitude_control->get_rate_yaw_pid().get_pid_info().desired * 100; + const float pitch_rate = attitude_control->get_rate_pitch_pid().get_pid_info().target * 100; + const float yaw_rate = attitude_control->get_rate_yaw_pid().get_pid_info().target * 100; const float speed_scaler = plane.get_speed_scaler(); // due to reference frame change roll and yaw are swapped, use roll as rudder input and output direct as with plane From 9c4ec7d71771e5e17916011524df3cc56a7e0b7c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 1 Aug 2019 08:12:02 -0600 Subject: [PATCH 5/5] AP_Motors: add copter tailsitter differential torque frames --- libraries/AP_Motors/AP_MotorsMatrixTS.cpp | 30 +++++++++++++++++++++++ libraries/AP_Motors/AP_MotorsMatrixTS.h | 2 ++ libraries/AP_Motors/AP_Motors_Class.h | 2 ++ 3 files changed, 34 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp index fa6bcb33d1afc..d7e0da6143510 100644 --- a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp @@ -67,6 +67,11 @@ void AP_MotorsMatrixTS::output_to_motors() // includes new scaling stability patch void AP_MotorsMatrixTS::output_armed_stabilizing() { + if (enable_yaw_torque) { + AP_MotorsMatrix::output_armed_stabilizing(); + return; + } + float roll_thrust; // roll thrust input value, +/- 1.0 float pitch_thrust; // pitch thrust input value, +/- 1.0 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 @@ -127,6 +132,7 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_ } bool success = false; + enable_yaw_torque = true; switch (frame_class) { @@ -140,6 +146,15 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_ case MOTOR_FRAME_QUAD: switch (frame_type) { case MOTOR_FRAME_TYPE_PLUS: + // differential torque for yaw: rotation directions specified below + add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2); + add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); + add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + + success = true; + break; + case MOTOR_FRAME_TYPE_NYT_PLUS: // motors 1,2 on wings, motors 3,4 on vertical tail/subfin // motors 1,2 are counter-rotating, as are motors 3,4 // left wing motor is CW (looking from front) @@ -148,14 +163,29 @@ void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_ add_motor(AP_MOTORS_MOT_2, -90, 0, 4); add_motor(AP_MOTORS_MOT_3, 0, 0, 1); add_motor(AP_MOTORS_MOT_4, 180, 0, 3); + + enable_yaw_torque = false; success = true; break; case MOTOR_FRAME_TYPE_X: // PLUS_TS layout rotated 45 degrees about X axis + // differential torque for yaw: rotation directions specified below + add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + + success = true; + break; + case MOTOR_FRAME_TYPE_NYT_X: + // PLUS_TS layout rotated 45 degrees about X axis + // no differential torque for yaw: wing and fin motors counter-rotating add_motor(AP_MOTORS_MOT_1, 45, 0, 1); add_motor(AP_MOTORS_MOT_2, -135, 0, 3); add_motor(AP_MOTORS_MOT_3, -45, 0, 4); add_motor(AP_MOTORS_MOT_4, 135, 0, 2); + + enable_yaw_torque = false; success = true; break; default: diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.h b/libraries/AP_Motors/AP_MotorsMatrixTS.h index 197e18739f669..3aa9e300e398e 100644 --- a/libraries/AP_Motors/AP_MotorsMatrixTS.h +++ b/libraries/AP_Motors/AP_MotorsMatrixTS.h @@ -22,6 +22,8 @@ class AP_MotorsMatrixTS : public AP_MotorsMatrix { virtual void output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override; protected: + bool enable_yaw_torque; // differential torque for yaw control + // configures the motors for the defined frame_class and frame_type virtual void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index d7d484fbffaa6..b3b9b8568260c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -59,6 +59,8 @@ class AP_Motors { MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only + MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw + MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw }; // Constructor