Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add option to scale FF in VTOL modes based on ratio of angle gains

Browse Source
This commit is contained in:
Andy Piper 2023-07-03 19:51:36 +01:00 committed by Andrew Tridgell
parent ab24f97275
commit f25e67e3eb
3 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/Attitude.cpp
View File

@ -131,6 +131,16 @@ float Plane::stabilize_roll_get_roll_out()
if (!quadplane.use_fw_attitude_controllers()) { if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency // use the VTOL rate for control, to ensure consistency
const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); 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); 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 /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
opposing integrators balancing between the two controllers opposing integrators balancing between the two controllers
@ -174,6 +184,16 @@ float Plane::stabilize_pitch_get_pitch_out()
if (!quadplane.use_fw_attitude_controllers()) { if (!quadplane.use_fw_attitude_controllers()) {
// use the VTOL rate for control, to ensure consistency // use the VTOL rate for control, to ensure consistency
const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); 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); 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 /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
opposing integrators balancing between the two controllers opposing integrators balancing between the two controllers

1
ArduPlane/quadplane.cpp
View File

@ -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: 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: 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: 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_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),

1
ArduPlane/quadplane.h
View File

@ -581,6 +581,7 @@ private:
TRANS_FAIL_TO_FW=(1<<19), TRANS_FAIL_TO_FW=(1<<19),
FS_RTL=(1<<20), FS_RTL=(1<<20),
DISARMED_TILT_UP=(1<<21), DISARMED_TILT_UP=(1<<21),
SCALE_FF_ANGLE_P=(1<<22),
}; };
bool option_is_set(OPTION option) const { bool option_is_set(OPTION option) const {
return (options.get() & int32_t(option)) != 0; return (options.get() & int32_t(option)) != 0;