/**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * 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.cpp * Driver for the GPS on a serial port */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "ubx.h" #include "mtk.h" #define TIMEOUT_5HZ 400 #define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR #endif static const int ERROR = -1; #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif class GPS : public device::CDev { public: GPS(const char* uart_path); ~GPS(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ void print_info(); private: bool _task_should_exit; ///< flag to make the main worker task exit int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path volatile int _task; //< worker task bool _healthy; ///< flag to signal if the GPS is ok 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_Helper *_Helper; ///< instance of GPS parser struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate /** * Try to configure the GPS, handle outgoing communication to the GPS */ void config(); /** * Trampoline to the worker task */ static void task_main_trampoline(void *arg); /** * Worker task: main GPS thread that configures the GPS and parses incoming data, always running */ void task_main(void); /** * Set the baudrate of the UART to the GPS */ int set_baudrate(unsigned baud); /** * Send a reset command to the GPS */ void cmd_reset(); }; /* * Driver 'main' command. */ extern "C" __EXPORT int gps_main(int argc, char *argv[]); namespace { GPS *g_dev; } GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), _mode_changed(false), _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), _rate(0.0f) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report, 0, sizeof(_report)); _debug_enabled = true; } GPS::~GPS() { /* tell the task we want it to go away */ _task_should_exit = true; /* spin waiting for the task to stop */ for (unsigned i = 0; (i < 10) && (_task != -1); i++) { /* give it another 100ms */ usleep(100000); } /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); g_dev = nullptr; } int GPS::init() { int ret = ERROR; /* do regular cdev init */ if (CDev::init() != OK) goto out; /* start the GPS driver worker task */ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); return -errno; } ret = OK; out: return ret; } int GPS::ioctl(struct file *filp, int cmd, unsigned long arg) { lock(); int ret = OK; switch (cmd) { case SENSORIOCRESET: cmd_reset(); break; } unlock(); return ret; } void GPS::task_main_trampoline(void *arg) { g_dev->task_main(); } void GPS::task_main() { log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); } uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ _Helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _Helper = new UBX(_serial_fd, &_report); break; case GPS_DRIVER_MODE_MTK: _Helper = new MTK(_serial_fd, &_report); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); //TODO: add NMEA break; default: break; } unlock(); if (_Helper->configure(_baudrate) == 0) { unlock(); while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } last_rate_count++; /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; } if (!_healthy) { warnx("module found"); _healthy = true; } } if (_healthy) { warnx("module lost"); _healthy = false; _rate = 0.0f; } lock(); } lock(); /* 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_UBX; break; // case GPS_DRIVER_MODE_NMEA: // _mode = GPS_DRIVER_MODE_UBX; // break; default: break; } } debug("exiting"); ::close(_serial_fd); /* tell the dtor that we are exiting */ _task = -1; _exit(0); } void GPS::cmd_reset() { //XXX add reset? } void GPS::print_info() { switch (_mode) { case GPS_DRIVER_MODE_UBX: warnx("protocol: UBX"); break; case GPS_DRIVER_MODE_MTK: warnx("protocol: MTK"); break; case GPS_DRIVER_MODE_NMEA: warnx("protocol: NMEA"); break; default: break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } usleep(100000); } /** * Local functions in support of the shell command. */ namespace gps { GPS *g_dev; void start(const char *uart_path); void stop(); void test(); void reset(); void info(); /** * Start the driver. */ void start(const char *uart_path) { int fd; if (g_dev != nullptr) errx(1, "already started"); /* create the driver */ g_dev = new GPS(uart_path); if (g_dev == nullptr) goto fail; if (OK != g_dev->init()) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } exit(0); fail: if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } errx(1, "driver start failed"); } /** * Stop the driver. */ void stop() { delete g_dev; g_dev = nullptr; exit(0); } /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test() { errx(0, "PASS"); } /** * Reset the driver. */ void reset() { int fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); exit(0); } /** * Print the status of the driver. */ void info() { if (g_dev == nullptr) errx(1, "driver not running"); g_dev->print_info(); exit(0); } } // namespace int gps_main(int argc, char *argv[]) { /* set to default */ char* device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) { /* work around getopt unreliability */ if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; } else { goto out; } } gps::start(device_name); } if (!strcmp(argv[1], "stop")) gps::stop(); /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) gps::test(); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) gps::reset(); /* * Print driver status. */ if (!strcmp(argv[1], "status")) gps::info(); out: errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); }