From 9da2b2b43000942070be0c7fe91f8e910bc5c9e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 9 Aug 2015 21:02:54 +1000 Subject: [PATCH] AP_Motors: added H_GYR_GAIN_ACRO --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 23 +++++++++++++++++--- libraries/AP_Motors/AP_MotorsHeli_Single.h | 10 ++++++--- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 2449dc590a..2b55dff517 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -71,7 +71,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = { // @Units: PWM // @Increment: 1 // @User: Standard - AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN), + AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain_std, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN), // @Param: PHANG // @DisplayName: Swashplate Phase Angle Compensation @@ -105,6 +105,15 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTORS_HELI_SINGLE_DDVPT_SPEED_DEFAULT), + // @Param: GYR_GAIN_ACRO + // @DisplayName: External Gyro Gain for ACRO + // @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro. A value of zero means to use H_GYR_GAIN + // @Range: 0 1000 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("GYR_GAIN_ACRO", 11, AP_MotorsHeli_Single, _ext_gyro_gain_acro, 0), + AP_GROUPEND }; @@ -181,7 +190,11 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) case 4: // external gyro & tail servo if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - write_aux(_ext_gyro_gain); + if (_acro_tail && _ext_gyro_gain_acro > 0) { + write_aux(_ext_gyro_gain_acro); + } else { + write_aux(_ext_gyro_gain_std); + } } hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); break; @@ -456,7 +469,11 @@ void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out) if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // output gain to exernal gyro - write_aux(_ext_gyro_gain); + if (_acro_tail && _ext_gyro_gain_acro > 0) { + write_aux(_ext_gyro_gain_acro); + } else { + write_aux(_ext_gyro_gain_std); + } } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) { // output yaw servo to tail rsc write_aux(_yaw_servo.servo_out); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index e06217ef61..71018fcd46 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -106,8 +106,8 @@ public: int16_t tail_type() const { return _tail_type; } // ext_gyro_gain - gets and sets external gyro gain as a pwm (1000~2000) - int16_t ext_gyro_gain() const { return _ext_gyro_gain; } - void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain = pwm; } + int16_t ext_gyro_gain() const { return _ext_gyro_gain_std; } + void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain_std = pwm; } // has_flybar - returns true if we have a mechical flybar bool has_flybar() const { return _flybar_mode; } @@ -121,6 +121,8 @@ public: // set_delta_phase_angle for setting variable phase angle compensation and force // recalculation of collective factors void set_delta_phase_angle(int16_t angle); + + void set_acro_tail(bool set) { _acro_tail = set; } // var_info static const struct AP_Param::GroupInfo var_info[]; @@ -161,12 +163,14 @@ protected: AP_Int16 _servo3_pos; // Angular location of swash servo #3 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_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro + 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 AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) + bool _acro_tail = false; }; #endif // __AP_MOTORS_HELI_SINGLE_H__