From 81f3d3edda3a06de4bba8c22f7428349a2e32de2 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 5 May 2023 20:06:42 +0100
Subject: [PATCH] AP_Motors: Heli: get_output_mask return only motors

---
 libraries/AP_Motors/AP_MotorsHeli.cpp        |  7 +++++++
 libraries/AP_Motors/AP_MotorsHeli.h          |  2 +-
 libraries/AP_Motors/AP_MotorsHeli_Dual.cpp   | 19 -------------------
 libraries/AP_Motors/AP_MotorsHeli_Dual.h     |  3 ---
 libraries/AP_Motors/AP_MotorsHeli_Quad.cpp   | 12 ------------
 libraries/AP_Motors/AP_MotorsHeli_Quad.h     |  3 ---
 libraries/AP_Motors/AP_MotorsHeli_RSC.cpp    |  9 +++++++++
 libraries/AP_Motors/AP_MotorsHeli_RSC.h      |  3 +++
 libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 18 +-----------------
 9 files changed, 21 insertions(+), 55 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp
index 43db815da7..00724bbe5c 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli.cpp
@@ -617,3 +617,10 @@ bool AP_MotorsHeli::arming_checks(size_t buflen, char *buffer) const
 
     return true;
 }
+
+// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
+//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
+uint32_t AP_MotorsHeli::get_motor_mask()
+{
+    return _main_rotor.get_output_mask();
+}
diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h
index 0e47f964e9..ef8ca00022 100644
--- a/libraries/AP_Motors/AP_MotorsHeli.h
+++ b/libraries/AP_Motors/AP_MotorsHeli.h
@@ -113,7 +113,7 @@ public:
 
     // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
-    virtual uint32_t get_motor_mask() override = 0;
+    virtual uint32_t get_motor_mask() override;
 
     virtual void set_acro_tail(bool set) {}
 
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
index 6dc77f5bd9..4e3613adcf 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
@@ -463,25 +463,6 @@ float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, f
     return swash_tilt;
 }
 
-// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
-//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
-uint32_t AP_MotorsHeli_Dual::get_motor_mask()
-{
-    // dual heli uses channels 1,2,3,4,5,6 and 8
-    uint32_t mask = 0;
-    for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
-        mask |= 1U << (AP_MOTORS_MOT_1+i);
-    }
-    if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        mask |= 1U << AP_MOTORS_MOT_7;
-    }
-    if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        mask |= 1U << AP_MOTORS_MOT_8;
-    }
-    mask |= 1U << AP_MOTORS_HELI_RSC;
-    return mask;
-}
-
 // update_motor_controls - sends commands to motor controllers
 void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state)
 {
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h
index f18a892672..2987d2fcd8 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h
@@ -76,9 +76,6 @@ public:
     // calculate_armed_scalars - recalculates scalars that can change while armed
     void calculate_armed_scalars() override;
 
-    // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
-    uint32_t get_motor_mask() override;
-
     // has_flybar - returns true if we have a mechical flybar
     bool has_flybar() const  override { return AP_MOTORS_HELI_NOFLYBAR; }
 
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
index 66c8782acb..1293674b47 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
@@ -173,18 +173,6 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors()
     }
 }
 
-// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
-//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
-uint32_t AP_MotorsHeli_Quad::get_motor_mask()
-{
-    uint32_t mask = 0;
-    for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
-        mask |= 1U << (AP_MOTORS_MOT_1+i);
-    }
-    mask |= 1U << AP_MOTORS_HELI_RSC;
-    return mask;
-}
-
 // update_motor_controls - sends commands to motor controllers
 void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state)
 {
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h
index d93182edee..19dc56f638 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h
@@ -55,9 +55,6 @@ public:
     // calculate_armed_scalars - recalculates scalars that can change while armed
     void calculate_armed_scalars() override;
 
-    // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
-    uint32_t get_motor_mask() override;
-
     // has_flybar - returns true if we have a mechanical flybar
     bool has_flybar() const  override { return AP_MOTORS_HELI_NOFLYBAR; }
 
diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
index 3a3413a4de..5af82956f0 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
@@ -510,6 +510,15 @@ void AP_MotorsHeli_RSC::write_rsc(float servo_out)
     }
 }
 
+// Return mask of output channels which the RSC is outputting on
+uint32_t AP_MotorsHeli_RSC::get_output_mask() const
+{
+    if (_control_mode == ROTOR_CONTROL_MODE_DISABLED) {
+        return 0;
+    }
+    return SRV_Channels::get_output_channel_mask(_aux_fn);
+}
+
 // calculate_throttlecurve - uses throttle curve and collective input to determine throttle setting
 float AP_MotorsHeli_RSC::calculate_throttlecurve(float collective_in)
 {
diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
index bf06f41756..6f606ca012 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
@@ -125,6 +125,9 @@ public:
     // output - update value to send to ESC/Servo
     void        output(RotorControlState state);
 
+    // Return mask of output channels which the RSC is outputting on
+    uint32_t    get_output_mask() const;
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
index 22d9719299..3269829ce5 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp
@@ -350,23 +350,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
 //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
 uint32_t AP_MotorsHeli_Single::get_motor_mask()
 {
-    // heli uses channels 1,2,3,4 and 8
-    // setup fast channels
-    uint32_t mask = 1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_RSC;
-
-    if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        mask |= 1U << 4;
-    }
-
-    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
-        mask |= 1U << AP_MOTORS_HELI_SINGLE_EXTGYRO;
-    }
-
-    if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
-        mask |= 1U << AP_MOTORS_HELI_SINGLE_TAILRSC;
-    }
-
-    return motor_mask_to_srv_channel_mask(mask);
+    return _main_rotor.get_output_mask() | _tail_rotor.get_output_mask();
 }
 
 // update_motor_controls - sends commands to motor controllers