mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: Heli_Dual: refactor swashplate mixing
This commit is contained in:
Iampete1
Andrew Tridgell
parent
2fe100d7bc
commit
aa8c477a26
|
|
@ -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);
|
||||
// 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;
|
||||
|
||||
// Calculate servo positions in swashplate library
|
||||
_swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll);
|
||||
_swashplate2.calculate(swash2_roll, swash2_pitch, swash2_coll);
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue