diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 361801c237..9bba585d0f 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -131,6 +131,16 @@ float Plane::stabilize_roll_get_roll_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); + + // scale FF to angle P + if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { + const float mc_angR = quadplane.attitude_control->get_angle_roll_p().kP() + * quadplane.attitude_control->get_last_angle_P_scale().x; + if (is_positive(mc_angR)) { + rollController.set_ff_scale(MIN(1.0, 1.0 / (mc_angR * rollController.tau()))); + } + } + const float roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers @@ -174,6 +184,16 @@ float Plane::stabilize_pitch_get_pitch_out() if (!quadplane.use_fw_attitude_controllers()) { // use the VTOL rate for control, to ensure consistency const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); + + // scale FF to angle P + if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) { + const float mc_angP = quadplane.attitude_control->get_angle_pitch_p().kP() + * quadplane.attitude_control->get_last_angle_P_scale().y; + if (is_positive(mc_angP)) { + pitchController.set_ff_scale(MIN(1.0, 1.0 / (mc_angP * pitchController.tau()))); + } + } + const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler); /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent opposing integrators balancing between the two controllers diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c88529fc27..a44f9d287f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -279,6 +279,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Bitmask: 19: CompleteTransition-to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND // @Bitmask: 20: Force RTL mode-forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL) // @Bitmask: 21: Tilt rotor-tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes. + // @Bitmask: 22: Scale FF by the ratio of VTOL/plane angle P gains in VTOL modes rather than reducing VTOL angle P based on airspeed. AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ae65c3768e..33d4254134 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -581,6 +581,7 @@ private: TRANS_FAIL_TO_FW=(1<<19), FS_RTL=(1<<20), DISARMED_TILT_UP=(1<<21), + SCALE_FF_ANGLE_P=(1<<22), }; bool option_is_set(OPTION option) const { return (options.get() & int32_t(option)) != 0;