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

AP_Motors:TradHeli - single rotor only - add support for H3-140 swashplates.

Browse Source 
Also changes the old definition to H3 for swashplates with fixed control radius and adjustable servo position.
This commit is contained in:
ChristopherOlson 2018-04-01 18:19:52 -05:00 committed by Randy Mackay
parent f073c58f68
commit 9aa685a6d8
3 changed files with 39 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -310,6 +310,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
}
// calculate_swash_factors - calculate factors based on swash type and servo position
// To Do: support H3-140 swashplates in Heli Dual?
void AP_MotorsHeli_Dual::calculate_roll_pitch_collective_factors()
{
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {

49
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -26,7 +26,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Param: SV1_POS
// @DisplayName: Servo 1 Position
// @Description: Angular location of swash servo #1
// @Description: Angular location of swash servo #1 - only used for H3 swash type
// @Range: -180 180
// @Units: deg
// @User: Standard
@ -35,7 +35,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Param: SV2_POS
// @DisplayName: Servo 2 Position
// @Description: Angular location of swash servo #2
// @Description: Angular location of swash servo #2 - only used for H3 swash type
// @Range: -180 180
// @Units: deg
// @User: Standard
@ -44,7 +44,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Param: SV3_POS
// @DisplayName: Servo 3 Position
// @Description: Angular location of swash servo #3
// @Description: Angular location of swash servo #3 - only used for H3 swash type
// @Range: -180 180
// @Units: deg
// @User: Standard
@ -60,10 +60,10 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Param: SWASH_TYPE
// @DisplayName: Swash Type
// @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
// @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing
// @Description: Swash Type Setting
// @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM
// @User: Standard
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_CCPM),
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_H3),
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
@ -76,8 +76,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Param: PHANG
// @DisplayName: Swashplate Phase Angle Compensation
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -90 90
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -30 30
// @Units: deg
// @User: Advanced
// @Increment: 1
@ -116,6 +116,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_GROUPINFO("GYR_GAIN_ACRO", 11, AP_MotorsHeli_Single, _ext_gyro_gain_acro, 0),
// Indices 16-18 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
// @Param: COL_CTRL_DIR
// @DisplayName: Collective Control Direction
// @Description: Collective Control Direction - 0 for Normal. 1 for Reversed
@ -123,7 +125,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Single, _collective_direction, AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL),
// Indices 16-18 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
// parameters up to and including 29 are reserved for tradheli
AP_GROUPEND
@ -284,17 +285,16 @@ void AP_MotorsHeli_Single::calculate_scalars()
}
}
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
// CCPM Mixers - calculate mixing scale factors by swashplate type
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
{
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
// roll factors
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3) { //Three-Servo adjustable CCPM mixer factors
// aileron factors
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
// pitch factors
// elevator factors
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
@ -303,15 +303,28 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
} else if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3_140) { //Three-Servo H3-140 CCPM mixer factors
// aileron factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = -1;
_rollFactor[CH_3] = 0;
}else{ //H1 Swashplate, keep servo outputs separated
// elevator factors
_pitchFactor[CH_1] = 1;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = -1;
// roll factors
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
} else { //H1 straight outputs, no mixing
// aileron factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
// elevator factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
@ -549,7 +562,7 @@ void AP_MotorsHeli_Single::servo_test()
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
{
// returns false if Phase Angle is outside of range
if ((_phase_angle > 90) || (_phase_angle < -90)){
if ((_phase_angle > 30) || (_phase_angle < -30)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_PHANG out of range");
}

5
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -18,8 +18,9 @@
#define AP_MOTORS_HELI_SINGLE_SERVO3_POS 180
// swash type definitions
#define AP_MOTORS_HELI_SINGLE_SWASH_CCPM 0
#define AP_MOTORS_HELI_SINGLE_SWASH_H3 0
#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1
#define AP_MOTORS_HELI_SINGLE_SWASH_H3_140 2
// collective control direction definitions
#define AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL 0
@ -144,7 +145,7 @@ protected:
AP_Int16 _servo3_pos; // Angular location of swash servo #3
AP_Int8 _collective_direction; // Collective control direction, normal or reversed
AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
AP_Int8 _swash_type; // Swash Type Setting
AP_Int16 _ext_gyro_gain_std; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
AP_Int16 _ext_gyro_gain_acro; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro in ACRO
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem