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.c505
1 files changed, 505 insertions, 0 deletions
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 <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>
+
+/**
+ * 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);
+}
+
+
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-21 23:13:27 +0000