Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add DCP trim feature for Dual Heli

Browse Source
This commit is contained in:
bnsgeyer 2020-12-16 08:47:07 -05:00 committed by Andrew Tridgell
parent 9706228fee
commit 06b4f259c0
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -196,6 +196,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
// @Increment: 1 // @Increment: 1
AP_SUBGROUPINFO(_swashplate2, "SW2_", 21, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash), AP_SUBGROUPINFO(_swashplate2, "SW2_", 21, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
// @Param: DCP_TRIM
// @DisplayName: Differential Collective Pitch Trim
// @Description: Removes I term bias due to center of gravity offsets or discrepancies between rotors in swashplate setup. If DCP axis has I term bias while hovering in calm winds, use value of bias in DCP_TRIM to re-center I term.
// @Range: -0.2 0.2
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("DCP_TRIM", 22, AP_MotorsHeli_Dual, _dcp_trim, 0.0f),
AP_GROUPEND AP_GROUPEND
}; };
@ -395,9 +403,9 @@ float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, f
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) {
// collective // collective
if (swash_num == 1) { if (swash_num == 1) {
swash_tilt = 0.45f * _dcp_scaler * roll_input + coll_input; swash_tilt = 0.45f * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input;
} else if (swash_num == 2) { } else if (swash_num == 2) {
swash_tilt = -0.45f * _dcp_scaler * roll_input + coll_input; swash_tilt = -0.45f * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input;
} }
} }
} else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM
@ -418,9 +426,9 @@ float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, f
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) { } else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) {
// collective // collective
if (swash_num == 1) { if (swash_num == 1) {
swash_tilt = 0.45f * _dcp_scaler * pitch_input + coll_input; swash_tilt = 0.45f * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input;
} else if (swash_num == 2) { } else if (swash_num == 2) {
swash_tilt = -0.45f * _dcp_scaler * pitch_input + coll_input; swash_tilt = -0.45f * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2f, 0.2f)) + coll_input;
} }
} }
} }

1
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -133,6 +133,7 @@ protected:
AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied. 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 _yaw_scaler; // scaling factor applied to the yaw mixing
AP_Float _dcp_trim; // used to easily trim dcp axis
// internal variables // internal variables
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range