From d63d82ec17ada8f3cf368acb56260d7ed4a8bb18 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 28 Apr 2014 16:29:09 +0900
Subject: [PATCH] MotorMatrix: output_test for individual motors

Based on original work by Nils Hogberg
---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 42 +++++++------------------
 libraries/AP_Motors/AP_MotorsMatrix.h   |  6 ++--
 2 files changed, 15 insertions(+), 33 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 7ec90804bd..e1be4dc740 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -329,43 +329,23 @@ void AP_MotorsMatrix::output_disarmed()
     output_min();
 }
 
-// output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction
-void AP_MotorsMatrix::output_test()
+// output_test - spin a motor at the pwm value specified
+//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
 {
-    uint8_t min_order, max_order;
-    uint8_t i,j;
-
-    // find min and max orders
-    min_order = _test_order[0];
-    max_order = _test_order[0];
-    for(i=1; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
-        if( _test_order[i] < min_order )
-            min_order = _test_order[i];
-        if( _test_order[i] > max_order )
-            max_order = _test_order[i];
+    // exit immediately if not armed
+    if (!_flags.armed) {
+        return;
     }
 
-    // shut down all motors
-    output_min();
-
-    // first delay is longer
-    hal.scheduler->delay(4000);
-
     // loop through all the possible orders spinning any motors that match that description
-    for( i=min_order; i<=max_order; i++ ) {
-        for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
-            if( motor_enabled[j] && _test_order[j] == i ) {
-                // turn on this motor and wait 1/3rd of a second
-                hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min + _min_throttle);
-                hal.scheduler->delay(300);
-                hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min);
-                hal.scheduler->delay(2000);
-            }
+    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i] && _test_order[i] == motor_seq) {
+            // turn on this motor
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
         }
     }
-
-    // shut down all motors
-    output_min();
 }
 
 // add_motor
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h
index 946e7bba50..550cfc8b4f 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.h
+++ b/libraries/AP_Motors/AP_MotorsMatrix.h
@@ -38,8 +38,10 @@ public:
     // enable - starts allowing signals to be sent to motors
     virtual void        enable();
 
-    // output_test - spin each motor for a moment to allow the user to confirm the motor order and spin direction
-    virtual void        output_test();
+    // output_test - spin a motor at the pwm value specified
+    //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
+    //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
+    virtual void        output_test(uint8_t motor_seq, int16_t pwm);
 
     // output_min - sends minimum values out to the motors
     virtual void        output_min();