Merge pull request #32 from julianoes/ardrone_testing

fix to set device for ardrone interface

1 files changed, 20 insertions, 6 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index dd7b1847b..8d77e7502 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -49,6 +49,7 @@
 #include <debug.h>
 #include <termios.h>
 #include <time.h>
+#include <systemlib/err.h>
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
 #include <uORB/uORB.h>
@@ -73,7 +74,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]);
 /**
  * Open the UART connected to the motor controllers
  */
-static int ardrone_open_uart(struct termios *uart_config_original);
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original);
 
 /**
  * Print the correct usage.
@@ -133,15 +134,15 @@ int ardrone_interface_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int ardrone_open_uart(struct termios *uart_config_original)
+static int ardrone_open_uart(char *uart_name, struct termios *uart_config_original)
 {
 	/* baud rate */
 	int speed = B115200;
 	int uart;
-	const char* uart_name = "/dev/ttyS1";
+
 
 	/* open uart */
-	printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200\n");
+	printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed);
 	uart = open(uart_name, O_RDWR | O_NOCTTY);
 
 	/* Try to set baud rate */
@@ -182,6 +183,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 {
 	thread_running = true;
 
+	char *device = "/dev/ttyS1";
+
 	/* welcome user */
 	printf("[ardrone_interface] Control started, taking over motors\n");
 
@@ -205,9 +208,20 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 				if (motor > 0 && motor < 5) {
 					test_motor = motor;
 				} else {
+					thread_running = false;
 					errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
 				}
 			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
+			}
+		}
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
 				errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
 			}
 		}
@@ -240,7 +254,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	fflush(stdout);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(&uart_config_original);
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
 	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
 	gpios = ar_multiplexing_init();
@@ -270,7 +284,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	//ar_multiplexing_deinit(gpios);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	ardrone_write = ardrone_open_uart(&uart_config_original);
+	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
 	/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
 	gpios = ar_multiplexing_init();