From 5974c37abba562c1372beb04490014db274fd175 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 14:06:23 +0200 Subject: Moved the bulk of sensor drivers to the new world --- src/drivers/gps/gps.cpp | 536 +++++++++++++++++++++++++++++ src/drivers/gps/gps_helper.cpp | 92 +++++ src/drivers/gps/gps_helper.h | 52 +++ src/drivers/gps/module.mk | 43 +++ src/drivers/gps/mtk.cpp | 274 +++++++++++++++ src/drivers/gps/mtk.h | 124 +++++++ src/drivers/gps/ubx.cpp | 755 +++++++++++++++++++++++++++++++++++++++++ src/drivers/gps/ubx.h | 395 +++++++++++++++++++++ 8 files changed, 2271 insertions(+) create mode 100644 src/drivers/gps/gps.cpp create mode 100644 src/drivers/gps/gps_helper.cpp create mode 100644 src/drivers/gps/gps_helper.h create mode 100644 src/drivers/gps/module.mk create mode 100644 src/drivers/gps/mtk.cpp create mode 100644 src/drivers/gps/mtk.h create mode 100644 src/drivers/gps/ubx.cpp create mode 100644 src/drivers/gps/ubx.h (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp new file mode 100644 index 000000000..e35bdb944 --- /dev/null +++ b/src/drivers/gps/gps.cpp @@ -0,0 +1,536 @@ +/**************************************************************************** + * + * 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 500 +#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); + virtual ~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]"); +} diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp new file mode 100644 index 000000000..9c1fad569 --- /dev/null +++ b/src/drivers/gps/gps_helper.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 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. + * + ****************************************************************************/ + +#include +#include +#include +#include "gps_helper.h" + +/* @file gps_helper.cpp */ + +int +GPS_Helper::set_baudrate(const int &fd, unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + warnx("try baudrate: %d\n", speed); + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + return -1; + } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; +} diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h new file mode 100644 index 000000000..f3d3bc40b --- /dev/null +++ b/src/drivers/gps/gps_helper.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 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_helper.h */ + +#ifndef GPS_HELPER_H +#define GPS_HELPER_H + +#include +#include + +class GPS_Helper +{ +public: + virtual int configure(unsigned &baud) = 0; + virtual int receive(unsigned timeout) = 0; + int set_baudrate(const int &fd, unsigned baud); +}; + +#endif /* GPS_HELPER_H */ diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk new file mode 100644 index 000000000..097db2abf --- /dev/null +++ b/src/drivers/gps/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# GPS driver +# + +MODULE_COMMAND = gps + +SRCS = gps.cpp \ + gps_helper.cpp \ + mtk.cpp \ + ubx.cpp diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp new file mode 100644 index 000000000..4762bd503 --- /dev/null +++ b/src/drivers/gps/mtk.cpp @@ -0,0 +1,274 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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 mkt.cpp */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mtk.h" + + +MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), +_mtk_revision(0) +{ + decode_init(); +} + +MTK::~MTK() +{ +} + +int +MTK::configure(unsigned &baudrate) +{ + /* set baudrate first */ + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + return -1; + + baudrate = MTK_BAUDRATE; + + /* Write config messages, don't wait for an answer */ + if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + warnx("mtk: config write failed"); + return -1; + } + + return 0; +} + +int +MTK::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + gps_mtk_packet_t packet; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* first read whatever is left */ + if (j < count) { + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j], packet) > 0) { + handle_message(packet); + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } + j++; + } + /* everything is read */ + j = count = 0; + } + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +void +MTK::decode_init(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = MTK_DECODE_UNINIT; +} +int +MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) +{ + int ret = 0; + + if (_decode_state == MTK_DECODE_UNINIT) { + + if (b == MTK_SYNC1_V16) { + _decode_state = MTK_DECODE_GOT_CK_A; + _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { + _decode_state = MTK_DECODE_GOT_CK_A; + _mtk_revision = 19; + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_A) { + if (b == MTK_SYNC2) { + _decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + decode_init(); + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (_rx_count < 33) + add_byte_to_checksum(b); + + // Fill packet buffer + ((uint8_t*)(&packet))[_rx_count] = b; + _rx_count++; + + /* Packet size minus checksum, XXX ? */ + if (_rx_count >= sizeof(packet)) { + /* Compare checksum */ + if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { + ret = 1; + } else { + warnx("MTK Checksum invalid"); + ret = -1; + } + // Reset state machine to decode next packet + decode_init(); + } + } + return ret; +} + +void +MTK::handle_message(gps_mtk_packet_t &packet) +{ + if (_mtk_revision == 16) { + _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + _gps_position->lat = packet.latitude; // both degrees*1e7 + _gps_position->lon = packet.longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + _gps_position->lat = 0; + _gps_position->lon = 0; + } + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + _gps_position->fix_type = packet.fix_type; + _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + _gps_position->satellites_visible = packet.satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet.date * 1e-4; + timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet.utc_time * 1e-7; + timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); + + return; +} + +void +MTK::add_byte_to_checksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h new file mode 100644 index 000000000..d4e390b01 --- /dev/null +++ b/src/drivers/gps/mtk.h @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 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 mtk.h */ + +#ifndef MTK_H_ +#define MTK_H_ + +#include "gps_helper.h" + +#define MTK_SYNC1_V16 0xd0 +#define MTK_SYNC1_V19 0xd1 +#define MTK_SYNC2 0xdd + +#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define WAAS_ON "$PMTK301,2*2E\r\n" +#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" + +#define MTK_TIMEOUT_5HZ 400 +#define MTK_BAUDRATE 38400 + +typedef enum { + MTK_DECODE_UNINIT = 0, + MTK_DECODE_GOT_CK_A = 1, + MTK_DECODE_GOT_CK_B = 2 +} mtk_decode_state_t; + +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint8_t payload; ///< Number of payload bytes + int32_t latitude; ///< Latitude in degrees * 10^7 + int32_t longitude; ///< Longitude in degrees * 10^7 + uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 + uint32_t ground_speed; ///< velocity in m/s + int32_t heading; ///< heading in degrees * 10^2 + uint8_t satellites; ///< number of sattelites used + uint8_t fix_type; ///< fix type: XXX correct for that + uint32_t date; + uint32_t utc_time; + uint16_t hdop; ///< horizontal dilution of position (without unit) + uint8_t ck_a; + uint8_t ck_b; +} gps_mtk_packet_t; + +#pragma pack(pop) + +#define MTK_RECV_BUFFER_SIZE 40 + +class MTK : public GPS_Helper +{ +public: + MTK(const int &fd, struct vehicle_gps_position_s *gps_position); + ~MTK(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + +private: + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b, gps_mtk_packet_t &packet); + + /** + * Handle the package once it has arrived + */ + void handle_message(gps_mtk_packet_t &packet); + + /** + * Reset the parse state machine for a fresh start + */ + void decode_init(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void add_byte_to_checksum(uint8_t); + + int _fd; + struct vehicle_gps_position_s *_gps_position; + mtk_decode_state_t _decode_state; + uint8_t _mtk_revision; + uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; +}; + +#endif /* MTK_H_ */ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp new file mode 100644 index 000000000..c150f5271 --- /dev/null +++ b/src/drivers/gps/ubx.cpp @@ -0,0 +1,755 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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 ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" + +#define UBX_CONFIG_TIMEOUT 100 + +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), +_waiting_for_ack(false) +{ + decode_init(); +} + +UBX::~UBX() +{ +} + +int +UBX::configure(unsigned &baudrate) +{ + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* Define the package contents, don't change the baudrate */ + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* Send a CFG-PRT message again, this time change the baudrate */ + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ + receive(UBX_CONFIG_TIMEOUT); + + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } + + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; +} + +int +UBX::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +int +UBX::parse_char(uint8_t b) +{ + switch (_decode_state) { + /* First, look for sync1 */ + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + break; + /* Second, look for sync2 */ + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + } + break; + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + /* check for known class */ + switch (b) { + case UBX_CLASS_ACK: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = ACK; + break; + + case UBX_CLASS_NAV: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = NAV; + break; + +// case UBX_CLASS_RXM: +// _decode_state = UBX_DECODE_GOT_CLASS; +// _message_class = RXM; +// break; + + case UBX_CLASS_CFG: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = CFG; + break; + default: //unknown class: reset state machine + decode_init(); + break; + } + break; + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + switch (_message_class) { + case NAV: + switch (b) { + case UBX_MESSAGE_NAV_POSLLH: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_POSLLH; + break; + + case UBX_MESSAGE_NAV_SOL: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SOL; + break; + + case UBX_MESSAGE_NAV_TIMEUTC: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_TIMEUTC; + break; + +// case UBX_MESSAGE_NAV_DOP: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = NAV_DOP; +// break; + + case UBX_MESSAGE_NAV_SVINFO: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SVINFO; + break; + + case UBX_MESSAGE_NAV_VELNED: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_VELNED; + break; + + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; +// case RXM: +// switch (b) { +// case UBX_MESSAGE_RXM_SVSI: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = RXM_SVSI; +// break; +// +// default: //unknown class: reset state machine, should not happen +// decode_init(); +// break; +// } +// break; + + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; + + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + decode_init(); + break; + } + break; + default: //should not happen because we set the class + warnx("UBX Error, we set a class that we don't know"); + decode_init(); +// config_needed = true; + break; + } + break; + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + case UBX_DECODE_GOT_LENGTH1: + add_byte_to_checksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + case UBX_DECODE_GOT_LENGTH2: + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) + add_byte_to_checksum(b); + _rx_buffer[_rx_count] = b; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); + } + + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + break; + default: + break; + } + return 0; //XXX ? +} + + +int +UBX::handle_message() +{ + int ret = 0; + + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + + _gps_position->timestamp_variance = hrt_absolute_time(); + } + break; + } + +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; +// +// _gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// break; +// } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + _gps_position->timestamp_time = hrt_absolute_time(); + } + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + _gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + _gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + _gps_position->satellite_used[i] = 0; + } + + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + } else { + _gps_position->satellite_info_available = false; + } + _gps_position->timestamp_satellites = hrt_absolute_time(); + } + + break; + } + + case NAV_VELNED: { + + if (!_waiting_for_ack) { + /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + } + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? +} + +void +UBX::decode_init(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = UBX_DECODE_UNINIT; + _message_class = CLASS_UNKNOWN; + _message_id = ID_UNKNOWN; + _payload_size = 0; +} + +void +UBX::add_byte_to_checksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} + +void +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) +{ + uint8_t ck_a = 0; + uint8_t ck_b = 0; + unsigned i; + + for (i = 0; i < length-2; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + /* The checksum is written to the last to bytes of a message */ + message[length-2] = ck_a; + message[length-1] = ck_b; +} + +void +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) +{ + ssize_t ret = 0; + + /* Calculate the checksum now */ + add_checksum_to_message(packet, length); + + const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + + /* Start with the two sync bytes */ + ret += write(fd, sync_bytes, sizeof(sync_bytes)); + ret += write(fd, packet, length); + + if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? + warnx("ubx: config write fail"); +} diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h new file mode 100644 index 000000000..e3dd1c7ea --- /dev/null +++ b/src/drivers/gps/ubx.h @@ -0,0 +1,395 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 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 U-Blox protocol definitions */ + +#ifndef UBX_H_ +#define UBX_H_ + +#include "gps_helper.h" + +#define UBX_SYNC1 0xB5 +#define UBX_SYNC2 0x62 + +/* ClassIDs (the ones that are used) */ +#define UBX_CLASS_NAV 0x01 +//#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 + +/* MessageIDs (the ones that are used) */ +#define UBX_MESSAGE_NAV_POSLLH 0x02 +#define UBX_MESSAGE_NAV_SOL 0x06 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +//#define UBX_MESSAGE_NAV_DOP 0x04 +#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_VELNED 0x12 +//#define UBX_MESSAGE_RXM_SVSI 0x20 +#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_PRT 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_CFG_MSG 0x01 +#define UBX_MESSAGE_CFG_RATE 0x08 + +#define UBX_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ + +#define UBX_CFG_RATE_LENGTH 6 +#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ +#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ +#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ + + +#define UBX_CFG_NAV5_LENGTH 36 +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ + +#define UBX_CFG_MSG_LENGTH 8 +#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ + +#define UBX_MAX_PAYLOAD_LENGTH 500 + +// ************ +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_posllh_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; + uint32_t reserved2; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_sol_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ + int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_timeutc_packet_t; + +//typedef struct { +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint8_t ck_a; +// uint8_t ck_b; +//} gps_bin_nav_dop_packet_t; + +typedef struct { + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ + uint8_t globalFlags; + uint16_t reserved2; + +} gps_bin_nav_svinfo_part1_packet_t; + +typedef struct { + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ + uint8_t flags; + uint8_t quality; + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ + +} gps_bin_nav_svinfo_part2_packet_t; + +typedef struct { + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_svinfo_part3_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t velN; //NED north velocity, cm/s + int32_t velE; //NED east velocity, cm/s + int32_t velD; //NED down velocity, cm/s + uint32_t speed; //Speed (3-D), cm/s + uint32_t gSpeed; //Ground Speed (2-D), cm/s + int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 + uint32_t sAcc; //Speed Accuracy Estimate, cm/s + uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_velned_packet_t; + +//typedef struct { +// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ +// +// //... rest of package is not used in this implementation +// +//} gps_bin_rxm_svsi_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_ack_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_nak_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t portID; + uint8_t res0; + uint16_t res1; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t pad; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_rate_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint32_t reserved2; + uint32_t reserved3; + uint32_t reserved4; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_nav5_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t msgClass_payload; + uint8_t msgID_payload; + uint8_t rate[6]; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_msg_packet_t; + + +// END the structures of the binary packets +// ************ + +typedef enum { + UBX_CONFIG_STATE_PRT = 0, + UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, + UBX_CONFIG_STATE_RATE, + UBX_CONFIG_STATE_NAV5, + UBX_CONFIG_STATE_MSG_NAV_POSLLH, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, + UBX_CONFIG_STATE_MSG_NAV_DOP, + UBX_CONFIG_STATE_MSG_NAV_SVINFO, + UBX_CONFIG_STATE_MSG_NAV_SOL, + UBX_CONFIG_STATE_MSG_NAV_VELNED, +// UBX_CONFIG_STATE_MSG_RXM_SVSI, + UBX_CONFIG_STATE_CONFIGURED +} ubx_config_state_t; + +typedef enum { + CLASS_UNKNOWN = 0, + NAV = 1, + RXM = 2, + ACK = 3, + CFG = 4 +} ubx_message_class_t; + +typedef enum { + //these numbers do NOT correspond to the message id numbers of the ubx protocol + ID_UNKNOWN = 0, + NAV_POSLLH, + NAV_SOL, + NAV_TIMEUTC, +// NAV_DOP, + NAV_SVINFO, + NAV_VELNED, +// RXM_SVSI, + CFG_NAV5, + ACK_ACK, + ACK_NAK, +} ubx_message_id_t; + +typedef enum { + UBX_DECODE_UNINIT = 0, + UBX_DECODE_GOT_SYNC1, + UBX_DECODE_GOT_SYNC2, + UBX_DECODE_GOT_CLASS, + UBX_DECODE_GOT_MESSAGEID, + UBX_DECODE_GOT_LENGTH1, + UBX_DECODE_GOT_LENGTH2 +} ubx_decode_state_t; + +//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; +#pragma pack(pop) + +#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer + +class UBX : public GPS_Helper +{ +public: + UBX(const int &fd, struct vehicle_gps_position_s *gps_position); + ~UBX(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + +private: + + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b); + + /** + * Handle the package once it has arrived + */ + int handle_message(void); + + /** + * Reset the parse state machine for a fresh start + */ + void decode_init(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void add_byte_to_checksum(uint8_t); + + /** + * Add the two checksum bytes to an outgoing message + */ + void add_checksum_to_message(uint8_t* message, const unsigned length); + + /** + * Helper to send a config packet + */ + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + + int _fd; + struct vehicle_gps_position_s *_gps_position; + ubx_config_state_t _config_state; + bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; + ubx_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; + ubx_message_id_t _message_id; + unsigned _payload_size; +}; + +#endif /* UBX_H_ */ -- cgit v1.2.3 From 3ec536a876a66f4e56549957e81ed4547c92a0c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 May 2013 17:13:38 +0200 Subject: Improved GPS update rate calculation --- src/drivers/gps/gps.cpp | 4 ++-- src/drivers/gps/gps_helper.cpp | 11 +++++++++-- src/drivers/gps/gps_helper.h | 4 ++++ 3 files changed, 15 insertions(+), 4 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7db257816..38835418b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -288,7 +288,6 @@ GPS::task_main() // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); - warnx("module configuration successful"); while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); @@ -306,6 +305,8 @@ GPS::task_main() _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); } if (!_healthy) { @@ -381,7 +382,6 @@ GPS::print_info() warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); - _Helper->reset_update_rates(); } usleep(100000); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 54ac90dab..ba86d370a 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -46,13 +46,13 @@ float GPS_Helper::get_position_update_rate() { - return _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); + return _rate_lat_lon; } float GPS_Helper::get_velocity_update_rate() { - return _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); + return _rate_vel; } float @@ -63,6 +63,13 @@ GPS_Helper::reset_update_rates() _interval_rate_start = hrt_absolute_time(); } +float +GPS_Helper::store_update_rates() +{ + _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); + _rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f); +} + int GPS_Helper::set_baudrate(const int &fd, unsigned baud) { diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 7e456a6aa..defc1a074 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -52,11 +52,15 @@ public: float get_position_update_rate(); float get_velocity_update_rate(); float reset_update_rates(); + float store_update_rates(); protected: uint8_t _rate_count_lat_lon; uint8_t _rate_count_vel; + float _rate_lat_lon; + float _rate_vel; + uint64_t _interval_rate_start; }; -- cgit v1.2.3