/**************************************************************************** * * 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() : _config_sent(false), _mtk_revision(0) { decodeInit(); } MTK::~MTK() { } void MTK::reset() { } void MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) { if (_config_sent == false) { 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"); _config_sent = true; } 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 == 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; } } else if (_decode_state == MTK_DECODE_GOT_CK_A) { if (b == MTK_SYNC2) { _decode_state = MTK_DECODE_GOT_CK_B; } else { // Second start symbol was wrong, reset state machine 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) { 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; } 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; }