From 8a365179eafdf3aea98e60ab9f5882b200d4c759 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 4 Aug 2012 15:12:36 -0700 Subject: Fresh import of the PX4 firmware sources. --- apps/gps/gps.c | 505 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 505 insertions(+) create mode 100644 apps/gps/gps.c (limited to 'apps/gps/gps.c') diff --git a/apps/gps/gps.c b/apps/gps/gps.c new file mode 100644 index 000000000..6d3ff0d6b --- /dev/null +++ b/apps/gps/gps.c @@ -0,0 +1,505 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 +#include +#include +#include +#include +#include +#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 +#include +#include +#include +#include +#include +#include +#include + +/** + * 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[]); + +/**************************************************************************** + * 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[] = {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 + +/**************************************************************************** + * Private functions + ****************************************************************************/ +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); + } + + /* 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); + } +} + + +/* + * Main function of gps app. + */ +int gps_main(int argc, char *argv[]) +{ + /* welcome message */ + 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"; + 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 = 1; i < argc; i++) { + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set + printf(commandline_usage, argv[0]); + 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]); + 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]); + 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]); + 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]); + 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); + return ERROR; + } + + + while (true) { + /* 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) { //TODO: make a small enum with all modes to avoid all the strcpy + + 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; + 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); + 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); + int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&fd); + + 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; + } + + close_port(fd); + } + + 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 */ + 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); + pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&fd); + sleep(2); + pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&fd); + + /* 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); + + } + + 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 */ + 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); + pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&fd); + sleep(2); + pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&fd); + + /* 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); + } + + /* 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); + } + + close(mavlink_fd); + + return ERROR; +} + +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); +} + + + -- cgit v1.2.3