From f707a2ce60ea0818f10900e467eef2505c0fc3ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Sep 2012 22:28:13 +0200 Subject: [PATCH] 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 | 167 ++++++++++++++++++----------------------- 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, 108 insertions(+), 133 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 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); - } +void close_port(int *fd); - /* 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,11 +492,19 @@ int gps_thread_main(int argc, char *argv[]) { sleep(RETRY_INTERVAL_SECONDS); } + printf("[gps] exiting.\n"); close(mavlink_fd); + thread_running = false; + return 0; +} - printf("[gps] exiting.\n"); - return 0; +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "\tusage: gps {start|status|stop} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n"); + exit(1); } int open_port(char *port) @@ -566,11 +517,39 @@ int open_port(char *port) } -void close_port(int fd) +void close_port(int *fd) { /* Close serial port */ - close(fd); + close(*fd); } +void setup_port(char *device, int speed, int *fd) +{ + /* open port (baud rate is set in defconfig file) */ + *fd = open_port(device); + + if (*fd != -1) { + if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed); + } else { + fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n"); + fflush(stdout); + } + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) { + fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state); + close(*fd); + } + if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed); + /* Set baud rate */ + cfsetispeed(&uart_config, speed); + cfsetospeed(&uart_config, speed); + if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device); + close(*fd); + } +} 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);