aboutsummaryrefslogtreecommitdiff
path: root/apps/gps/gps.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/gps/gps.c')
-rw-r--r--apps/gps/gps.c589
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);
- }
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 11:57:57 +0000