fixed stacktrace which happened in configure_gps_ubx(int *fd) because of faulty file descriptor argument, added possibility to stop gps daemon (only tested without gps attached)

apps/gps/gps.c

@@ -58,7 +58,7 @@
 #include <v1.0/common/mavlink.h>
 #include <mavlink/mavlink_log.h>
 
-static bool thread_should_exit = false;		/**< Deamon exit flag */
+bool gps_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;				/**< Deamon status flag */
 static int deamon_task;				/**< Handle of deamon task / thread */
 
@@ -82,16 +82,6 @@ int gps_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
-	exit(1);
-}
-
 /****************************************************************************
  * Definitions
  ****************************************************************************/
@@ -125,40 +115,9 @@ const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS
  ****************************************************************************/
 int open_port(char *port);
 
-void close_port(int fd);
+void close_port(int *fd);
 
-void setup_port(char *device, int speed, int *fd)
-{
-
-	/* open port (baud rate is set in defconfig file) */
-	*fd = open_port(device);
-
-	if (*fd != -1) {
-		if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
-
-	} else {
-		fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
-		fflush(stdout);
-	}
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-
-	if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
-		fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
-		close(*fd);
-	}
-
-	/* Set baud rate */
-	cfsetispeed(&uart_config, speed);
-	cfsetospeed(&uart_config, speed);
-
-	if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
-		close(*fd);
-	}
-}
+void setup_port(char *device, int speed, int *fd);
 
 
 /**
@@ -182,14 +141,14 @@ int gps_main(int argc, char *argv[])
 			exit(0);
 		}
 
-		thread_should_exit = false;
+		gps_thread_should_exit = false;
 		deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
+		gps_thread_should_exit = true;
 		exit(0);
 	}
 
@@ -215,7 +174,7 @@ int gps_thread_main(int argc, char *argv[]) {
 	printf("[gps] Initialized. Searching for GPS receiver..\n");
 
 	/* default values */
-	const char *commandline_usage = "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
+	const char *commandline_usage = "\tusage: %s {start|stop|status} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
 	char *device = "/dev/ttyS3";
 	char mode[10];
 	strcpy(mode, "all");
@@ -294,45 +253,25 @@ int gps_thread_main(int argc, char *argv[]) {
 	case -1:     gps_baud_try_all = true; break;
 
 	case 0:      current_gps_speed = B0;      break;
-
 	case 50:     current_gps_speed = B50;     break;
-
 	case 75:     current_gps_speed = B75;     break;
-
 	case 110:    current_gps_speed = B110;    break;
-
 	case 134:    current_gps_speed = B134;    break;
-
 	case 150:    current_gps_speed = B150;    break;
-
 	case 200:    current_gps_speed = B200;    break;
-
 	case 300:    current_gps_speed = B300;    break;
-
 	case 600:    current_gps_speed = B600;    break;
-
 	case 1200:   current_gps_speed = B1200;   break;
-
 	case 1800:   current_gps_speed = B1800;   break;
-
 	case 2400:   current_gps_speed = B2400;   break;
-
 	case 4800:   current_gps_speed = B4800;   break;
-
 	case 9600:   current_gps_speed = B9600;   break;
-
 	case 19200:  current_gps_speed = B19200;  break;
-
 	case 38400:  current_gps_speed = B38400;  break;
-
 	case 57600:  current_gps_speed = B57600;  break;
-
 	case 115200: current_gps_speed = B115200; break;
-
 	case 230400: current_gps_speed = B230400; break;
-
 	case 460800: current_gps_speed = B460800; break;
-
 	case 921600: current_gps_speed = B921600; break;
 
 	default:
@@ -362,8 +301,10 @@ int gps_thread_main(int argc, char *argv[]) {
 		return ERROR;
 	}
 
+	/* Declare file descriptor for gps here, open port later in setup_port */
+	int fd;
 
-	while (!thread_should_exit) {
+	while (!gps_thread_should_exit) {
 		/* Infinite retries or break if retry == false */
 
 		/* Loop over all configurations of baud rate and protocol */
@@ -392,9 +333,8 @@ int gps_thread_main(int argc, char *argv[]) {
 
 				if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed);
 
-				mavlink_log_info(mavlink_fd, "GPS: trying to connect to a ubx module");
+				mavlink_log_info(mavlink_fd, "[gps] trying to connect to a ubx module");
                 
-				int fd;
 				setup_port(device, current_gps_speed, &fd);
                 
 				/* start ubx thread and watchdog */
@@ -411,6 +351,7 @@ int gps_thread_main(int argc, char *argv[]) {
 				pthread_attr_t ubx_loop_attr;
 				pthread_attr_init(&ubx_loop_attr);
 				pthread_attr_setstacksize(&ubx_loop_attr, 3000);
+
 				pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd);
 				sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
                 
@@ -433,17 +374,12 @@ int gps_thread_main(int argc, char *argv[]) {
 					gps_mode_success = true;
 					terminate_gps_thread = false;
 				}
-
-				close_port(fd);
-			}
-
-			if (current_gps_mode == GPS_MODE_MTK) {
+				close_port(&fd);
+			} else if (current_gps_mode == GPS_MODE_MTK) {
 				if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed);
 
 				mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module");
 
-
-				int fd;
 				setup_port(device, current_gps_speed, &fd);
 
 				/* start mtk thread and watchdog */
@@ -476,23 +412,21 @@ int gps_thread_main(int argc, char *argv[]) {
 					terminate_gps_thread = true;
 					pthread_join(mtk_thread, NULL);
 
-					//				if(true == gps_mode_try_all)
-					//					strcpy(mode, "nmea");
+					//if(true == gps_mode_try_all)
+					//strcpy(mode, "nmea");
 
 					gps_mode_success = true;
 					terminate_gps_thread = false;
 				}
 
-				close_port(fd);
+				close_port(&fd);
 
-			}
-
-			if (current_gps_mode == GPS_MODE_NMEA) {
+			} else if (current_gps_mode == GPS_MODE_NMEA) {
 				if (gps_verbose) printf("[gps] Trying NMEA mode at %d baud\n", current_gps_speed);
 
 				mavlink_log_info(mavlink_fd, "[gps] trying to connect to a NMEA module");
 
-				int fd;
+
 				setup_port(device, current_gps_speed, &fd);
 
 				/* start nmea thread and watchdog */
@@ -526,7 +460,15 @@ int gps_thread_main(int argc, char *argv[]) {
 					terminate_gps_thread = false;
 				}
 
-				close_port(fd);
+				close_port(&fd);
+			}
+
+			/* exit quickly if stop command has been received */
+			if (gps_thread_should_exit) {
+	        		printf("[gps] stopped, exiting.\n");
+	        		close(mavlink_fd);
+	        		thread_running = false;
+				return 0;
 	        	}
 
 			/* if both, mode and baud is set by argument, we only need one loop*/
@@ -534,6 +476,7 @@ int gps_thread_main(int argc, char *argv[]) {
 				break;
 		}
 
+	        
 		if (retry) {
 			printf("[gps] No configuration was successful, retrying in %d seconds \n", RETRY_INTERVAL_SECONDS);
 			mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, retrying...");
@@ -549,11 +492,19 @@ int gps_thread_main(int argc, char *argv[]) {
 		sleep(RETRY_INTERVAL_SECONDS);
 	}
 
+	printf("[gps] exiting.\n");
 	close(mavlink_fd);
+	thread_running = false;
+	return 0;
+}
 
-	printf("[gps] exiting.\n");
 
-	return 0;
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "\tusage: gps {start|status|stop} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
+	exit(1);
 }
 
 int open_port(char *port)
@@ -566,11 +517,39 @@ int open_port(char *port)
 }
 
 
-void close_port(int fd)
+void close_port(int *fd)
 {
 	/* Close serial port */
-	close(fd);
+	close(*fd);
 }
 
+void setup_port(char *device, int speed, int *fd)
+{
+	/* open port (baud rate is set in defconfig file) */
+	*fd = open_port(device);
+
+	if (*fd != -1) {
+		if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
+
+	} else {
+		fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
+		fflush(stdout);
+	}
 
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
 
+	if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
+		fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
+		close(*fd);
+	}
+	if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed);
+	/* Set baud rate */
+	cfsetispeed(&uart_config, speed);
+	cfsetospeed(&uart_config, speed);
+	if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
+		fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
+		close(*fd);
+	}
+}

apps/gps/gps.h

@@ -6,15 +6,10 @@
  */
 
 #ifndef GPS_H_
-#define GPS_H_
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-int gps_fd;
-
-//extern gps_bin_ubx_state_t * ubx_state;
+#define GPS_H
 
+#include <stdbool.h>
 
+extern bool gps_thread_should_exit;		/**< Deamon status flag */
 
 #endif /* GPS_H_ */

apps/gps/mtk.c

@@ -35,6 +35,7 @@
 
 /* @file MTK custom binary (3DR) protocol implementation */
 
+#include "gps.h"
 #include "mtk.h"
 #include <nuttx/config.h>
 #include <unistd.h>
@@ -313,7 +314,7 @@ void *mtk_loop(void *arg)
 	mtk_gps = &mtk_gps_d;
 	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		/* Parse a message from the gps receiver */
 		if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
 
@@ -349,7 +350,7 @@ void *mtk_watchdog_loop(void *arg)
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		fflush(stdout);
 
 		/* if we have no update for a long time reconfigure gps */

apps/gps/nmea_helper.c

@@ -34,7 +34,7 @@
  ****************************************************************************/
 
 /* @file NMEA protocol implementation */
-
+#include "gps.h"
 #include "nmea_helper.h"
 #include <sys/prctl.h>
 #include <poll.h>
@@ -75,7 +75,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
 	// NMEA or SINGLE-SENTENCE GPS mode
 
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		//check if the thread should terminate
 		if (terminate_gps_thread == true) {
 //			printf("terminate_gps_thread=%u ", terminate_gps_thread);
@@ -178,7 +178,7 @@ void *nmea_loop(void *arg)
 	nmea_gps = &nmea_gps_d;
 	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		/* Parse a message from the gps receiver */
 		uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser);
 
@@ -269,7 +269,7 @@ void *nmea_watchdog_loop(void *arg)
 
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 //		printf("nmea_watchdog_loop : while ");
 		/* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */
 		pthread_mutex_lock(nmea_mutex);

apps/gps/ubx.c

@@ -35,6 +35,7 @@
 
 /* @file U-Blox protocol implementation */
 
+#include "gps.h"
 #include "ubx.h"
 #include <sys/prctl.h>
 #include <poll.h>
@@ -570,44 +571,44 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
 //	printf("[%x,%x]\n", message[length-2], message[length-1]);
 }
 
-int configure_gps_ubx(int fd)
+int configure_gps_ubx(int *fd)
 {
-	fflush((FILE *)fd);
+	fflush(((FILE *)fd));
 
 	//TODO: write this in a loop once it is tested
 	//UBX_CFG_PRT_PART:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
 
 	usleep(100000);
 
 	//NAV_POSLLH:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 	//NAV_TIMEUTC:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 	//NAV_DOP:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 	//NAV_SOL:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 
 	//NAV_SVINFO:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 	//NAV_VELNED:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 
 	//RXM_SVSI:
-	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , fd);
+	write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
 	usleep(100000);
 
 	return 0;
@@ -721,7 +722,7 @@ void *ubx_watchdog_loop(void *arg)
 
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		/* if some values are to old reconfigure gps */
 		int i;
 		pthread_mutex_lock(ubx_mutex);
@@ -768,7 +769,7 @@ void *ubx_watchdog_loop(void *arg)
 			}
 
 			/* trying to reconfigure the gps configuration */
-			configure_gps_ubx(fd);
+			configure_gps_ubx(&fd);
 			fflush(stdout);
 			sleep(1);
 
@@ -809,23 +810,22 @@ void *ubx_loop(void *arg)
 
 	if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n");
 
-	//set parameters for ubx
-
+	//set parameters for ubx_state
 
-//	    	//ubx state
-//			gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
-//		   	printf("gps: ubx_state created\n");
-//			ubx_decode_init();
-//			ubx_state->print_errors = false;
+	//ubx state
+	gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
+	//printf("gps: ubx_state created\n");
+	ubx_decode_init();
+	ubx_state->print_errors = false;
 
 
 	/* set parameters for ubx */
 
-	if (configure_gps_ubx(fd) != 0) {
+	if (configure_gps_ubx(&fd) != 0) {
 		printf("[gps] Configuration of gps module to ubx failed\r\n");
 
 		/* Write shared variable sys_status */
-
+		// TODO enable this again
 		//global_data_send_subsystem_info(&ubx_present);
 
 	} else {
@@ -834,7 +834,7 @@ void *ubx_loop(void *arg)
 		// XXX Shouldn't the system status only change if the module is known to work ok?
 
 		/* Write shared variable sys_status */
-
+		// TODO enable this again
 		//global_data_send_subsystem_info(&ubx_present_enabled);
 	}
 
@@ -844,7 +844,7 @@ void *ubx_loop(void *arg)
 
 	orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
 
-	while (1) {
+	while (!gps_thread_should_exit) {
 		/* Parse a message from the gps receiver */
 		if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
 			/* publish new GPS position */

apps/gps/ubx.h

@@ -298,7 +298,7 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
 
 int ubx_parse(uint8_t b,  char *gps_rx_buffer);
 
-int configure_gps_ubx(int fd);
+int configure_gps_ubx(int *fd);
 
 int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size);