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

AP_Motors: Dual Heli-add support for intermeshing rotor

This commit is contained in:
PittRBM 2020-12-03 00:02:34 +07:00 committed by Bill Geyer
parent ebd83b756b
commit 3f405590c7
2 changed files with 64 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -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;

2
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -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