From 3f405590c7317497a0e61c71e75d247c817500e5 Mon Sep 17 00:00:00 2001 From: PittRBM Date: Thu, 3 Dec 2020 00:02:34 +0700 Subject: [PATCH] AP_Motors: Dual Heli-add support for intermeshing rotor --- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 87 +++++++++++++++------- libraries/AP_Motors/AP_MotorsHeli_Dual.h | 2 + 2 files changed, 64 insertions(+), 25 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index bf78c8beb6..51388fb707 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = { // @Param: DUAL_MODE // @DisplayName: Dual Mode // @Description: Sets the dual mode of the heli, either as tandem or as transverse. - // @Values: 0:Longitudinal, 1:Transverse + // @Values: 0:Longitudinal, 1:Transverse, 2:Intermeshing // @User: Standard AP_GROUPINFO("DUAL_MODE", 9, AP_MotorsHeli_Dual, _dual_mode, AP_MOTORS_HELI_DUAL_MODE_TANDEM), @@ -204,6 +204,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = { // @User: Standard AP_GROUPINFO("DCP_TRIM", 22, AP_MotorsHeli_Dual, _dcp_trim, 0.0f), + // @Param: YAW_REV_EXPO + // @DisplayName: Yaw reverser expo + // @Description: For intermeshing mode only. Yaw revereser smoothing exponent, smoothen transition near zero collective region. Increase this parameter to shink smoothing range. Set to -1 to disable reverser. + // @Range: -1 1000 + // @Increment: 1.0 + // @User: Standard + AP_GROUPINFO("YAW_REV_EXPO", 23, AP_MotorsHeli_Dual, _yaw_rev_expo, -1), + AP_GROUPEND }; @@ -421,6 +429,29 @@ float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, f swash_tilt = -0.45f * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input; } } + } else if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING) { + // roll tilt + if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { + if (swash_num == 1) { + swash_tilt = roll_input; + } else if (swash_num == 2) { + swash_tilt = roll_input; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { + // pitch tilt + if (swash_num == 1) { + swash_tilt = pitch_input - _yaw_scaler * yaw_input; + } else if (swash_num == 2) { + swash_tilt = pitch_input + _yaw_scaler * yaw_input; + } + } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { + // collective + if (swash_num == 1) { + swash_tilt = 0.45f * _dcp_scaler * yaw_input + coll_input; + } else if (swash_num == 2) { + swash_tilt = -0.45f * _dcp_scaler * yaw_input + coll_input; + } + } } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM // roll tilt if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { @@ -502,7 +533,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c limit.throttle_lower = false; limit.throttle_upper = false; - if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { + if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE || _dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING) { if (pitch_out < -_cyclic_max/4500.0f) { pitch_out = -_cyclic_max/4500.0f; limit.pitch = true; @@ -528,29 +559,6 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c collective_in = 1 - collective_in; } - float yaw_compensation = 0.0f; - - // if servo output not in manual mode, process pre-compensation factors - if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) { - // add differential collective pitch yaw compensation - if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { - yaw_compensation = _dcp_yaw_effect * roll_out; - } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM - yaw_compensation = _dcp_yaw_effect * pitch_out; - } - yaw_out = yaw_out + yaw_compensation; - } - - // scale yaw and update limits - if (yaw_out < -_cyclic_max/4500.0f) { - yaw_out = -_cyclic_max/4500.0f; - limit.yaw = true; - } - if (yaw_out > _cyclic_max/4500.0f) { - yaw_out = _cyclic_max/4500.0f; - limit.yaw = true; - } - // constrain collective input float collective_out = collective_in; if (collective_out <= 0.0f) { @@ -574,6 +582,35 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c collective2_out = _collective2_mid_pct; } + // if servo output not in manual mode, process pre-compensation factors + if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) { + // add differential collective pitch yaw compensation + float yaw_compensation = 0.0f; + if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING && _yaw_rev_expo > 0.01f) { + // for intermeshing, reverse yaw in negative collective region and smoothen transition near zero collective + // yaw_compensation range: (-1,1) S-shaped curve (Logistic Model) 1/(1 + e^kt) + yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_mid_pct)))); + yaw_out = yaw_out * yaw_compensation; + } else { + if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { + yaw_compensation = _dcp_yaw_effect * roll_out; + } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM + yaw_compensation = _dcp_yaw_effect * pitch_out; + } + yaw_out = yaw_out + yaw_compensation; + } + } + + // scale yaw and update limits + if (yaw_out < -_cyclic_max/4500.0f) { + yaw_out = -_cyclic_max/4500.0f; + limit.yaw = true; + } + if (yaw_out > _cyclic_max/4500.0f) { + yaw_out = _cyclic_max/4500.0f; + limit.yaw = true; + } + // scale collective pitch for front swashplate (servos 1,2,3) float collective_scaler = ((float)(_collective_max-_collective_min))*0.001f; float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)*0.001f; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 85473cd8f5..c241e297a6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -15,6 +15,7 @@ // tandem modes #define AP_MOTORS_HELI_DUAL_MODE_TANDEM 0 // tandem mode (rotors front and aft) #define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE 1 // transverse mode (rotors side by side) +#define AP_MOTORS_HELI_DUAL_MODE_INTERMESHING 2 // intermeshing mode (rotors side by side) // tandem modes #define AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH 0 // swashplate pitch tilt axis @@ -133,6 +134,7 @@ protected: AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied. AP_Float _yaw_scaler; // scaling factor applied to the yaw mixing AP_Float _dcp_trim; // used to easily trim dcp axis + AP_Float _yaw_rev_expo; // yaw reverser smoothing exponent, for intermeshing mode only. // internal variables float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range