From f707a2ce60ea0818f10900e467eef2505c0fc3ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Sep 2012 22:28:13 +0200 Subject: [PATCH 1/5] 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 | 169 ++++++++++++++++++----------------------- apps/gps/gps.h | 11 +-- apps/gps/mtk.c | 5 +- apps/gps/nmea_helper.c | 8 +- apps/gps/ubx.c | 48 ++++++------ apps/gps/ubx.h | 2 +- 6 files changed, 109 insertions(+), 134 deletions(-) diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 309b9a17a8..9b62495462 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -58,8 +58,8 @@ #include #include -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status 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,21 +141,21 @@ 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); } if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\gps is running\n"); - } else { + } else { printf("\tgps not started\n"); } 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,11 +333,10 @@ 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"); - - int fd; + mavlink_log_info(mavlink_fd, "[gps] trying to connect to a ubx module"); + setup_port(device, current_gps_speed, &fd); - + /* start ubx thread and watchdog */ pthread_t ubx_thread; pthread_t ubx_watchdog_thread; @@ -411,9 +351,10 @@ 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 - + pthread_attr_t ubx_wd_attr; pthread_attr_init(&ubx_wd_attr); pthread_attr_setstacksize(&ubx_wd_attr, 1400); @@ -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,14 +460,23 @@ 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*/ if (gps_mode_try_all == false && gps_baud_try_all == false) 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,13 +492,21 @@ int gps_thread_main(int argc, char *argv[]) { sleep(RETRY_INTERVAL_SECONDS); } - close(mavlink_fd); - printf("[gps] exiting.\n"); - + close(mavlink_fd); + thread_running = false; 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) { int fd; /**< File descriptor for the gps 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); + } +} diff --git a/apps/gps/gps.h b/apps/gps/gps.h index 313a3a2c2a..80d4c7e1d8 100644 --- a/apps/gps/gps.h +++ b/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 +extern bool gps_thread_should_exit; /**< Deamon status flag */ #endif /* GPS_H_ */ diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 290fa61292..e00b863b06 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -35,6 +35,7 @@ /* @file MTK custom binary (3DR) protocol implementation */ +#include "gps.h" #include "mtk.h" #include #include @@ -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 */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index fccc7414ad..22e19df637 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -34,7 +34,7 @@ ****************************************************************************/ /* @file NMEA protocol implementation */ - +#include "gps.h" #include "nmea_helper.h" #include #include @@ -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); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index a629e904d8..a3ff2faec1 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -35,6 +35,7 @@ /* @file U-Blox protocol implementation */ +#include "gps.h" #include "ubx.h" #include #include @@ -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); @@ -799,7 +800,7 @@ void *ubx_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "gps ubx read", getpid()); - + /* Retrieve file descriptor */ int fd = *((int *)arg); @@ -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 */ diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index 5def1ed33d..73f0c369ab 100644 --- a/apps/gps/ubx.h +++ b/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); From 71b37a859c9f7bca5107aa224cfd2b11e08e191a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Sep 2012 08:17:01 +0200 Subject: [PATCH 2/5] the gps_thread_should_exit flag is now static again, hope it works like this --- apps/gps/gps.c | 13 ++++++------- apps/gps/gps.h | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 9b62495462..055b0138df 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -58,7 +58,6 @@ #include #include -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 */ @@ -154,7 +153,7 @@ int gps_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\gps is running\n"); + printf("\tgps is running\n"); } else { printf("\tgps not started\n"); } @@ -329,7 +328,7 @@ int gps_thread_main(int argc, char *argv[]) { * if the gps was once running the wtachdog thread will not return but instead try to reconfigure the gps (depending on the mode/protocol) */ - if (current_gps_mode == GPS_MODE_UBX) { //TODO: make a small enum with all modes to avoid all the strcpy + if (current_gps_mode == GPS_MODE_UBX) { if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed); @@ -465,11 +464,11 @@ int gps_thread_main(int argc, char *argv[]) { /* exit quickly if stop command has been received */ if (gps_thread_should_exit) { - printf("[gps] stopped, exiting.\n"); - close(mavlink_fd); - thread_running = false; + 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*/ if (gps_mode_try_all == false && gps_baud_try_all == false) diff --git a/apps/gps/gps.h b/apps/gps/gps.h index 80d4c7e1d8..0607e5796a 100644 --- a/apps/gps/gps.h +++ b/apps/gps/gps.h @@ -10,6 +10,6 @@ #include -extern bool gps_thread_should_exit; /**< Deamon status flag */ +static bool gps_thread_should_exit; /**< Deamon status flag */ #endif /* GPS_H_ */ From e7241fb37f59d53a65a447058ad0d3ce8249a82e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Sep 2012 11:56:30 +0200 Subject: [PATCH 3/5] 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(-) diff --git a/apps/gps/gps.c b/apps/gps/gps.c index 055b0138df..976beeaab8 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 0607e5796a..499a6164f7 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 e00b863b06..2e90f50b1b 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 1c65a5865c..9fc1caec83 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 22e19df637..d1c9d364b7 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 8fd630bcd3..3a853dd134 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 a3ff2faec1..367dd8a8c1 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 73f0c369ab..8e98cca5f3 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_ */ From df8bbb2d308adc6fa46a2f5299913b9044a53b01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Sep 2012 11:57:29 +0200 Subject: [PATCH 4/5] workaround in nuttx to allow for more than 6 arguments when starting an app (in my opinion needed) --- apps/nshlib/nsh.h | 8 +++++--- nuttx/configs/px4fmu/nsh/defconfig | 2 ++ 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h index dac91ba05d..e6dd2683b2 100644 --- a/apps/nshlib/nsh.h +++ b/apps/nshlib/nsh.h @@ -234,9 +234,11 @@ #endif /* This is the maximum number of arguments that will be accepted for a command */ - -#define NSH_MAX_ARGUMENTS 6 - +#ifdef CONFIG_NSH_MAX_ARGUMENTS +# define NSH_MAX_ARGUMENTS CONFIG_NSH_MAX_ARGUMENTS +#else +# define NSH_MAX_ARGUMENTS 10 +#endif /* strerror() produces much nicer output but is, however, quite large and * will only be used if CONFIG_NSH_STRERROR is defined. Note that the strerror * interface must also have been enabled with CONFIG_LIBC_STRERROR. diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index bc1543d569..e1ba862cf7 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -941,6 +941,7 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" # CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer # CONFIG_NSH_STRERROR - Use strerror(errno) # CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line # CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi # CONFIG_NSH_DISABLESCRIPT - Disable scripting support # CONFIG_NSH_DISABLEBG - Disable background commands @@ -972,6 +973,7 @@ CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_FILEIOSIZE=512 CONFIG_NSH_STRERROR=y CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAX_ARGUMENTS=12 CONFIG_NSH_NESTDEPTH=8 CONFIG_NSH_DISABLESCRIPT=n CONFIG_NSH_DISABLEBG=n From d7085ba9e30f5166ec583eeec6bb018364a14ce0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Sep 2012 13:09:32 +0200 Subject: [PATCH 5/5] forgot to remove some rprintfs --- apps/gps/ubx.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 367dd8a8c1..8a658b6230 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -726,7 +726,7 @@ void *ubx_watchdog_loop(void *args) int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - int err_skip_counter = 0; + //int err_skip_counter = 0; while (!(*thread_should_exit)) { /* if some values are to old reconfigure gps */ @@ -739,7 +739,7 @@ void *ubx_watchdog_loop(void *args) // 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; } } @@ -749,13 +749,13 @@ void *ubx_watchdog_loop(void *args) 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... */