From 1c8ab3853ce4c3d9df7ea4b4da66f658a06bc9ee Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 22 Nov 2023 14:07:37 +0000 Subject: [PATCH] AP_Motors: Heli: Single: tail type tidyup --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 210 +++++++++++-------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 34 ++- 2 files changed, 148 insertions(+), 96 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index fe13951f4d..30f10cda24 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -31,7 +31,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected. Direct Drive Variable Pitch is used for tails that have a motor that is governed at constant speed by an ESC. Tail pitch is still accomplished with a servo. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above. In both DDFP cases, no servo is used for the tail and the tail motor esc is controlled by the yaw axis. // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW,5:DDVP with external governor // @User: Standard - AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO), + AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, float(TAIL_TYPE::SERVO)), // Indice 5 was used by SWASH_TYPE and should not be used @@ -210,27 +210,35 @@ void AP_MotorsHeli_Single::init_outputs() // initialize main rotor servo _main_rotor.init_servo(); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - _tail_rotor.init_servo(); - } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // external gyro output - add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. + SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); + break; + + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + _tail_rotor.init_servo(); + break; + + case TAIL_TYPE::SERVO_EXTGYRO: + // external gyro output + add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); + + + // External Gyro uses PWM output thus servo endpoints are forced + SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); + FALLTHROUGH; + + case TAIL_TYPE::SERVO: + default: + // yaw servo is an angle from -4500 to 4500 + SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); + break; } } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // External Gyro uses PWM output thus servo endpoints are forced - SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); - } - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. - SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); - } else if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - // yaw servo is an angle from -4500 to 4500 - SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); - } - set_initialised_ok(_frame_class == MOTOR_FRAME_HELI); } @@ -266,13 +274,13 @@ void AP_MotorsHeli_Single::calculate_armed_scalars() _main_rotor.set_autorotation_flag(_heliflags.in_autorotation); // set bailout ramp time _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(_heliflags.in_autorotation); _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } } else { _main_rotor.set_autorotation_flag(false); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(false); } } @@ -310,7 +318,7 @@ void AP_MotorsHeli_Single::calculate_scalars() calculate_armed_scalars(); // send setpoints to DDVP rotor controller and trigger recalculation of scalars - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SETPOINT); _tail_rotor.set_ramp_time(_main_rotor._ramp_time.get()); _tail_rotor.set_runup_time(_main_rotor._runup_time.get()); @@ -370,8 +378,6 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) // void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) { - float yaw_offset = 0.0f; - // initialize limits flag limit.throttle_lower = false; limit.throttle_upper = false; @@ -422,26 +428,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // updates takeoff collective flag based on 50% hover collective update_takeoff_collective_flag(collective_out); - // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors - if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) { - // rudder feed forward based on collective - // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque - // also not required if we are using external gyro - if ((get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // sanity check collective_yaw_scale - _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); - // This feedforward compensation follows the hover performance theory that hover power required - // is a function of gross weight to the 3/2 power - yaw_offset = _collective_yaw_scale * powf(fabsf(collective_out - _collective_zero_thrust_pct),1.5f); - - // Add yaw trim for DDFP tails - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - yaw_offset += _yaw_trim.get(); - } - } - } else { - yaw_offset = 0.0f; - } + // Get yaw offset required to cancel out steady state main rotor torque + const float yaw_offset = get_yaw_offset(collective_out); // feed power estimate into main rotor controller // ToDo: include tail rotor power? @@ -475,6 +463,34 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out) _servo4_out = yaw_out; } +// Get yaw offset required to cancel out steady state main rotor torque +float AP_MotorsHeli_Single::get_yaw_offset(float collective) +{ + if ((get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO) || (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED)) { + // Not in direct control of tail with external gyro or manual servo mode + return 0.0; + } + + if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) { + // Motor is stopped or at idle, and thus not creating torque + return 0.0; + } + + // sanity check collective_yaw_scale + _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); + + // This feedforward compensation follows the hover performance theory that hover power required + // is a function of gross weight to the 3/2 power + float yaw_offset = _collective_yaw_scale * powf(fabsf(collective - _collective_zero_thrust_pct),1.5f); + + // Add yaw trim for DDFP tails + if (have_DDFP_tail()) { + yaw_offset += _yaw_trim.get(); + } + + return yaw_offset; +} + void AP_MotorsHeli_Single::output_to_motors() { if (!initialised_ok()) { @@ -484,63 +500,73 @@ void AP_MotorsHeli_Single::output_to_motors() // Write swashplate outputs _swashplate.output(); - if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); - } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // output gain to exernal gyro - if (_acro_tail && _ext_gyro_gain_acro > 0) { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); - } else { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); - } - } - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - _servo4_out = -_servo4_out; - } - - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // calc filtered battery voltage and lift_max - thr_lin.update_lift_max_from_batt_voltage(); - } - - // Warning: This spool state logic is what prevents DDFP tails from actuating when doing H_SV_MAN and H_SV_TEST tests + // Output main rotor switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors update_motor_control(ROTOR_CONTROL_STOP); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } break; case SpoolState::GROUND_IDLE: // sends idle output to motors when armed. rotor could be static or turning (autorotation) update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } break; case SpoolState::SPOOLING_UP: case SpoolState::THROTTLE_UNLIMITED: // set motor output based on thrust requests update_motor_control(ROTOR_CONTROL_ACTIVE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation - output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); - } break; case SpoolState::SPOOLING_DOWN: // sends idle output to motors and wait for rotor to stop update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){ - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } break; } + + // Output tail rotor + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // Invert output for CCW tail + _servo4_out *= -1.0; + FALLTHROUGH; + + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: { + // calc filtered battery voltage and lift_max + thr_lin.update_lift_max_from_batt_voltage(); + + // Only throttle up if in active spool state + switch (_spool_state) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // Set DDFP to servo min + output_to_ddfp_tail(0.0); + break; + + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation + output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); + break; + } + break; + } + + case TAIL_TYPE::SERVO_EXTGYRO: + // output gain to external gyro + if (_acro_tail && _ext_gyro_gain_acro > 0) { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); + } else { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); + } + FALLTHROUGH; + + case TAIL_TYPE::SERVO: + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + default: + rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); + break; + } + } // handle output limit flags and send throttle to servos lib @@ -655,9 +681,7 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) // Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim // to H_YAW_TRIM. Default thrust linearisation gives linear thrust to throttle relationship to preserve previous setup behaviours so // we can assume linear relationship in the conversion. - if ((_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || - _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) && - !_yaw_trim.configured()) { + if (have_DDFP_tail() && !_yaw_trim.configured()) { SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4); if (c != nullptr) { @@ -673,3 +697,19 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) _yaw_trim.save(); } } + +// Helper to return true for direct drive fixed pitch tail, either CW or CCW +bool AP_MotorsHeli_Single::have_DDFP_tail() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) || + (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW); +} + +// Helper to return true if the tail RSC should be used +bool AP_MotorsHeli_Single::use_tail_RSC() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) || + (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 595ce98a04..870d4801c9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -14,15 +14,6 @@ #define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7 #define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7 -// tail types -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW 3 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW 4 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV 5 - - // direct-drive variable pitch defaults #define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 50 @@ -72,8 +63,8 @@ public: // has_flybar - returns true if we have a mechical flybar bool has_flybar() const override { return _flybar_mode; } - // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } + // supports_yaw_passthrough - returns true if we support yaw passthrough + bool supports_yaw_passthrough() const override { return get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO; } void set_acro_tail(bool set) override { _acro_tail = set; } @@ -103,12 +94,33 @@ protected: // move_yaw - moves the yaw servo void move_yaw(float yaw_out); + // Get yaw offset required to cancel out steady state main rotor torque + float get_yaw_offset(float collective); + // handle output limit flags and send throttle to servos lib void output_to_ddfp_tail(float throttle); // servo_test - move servos through full range of movement void servo_test() override; + // Tail types + enum class TAIL_TYPE { + SERVO = 0, + SERVO_EXTGYRO = 1, + DIRECTDRIVE_VARPITCH = 2, + DIRECTDRIVE_FIXEDPITCH_CW = 3, + DIRECTDRIVE_FIXEDPITCH_CCW = 4, + DIRECTDRIVE_VARPIT_EXT_GOV = 5 + }; + + TAIL_TYPE get_tail_type() const { return TAIL_TYPE(_tail_type.get()); } + + // Helper to return true for direct drive fixed pitch tail, either CW or CCW + bool have_DDFP_tail() const; + + // Helper to return true if the tail RSC should be used + bool use_tail_RSC() const; + // external objects we depend upon AP_MotorsHeli_RSC _tail_rotor; // tail rotor AP_MotorsHeli_Swash _swashplate; // swashplate