Beat up on the mavlink app startup a bit.

1 files changed, 62 insertions, 93 deletions

diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 3a1365c66..3bb70973d 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -61,6 +61,8 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/getopt_long.h>
 
 #include "waypoints.h"
 #include "mavlink_log.h"
@@ -118,7 +120,7 @@ static char mavlink_message_string[51] = {0};
 
 /* protocol interface */
 static int uart;
-static int baudrate = 57600;
+static int baudrate;
 
 /* interface mode */
 static enum {
@@ -128,12 +130,7 @@ static enum {
 
 static void mavlink_update_system(void);
 static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-
-
-/**
- * Print the usage
- */
-static void usage(const char *reason);
+static void usage(void);
 
 
 
@@ -493,64 +490,60 @@ void mavlink_update_system(void)
  */
 int mavlink_thread_main(int argc, char *argv[])
 {
-	/* print welcome text */
-	printf("[mavlink] MAVLink v1.0 serial interface starting..\n");
+	static GETOPT_LONG_OPTION_T options[] = {
+		{"baud",	REQUIRED_ARG,	NULL,	'b'},
+		{"speed",	REQUIRED_ARG,	NULL,	'b'},
+		{"device",	REQUIRED_ARG,	NULL,	'd'},
+		{"exit-allowed", NO_ARG,	NULL,	'e'},
+		{"onboard",	NO_ARG,		NULL,	'o'}
+	};
+
+	int ch, longind;
+	char *device_name = "/dev/ttyS1";
+	baudrate = 57600;
 
-	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
+	while ((ch = getopt_long(argc, argv, "b:d:eo", options, &longind)) != EOF) {
+		switch (ch) {
+		case 'b':
+			baudrate = strtoul(optarg, NULL, 10);
+			if (baudrate == 0)
+				errx(1, "invalid baud rate '%s'", optarg);
+			break;
 
-	/* default values for arguments */
-	char *uart_name = "/dev/ttyS1";
-	baudrate = 57600;
+		case 'd':
+			device_name = optarg;
+			break;
 
-	/* read program arguments */
-	int i;
-
-	for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
-
-		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
-			usage("");
-			return 0;
-		} else if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
-			if (argc > i + 1) {
-				uart_name = argv[i + 1];
-				i++;
-			} else {
-				usage("missing argument for device (-d)");
-				return 1;
-			}
-		} else if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
-			if (argc > i + 1) {
-				baudrate = atoi(argv[i + 1]);
-				i++;
-			} else {
-				usage("missing argument for baud rate (-b)");
-				return 1;
-			}
-		} else if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
+		case 'e':
 			mavlink_link_termination_allowed = true;
-		} else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
+			break;
+
+		case 'o':
 			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-		} else {
-			usage("out of order or invalid argument");
-			return 1;
+			break;
+
+		default:
+			usage();
 		}
 	}
 
 	struct termios uart_config_original;
-
 	bool usb_uart;
 
-	uart = mavlink_open_uart(baudrate, uart_name, &uart_config_original, &usb_uart);
-
-	if (uart < 0) {
-		printf("[mavlink] FAILED to open %s, terminating.\n", uart_name);
-		goto exit_cleanup;
-	}
+	/* print welcome text */
+	warnx("MAVLink v1.0 serial interface starting...");
 
-	/* Flush UART */
+	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+	/* default values for arguments */
+	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
+	if (uart < 0)
+		err(1, "could not open %s", device_name);
+
+	/* create the device node that's used for sending text log messages, etc. */
+	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
+
 	/* Initialize system properties */
 	mavlink_update_system();
 
@@ -563,8 +556,6 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
 
-	uint16_t counter = 0;
-
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 230400) {
 		/* 200 Hz / 5 ms */
@@ -616,7 +607,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	thread_running = true;
 
 	/* arm counter to go off immediately */
-	int lowspeed_counter = 10;
+	unsigned lowspeed_counter = 10;
 
 	while (!thread_should_exit) {
 
@@ -680,7 +671,6 @@ int mavlink_thread_main(int argc, char *argv[])
 			mavlink_missionlib_send_gcs_string(mavlink_message_string);
 			mavlink_message_string[0] = '\0';
 		}
-		counter++;
 
 		/* sleep 15 ms */
 		usleep(15000);
@@ -691,54 +681,34 @@ int mavlink_thread_main(int argc, char *argv[])
 	pthread_join(uorb_receive_thread, NULL);
 
 	/* Reset the UART flags to original state */
-	if (!usb_uart) {
-		int termios_state;
-
-		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config_original)) < 0) {
-			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", uart_name);
-		}
-
-		printf("[mavlink] Restored original UART config, exiting..\n");
-	}
-
-exit_cleanup:
-
-	/* close uart */
-	close(uart);
-
-	/* close subscriptions */
-	close(mavlink_subs.global_pos_sub);
-	close(mavlink_subs.local_pos_sub);
-
-	fflush(stdout);
-	fflush(stderr);
+	if (!usb_uart)
+		tcsetattr(uart, TCSANOW, &uart_config_original);
 
 	thread_running = false;
 
-	return 0;
+	exit(0);
 }
 
 static void
-usage(const char *reason)
+usage()
 {
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: mavlink {start|stop|status} [-d <devicename>] [-b <baudrate>] [-e/--exit-allowed]\n\n");
+	fprintf(stderr, "usage: mavlink start [--device <devicename>] [--speed <device speed>] [--exit-allowed] [--onboard]\n"
+			"       mavlink stop\n"
+			"       mavlink status\n");
 	exit(1);
 }
 
 int mavlink_main(int argc, char *argv[])
 {
+
 	if (argc < 1)
-		usage("missing command");
+		errx(1, "missing command");
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (thread_running) {
-			printf("mavlink already running\n");
-			/* this is not an error */
-			exit(0);
-		}
+		/* this is not an error */
+		if (thread_running)
+			errx(0, "mavlink already running\n");
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn("mavlink",
@@ -746,25 +716,24 @@ int mavlink_main(int argc, char *argv[])
 					  SCHED_PRIORITY_DEFAULT,
 					  6000,
 					  mavlink_thread_main,
-					  (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					  NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
+		/* XXX should wait for it to actually exit here */
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tmavlink app is running\n");
+			errx(0, "running");
 		} else {
-			printf("\tmavlink app not started\n");
+			errx(1, "not running");
 		}
-		exit(0);
 	}
 
-	usage("unrecognized command");
-	exit(1);
+	errx(1, "unrecognized command");
 }