AP_Motors: Heli: Single: tail type tidyup

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) {
+        switch (get_tail_type()) {
-            _tail_rotor.init_servo();
+            case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW:
-        } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
+            case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
-            // external gyro output
+                // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation.
-            add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO);
+                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
+    // Get yaw offset required to cancel out steady state main rotor torque
-    if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) {
+    const float yaw_offset = get_yaw_offset(collective_out);
-        // 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;
-    }
 
     // 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) {
+    // Output main rotor
-        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
     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 ||
+    if (have_DDFP_tail() && !_yaw_trim.configured()) {
-        _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) &&
-        !_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);
+}

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
+    // supports_yaw_passthrough - returns true if we support yaw passthrough
-    bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; }
+    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