From 9aa685a6d81e23a9cc39402b25ca4cba6c5b6a4e Mon Sep 17 00:00:00 2001 From: ChristopherOlson Date: Sun, 1 Apr 2018 18:19:52 -0500 Subject: [PATCH] AP_Motors:TradHeli - single rotor only - add support for H3-140 swashplates. Also changes the old definition to H3 for swashplates with fixed control radius and adjustable servo position. --- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 1 + libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 51 ++++++++++++-------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 11 +++-- 3 files changed, 39 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 329187c2b2..b6435b8eeb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -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) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index bd92cac340..e371bb9d49 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -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; @@ -548,8 +561,8 @@ void AP_MotorsHeli_Single::servo_test() // parameter_check - check if helicopter specific parameters are sensible 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)){ + // returns false if Phase Angle is outside of range + if ((_phase_angle > 30) || (_phase_angle < -30)){ if (display_msg) { gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_PHANG out of range"); } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 233266ce4b..d3fe441a4f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -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 @@ -38,10 +39,10 @@ #define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350 // COLYAW parameter min and max values -#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f +#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f // maximum number of swashplate servos -#define AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS 3 +#define AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS 3 /// @class AP_MotorsHeli_Single class AP_MotorsHeli_Single : public AP_MotorsHeli { @@ -142,9 +143,9 @@ protected: AP_Int16 _servo1_pos; // Angular location of swash servo #1 AP_Int16 _servo2_pos; // Angular location of swash servo #2 AP_Int16 _servo3_pos; // Angular location of swash servo #3 - AP_Int8 _collective_direction; // Collective control direction, normal or reversed + 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