From aa8c477a26fea9034208a65fd937f70fab661ff3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 30 Oct 2023 21:24:56 +0000 Subject: [PATCH] AP_Motors: Heli_Dual: refactor swashplate mixing --- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 155 ++++++++++----------- libraries/AP_Motors/AP_MotorsHeli_Dual.h | 17 ++- 2 files changed, 82 insertions(+), 90 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 290a3f9a2e..06abef19c2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -302,81 +302,61 @@ void AP_MotorsHeli_Dual::calculate_scalars() calculate_armed_scalars(); } -// get_swashplate - calculate movement of each swashplate based on configuration -float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input) +// Mix and output swashplates for tandem +void AP_MotorsHeli_Dual::mix_tandem(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input) { - float swash_tilt = 0.0f; - if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { - // roll tilt - if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { - if (swash_num == 1) { - swash_tilt = 0.0f; - } else if (swash_num == 2) { - swash_tilt = 0.0f; - } - } 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 * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input; - } else if (swash_num == 2) { - 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) { - if (swash_num == 1) { - swash_tilt = roll_input + _yaw_scaler * yaw_input; - } else if (swash_num == 2) { - swash_tilt = roll_input - _yaw_scaler * yaw_input; - } - } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { - // pitch tilt - if (swash_num == 1) { - swash_tilt = 0.0f; - } else if (swash_num == 2) { - swash_tilt = 0.0f; - } - } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { - // collective - if (swash_num == 1) { - swash_tilt = 0.45f * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input; - } else if (swash_num == 2) { - swash_tilt = -0.45f * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input; - } - } - } - return swash_tilt; + // Differential cyclic roll is used for yaw and combined for roll + const float swash1_roll = roll_input + _yaw_scaler * yaw_input; + const float swash2_roll = roll_input - _yaw_scaler * yaw_input; + + // cyclic is not used for pitch control + const float swash_pitch = 0.0; + + // Differential collective for pitch and combined for thrust + const float swash1_coll = 0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input; + const float swash2_coll = -0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input; + + // Calculate servo positions in swashplate library + _swashplate1.calculate(swash1_roll, swash_pitch, swash1_coll); + _swashplate2.calculate(swash2_roll, swash_pitch, swash2_coll); +} + +// Mix and output swashplates for transverse +void AP_MotorsHeli_Dual::mix_transverse(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input) +{ + // cyclic is not used for roll control + const float swash_roll = 0.0; + + // Differential cyclic pitch is used for yaw + const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input; + const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input; + + // Differential collective for roll and combined for thrust + const float swash1_coll = 0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input; + const float swash2_coll = -0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input; + + // Calculate servo positions in swashplate library + _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll); + _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll); +} + +// Mix and output swashplates for intermeshing +void AP_MotorsHeli_Dual::mix_intermeshing(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input) +{ + // Direct roll control on both swash plates + const float swash_roll = roll_input; + + // Differential cyclic pitch is used for yaw and combined for pitch + const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input; + const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input; + + // Differential collective for yaw and combined for thrust + const float swash1_coll = 0.45 * _dcp_scaler * yaw_input + collective1_input; + const float swash2_coll = -0.45 * _dcp_scaler * yaw_input + collective2_input; + + // Calculate servo positions in swashplate library + _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll); + _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll); } // update_motor_controls - sends commands to motor controllers @@ -516,17 +496,22 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c // ToDo: add main rotor cyclic power? _main_rotor.set_collective(fabsf(collective_out)); - // compute swashplate tilt - float swash1_pitch = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective_out_scaled); - float swash1_roll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective_out_scaled); - float swash1_coll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective_out_scaled); - float swash2_pitch = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective2_out_scaled); - float swash2_roll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective2_out_scaled); - float swash2_coll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective2_out_scaled); - - // Calculate servo positions in swashplate library - _swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll); - _swashplate2.calculate(swash2_roll, swash2_pitch, swash2_coll); + // Mix swash plate + switch (_dual_mode) { + case AP_MOTORS_HELI_DUAL_MODE_TANDEM: + default: + mix_tandem(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled); + break; + + case AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE: + mix_transverse(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled); + break; + + case AP_MOTORS_HELI_DUAL_MODE_INTERMESHING: + mix_intermeshing(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled); + break; + + } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 5fefd3201b..1c56e07367 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -17,11 +17,6 @@ #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 -#define AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL 1 // swashplate roll tilt axis -#define AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL 2 // swashplate collective axis - // default differential-collective-pitch scaler #define AP_MOTORS_HELI_DUAL_DCP_SCALER 0.25f @@ -110,4 +105,16 @@ protected: // internal variables float _collective2_zero_thrst_pct; + +private: + + // Mix and output swashplates for tandem + void mix_tandem(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input); + + // Mix and output swashplates for transverse + void mix_transverse(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input); + + // Mix and output swashplates for intermeshing + void mix_intermeshing(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input); + };