From 50b736333f97761e4613354ee853071c652736ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 15:57:12 +0100 Subject: [PATCH] Reduced, but functional u-blox series driver --- apps/drivers/drv_gps.h | 80 ++++ apps/drivers/gps/Makefile | 42 ++ apps/drivers/gps/gps.cpp | 627 ++++++++++++++++++++++++++ apps/drivers/gps/gps_helper.h | 47 ++ apps/drivers/gps/ubx.cpp | 818 ++++++++++++++++++++++++++++++++++ apps/drivers/gps/ubx.h | 369 +++++++++++++++ 6 files changed, 1983 insertions(+) create mode 100644 apps/drivers/drv_gps.h create mode 100644 apps/drivers/gps/Makefile create mode 100644 apps/drivers/gps/gps.cpp create mode 100644 apps/drivers/gps/gps_helper.h create mode 100644 apps/drivers/gps/ubx.cpp create mode 100644 apps/drivers/gps/ubx.h diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h new file mode 100644 index 0000000000..89fc54b0d9 --- /dev/null +++ b/apps/drivers/drv_gps.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 driver interface. + */ + +#ifndef _DRV_GPS_H +#define _DRV_GPS_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define GPS_DEVICE_PATH "/dev/gps" + +typedef enum { + GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_MTK19, + GPS_DRIVER_MODE_MTK16, + GPS_DRIVER_MODE_NMEA, +} gps_driver_mode_t; + + + + +/* + * ObjDev tag for GPS data. + */ +ORB_DECLARE(gps); + +/* + * ioctl() definitions + */ +#define _GPSIOCBASE (0x2800) //TODO: arbitrary choice... +#define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n)) + +/** configure ubx mode */ +#define GPS_CONFIGURE_UBX _GPSIOC(0) + +/** configure mtk mode */ +#define GPS_CONFIGURE_MTK19 _GPSIOC(1) +#define GPS_CONFIGURE_MTK16 _GPSIOC(2) + +/** configure mtk mode */ +#define GPS_CONFIGURE_NMEA _GPSIOC(3) + +#endif /* _DRV_GPS_H */ diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile new file mode 100644 index 0000000000..3859a88a5a --- /dev/null +++ b/apps/drivers/gps/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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 +# + +APPNAME = gps +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp new file mode 100644 index 0000000000..7d6f7144ef --- /dev/null +++ b/apps/drivers/gps/gps.cpp @@ -0,0 +1,627 @@ +/**************************************************************************** + * + * 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 UART6 + */ + +#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" + +#define SEND_BUFFER_LENGTH 100 +#define TIMEOUT 1000000 //1s + +#define NUMBER_OF_BAUDRATES 4 +#define CONFIG_TIMEOUT 2000000 + +/* 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(); + ~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: + + int _task_should_exit; + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; + uint8_t _send_buffer[SEND_BUFFER_LENGTH]; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + volatile int _task; ///< worker task + + bool _response_received; + bool _config_needed; + bool _baudrate_changed; + bool _mode_changed; + bool _healthy; + gps_driver_mode_t _mode; + unsigned _messages_received; + + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + + struct vehicle_gps_position_s _report; + orb_advert_t _report_pub; + + orb_advert_t _gps_topic; + + void recv(); + + void config(); + + static void task_main_trampoline(void *arg); + + + /** + * worker task + */ + void task_main(void); + + 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() : + CDev("gps", GPS_DEVICE_PATH), + _task_should_exit(false), + _baudrates_to_try({9600, 38400, 57600, 115200}), + _config_needed(true), + _baudrate_changed(false), + _mode_changed(true), + _healthy(false), + _mode(GPS_DRIVER_MODE_UBX), + _messages_received(0), + _Helper(nullptr) +{ + /* we need this potentially before it could be set in task_main */ + g_dev = this; + + _debug_enabled = true; + debug("[gps driver] instantiated"); +} + +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 IO interface task */ + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + +// if (_gps_topic < 0) +// debug("failed to create GPS object"); + + ret = OK; +out: + return ret; +} + +int +GPS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case GPS_CONFIGURE_UBX: + if (_mode != GPS_DRIVER_MODE_UBX) { + _mode = GPS_DRIVER_MODE_UBX; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK19: + if (_mode != GPS_DRIVER_MODE_MTK19) { + _mode = GPS_DRIVER_MODE_MTK19; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK16: + if (_mode != GPS_DRIVER_MODE_MTK16) { + _mode = GPS_DRIVER_MODE_MTK16; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_NMEA: + if (_mode != GPS_DRIVER_MODE_NMEA) { + _mode = GPS_DRIVER_MODE_NMEA; + _mode_changed = true; + } + break; + case SENSORIOCRESET: + cmd_reset(); + break; + } + + return ret; +} + +void +GPS::config() +{ + int length = 0; + + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + + /* the message needs to be sent at the old baudrate */ + if (length > 0) { + if (length != ::write(_serial_fd, _send_buffer, length)) { + debug("write config failed"); + return; + } + } + + if (_baudrate_changed) { + set_baudrate(_baudrate); + _baudrate_changed = false; + } +} + +void +GPS::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +void +GPS::task_main() +{ + log("starting"); + + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + + memset(&_report, 0, sizeof(_report)); + + /* open the serial port */ + _serial_fd = ::open("/dev/ttyS3", O_RDWR); + + uint8_t buf[256]; + int baud_i = 0; + uint64_t time_before_configuration; + + if (_serial_fd < 0) { + log("failed to open serial port: %d", errno); + goto out; + } + + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + debug("ready"); + + /* lock against the ioctl handler */ + lock(); + + + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + + time_before_configuration = hrt_absolute_time(); + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + if (_mode_changed) { + if (_Helper != nullptr) { + delete(_Helper); + _Helper = nullptr; // XXX is this needed? + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(); + break; + case GPS_DRIVER_MODE_MTK19: + //_Helper = new MTK19(); + break; + case GPS_DRIVER_MODE_MTK16: + //_Helper = new MTK16(); + break; + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); + break; + default: + break; + } + _mode_changed = false; + } + + if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { + baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + time_before_configuration = hrt_absolute_time(); + } + + + int poll_timeout; + if (_config_needed) { + poll_timeout = 50; + } else { + poll_timeout = 250; + } + /* sleep waiting for data, but no more than 1000ms */ + unlock(); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); + lock(); + + + + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + } else if (ret == 0) { + config(); + if (_config_needed == false) { + _config_needed = true; + debug("Lost GPS module"); + } + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + int count; + + /* + * 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(_serial_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ + int j; + for (j = 0; j < count; j++) { + _messages_received += _Helper->parse(buf[j], &_report); + } + if (_messages_received > 0) { + if (_config_needed == true) { + _config_needed = false; + debug("Found GPS module"); + } + + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + _messages_received = 0; + } + } + } + } +out: + debug("exiting"); + + ::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +GPS::set_baudrate(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; + + 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(_serial_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 (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + return -1; + } + + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } +} + +void +GPS::cmd_reset() +{ + _healthy = false; +} + +void +GPS::print_info() +{ + +} + +/** + * Local functions in support of the shell command. + */ +namespace gps +{ + +GPS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new GPS; + + 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) { + printf("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"); +} + +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"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +int +gps_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + gps::start(); + + 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(); + + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); +} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h new file mode 100644 index 0000000000..baacf7cb47 --- /dev/null +++ b/apps/drivers/gps/gps_helper.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 GPS_HELPER_H + +class GPS_Helper +{ +public: + virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; + virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; +}; + +#endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp new file mode 100644 index 0000000000..981b023e55 --- /dev/null +++ b/apps/drivers/gps/ubx.cpp @@ -0,0 +1,818 @@ +/**************************************************************************** + * + * 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 U-Blox protocol implementation */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" + + +UBX::UBX() : +_waited(0), +_config_state(UBX_CONFIG_STATE_PRT), +_new_nav_posllh(false), +_new_nav_timeutc(false), +_new_nav_dop(false), +_new_nav_sol(false), +_new_nav_velned(false) +{ + decodeInit(); + _waiting_for_ack = false; +} + +UBX::~UBX() +{ +} + +void +UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) +{ + /* make sure the buffer to write the message is long enough */ + assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); + + /* try again after 10 times */ + if (_waited > 10) { + _waiting_for_ack = false; + } + + if (!_waiting_for_ack) { + _waiting_for_ack = true; + _waited = 0; + if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { + config_needed = false; + length = 0; + _config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */ + _waiting_for_ack = false; + return; + } else if (_config_state == UBX_CONFIG_STATE_PRT) { + /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + 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; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { + +// printf("Send change of baudrate now\n"); + + /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + 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; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + /* detection when the baudrate has been changed in the first step */ + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + /* set a flag and exit */ + baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; + baudrate_changed = true; + _config_state = UBX_CONFIG_STATE_RATE; + _waiting_for_ack = false; + return; + } + + + + } else if (_config_state == UBX_CONFIG_STATE_RATE) { + + /* 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)); + + 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; + + addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); + length = sizeof(cfg_rate_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_NAV5) { + /* send a NAV5 message to set the options for the internal estimator */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + + 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; + + addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); + length = sizeof(cfg_nav5_packet)+2; + + } else { + /* catch the remaining config states here */ + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + + switch (_config_state) { + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + cfg_msg_packet.rate[1] = 5; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; +// break; + default: + break; + } + + addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); + length = sizeof(cfg_msg_packet)+2; + } + } else { + _waited++; + } +} + +int +UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +{ + int ret = 0; + //printf("Received char: %c\n", 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 */ + decodeInit(); + } + break; + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + addByteToChecksum(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 + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_CLASS: + addByteToChecksum(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 + decodeInit(); + 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 +// decodeInit(); +// 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 + decodeInit(); + 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 + decodeInit(); + break; + } + break; + default: //should not happen because we set the class + printf("UBX Error, we set a class that we don't know\n"); + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_MESSAGEID: + addByteToChecksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + case UBX_DECODE_GOT_LENGTH1: + addByteToChecksum(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) + addByteToChecksum(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 + 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"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + gps_position->lat = packet->lat; + gps_position->lon = packet->lon; + gps_position->alt = packet->height_msl; + + gps_position->counter_pos_valid++; + gps_position->counter++; + + _new_nav_posllh = true; + + } else { + printf("[gps] NAV_POSLLH: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->fix_type = packet->gpsFix; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + gps_position->s_variance = packet->sAcc; + gps_position->p_variance = packet->pAcc; + + _new_nav_sol = true; + + } else { + printf("[gps] NAV_SOL: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + 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; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->eph = packet->hDOP; + gps_position->epv = packet->vDOP; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_dop = true; + + } else { + printf("[gps] NAV_DOP: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + //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 = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_timeutc = true; + + } else { + printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + //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); + gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; + + //Check if checksum is valid and then store the gps information + if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { + //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; + } + + /* set flag if any sat info is available */ + if (!packet_part1->numCh > 0) { + gps_position->satellite_info_available = 1; + + } else { + gps_position->satellite_info_available = 0; + } + + gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report + + } else { + printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->vel = (uint16_t)packet->speed; + gps_position->vel_n = packet->velN / 100.0f; + gps_position->vel_e = packet->velE / 100.0f; + gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_ned_valid = true; + gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + + _new_nav_velned = true; + + } else { + printf("[gps] NAV_VELNED: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + 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; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { +// +// gps_position->satellites_visible = packet->numVis; +// gps_position->timestamp = hrt_absolute_time(); +// gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// ret = 1; +// +// } else { +// printf("[gps] RXM_SVSI: checksum invalid\n"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// return ret; +// +// break; +// } + + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + _waiting_for_ack = false; + + switch (_config_state) { + case UBX_CONFIG_STATE_PRT: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; + break; + case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_RATE; + break; + case UBX_CONFIG_STATE_RATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) + _config_state = UBX_CONFIG_STATE_NAV5; + break; + case UBX_CONFIG_STATE_NAV5: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) + _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_CONFIGURED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_CONFIGURED; +// break; + default: + break; + } + } else { + printf("[gps] ACK_ACK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + printf("[gps] the ubx gps returned: not acknowledged\n"); + ret = 1; + + } else { + printf("[gps] ACK_NAK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + default: //we don't know the message + printf("Unknown message received: %d-%d\n",_message_class,_message_id); + decodeInit(); + + break; + } + } // end if _rx_count high enough + if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + printf("Buffer full"); + decodeInit(); + } + break; + default: + break; + } + + /* return 1 when all needed messages have arrived */ + if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + + gps_position->timestamp = hrt_absolute_time(); + ret = 1; + _new_nav_posllh = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + } + + return ret; +} + +void +UBX::decodeInit(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::addByteToChecksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} + +void +UBX::addChecksumToMessage(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; + } + message[length-2] = ck_a; + message[length-1] = ck_b; +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h new file mode 100644 index 0000000000..dff25a518f --- /dev/null +++ b/apps/drivers/gps/ubx.h @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * 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_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ + +#define UBX_SYNC1 0xB5 +#define UBX_SYNC2 0x62 + +//UBX Protocol definitions (this is the subset of the messages that are parsed) +#define UBX_CLASS_NAV 0x01 +//#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#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 /**< port 1 */ +#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 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ + + +// ************ +/** 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(); + ~UBX(); + + void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + int parse(uint8_t, struct vehicle_gps_position_s*); + +private: + void decodeInit(void); + void addByteToChecksum(uint8_t); + void addChecksumToMessage(uint8_t*, const unsigned); + unsigned _waited; + bool _waiting_for_ack; + ubx_config_state_t _config_state; + 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; + bool _new_nav_posllh; + bool _new_nav_timeutc; + bool _new_nav_dop; + bool _new_nav_sol; + bool _new_nav_velned; +}; + +#endif /* UBX_H_ */