From 279952546257b3b284ec03ce65d1667b728bad93 Mon Sep 17 00:00:00 2001 From: Miguel Arroyo Date: Sat, 3 Sep 2016 21:49:00 -0400 Subject: [PATCH] Commandline Interface and Mode options --- src/drivers/drv_gps.h | 4 ++ src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 107 ++++++++++++++++++++++++++-------------- 3 files changed, 75 insertions(+), 38 deletions(-) diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 8d7b5c1dfe..e03cc0cfcf 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -58,3 +58,7 @@ typedef enum { GPS_DRIVER_MODE_ASHTECH } gps_driver_mode_t; +typedef enum { + GPS_DRIVER_UART = 0, + GPS_DRIVER_SPI +} gps_driver_interface_t; diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 3cac6a5bc5..3fa8711feb 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 3cac6a5bc5826d7ac495827a46b63f476d82ba86 +Subproject commit 3fa8711feb16b13f4b6d47bb97cdd0cf34362019 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index d8a0bb5c53..1a3915afc0 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -103,7 +103,8 @@ public: class GPS { public: - GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); + GPS(const char *path, gps_driver_mode_t mode, gps_driver_interface_t interface, bool fake_gps, bool enable_sat_info, + int gps_num); virtual ~GPS(); virtual int init(); @@ -124,6 +125,7 @@ private: bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode + gps_driver_interface_t _interface; GPSHelper *_helper; ///< instance of GPS parser GPS_Sat_Info *_sat_info; ///< instance of GPS sat info data object struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position @@ -241,11 +243,13 @@ volatile bool is_gps1_advertised = false; ///< for the second gps we want to mak } -GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) : +GPS::GPS(const char *path, gps_driver_mode_t mode, gps_driver_interface_t interface, bool fake_gps, + bool enable_sat_info, int gps_num) : _task_should_exit(false), _healthy(false), _mode_changed(false), - _mode(GPS_DRIVER_MODE_UBX), + _mode(mode), + _interface(interface), _helper(nullptr), _sat_info(nullptr), _report_gps_pos_pub{nullptr}, @@ -263,7 +267,7 @@ GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num _dump_from_device(nullptr) { /* store port name */ - strncpy(_port, uart_path, sizeof(_port)); + strncpy(_port, path, sizeof(_port)); /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; @@ -699,13 +703,24 @@ GPS::task_main() _helper = nullptr; } + GPSHelper::Interface helper_interface = GPSHelper::Interface::UART; + + switch (_interface) { + case GPS_DRIVER_UART: + helper_interface = GPSHelper::Interface::UART; + break; + + case GPS_DRIVER_SPI: + helper_interface = GPSHelper::Interface::SPI; + break; + + default: + break; + } + switch (_mode) { case GPS_DRIVER_MODE_UBX: -#ifndef __PX4_POSIX_RPI - _helper = new GPSDriverUBX(GPSHelper::Interface::UART, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info); -#else - _helper = new GPSDriverUBX(GPSHelper::Interface::SPI, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info); -#endif + _helper = new GPSDriverUBX(helper_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: @@ -810,30 +825,7 @@ GPS::task_main() _rate_rtcm_injection = 0.0f; } } - -#if !defined(__PX4_POSIX_RPI) - - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; - break; - - case GPS_DRIVER_MODE_ASHTECH: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; - } - -#endif } - } PX4_INFO("exiting"); @@ -955,7 +947,8 @@ namespace gps { -void start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num); +void start(const char *path, gps_driver_mode_t mode, gps_driver_interface_t interface, bool fake_gps, + bool enable_sat_info, int gps_num); void stop(); void test(); void reset(); @@ -965,7 +958,8 @@ void info(); * Start the driver. */ void -start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) +start(const char *path, gps_driver_mode_t mode, gps_driver_interface_t interface, bool fake_gps, bool enable_sat_info, + int gps_num) { if (g_dev[gps_num - 1] != nullptr) { PX4_WARN("GPS %i already started", gps_num); @@ -973,7 +967,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info, int gps_num) } /* create the driver */ - g_dev[gps_num - 1] = new GPS(uart_path, fake_gps, enable_sat_info, gps_num); + g_dev[gps_num - 1] = new GPS(path, mode, interface, fake_gps, enable_sat_info, gps_num); if (!g_dev[gps_num - 1] || OK != g_dev[gps_num - 1]->init()) { if (g_dev[gps_num - 1] != nullptr) { @@ -1054,6 +1048,8 @@ gps_main(int argc, char *argv[]) const char *device_name2 = nullptr; bool fake_gps = false; bool enable_sat_info = false; + gps_driver_interface_t interface = GPS_DRIVER_UART; + gps_driver_mode_t mode = GPS_DRIVER_MODE_UBX; if (argc < 2) { goto out; @@ -1088,6 +1084,43 @@ gps_main(int argc, char *argv[]) } } + /* Detect interface option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-i")) { + + int interface_arg = i + 1; + + if (interface_arg < argc) { + if (!strcmp(argv[interface_arg], "spi")) { + interface = GPS_DRIVER_SPI; + + } else if (!strcmp(argv[interface_arg], "uart")) { + interface = GPS_DRIVER_UART; + } + } + } + } + + /* Detect mode/protocol option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-p")) { + + int mode_arg = i + 1; + + if (mode_arg < argc) { + if (!strcmp(argv[mode_arg], "ubx")) { + mode = GPS_DRIVER_MODE_UBX; + + } else if (!strcmp(argv[mode_arg], "mtk")) { + mode = GPS_DRIVER_MODE_MTK; + + } else if (!strcmp(argv[mode_arg], "ash")) { + mode = GPS_DRIVER_MODE_ASHTECH; + } + } + } + } + /* Allow to use a second gps device */ for (int i = 2; i < argc; i++) { if (!strcmp(argv[i], "-dualgps")) { @@ -1100,10 +1133,10 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name, fake_gps, enable_sat_info, 1); + gps::start(device_name, mode, interface, fake_gps, enable_sat_info, 1); if (device_name2) { - gps::start(device_name2, fake_gps, enable_sat_info, 2); + gps::start(device_name2, mode, interface, fake_gps, enable_sat_info, 2); } }