gps starting and stopping should be working correctly now, ubx not continuing whith configuring should be fixed

8 files changed, 148 insertions, 90 deletions

diff --git a/apps/gps/gps.c b/apps/gps/gps.c
index 055b0138d..976beeaab 100644
--- a/apps/gps/gps.c
+++ b/apps/gps/gps.c
@@ -38,7 +38,6 @@
  */
 
 #include "gps.h"
-
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <stdlib.h>
@@ -58,8 +57,9 @@
 #include <v1.0/common/mavlink.h>
 #include <mavlink/mavlink_log.h>
 
+static bool thread_should_exit;				/**< Deamon status flag */
 static bool thread_running = false;				/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
+static int deamon_task;							/**< Handle of deamon task / thread */
 
 /**
  * GPS module readout and publishing.
@@ -104,7 +104,6 @@ enum GPS_MODES {
 };
 
 
-
 #define AUTO_DETECTION_COUNT 8
 const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400};
 const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
@@ -140,14 +139,14 @@ int gps_main(int argc, char *argv[])
 			exit(0);
 		}
 
-		gps_thread_should_exit = false;
+		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")) {
-		gps_thread_should_exit = true;
+		thread_should_exit = true;
 		exit(0);
 	}
 
@@ -193,6 +192,7 @@ int gps_thread_main(int argc, char *argv[]) {
 	for (i = 0; i < argc; i++) {
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
 			printf(commandline_usage, argv[0]);
+			thread_running = false;
 			return 0;
 		}
 
@@ -202,6 +202,7 @@ int gps_thread_main(int argc, char *argv[]) {
 
 			} else {
 				printf(commandline_usage, argv[0]);
+				thread_running = false;
 				return 0;
 			}
 		}
@@ -212,6 +213,7 @@ int gps_thread_main(int argc, char *argv[]) {
 
 			} else {
 				printf(commandline_usage, argv[0]);
+				thread_running = false;
 				return 0;
 			}
 		}
@@ -222,6 +224,7 @@ int gps_thread_main(int argc, char *argv[]) {
 
 			} else {
 				printf(commandline_usage, argv[0]);
+				thread_running = false;
 				return 0;
 			}
 		}
@@ -232,6 +235,7 @@ int gps_thread_main(int argc, char *argv[]) {
 
 			} else {
 				printf(commandline_usage, argv[0]);
+				thread_running = false;
 				return 0;
 			}
 		}
@@ -297,13 +301,14 @@ int gps_thread_main(int argc, char *argv[]) {
 	} else {
 		fprintf(stderr, "\t[gps] Invalid mode argument\n");
 		printf(commandline_usage);
+		thread_running = false;
 		return ERROR;
 	}
 
 	/* Declare file descriptor for gps here, open port later in setup_port */
 	int fd;
 
-	while (!gps_thread_should_exit) {
+	while (!thread_should_exit) {
 		/* Infinite retries or break if retry == false */
 
 		/* Loop over all configurations of baud rate and protocol */
@@ -351,13 +356,17 @@ int gps_thread_main(int argc, char *argv[]) {
 				pthread_attr_init(&ubx_loop_attr);
 				pthread_attr_setstacksize(&ubx_loop_attr, 3000);
 
-				pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd);
+				struct arg_struct args;
+				args.fd_ptr = &fd;
+				args.thread_should_exit_ptr = &thread_should_exit;
+
+				pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args);
 				sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
                 
 				pthread_attr_t ubx_wd_attr;
 				pthread_attr_init(&ubx_wd_attr);
 				pthread_attr_setstacksize(&ubx_wd_attr, 1400);
-				int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&fd);
+				int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&args);
 
 				if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res);
 
@@ -372,7 +381,17 @@ int gps_thread_main(int argc, char *argv[]) {
 
 					gps_mode_success = true;
 					terminate_gps_thread = false;
+
+				/* maybe the watchdog was stopped through the thread_should_exit flag */
+				} else if (thread_should_exit) {
+					pthread_join(ubx_thread, NULL);
+					if (gps_verbose) printf("[gps] ubx watchdog and ubx loop threads have been terminated, exiting.");
+					close(mavlink_fd);
+					close_port(&fd);
+					thread_running = false;
+					return 0;
 				}
+
 				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);
@@ -398,9 +417,14 @@ int gps_thread_main(int argc, char *argv[]) {
 				pthread_attr_t mtk_loop_attr;
 				pthread_attr_init(&mtk_loop_attr);
 				pthread_attr_setstacksize(&mtk_loop_attr, 2048);
-				pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&fd);
+
+				struct arg_struct args;
+				args.fd_ptr = &fd;
+				args.thread_should_exit_ptr = &thread_should_exit;
+
+				pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&args);
 				sleep(2);
-				pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&fd);
+				pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&args);
 
 				/* wait for threads to complete */
 				pthread_join(mtk_watchdog_thread, (void *)&fd);
@@ -442,9 +466,14 @@ int gps_thread_main(int argc, char *argv[]) {
 				pthread_attr_t nmea_loop_attr;
 				pthread_attr_init(&nmea_loop_attr);
 				pthread_attr_setstacksize(&nmea_loop_attr, 4096);
-				pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&fd);
+
+				struct arg_struct args;
+				args.fd_ptr = &fd;
+				args.thread_should_exit_ptr = &thread_should_exit;
+
+				pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&args);
 				sleep(2);
-				pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&fd);
+				pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&args);
 
 				/* wait for threads to complete */
 				pthread_join(nmea_watchdog_thread, (void *)&fd);
@@ -463,7 +492,7 @@ int gps_thread_main(int argc, char *argv[]) {
 			}
 
 			/* exit quickly if stop command has been received */
-			if (gps_thread_should_exit) {
+			if (thread_should_exit) {
 				printf("[gps] stopped, exiting.\n");
 				close(mavlink_fd);
 				thread_running = false;
diff --git a/apps/gps/gps.h b/apps/gps/gps.h
index 0607e5796..499a6164f 100644
--- a/apps/gps/gps.h
+++ b/apps/gps/gps.h
@@ -10,6 +10,9 @@
 
 #include <stdbool.h>
 
-static bool gps_thread_should_exit;		/**< Deamon status flag */
+struct arg_struct {
+    int *fd_ptr;
+    bool *thread_should_exit_ptr;
+};
 
 #endif /* GPS_H_ */
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index e00b863b0..2e90f50b1 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -42,6 +42,7 @@
 #include <sys/prctl.h>
 #include <pthread.h>
 #include <poll.h>
+#include <fcntl.h>
 #include <arch/board/up_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -181,7 +182,7 @@ int mtk_parse(uint8_t b,  char *gps_rx_buffer)
 
 }
 
-int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size)  // returns 1 if the thread should terminate
+int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size)  // returns 1 if the thread should terminate
 {
 //	printf("in read_gps_mtk\n");
 	uint8_t ret = 0;
@@ -192,7 +193,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size)  // returns 1 if
 	int gpsRxOverflow = 0;
 
 	struct pollfd fds;
-	fds.fd = fd;
+	fds.fd = *fd;
 	fds.events = POLLIN;
 
 	// This blocks the task until there is something on the buffer
@@ -207,7 +208,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size)  // returns 1 if
 		}
 
 		if (poll(&fds, 1, 1000) > 0) {
-			if (read(fd, &c, 1) > 0) {
+			if (read(*fd, &c, 1) > 0) {
 //				printf("Read %x\n",c);
 				if (rx_count >= buffer_size) {
 					// The buffer is already full and we haven't found a valid NMEA sentence.
@@ -244,11 +245,11 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size)  // returns 1 if
 	return ret;
 }
 
-int configure_gps_mtk(int fd)
+int configure_gps_mtk(int *fd)
 {
 	int success = 0;
 	size_t result_write;
-	result_write =  write(fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ));
+	result_write =  write(*fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ));
 
 	if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) {
 		printf("[gps] Set update speed to 10 Hz failed\r\n");
@@ -259,7 +260,7 @@ int configure_gps_mtk(int fd)
 	}
 
 	//set custom mode
-	result_write =  write(fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE));
+	result_write =  write(*fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE));
 
 	if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) {
 		//global_data_send_subsystem_info(&mtk_present);
@@ -274,7 +275,7 @@ int configure_gps_mtk(int fd)
 	return success;
 }
 
-void *mtk_loop(void *arg)
+void *mtk_loop(void *args)
 {
 //	int oldstate;
 //	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate);
@@ -283,8 +284,10 @@ void *mtk_loop(void *arg)
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps mtk read", getpid());
 
-	/* Retrieve file descriptor */
-	int fd = *((int *)arg);
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
 
 	/* Initialize gps stuff */
 //	int buffer_size = 1000;
@@ -314,7 +317,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 (!gps_thread_should_exit) {
+	while (!(*thread_should_exit)) {
 		/* Parse a message from the gps receiver */
 		if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
 
@@ -322,15 +325,19 @@ void *mtk_loop(void *arg)
 			orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
 
 		} else {
+			/* de-advertise */
+			close(gps_handle);
 			break;
 		}
 
 	}
 
+	close(gps_handle);
+	if(gps_verbose) printf("[gps] mtk loop is going to terminate\n");
 	return NULL;
 }
 
-void *mtk_watchdog_loop(void *arg)
+void *mtk_watchdog_loop(void *args)
 {
 //	printf("in mtk watchdog loop\n");
 	fflush(stdout);
@@ -338,8 +345,10 @@ void *mtk_watchdog_loop(void *arg)
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps mtk watchdog", getpid());
 
-	/* Retrieve file descriptor */
-	int fd = *((int *)arg);
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
 
 	bool mtk_healthy = false;
 
@@ -350,7 +359,7 @@ void *mtk_watchdog_loop(void *arg)
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 
-	while (!gps_thread_should_exit) {
+	while (!(*thread_should_exit)) {
 		fflush(stdout);
 
 		/* if we have no update for a long time reconfigure gps */
@@ -417,8 +426,7 @@ void *mtk_watchdog_loop(void *arg)
 
 		usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
 	}
-
+	if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n");
 	close(mavlink_fd);
-
 	return NULL;
 }
diff --git a/apps/gps/mtk.h b/apps/gps/mtk.h
index 1c65a5865..9fc1caec8 100644
--- a/apps/gps/mtk.h
+++ b/apps/gps/mtk.h
@@ -87,12 +87,12 @@ void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
 
 int mtk_parse(uint8_t b,  char *gps_rx_buffer);
 
-int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size);
+int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size);
 
-int configure_gps_mtk(int fd);
+int configure_gps_mtk(int *fd);
 
-void *mtk_loop(void *arg);
+void *mtk_loop(void *args);
 
-void *mtk_watchdog_loop(void *arg);
+void *mtk_watchdog_loop(void *args);
 
 #endif /* MTK_H_ */
diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c
index 22e19df63..d1c9d364b 100644
--- a/apps/gps/nmea_helper.c
+++ b/apps/gps/nmea_helper.c
@@ -37,10 +37,14 @@
 #include "gps.h"
 #include "nmea_helper.h"
 #include <sys/prctl.h>
+#include <unistd.h>
 #include <poll.h>
+#include <fcntl.h>
+#include <unistd.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
+#include <arch/board/up_hrt.h>
 
 #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
@@ -59,7 +63,7 @@ extern bool gps_verbose;
 extern int current_gps_speed;
 
 
-int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
+int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
 {
 	int ret = 1;
 	char c;
@@ -69,13 +73,13 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
 	int gpsRxOverflow = 0;
 
 	struct pollfd fds;
-	fds.fd = fd;
+	fds.fd = *fd;
 	fds.events = POLLIN;
 
 	// NMEA or SINGLE-SENTENCE GPS mode
 
 
-	while (!gps_thread_should_exit) {
+	while (1) {
 		//check if the thread should terminate
 		if (terminate_gps_thread == true) {
 //			printf("terminate_gps_thread=%u ", terminate_gps_thread);
@@ -86,7 +90,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
 		}
 
 		if (poll(&fds, 1, 1000) > 0) {
-			if (read(fd, &c, 1) > 0) {
+			if (read(*fd, &c, 1) > 0) {
 				// detect start while acquiring stream
 				//		printf("Char = %c\n", c);
 				if (!start_flag && (c == '$')) {
@@ -155,13 +159,15 @@ float ndeg2degree(float val)
 	return val;
 }
 
-void *nmea_loop(void *arg)
+void *nmea_loop(void *args)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps nmea read", getpid());
 
-	/* Retrieve file descriptor */
-	int fd = *((int *)arg);
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
 
 	/* Initialize gps stuff */
 	nmeaINFO info_d;
@@ -174,11 +180,11 @@ void *nmea_loop(void *arg)
 	nmea_zero_INFO(info);
 
 	/* advertise GPS topic */
-	struct vehicle_gps_position_s nmea_gps_d = {0};
+	struct vehicle_gps_position_s nmea_gps_d = {.counter=0};
 	nmea_gps = &nmea_gps_d;
 	orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
 
-	while (!gps_thread_should_exit) {
+	while (!(*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);
 
@@ -200,11 +206,11 @@ void *nmea_loop(void *arg)
 			//			printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, info->utc.hsec);
 
 			nmea_gps->timestamp = hrt_absolute_time();
-			nmea_gps->time_gps_usec =  epoch * 1e6 + info->utc.hsec * 1e4;
+			nmea_gps->time_gps_usec = (uint64_t)((epoch)*1000000 + (info->utc.hsec)*10000);
 			nmea_gps->fix_type = (uint8_t)info->fix;
-			nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7);
-			nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7);
-			nmea_gps->alt = (int32_t)(info->elv * 1e3);
+			nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7f);
+			nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7f);
+			nmea_gps->alt = (int32_t)(info->elv * 1000.0f);
 			nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
 			nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling
 			nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100
@@ -251,12 +257,12 @@ void *nmea_loop(void *arg)
 
 	//destroy gps parser
 	nmea_parser_destroy(&parser);
-
+	if(gps_verbose) printf("[gps] nmea loop is going to terminate\n");
 	return NULL;
 
 }
 
-void *nmea_watchdog_loop(void *arg)
+void *nmea_watchdog_loop(void *args)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps nmea watchdog", getpid());
@@ -267,9 +273,14 @@ void *nmea_watchdog_loop(void *arg)
 	uint8_t nmea_success_count = 0;
 	bool once_ok = false;
 
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	//int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
+
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	while (!gps_thread_should_exit) {
+	while (!(*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);
@@ -328,8 +339,7 @@ void *nmea_watchdog_loop(void *arg)
 
 		usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS);
 	}
-
+	if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n");
 	close(mavlink_fd);
-
 	return NULL;
 }
diff --git a/apps/gps/nmea_helper.h b/apps/gps/nmea_helper.h
index 8fd630bcd..3a853dd13 100644
--- a/apps/gps/nmea_helper.h
+++ b/apps/gps/nmea_helper.h
@@ -30,7 +30,7 @@ extern pthread_mutex_t *nmea_mutex;
 
 
 
-int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
+int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
 
 void *nmea_loop(void *arg);
 
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index a3ff2faec..367dd8a8c 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -35,13 +35,16 @@
 
 /* @file U-Blox protocol implementation */
 
-#include "gps.h"
+
 #include "ubx.h"
+#include "gps.h"
 #include <sys/prctl.h>
 #include <poll.h>
 #include <arch/board/up_hrt.h>
 #include <uORB/uORB.h>
 #include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
 
@@ -341,8 +344,8 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 
 
 
-						ubx_gps->time_gps_usec = (uint64_t)epoch * 1e6; //TODO: test this
-						ubx_gps->time_gps_usec += packet->time_nanoseconds * 1e-3;
+						ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+						ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 
 						ubx_gps->timestamp = hrt_absolute_time();
 						ubx_gps->counter++;
@@ -475,7 +478,7 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 					if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
 
 						ubx_gps->vel = (uint16_t)packet->speed;
-						ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3);
+						ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
 
 						ubx_gps->timestamp = hrt_absolute_time();
 						ubx_gps->counter++;
@@ -573,7 +576,7 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
 
 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:
@@ -616,16 +619,15 @@ int configure_gps_ubx(int *fd)
 
 
 
-int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
+int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
 {
-
 	uint8_t ret = 0;
 	uint8_t c;
 	int rx_count = 0;
 	int gpsRxOverflow = 0;
 
 	struct pollfd fds;
-	fds.fd = fd;
+	fds.fd = *fd;
 	fds.events = POLLIN;
 
 	// UBX GPS mode
@@ -642,7 +644,7 @@ int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
 		}
 
 		if (poll(&fds, 1, 1000) > 0) {
-			if (read(fd, &c, 1) > 0) {
+			if (read(*fd, &c, 1) > 0) {
 
 				//		printf("Read %x\n",c);
 				if (rx_count >= buffer_size) {
@@ -686,7 +688,7 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
 	uint8_t ck_a = 0;
 	uint8_t ck_b = 0;
 
-	int i;
+	unsigned int i;
 
 	for (i = 2; i < length; i++) {
 		ck_a = ck_a + message[i];
@@ -703,14 +705,16 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
 
 }
 
-void *ubx_watchdog_loop(void *arg)
+void *ubx_watchdog_loop(void *args)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps ubx watchdog", getpid());
 
 
-	/* Retrieve file descriptor */
-	int fd = *((int *)arg);
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
 
 	/* GPS watchdog error message skip counter */
 
@@ -722,7 +726,9 @@ void *ubx_watchdog_loop(void *arg)
 
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	while (!gps_thread_should_exit) {
+	int err_skip_counter = 0;
+
+	while (!(*thread_should_exit)) {
 		/* if some values are to old reconfigure gps */
 		int i;
 		pthread_mutex_lock(ubx_mutex);
@@ -733,7 +739,7 @@ void *ubx_watchdog_loop(void *arg)
 //			printf("timestamp_now=%llu\n", timestamp_now);
 //			printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]);
 			if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
-//				printf("Warning: GPS ubx message %d not received for a long time\n", i);
+				printf("Warning: GPS ubx message %d not received for a long time\n", i);
 				all_okay = false;
 			}
 		}
@@ -743,13 +749,13 @@ void *ubx_watchdog_loop(void *arg)
 		if (!all_okay) {
 			/* gps error */
 			ubx_fail_count++;
-//			if (err_skip_counter == 0)
-//			{
-//				printf("GPS Watchdog detected gps not running or having problems\n");
-//				err_skip_counter = 20;
-//			}
-//			err_skip_counter--;
-//			printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
+			if (err_skip_counter == 0)
+			{
+				printf("GPS Watchdog detected gps not running or having problems\n");
+				err_skip_counter = 20;
+			}
+			err_skip_counter--;
+			printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
 
 
 			/* If we have too many failures and another mode or baud should be tried, exit... */
@@ -767,9 +773,8 @@ void *ubx_watchdog_loop(void *arg)
 				ubx_healthy = false;
 				ubx_success_count = 0;
 			}
-
 			/* trying to reconfigure the gps configuration */
-			configure_gps_ubx(&fd);
+			configure_gps_ubx(fd);
 			fflush(stdout);
 			sleep(1);
 
@@ -778,7 +783,7 @@ void *ubx_watchdog_loop(void *arg)
 			ubx_success_count++;
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
-				printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
+				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
 				// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
 				ubx_healthy = true;
@@ -791,18 +796,20 @@ void *ubx_watchdog_loop(void *arg)
 		usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
 	}
 
+	if(gps_verbose) printf("[gps] ubx loop is going to terminate\n");
 	close(mavlink_fd);
-
 	return NULL;
 }
 
-void *ubx_loop(void *arg)
+void *ubx_loop(void *args)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "gps ubx read", getpid());
 	
-	/* Retrieve file descriptor */
-	int fd = *((int *)arg);
+	/* Retrieve file descriptor and thread flag */
+	struct arg_struct *arguments = (struct arg_struct *)args;
+	int *fd = arguments->fd_ptr;
+	bool *thread_should_exit = arguments->thread_should_exit_ptr;
 
 	/* Initialize gps stuff */
 	char gps_rx_buffer[UBX_BUFFER_SIZE];
@@ -813,7 +820,7 @@ void *ubx_loop(void *arg)
 	//set parameters for ubx_state
 
 	//ubx state
-	gps_bin_ubx_state_t * ubx_state = malloc(sizeof(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;
@@ -821,7 +828,7 @@ void *ubx_loop(void *arg)
 
 	/* 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 */
@@ -838,13 +845,13 @@ void *ubx_loop(void *arg)
 		//global_data_send_subsystem_info(&ubx_present_enabled);
 	}
 
-	struct vehicle_gps_position_s ubx_gps_d = {0};
+	struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
 
 	ubx_gps = &ubx_gps_d;
 
 	orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
 
-	while (!gps_thread_should_exit) {
+	while (!(*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 */
@@ -857,7 +864,8 @@ void *ubx_loop(void *arg)
 		}
 	}
 
-
+	if(gps_verbose) printf("[gps] ubx read is going to terminate\n");
+	close(gps_pub);
 	return NULL;
 
 }
diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h
index 73f0c369a..8e98cca5f 100644
--- a/apps/gps/ubx.h
+++ b/apps/gps/ubx.h
@@ -300,15 +300,15 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer);
 
 int configure_gps_ubx(int *fd);
 
-int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size);
+int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
 
 int write_config_message_ubx(uint8_t *message, size_t length, int fd);
 
 void calculate_ubx_checksum(uint8_t *message, uint8_t length);
 
-void *ubx_watchdog_loop(void *arg);
+void *ubx_watchdog_loop(void *args);
 
-void *ubx_loop(void *arg);
+void *ubx_loop(void *args);
 
 
 #endif /* UBX_H_ */