diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a066bf9b6b..58ef2c1a06 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2232,7 +2232,7 @@ static void tuning(){ #if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO: - motors.ext_gyro_gain(tuning_value); + motors.ch7_pwm_setpoint(tuning_value); break; #endif diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index eed103e5d2..45aa0da62b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -83,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle) // do not use rate controllers for helicotpers with external gyros #if FRAME_CONFIG == HELI_FRAME - if(motors.ext_gyro_enabled()) { + if(motors.tail_type() == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500); } #endif @@ -446,7 +446,7 @@ run_rate_controllers() // convert desired roll and pitch rate to roll and pitch swash angles heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf); // helicopters only use rate controllers for yaw and only when not using an external gyro - if(!motors.ext_gyro_enabled()) { + if(motors.tail_type() != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf); }else{ // do not use rate controllers for helicotpers with external gyros diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index aeaf62912b..5a547d6dc2 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -239,7 +239,7 @@ struct PACKED log_Motors { int16_t motor_out[6]; #elif FRAME_CONFIG == HELI_FRAME int16_t motor_out[4]; - int16_t ext_gyro_gain; + int16_t ch7_pwm_setpoint; #else // quads & TRI_FRAME int16_t motor_out[4]; #endif @@ -271,7 +271,7 @@ static void Log_Write_Motors() motors.motor_out[AP_MOTORS_MOT_2], motors.motor_out[AP_MOTORS_MOT_3], motors.motor_out[AP_MOTORS_MOT_4]}, - ext_gyro_gain : motors.ext_gyro_gain() + ch7_pwm_setpoint:motors.ch7_pwm_setpoint() #elif FRAME_CONFIG == TRI_FRAME motor_out : {motors.motor_out[AP_MOTORS_MOT_1], motors.motor_out[AP_MOTORS_MOT_2], diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 07288e42c6..56c91920bd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -100,12 +100,12 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID), - // @Param: GYR_ENABLE - // @DisplayName: External Gyro Enabled - // @Description: Enabled/Disable an external rudder gyro connected to channel 7. With no external gyro a more complex yaw controller is used - // @Values: 0:Disabled,1:Enabled + // @Param: TAIL_TYPE + // @DisplayName: Tail Type + // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected + // @Values: 0:Servo only,1:Servo w/ ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch // @User: Standard - AP_GROUPINFO("GYR_ENABLE",9, AP_MotorsHeli, _ext_gyro_enabled, 0), + AP_GROUPINFO("TAIL_TYPE",9, AP_MotorsHeli, _tail_type, AP_MOTORS_HELI_TAILTYPE_SERVO), // @Param: SWASH_TYPE // @DisplayName: Swash Type @@ -114,14 +114,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("SWASH_TYPE",10, AP_MotorsHeli, _swash_type, AP_MOTORS_HELI_SWASH_CCPM), - // @Param: GYR_GAIN - // @DisplayName: External Gyro Gain - // @Description: PWM sent to the external gyro on Ch7 + // @Param: CH7_SETPOINT + // @DisplayName: Ch7 PWM Setpoint + // @Description: PWM output on Ch7 for External Gyro gain or Variable Pitch Direct Drive speed // @Range: 1000 2000 // @Units: PWM // @Increment: 10 // @User: Standard - AP_GROUPINFO("GYR_GAIN",11, AP_MotorsHeli, _ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN), + AP_GROUPINFO("CH7_SETPOINT", 11, AP_MotorsHeli, _ch7_pwm_setpoint, 1000), // @Param: SV_MAN // @DisplayName: Manual Servo Mode @@ -242,12 +242,12 @@ void AP_MotorsHeli::set_update_rate( uint16_t speed_hz ) void AP_MotorsHeli::enable() { // enable output channels - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1 - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]); // swash servo 2 - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3 - hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw - hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_GYRO); // for external gyro - hal.rcout->enable_ch(AP_MOTORS_HELI_EXT_RSC); // for external RSC + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_1]); // swash servo 1 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_2]); // swash servo 2 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3 + hal.rcout->enable_ch(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw + hal.rcout->enable_ch(AP_MOTORS_HELI_AUX); // output for gyro gain or direct drive variable pitch tail motor + hal.rcout->enable_ch(AP_MOTORS_HELI_RSC); // output for main rotor esc } // output_min - sends minimum values out to the motors @@ -302,8 +302,8 @@ void AP_MotorsHeli::output_test() } // external gyro - if (_ext_gyro_enabled) { - hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, _ext_gyro_gain); + if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { + hal.rcout->write(AP_MOTORS_HELI_AUX, _ch7_pwm_setpoint); } // servo 4 @@ -578,7 +578,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; // rudder feed forward based on collective - if (!_ext_gyro_enabled) { + if (_tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { yaw_offset = _collective_yaw_effect * abs(coll_out_scaled - _collective_mid_pwm); } } @@ -621,9 +621,33 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll motor_out[AP_MOTORS_MOT_3] = _servo_3->radio_out; motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out; - // output gyro value - if (_ext_gyro_enabled) { - hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, _ext_gyro_gain); + switch (_tail_type) { + case AP_MOTORS_HELI_TAILTYPE_SERVO: + // do nothing + break; + + case AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO: + // output gyro value + hal.rcout->write(AP_MOTORS_HELI_AUX, _ch7_pwm_setpoint); + break; + + case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH: + // switch on the motor if Ch8 is not low + if (armed() && _rc_8->control_in > 100) { + hal.rcout->write(AP_MOTORS_HELI_AUX, _ch7_pwm_setpoint); + } else { + hal.rcout->write(AP_MOTORS_HELI_AUX, AP_MOTOR_HELI_TAIL_TYPE_DIRECTDRIVE_PWM_MIN); + } + break; + + case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH: + // output fixed-pitch speed control if Ch8 is high + if (armed() && _rc_8->control_in > 100) { + hal.rcout->write(AP_MOTORS_HELI_AUX, _servo_4->radio_out); + } else { + hal.rcout->write(AP_MOTORS_HELI_AUX, AP_MOTOR_HELI_TAIL_TYPE_DIRECTDRIVE_PWM_MIN); + } + break; } } @@ -668,7 +692,7 @@ void AP_MotorsHeli::rsc_control() case AP_MOTORS_HELI_RSC_MODE_EXT_GOVERNOR: - if (armed() && _rc_8->control_in > 100) { + if (armed() && _rc_8->control_in > 400) { if (_rsc_ramp < _rsc_ramp_up_rate) { _rsc_ramp++; _rsc_output = map(_rsc_ramp, 0, _rsc_ramp_up_rate, 1000, _ext_gov_setpoint); diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 44188d5578..0c18d47973 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -24,6 +24,10 @@ #define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos #define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos +// TradHeli Aux Function Output Channels +#define AP_MOTORS_HELI_AUX CH_7 +#define AP_MOTORS_HELI_RSC CH_8 + // servo position defaults #define AP_MOTORS_HELI_SERVO1_POS -60 #define AP_MOTORS_HELI_SERVO2_POS 60 @@ -47,8 +51,18 @@ // swash min while landed or landing (as a number from 0 ~ 1000 #define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0 +// tail types +#define AP_MOTORS_HELI_TAILTYPE_SERVO 0 +#define AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO 1 +#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH 2 +#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3 + // default external gyro gain (ch7 out) -#define AP_MOTORS_HELI_EXT_GYRO_GAIN 1350 +#define AP_MOTORS_HELI_CH7_PWM_SETPOINT 1350 + +// minimum outputs for direct drive motors +#define AP_MOTOR_HELI_TAIL_TYPE_DIRECTDRIVE_PWM_MIN 1000 + // main rotor speed control types (ch8 out) #define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver @@ -131,12 +145,12 @@ public: // allow_arming - returns true if main rotor is spinning and it is ok to arm bool allow_arming(); - // ext_gyro_enabled - returns true if we have an external gyro for yaw control - bool ext_gyro_enabled() { return _ext_gyro_enabled; } + // _tail_type - returns the tail type (servo, servo with ext gyro, direct drive var pitch, direct drive fixed pitch) + int16_t tail_type() { return _tail_type; } - // ext_gyro_gain - gets and sets external gyro gain output on ch7 - int16_t ext_gyro_gain() { return _ext_gyro_gain; } - void ext_gyro_gain(int16_t gain) { _ext_gyro_gain = gain; } + // ch7_pwm_setpoint - gets and sets pwm output on ch7 (for gyro gain or direct drive tail motors) + int16_t ch7_pwm_setpoint() { return _ch7_pwm_setpoint; } + void ch7_pwm_setpoint(int16_t pwm) { _ch7_pwm_setpoint = pwm; } // has_flybar - returns true if we have a mechical flybar bool has_flybar() { return _flybar_mode; } @@ -209,9 +223,9 @@ private: AP_Int16 _collective_min; // Lowest possible servo position for the swashplate AP_Int16 _collective_max; // Highest possible servo position for the swashplate AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades) - AP_Int16 _ext_gyro_enabled; // Enabled/Disable an external rudder gyro connected to channel 7. With no external gyro a more complex yaw controller is used + 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 the external gyro on Ch7 + AP_Int16 _ch7_pwm_setpoint; // PWM sent to Ch7 for ext gyro gain or direct drive variable pitch motor AP_Int8 _servo_manual; // Pass radio inputs directly to servos during set-up through mission planner 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_Int16 _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.