From 0d54661ce90dfe2440daea2639a9853520d8366c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 20:04:49 -0800 Subject: Added MTK 1.6, works after some seconds, work in progress --- apps/drivers/gps/gps.cpp | 43 +++++++---- apps/drivers/gps/mtk.cpp | 184 +++++++++++++++++++++++++++++++++++++++++++++++ apps/drivers/gps/mtk.h | 108 ++++++++++++++++++++++++++++ 3 files changed, 320 insertions(+), 15 deletions(-) create mode 100644 apps/drivers/gps/mtk.cpp create mode 100644 apps/drivers/gps/mtk.h (limited to 'apps/drivers/gps') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 3e1aca810..c749e8b7f 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -77,7 +77,7 @@ #define SEND_BUFFER_LENGTH 100 #define TIMEOUT 1000000 //1s -#define NUMBER_OF_BAUDRATES 4 +#define NUMBER_OF_TRIES 5 #define CONFIG_TIMEOUT 2000000 /* oddly, ERROR is not defined for c++ */ @@ -113,7 +113,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_BAUDRATES]; ///< try different baudrates that GPS could be set to + 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 _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed @@ -170,11 +171,11 @@ 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}), + _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), - _mode_changed(true), - _mode(GPS_DRIVER_MODE_UBX), + _mode_changed(false), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -295,8 +296,10 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - unsigned baud_i = 0; - _baudrate = _baudrates_to_try[baud_i]; + 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(); @@ -310,6 +313,23 @@ GPS::task_main() /* 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 (_mode_changed) { if (_Helper != nullptr) { delete(_Helper); @@ -333,14 +353,7 @@ GPS::task_main() _mode_changed = false; } - /* If a configuration does not finish in the config timeout, change the baudrate */ - 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); - _Helper->reset(); - time_before_configuration = hrt_absolute_time(); - } + /* 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 */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp new file mode 100644 index 000000000..555fb7a51 --- /dev/null +++ b/apps/drivers/gps/mtk.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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 + +#include "mtk.h" + + +MTK::MTK() +{ + decodeInit(); +} + +MTK::~MTK() +{ +} + +void +MTK::reset() +{ + +} + +void +MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) +{ + 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"); + 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"); + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) + warnx("mtk: config write failed"); + + return; +} + +void +MTK::decodeInit(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) +{ + if (_decode_state == MTK_DECODE_UNINIT) { + + if (b == 0xd0) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_A) { + if (b == 0xdd) { + _decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + decodeInit(); + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (_rx_count < 33) + addByteToChecksum(b); + + // Fill packet buffer + _rx_buffer[_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) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + 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; + 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; + + + } 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); + } + + // Reset state machine to decode next packet + decodeInit(); + } + } + return; +} + +void +MTK::addByteToChecksum(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 new file mode 100644 index 000000000..8929eb041 --- /dev/null +++ b/apps/drivers/gps/mtk.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 0xd0 +#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" + +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; ///< FIXME SPEC UNCLEAR + int32_t heading; + uint8_t satellites; + uint8_t fix_type; + uint32_t date; + uint32_t utc_time; + uint16_t hdop; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_mtk_packet; + +#pragma pack(pop) + +#define MTK_RECV_BUFFER_SIZE 40 + +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); + +private: + /** + * Reset the parse state machine for a fresh start + */ + void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void addByteToChecksum(uint8_t); + + mtk_decode_state_t _decode_state; + uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; +}; + +#endif /* MTK_H_ */ -- cgit v1.2.3