From 9c01df734a337878c0c2a7679aa12cac426c38c8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 7 Sep 2012 12:40:18 +0200
Subject: [PATCH] Added per-motor test routine, test came clean. Worth trying
 PID tuning.

---
 apps/ardrone_interface/ardrone_interface.c | 23 +++++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index c4cceaf907..af88684bb8 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -191,12 +191,26 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
 
 	bool motor_test_mode = false;
+	int test_motor = -1;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
 		if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
 			motor_test_mode = true;
 		}
+
+		if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
+			if (i+1 < argc) {
+				int motor = atoi(argv[i+1]);
+				if (motor > 0 && motor < 5) {
+					test_motor = motor;
+				} else {
+					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
+				}
+			} else {
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
 	}
 
 	struct termios uart_config_original;
@@ -279,7 +293,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 		if (motor_test_mode) {
 			/* set motors to idle speed */
-			ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
+			if (test_motor > 0 && test_motor < 5) {
+				int motors[4] = {0, 0, 0, 0};
+				motors[test_motor - 1] = 10;
+				ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
+			} else {
+				ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
+			}
+
 		} else {
 			/* MAIN OPERATION MODE */