Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Heli_Dual: refactor swashplate mixing

This commit is contained in:
Iampete1 2023-10-30 21:24:56 +00:00 committed by Andrew Tridgell
parent 2fe100d7bc
commit aa8c477a26
2 changed files with 82 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -302,81 +302,61 @@ void AP_MotorsHeli_Dual::calculate_scalars()
calculate_armed_scalars(); calculate_armed_scalars();
} }
// get_swashplate - calculate movement of each swashplate based on configuration // Mix and output swashplates for tandem
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) 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; // Differential cyclic roll is used for yaw and combined for roll
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { const float swash1_roll = roll_input + _yaw_scaler * yaw_input;
// roll tilt const float swash2_roll = roll_input - _yaw_scaler * yaw_input;
if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) {
if (swash_num == 1) { // cyclic is not used for pitch control
swash_tilt = 0.0f; const float swash_pitch = 0.0;
} else if (swash_num == 2) {
swash_tilt = 0.0f; // 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;
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { const float swash2_coll = -0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
// pitch tilt
if (swash_num == 1) { // Calculate servo positions in swashplate library
swash_tilt = pitch_input - _yaw_scaler * yaw_input; _swashplate1.calculate(swash1_roll, swash_pitch, swash1_coll);
} else if (swash_num == 2) { _swashplate2.calculate(swash2_roll, swash_pitch, swash2_coll);
swash_tilt = pitch_input + _yaw_scaler * yaw_input; }
}
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { // Mix and output swashplates for transverse
// collective void AP_MotorsHeli_Dual::mix_transverse(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input)
if (swash_num == 1) { {
swash_tilt = 0.45f * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input; // cyclic is not used for roll control
} else if (swash_num == 2) { const float swash_roll = 0.0;
swash_tilt = -0.45f * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input;
} // Differential cyclic pitch is used for yaw
} const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input;
} else if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING) { const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input;
// roll tilt
if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { // Differential collective for roll and combined for thrust
if (swash_num == 1) { const float swash1_coll = 0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input;
swash_tilt = roll_input; const float swash2_coll = -0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
} else if (swash_num == 2) {
swash_tilt = roll_input; // Calculate servo positions in swashplate library
} _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) { _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
// pitch tilt }
if (swash_num == 1) {
swash_tilt = pitch_input - _yaw_scaler * yaw_input; // Mix and output swashplates for intermeshing
} else if (swash_num == 2) { void AP_MotorsHeli_Dual::mix_intermeshing(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input)
swash_tilt = pitch_input + _yaw_scaler * yaw_input; {
} // Direct roll control on both swash plates
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { const float swash_roll = roll_input;
// collective
if (swash_num == 1) { // Differential cyclic pitch is used for yaw and combined for pitch
swash_tilt = 0.45f * _dcp_scaler * yaw_input + coll_input; const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input;
} else if (swash_num == 2) { const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input;
swash_tilt = -0.45f * _dcp_scaler * yaw_input + coll_input;
} // Differential collective for yaw and combined for thrust
} const float swash1_coll = 0.45 * _dcp_scaler * yaw_input + collective1_input;
} else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM const float swash2_coll = -0.45 * _dcp_scaler * yaw_input + collective2_input;
// roll tilt
if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) { // Calculate servo positions in swashplate library
if (swash_num == 1) { _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
swash_tilt = roll_input + _yaw_scaler * yaw_input; _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
} 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;
} }
// update_motor_controls - sends commands to motor controllers // 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? // ToDo: add main rotor cyclic power?
_main_rotor.set_collective(fabsf(collective_out)); _main_rotor.set_collective(fabsf(collective_out));
// compute swashplate tilt // Mix swash plate
float swash1_pitch = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective_out_scaled); switch (_dual_mode) {
float swash1_roll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective_out_scaled); case AP_MOTORS_HELI_DUAL_MODE_TANDEM:
float swash1_coll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective_out_scaled); default:
float swash2_pitch = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective2_out_scaled); mix_tandem(pitch_out, roll_out, yaw_out, collective_out_scaled, 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); break;
float swash2_coll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective2_out_scaled);
case AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE:
// Calculate servo positions in swashplate library mix_transverse(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled);
_swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll); break;
_swashplate2.calculate(swash2_roll, swash2_pitch, swash2_coll);
case AP_MOTORS_HELI_DUAL_MODE_INTERMESHING:
mix_intermeshing(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled);
break;
}
} }

17
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -17,11 +17,6 @@
#define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE 1 // transverse mode (rotors side by side) #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) #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 // default differential-collective-pitch scaler
#define AP_MOTORS_HELI_DUAL_DCP_SCALER 0.25f #define AP_MOTORS_HELI_DUAL_DCP_SCALER 0.25f
@ -110,4 +105,16 @@ protected:
// internal variables // internal variables
float _collective2_zero_thrst_pct; 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);
}; };