Changed mavlink and sensors apps to deamons, now started with mavlink start and sensors start.

apps/mavlink/mavlink.c

@@ -78,6 +78,11 @@
 
 
 __EXPORT int mavlink_main(int argc, char *argv[]);
+int mavlink_thread_main(int argc, char *argv[]);
+
+static bool thread_should_exit = false;
+static bool thread_running = false;
+static int mavlink_task;
 
 /* terminate MAVLink on user request - disabled by default */
 static bool mavlink_link_termination_allowed = false;
@@ -170,6 +175,11 @@ mavlink_wpm_storage *wpm;
 
 #include "mavlink_parameters.h"
 
+/**
+ * Print the usage
+ */
+static void usage(const char *reason);
+
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 
 void mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -1194,7 +1204,7 @@ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_
 /**
  * MAVLink Protocol main function.
  */
-int mavlink_main(int argc, char *argv[])
+int mavlink_thread_main(int argc, char *argv[])
 {
 	wpm = &wpm_s;
 
@@ -1218,46 +1228,38 @@ int mavlink_main(int argc, char *argv[])
 	/* default values for arguments */
 	char *uart_name = "/dev/ttyS0";
 	int baudrate = 57600;
-	const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
 
 	/* 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) {
-			printf(commandline_usage, argv[0], uart_name, baudrate);
-			return 0;
-		}
 
-		/* UART device ID */
+		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
-		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 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 {
-				printf(commandline_usage, argv[0], uart_name, baudrate);
+				usage("missing argument for device (-d)");
-				return 0;
+				return 1;
 			}
-		}
+		} else if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
-
-		/* baud rate */
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
 			if (argc > i + 1) {
 				baudrate = atoi(argv[i + 1]);
-
+				i++;
 			} else {
-				printf(commandline_usage, argv[0], uart_name, baudrate);
+				usage("missing argument for baud rate (-b)");
-				return 0;
+				return 1;
 			}
-		}
+		} else if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
-
-		/* terminating MAVLink is allowed - yes/no */
-		if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
 			mavlink_link_termination_allowed = true;
-		}
+		} else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
-
-		if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
 			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
+		} else {
+			usage("out of order or invalid argument");
+			return 1;
 		}
 	}
 
@@ -1392,7 +1394,54 @@ int mavlink_main(int argc, char *argv[])
 	fflush(stdout);
 	fflush(stderr);
 
+	thread_running = false;
+
 	return 0;
 }
 
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: mavlink {start|stop|status} [-d <devicename>] [-b <baudrate>] [-e/--exit-allowed]\n\n");
+	exit(1);
+}
+
+int mavlink_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("mavlink already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4096, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmavlink app is running\n");
+		} else {
+			printf("\tmavlink app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
 

apps/sensors/sensors.c

@@ -111,6 +111,9 @@ static int	fd_magnetometer = -1;
 static int fd_barometer = -1;
 static int fd_adc = -1;
 
+static bool thread_should_exit = false;
+static int sensors_task;
+
 /* Private functions declared static */
 static void sensors_timer_loop(void *arg);
 
@@ -243,7 +246,7 @@ static void sensors_timer_loop(void *arg)
 	pthread_cond_broadcast(&sensors_read_ready);
 }
 
-int sensors_main(int argc, char *argv[])
+int sensors_thread_main(int argc, char *argv[])
 {
 	/* inform about start */
 	printf("[sensors] Initializing..\n");
@@ -922,3 +925,40 @@ int sensors_main(int argc, char *argv[])
 	return ret;
 }
 
+/**
+ * Print the usage
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: sensors {start|stop}\n");
+	exit(1);
+}
+
+int sensors_main(int argc, char *argv[])
+{
+	int	ch;
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		thread_should_exit = false;
+		sensors_task = task_create("sensors", SCHED_PRIORITY_MAX - 5, 4096, sensors_thread_main, NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+