diff options
Diffstat (limited to 'apps/gps/gps.c')
-rw-r--r-- | apps/gps/gps.c | 589 |
1 files changed, 0 insertions, 589 deletions
diff --git a/apps/gps/gps.c b/apps/gps/gps.c deleted file mode 100644 index 8a9512054..000000000 --- a/apps/gps/gps.c +++ /dev/null @@ -1,589 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file gps.c - * GPS app main loop. - */ - -#include "gps.h" -#include <nuttx/config.h> -#include <unistd.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include "nmealib/nmea/nmea.h" // the nmea library -#include "nmea_helper.h" //header files for interacting with the nmea library -#include "mtk.h" //header files for the custom protocol for the mediatek diydrones chip -#include "ubx.h" //header files for the ubx protocol -#include <termios.h> -#include <signal.h> -#include <pthread.h> -#include <sys/prctl.h> -#include <errno.h> -#include <signal.h> -#include <v1.0/common/mavlink.h> -#include <mavlink/mavlink_log.h> - -#include <systemlib/systemlib.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 */ - -/** - * GPS module readout and publishing. - * - * This function reads the onboard gps and publishes the vehicle_gps_positon topic. - * - * @see vehicle_gps_position_s - * @ingroup apps - */ -__EXPORT int gps_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int gps_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/**************************************************************************** - * Definitions - ****************************************************************************/ -#define IMPORTANT_GPS_BAUD_RATES_N 2 -#define RETRY_INTERVAL_SECONDS 10 - -//gps_bin_ubx_state_t * ubx_state; -bool gps_mode_try_all; -bool gps_baud_try_all; -bool gps_mode_success; -bool terminate_gps_thread; -bool gps_verbose; -int current_gps_speed; - -enum GPS_MODES { - GPS_MODE_START = 0, - GPS_MODE_UBX = 1, - GPS_MODE_MTK = 2, - GPS_MODE_NMEA = 3, - GPS_MODE_END = 4 -}; - - -#define AUTO_DETECTION_COUNT 8 -const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600}; -const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, 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 - -/**************************************************************************** - * Private functions - ****************************************************************************/ -int open_port(char *port); - -void close_port(int *fd); - -void setup_port(char *device, int speed, int *fd); - - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int gps_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("gps already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("gps", - SCHED_DEFAULT, - 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; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tgps is running\n"); - } else { - printf("\tgps not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/* - * Main function of gps app. - */ -int gps_thread_main(int argc, char *argv[]) { - - /* welcome message */ - printf("[gps] Initialized. Searching for GPS receiver..\n"); - - /* default values */ - 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"); - int baudrate = -1; - gps_mode_try_all = false; - gps_baud_try_all = false; - gps_mode_success = true; - terminate_gps_thread = false; - bool retry = false; - gps_verbose = false; - - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - /* read arguments */ - int i; - - 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; - } - - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set - if (argc > i + 1) { - device = argv[i + 1]; - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--retry") == 0) { - if (argc > i + 1) { - retry = atoi(argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { - if (argc > i + 1) { - baudrate = atoi(argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { - if (argc > i + 1) { - strcpy(mode, argv[i + 1]); - - } else { - printf(commandline_usage, argv[0]); - thread_running = false; - return 0; - } - } - - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - gps_verbose = true; - } - } - - /* - * In case a baud rate is set only this baud rate will be tried, - * otherwise a array of usual baud rates for gps receivers is used - */ - - -// printf("baudrate = %d\n",baudrate); - switch (baudrate) { - 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: - fprintf(stderr, "[gps] ERROR: Unsupported baudrate: %d\n", baudrate); - fflush(stdout); - return -EINVAL; - } - - - enum GPS_MODES current_gps_mode = GPS_MODE_UBX; - - if (strcmp(mode, "ubx") == 0) { - current_gps_mode = GPS_MODE_UBX; - - } else if (strcmp(mode, "mtkcustom") == 0) { - current_gps_mode = GPS_MODE_MTK; - - } else if (strcmp(mode, "nmea") == 0) { - current_gps_mode = GPS_MODE_NMEA; - - } else if (strcmp(mode, "all") == 0) { - gps_mode_try_all = true; - - } 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 (!thread_should_exit) { - /* Infinite retries or break if retry == false */ - - /* Loop over all configurations of baud rate and protocol */ - for (i = 0; i < AUTO_DETECTION_COUNT; i++) { - if (gps_mode_try_all) { - current_gps_mode = autodetection_gpsmodes[i]; - - if (false == gps_baud_try_all && autodetection_baudrates[i] != current_gps_speed) //there is no need to try modes which are not configured to run with the selcted baud rate - continue; - } - - if (gps_baud_try_all) { - current_gps_speed = autodetection_baudrates[i]; - - if (false == gps_mode_try_all && autodetection_gpsmodes[i] != current_gps_mode) //there is no need to try baud rates which are not usual for the selected mode - continue; - } - - - /* - * The watchdog_thread will return and set gps_mode_success to false if no data can be parsed. - * 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) { - - 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"); - - setup_port(device, current_gps_speed, &fd); - - /* start ubx thread and watchdog */ - pthread_t ubx_thread; - pthread_t ubx_watchdog_thread; - - pthread_mutex_t ubx_mutex_d; - ubx_mutex = &ubx_mutex_d; - pthread_mutex_init(ubx_mutex, NULL); - gps_bin_ubx_state_t ubx_state_d; - ubx_state = &ubx_state_d; - ubx_decode_init(); - - pthread_attr_t ubx_loop_attr; - pthread_attr_init(&ubx_loop_attr); - pthread_attr_setstacksize(&ubx_loop_attr, 3000); - - 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); - - 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 *)&args); - - if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res); - - /* wait for threads to complete */ - pthread_join(ubx_watchdog_thread, NULL); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] no success with UBX mode and %d baud\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(ubx_thread, NULL); - - 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); - - mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module"); - - setup_port(device, current_gps_speed, &fd); - - /* start mtk thread and watchdog */ - pthread_t mtk_thread; - pthread_t mtk_watchdog_thread; - - pthread_mutex_t mtk_mutex_d; - mtk_mutex = &mtk_mutex_d; - pthread_mutex_init(mtk_mutex, NULL); - - - gps_bin_mtk_state_t mtk_state_d; - mtk_state = &mtk_state_d; - mtk_decode_init(); - - - pthread_attr_t mtk_loop_attr; - pthread_attr_init(&mtk_loop_attr); - pthread_attr_setstacksize(&mtk_loop_attr, 2048); - - 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 *)&args); - - /* wait for threads to complete */ - pthread_join(mtk_watchdog_thread, (void *)&fd); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] No success with MTK binary mode and %d baud\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(mtk_thread, NULL); - - //if(true == gps_mode_try_all) - //strcpy(mode, "nmea"); - - gps_mode_success = true; - terminate_gps_thread = false; - } - - close_port(&fd); - - } 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"); - - - setup_port(device, current_gps_speed, &fd); - - /* start nmea thread and watchdog */ - pthread_t nmea_thread; - pthread_t nmea_watchdog_thread; - - pthread_mutex_t nmea_mutex_d; - nmea_mutex = &nmea_mutex_d; - pthread_mutex_init(nmea_mutex, NULL); - - gps_bin_nmea_state_t nmea_state_d; - nmea_state = &nmea_state_d; - - pthread_attr_t nmea_loop_attr; - pthread_attr_init(&nmea_loop_attr); - pthread_attr_setstacksize(&nmea_loop_attr, 4096); - - 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 *)&args); - - /* wait for threads to complete */ - pthread_join(nmea_watchdog_thread, (void *)&fd); - - if (gps_mode_success == false) { - if (gps_verbose) printf("[gps] No success with NMEA mode and %d baud\r\n", current_gps_speed); - - terminate_gps_thread = true; - pthread_join(nmea_thread, NULL); - - gps_mode_success = true; - terminate_gps_thread = false; - } - - close_port(&fd); - } - - /* exit quickly if stop command has been received */ - if (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..."); - fflush(stdout); - - } else { - fprintf(stderr, "[gps] No configuration was successful, exiting... \n"); - fflush(stdout); - mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, exiting..."); - break; - } - - sleep(RETRY_INTERVAL_SECONDS); - } - - 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 */ - - /* Open serial port */ - fd = open(port, O_CREAT | O_RDWR | O_NOCTTY); /* O_RDWR - Read and write O_NOCTTY - Ignore special chars like CTRL-C */ - return (fd); -} - - -void close_port(int *fd) -{ - /* Close serial port */ - 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 baud\n", device, speed); - - } else { - fprintf(stderr, "[gps] Could not open port, exiting gps app!\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\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)\n", device); - close(*fd); - } -} |