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

AP_Motors: add DCP trim feature for Dual Heli

This commit is contained in:
bnsgeyer 2020-12-16 08:47:07 -05:00 committed by Andrew Tridgell
parent 2763ce027e
commit ebd83b756b
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
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
};
@ -408,9 +416,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) {
// collective
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) {
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
@ -431,9 +439,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) {
// collective
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) {
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

@ -132,6 +132,7 @@ protected:
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 _yaw_scaler; // scaling factor applied to the yaw mixing
AP_Float _dcp_trim; // used to easily trim dcp axis
// internal variables
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range