diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 58d4afd6f3..25c255d6d8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2615,6 +2615,10 @@ void QuadPlane::vtol_position_controller(void) // call attitude controller disable_yaw_rate_time_constant(); + + // setup scaling of roll and pitch angle P gains to match fixed wing gains + setup_rp_fw_angle_gains(); + if (have_target_yaw) { attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, @@ -4330,4 +4334,45 @@ bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH)); } +/* + setup scaling of roll and pitch angle P gains to match fixed wing gains + + we setup the angle P gain to match fixed wing at high speed (above + ARSPD_FBW_MIN) where fixed wing surfaces are presumed to + dominate. At lower speeds we use the multicopter angle P gains. +*/ +void QuadPlane::setup_rp_fw_angle_gains(void) +{ + const float mc_angR = attitude_control->get_angle_roll_p().kP(); + const float mc_angP = attitude_control->get_angle_pitch_p().kP(); + const float fw_angR = 1.0/plane.rollController.tau(); + const float fw_angP = 1.0/plane.pitchController.tau(); + + if (!is_positive(mc_angR) || !is_positive(mc_angP)) { + // bad configuration, don't scale + return; + } + + float aspeed; + if (!ahrs.airspeed_estimate(aspeed)) { + // can't get airspeed, no scaling of VTOL angle gains + return; + } + + const float low_airspeed = 3.0; + if (aspeed <= low_airspeed || plane.aparm.airspeed_min <= low_airspeed) { + // no scaling + return; + } + + const float angR_scale = linear_interpolate(mc_angR, fw_angR, + aspeed, + low_airspeed, plane.aparm.airspeed_min) / mc_angR; + const float angP_scale = linear_interpolate(mc_angP, fw_angP, + aspeed, + low_airspeed, plane.aparm.airspeed_min) / mc_angP; + const Vector3f gain_scale{angR_scale, angP_scale, 1.0}; + attitude_control->set_angle_P_scale(gain_scale); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 72de5c0897..2ce137b022 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -665,6 +665,11 @@ private: */ float get_scaled_wp_speed(float target_bearing_deg) const; + /* + setup scaling of roll and pitch angle P gains to match fixed wing gains + */ + void setup_rp_fw_angle_gains(void); + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,