From e7241fb37f59d53a65a447058ad0d3ce8249a82e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Sep 2012 11:56:30 +0200 Subject: gps starting and stopping should be working correctly now, ubx not continuing whith configuring should be fixed --- apps/gps/gps.c | 55 +++++++++++++++++++++++++++--------- apps/gps/gps.h | 5 +++- apps/gps/mtk.c | 40 +++++++++++++++----------- apps/gps/mtk.h | 8 +++--- apps/gps/nmea_helper.c | 46 ++++++++++++++++++------------ apps/gps/nmea_helper.h | 2 +- apps/gps/ubx.c | 76 ++++++++++++++++++++++++++++---------------------- apps/gps/ubx.h | 6 ++-- 8 files changed, 148 insertions(+), 90 deletions(-) (limited to 'apps/gps') 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 #include #include @@ -58,8 +57,9 @@ #include #include +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 -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 #include #include +#include #include #include #include @@ -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 +#include #include +#include +#include #include #include #include +#include #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 #include #include #include #include +#include +#include #include #include @@ -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_ */ -- cgit v1.2.3