From 4aa0dfa2aaa3c6e6d5406775caaf4a3712625691 Mon Sep 17 00:00:00 2001
From: Peter Hall <33176108+IamPete1@users.noreply.github.com>
Date: Fri, 3 Jan 2020 00:12:59 +0000
Subject: [PATCH] AP_Motors: output_motor_mask update

---
 libraries/AP_Motors/AP_MotorsMatrixTS.cpp    | 27 --------------------
 libraries/AP_Motors/AP_MotorsMatrixTS.h      |  2 --
 libraries/AP_Motors/AP_MotorsMulticopter.cpp |  7 +++--
 3 files changed, 5 insertions(+), 31 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
index d7e0da6143..2154340f29 100644
--- a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
@@ -25,33 +25,6 @@ extern const AP_HAL::HAL& hal;
 
 #define SERVO_OUTPUT_RANGE  4500
 
-// output a thrust to all motors that match a given motor mask. This
-// is used to control motors enabled for forward flight. Thrust is in
-// the range 0 to 1
-void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
-{
-    const int16_t pwm_min = get_pwm_output_min();
-    const int16_t pwm_range = get_pwm_output_max() - pwm_min;
-
-    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
-        if (motor_enabled[i]) {
-            int16_t motor_out;
-            if (mask & (1U<<i)) {
-                /*
-                    apply rudder mixing differential thrust
-                    copter frame roll is plane frame yaw (this is only
-                    used by tiltrotors and tailsitters)
-                */
-                float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
-                motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f);
-            } else {
-                motor_out = pwm_min;
-            }
-            rc_write(i, motor_out);
-        }
-    }
-}
-
 void AP_MotorsMatrixTS::output_to_motors()
 {
     // calls calc_thrust_to_pwm(_thrust_rpyt_out[i]) for each enabled motor
diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.h b/libraries/AP_Motors/AP_MotorsMatrixTS.h
index 3aa9e300e3..70085586c1 100644
--- a/libraries/AP_Motors/AP_MotorsMatrixTS.h
+++ b/libraries/AP_Motors/AP_MotorsMatrixTS.h
@@ -19,8 +19,6 @@ public:
 
     virtual void        output_to_motors() override;
 
-    virtual void        output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override;
-
 protected:
     bool enable_yaw_torque;    // differential torque for yaw control
 
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
index c92cc18fab..602ac02093 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
@@ -732,6 +732,9 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
 // the range 0 to 1
 void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
 {
+    const int16_t pwm_min = get_pwm_output_min();
+    const int16_t pwm_range = get_pwm_output_max() - pwm_min;
+
     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             if (mask & (1U << i)) {
@@ -742,10 +745,10 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float r
                  */
                 float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
                 set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
-                int16_t pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * _actuator[i];
+                int16_t pwm_output = pwm_min + pwm_range * _actuator[i];
                 rc_write(i, pwm_output);
             } else {
-                rc_write(i, get_pwm_output_min());
+                rc_write(i, pwm_min);
             }
         }
     }