diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 7aca12806a..ddb7846de4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -144,6 +144,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @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 AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, _collective_yaw_effect, 0), // @Param: GOV_SETPOINT @@ -606,6 +607,8 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // the feed-forward is not required when the motor is shut down and not creating torque // also not required if we are using external gyro if ((_rotor_desired > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { + // sanity check collective_yaw_effect + _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index c56645da21..01ce119370 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -59,6 +59,8 @@ // minimum outputs for direct drive motors #define AP_MOTOR_HELI_DDTAIL_DEFAULT 500 +// COLYAW parameter min and max values +#define AP_MOTOR_HELI_COLYAW_RANGE 10.0f // main rotor speed control types (ch8 out) #define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver, pilot controls ESC speed through transmitter directly @@ -275,7 +277,7 @@ private: AP_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro AP_Int8 _servo_manual; // Pass radio inputs directly to servos during set-up through mission planner AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem - AP_Int16 _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_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_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed