From 93b0519ad258546b80d775d6de89ad2cce676ab0 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Wed, 21 Dec 2022 19:53:04 -0500 Subject: [PATCH] AP_MotorsHeli: improve collective to yaw compensation --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 25 +++++++++++--------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 4 ++-- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index ce33484e14..51097e2f80 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -46,13 +46,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // Index 7 was used for phase angle and should not be used - // @Param: COLYAW - // @DisplayName: Collective-Yaw Mixing - // @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. - // @Range: -10 10 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("COLYAW", 8, AP_MotorsHeli_Single, _collective_yaw_effect, 0), + // Indice 8 was used by COLYAW and should not be used // @Param: FLYBAR_MODE // @DisplayName: Flybar Mode Selector @@ -135,6 +129,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Increment: 1 AP_SUBGROUPINFO(_swashplate, "SW_", 20, AP_MotorsHeli_Single, AP_MotorsHeli_Swash), + // @Param: COL2YAW + // @DisplayName: Collective-Yaw Mixing + // @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. + // @Range: -2 2 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("COL2YAW", 21, AP_MotorsHeli_Single, _collective_yaw_scale, 0), + AP_GROUPEND }; @@ -466,10 +468,11 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque // also not required if we are using external gyro if ((_main_rotor.get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // sanity check collective_yaw_effect - _collective_yaw_effect.set(constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); - // the 4.5 scaling factor is to bring the values in line with previous releases - yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_zero_thrust_pct) / 4.5f; + // sanity check collective_yaw_scale + _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); + // This feedforward compensation follows the hover performance theory that hover power required + // is a function of gross weight to the 3/2 power + yaw_offset = _collective_yaw_scale * powf(fabsf(collective_out - _collective_zero_thrust_pct),1.5f); } } else { yaw_offset = 0.0f; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index c2079e7afa..66bfd8f6dc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -29,7 +29,7 @@ #define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350 // COLYAW parameter min and max values -#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f +#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 5.0f // maximum number of swashplate servos #define AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS 3 @@ -140,9 +140,9 @@ protected: AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch AP_Int16 _ext_gyro_gain_std; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro AP_Int16 _ext_gyro_gain_acro; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro in ACRO - AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) + AP_Float _collective_yaw_scale; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. bool _acro_tail = false; };