diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 16568f65452..9567f78f880 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 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 @@ -470,6 +470,20 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0), + // @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", 17, 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", 18, QuadPlane, tailsitter.gain_scaling_min, 0.4), + AP_GROUPEND }; @@ -811,21 +825,41 @@ 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; } } @@ -1023,9 +1057,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 128d8a014bd..3d3a85ca495 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -124,9 +124,9 @@ public: // check if we have completed transition to vtol bool tailsitter_transition_vtol_complete(void) const; - // account for surface speed scaling in hover + // account for control surface speed scaling in VTOL modes void tailsitter_speed_scaling(void); - + // user initiated takeoff for guided mode bool do_user_takeoff(float takeoff_altitude); @@ -455,7 +455,13 @@ private: 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; @@ -466,10 +472,17 @@ private: 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 9f54cabfc5a..ebece0f9710 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -18,6 +18,7 @@ to a configuration supported by AP_MotorsMatrix */ +#include #include "Plane.h" /* @@ -103,7 +104,7 @@ void QuadPlane::tailsitter_output(void) } // handle VTOL modes - // the MultiCopter rate controller has already been run in an earlier call + // 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(); @@ -129,11 +130,12 @@ void QuadPlane::tailsitter_output(void) 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; - 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; @@ -215,8 +217,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 @@ -244,95 +246,70 @@ void QuadPlane::tailsitter_speed_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) + // 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) + // 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; + // 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, max_atten, 1.0f); - // reduce throttle attenuation threshold too - tthr = 0.5f * hover_throttle; - } + 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 = 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; - } else { - scale = spd_scaler; - } - } - last_scale = scale; - const SRV_Channel::Aux_servo_function_t functions[5] = { + // 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; + } + + // 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 { + spd_scaler = constrain_float(hover_throttle / throttle, 1.0f, tailsitter.throttle_scale_max); + } + } + + const SRV_Channel::Aux_servo_function_t functions[] = { 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}; + SRV_Channel::Aux_servo_function_t::k_rudder}; for (uint8_t i=0; i