diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-07 14:48:00 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-07 14:48:00 -0800 |
commit | a88b9f4eefe8315cb692779dd300400d8052eb44 (patch) | |
tree | 211753642a2618aafe1c31b732b17c5c5dc1b198 /apps | |
parent | d36eb8a3fcce358409a7205bbd75576a447ac7b4 (diff) | |
download | px4-firmware-a88b9f4eefe8315cb692779dd300400d8052eb44.tar.gz px4-firmware-a88b9f4eefe8315cb692779dd300400d8052eb44.tar.bz2 px4-firmware-a88b9f4eefe8315cb692779dd300400d8052eb44.zip |
Restructered the parsing/configuring, MTK working
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/gps/gps.cpp | 243 | ||||
-rw-r--r-- | apps/drivers/gps/gps_helper.cpp | 90 | ||||
-rw-r--r-- | apps/drivers/gps/gps_helper.h | 11 | ||||
-rw-r--r-- | apps/drivers/gps/mtk.cpp | 238 | ||||
-rw-r--r-- | apps/drivers/gps/mtk.h | 35 |
5 files changed, 314 insertions, 303 deletions
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index c749e8b7f..76b5f4533 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -53,7 +53,6 @@ #include <stdio.h> #include <math.h> #include <unistd.h> -#include <termios.h> #include <fcntl.h> #include <nuttx/arch.h> @@ -71,7 +70,7 @@ #include <uORB/topics/vehicle_gps_position.h> -#include "ubx.h" +//#include "ubx.h" #include "mtk.h" #define SEND_BUFFER_LENGTH 100 @@ -113,10 +112,8 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - const unsigned _baudrates_to_try[NUMBER_OF_TRIES]; ///< try different baudrates that GPS could be set to - const gps_driver_mode_t _modes_to_try[NUMBER_OF_TRIES]; ///< try different modes volatile int _task; //< worker task - bool _config_needed; ///< flag to signal that configuration of GPS is needed + 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 @@ -171,11 +168,9 @@ GPS *g_dev; GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), - _baudrates_to_try({9600, 38400, 57600, 115200, 38400}), - _modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}), - _config_needed(true), - _baudrate_changed(false), + _healthy(false), _mode_changed(false), + _mode(GPS_DRIVER_MODE_MTK), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -251,20 +246,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) } void -GPS::config() -{ - _Helper->configure(_serial_fd, _baudrate_changed, _baudrate); - - /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed - * from 9600 to 38400 - */ - if (_baudrate_changed) { - set_baudrate(_baudrate); - _baudrate_changed = false; - } -} - -void GPS::task_main_trampoline(void *arg) { g_dev->task_main(); @@ -276,10 +257,7 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters - - /* buffer to read from the serial port */ - uint8_t buf[32]; + _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); @@ -288,151 +266,65 @@ GPS::task_main() _exit(1); } - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - /* lock against the ioctl handler */ - lock(); - - unsigned try_i = 0; - _baudrate = _baudrates_to_try[try_i]; - _mode = _modes_to_try[try_i]; - _mode_changed = true; - set_baudrate(_baudrate); - - uint64_t time_before_configuration = hrt_absolute_time(); - uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - bool pos_updated; - bool config_needed_res; - /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - /* If a configuration does not finish in the config timeout, change the baudrate */ - if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { - /* loop through possible modes/baudrates */ - try_i = (try_i + 1) % NUMBER_OF_TRIES; - _baudrate = _baudrates_to_try[try_i]; - set_baudrate(_baudrate); - if (_mode != _modes_to_try[try_i]) { - _mode_changed = true; - } - _mode = _modes_to_try[try_i]; - - if (_Helper != nullptr) { - _Helper->reset(); - } - time_before_configuration = hrt_absolute_time(); + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; } - if (_mode_changed) { - 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(); - break; + switch (_mode) { +// case GPS_DRIVER_MODE_UBX: +// _Helper = new UBX(); +// break; case GPS_DRIVER_MODE_MTK: + printf("try new mtk\n"); _Helper = new MTK(); break; case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); + //_Helper = new NMEA(); //TODO: add NMEA break; default: break; - } - _mode_changed = false; } - - - - /* during configuration, the timeout should be small, so that we can send config messages in between parsing, - * but during normal operation, it should never timeout because updates should arrive with 5Hz */ - int poll_timeout; - if (_config_needed) { - poll_timeout = 50; - } else { - poll_timeout = 400; - } - /* sleep waiting for data, but no more than the poll timeout */ + // XXX unlock/lock makes sense? unlock(); - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); - lock(); + if (_Helper->configure(_serial_fd, _baudrate) == 0) { + + while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + /* 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++; - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - } else if (ret == 0) { - /* no response from the GPS */ - if (_config_needed == false) { - _config_needed = true; - warnx("GPS module timeout"); - _Helper->reset(); - } - config(); - } 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++) { - pos_updated = false; - config_needed_res = _config_needed; - _Helper->parse(buf[j], &_report, config_needed_res, pos_updated); - - if (pos_updated) { - /* 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 > 5000000) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } - - } - if (config_needed_res == true && _config_needed == false) { - /* the parser told us that an error happened and reconfiguration is needed */ - _config_needed = true; - warnx("GPS module lost"); - _Helper->reset(); - config(); - } else if (config_needed_res == true && _config_needed == true) { - /* we are still configuring */ - config(); - } else if (config_needed_res == false && _config_needed == true) { - /* the parser is happy, stop configuring */ - warnx("GPS module found"); - _config_needed = false; - } + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _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(); } debug("exiting"); @@ -443,59 +335,12 @@ GPS::task_main() _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 ((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(_serial_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; -} void GPS::cmd_reset() { - _config_needed = true; + //XXX add reset? } void @@ -514,7 +359,7 @@ GPS::print_info() default: break; } - warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); + 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)); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp new file mode 100644 index 000000000..2caa82af1 --- /dev/null +++ b/apps/drivers/gps/gps_helper.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include <termios.h> +#include <errno.h> +#include <systemlib/err.h> +#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; + + 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/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 576692c2a..537ca819c 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -33,17 +33,20 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/* @file gps_helper.h */ #ifndef GPS_HELPER_H #define GPS_HELPER_H +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_gps_position.h> + class GPS_Helper { public: - virtual void reset(void) = 0; - virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0; - virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; + virtual int configure(const int &fd, unsigned &baud) = 0; + virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + int set_baudrate(const int &fd, unsigned baud); }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 026b0660b..4ccbbfbe4 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -37,85 +37,143 @@ #include <unistd.h> #include <stdio.h> +#include <poll.h> #include <math.h> #include <string.h> #include <assert.h> #include <systemlib/err.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_gps_position.h> #include <drivers/drv_hrt.h> #include "mtk.h" MTK::MTK() : -_config_sent(false), _mtk_revision(0) { - decodeInit(); + decode_init(); } MTK::~MTK() { } -void -MTK::reset() +int +MTK::configure(const int &fd, unsigned &baudrate) { + /* set baudrate first */ + if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) + return -1; -} + baudrate = MTK_BAUDRATE; -void -MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - if (_config_sent == false) { + /* 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_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) - warnx("mtk: config write failed"); - 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(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + 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"); - usleep(10000); + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_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"); - usleep(10000); + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + warnx("mtk: config write failed"); + return -1; + } - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) - warnx("mtk: config write failed"); + return 0; +} - _config_sent = true; - } +int +MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + gps_mtk_packet_t packet; + + 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, gps_position); + return 1; + } + j++; + } + /* everything is read */ + j = count = 0; + } - return; + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + + 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::decodeInit(void) +MTK::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; _rx_count = 0; _decode_state = MTK_DECODE_UNINIT; } - -void -MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +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; - config_needed = false; _mtk_revision = 16; } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; - config_needed = false; _mtk_revision = 19; } @@ -125,78 +183,82 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } else { // Second start symbol was wrong, reset state machine - decodeInit(); + decode_init(); } } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum if (_rx_count < 33) - addByteToChecksum(b); + add_byte_to_checksum(b); // Fill packet buffer - _rx_buffer[_rx_count] = b; + ((uint8_t*)(&packet))[_rx_count] = b; _rx_count++; - /* Packet size minus checksum */ - if (_rx_count >= 35) { - type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;; - - /* Check if checksum is valid */ - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - 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->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(); - - pos_updated = true; - - + /* 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, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b); + warnx("MTK Checksum invalid"); + ret = -1; } - // Reset state machine to decode next packet - decodeInit(); + decode_init(); } } + return ret; +} + +void +MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +{ + 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::addByteToChecksum(uint8_t b) +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/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index e43637195..39063beab 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -50,6 +50,9 @@ #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, @@ -64,16 +67,16 @@ typedef struct { 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; ///< FIXME SPEC UNCLEAR - int32_t heading; - uint8_t satellites; - uint8_t fix_type; + 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; + uint16_t hdop; ///< horizontal dilution of position (without unit) uint8_t ck_a; uint8_t ck_b; -} type_gps_mtk_packet; +} gps_mtk_packet_t; #pragma pack(pop) @@ -84,23 +87,31 @@ class MTK : public GPS_Helper public: MTK(); ~MTK(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(const int &fd, struct vehicle_gps_position_s &gps_position); + int configure(const int &fd, 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, struct vehicle_gps_position_s &gps_position); + + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); mtk_decode_state_t _decode_state; - bool _config_sent; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; |