From 24e21b57cae900f09c51ecefb2fabb9ddafa61d2 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Thu, 19 Apr 2018 15:12:28 -0400
Subject: [PATCH] AP_Motors: add output_test_num api for motor testing

---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 25 +++++++++++++++++++++++++
 libraries/AP_Motors/AP_MotorsMatrix.h   |  7 +++++++
 2 files changed, 32 insertions(+)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 542ea48ded..a921836219 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -299,6 +299,31 @@ void AP_MotorsMatrix::output_test_seq(uint8_t motor_seq, int16_t pwm)
     }
 }
 
+// output_test_num - spin a motor connected to the specified output channel
+//  (should only be performed during testing)
+//  If a motor output channel is remapped, the mapped channel is used.
+//  Returns true if motor output is set, false otherwise
+//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
+{
+    if (!armed()) {
+        return false;
+    }
+
+    // Is channel in supported range?
+    if (output_channel > AP_MOTORS_MAX_NUM_MOTORS -1) {
+        return false;
+    }
+
+    // Is motor enabled?
+    if (!motor_enabled[output_channel]) {
+        return false;
+    }
+
+    rc_write(output_channel, pwm); // output
+    return true;
+}
+
 // add_motor
 void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
 {
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h
index 36723ca70e..4b679ec968 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.h
+++ b/libraries/AP_Motors/AP_MotorsMatrix.h
@@ -34,6 +34,13 @@ public:
     //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
     virtual void        output_test_seq(uint8_t motor_seq, int16_t pwm) override;
 
+    // output_test_num - spin a motor connected to the specified output channel
+    //  (should only be performed during testing)
+    //  If a motor output channel is remapped, the mapped channel is used.
+    //  Returns true if motor output is set, false otherwise
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    bool                output_test_num(uint8_t motor, int16_t pwm);
+
     // output_to_motors - sends minimum values out to the motors
     void                output_to_motors();