From 2be99d7a92f43f7cd395db64236d46747c68629b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 28 Apr 2014 16:30:26 +0900
Subject: [PATCH] TriCopter: output_test for individual motors

Based on original work by Nils Hogberg
---
 libraries/AP_Motors/AP_MotorsTri.cpp | 51 ++++++++++++++++------------
 libraries/AP_Motors/AP_MotorsTri.h   |  6 ++--
 2 files changed, 34 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
index a29b7ba4c..14163bee8 100644
--- a/libraries/AP_Motors/AP_MotorsTri.cpp
+++ b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -185,27 +185,36 @@ void AP_MotorsTri::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_MotorsTri::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_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm)
 {
-    // Send minimum values to all motors
-    output_min();
-
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min);
-    hal.scheduler->delay(4000);
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min + _min_throttle);
-    hal.scheduler->delay(300);
-
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min);
-    hal.scheduler->delay(2000);
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min + _min_throttle);
-    hal.scheduler->delay(300);
-
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min);
-    hal.scheduler->delay(2000);
-    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min + _min_throttle);
-    hal.scheduler->delay(300);
+    // exit immediately if not armed
+    if (!_flags.armed) {
+        return;
+    }
 
-    // Send minimum values to all motors
-    output_min();
+    // output to motors and servos
+    switch (motor_seq) {
+        case 1:
+            // front right motor
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm);
+            break;
+        case 2:
+            // back motor
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm);
+            break;
+        case 3:
+            // back servo
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), pwm);
+            break;
+        case 4:
+            // front left motor
+            hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm);
+            break;
+        default:
+            // do nothing
+            break;
+    }
 }
diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h
index 679a0ef51..c7522d675 100644
--- a/libraries/AP_Motors/AP_MotorsTri.h
+++ b/libraries/AP_Motors/AP_MotorsTri.h
@@ -33,8 +33,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();
-- 
GitLab