From 727f8ff029aec8104747ccdee596de7ee8415604 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Feb 2013 22:03:53 +1100 Subject: [PATCH] AP_GPS: removed the old MTK16 driver the MTK19 driver supports both the 1.6 and 1.9 protocol --- libraries/AP_GPS/AP_GPS_MTK16.cpp | 230 ------------------------------ libraries/AP_GPS/AP_GPS_MTK16.h | 72 ---------- libraries/AP_GPS/AP_GPS_MTK19.cpp | 2 + 3 files changed, 2 insertions(+), 302 deletions(-) delete mode 100644 libraries/AP_GPS/AP_GPS_MTK16.cpp delete mode 100644 libraries/AP_GPS/AP_GPS_MTK16.h diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp deleted file mode 100644 index 4d40d0012c..0000000000 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, Craig Elder, DIYDrones.com -// -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. -// -// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.6, v1.7, v1.8" -// -#include -#include "AP_GPS_MTK16.h" -#include - -extern const AP_HAL::HAL& hal; - -// Public Methods ////////////////////////////////////////////////////////////// -void -AP_GPS_MTK16::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) -{ - _port = s; - _port->flush(); - _step = 0; - - // initialize serial port for binary protocol use - // XXX should assume binary, let GPS_AUTO handle dynamic config? - _port->print(MTK_SET_BINARY); - - // set 5Hz update rate - _port->print(MTK_OUTPUT_5HZ); - - // set SBAS on - _port->print(SBAS_ON); - - // set WAAS on - _port->print(WAAS_ON); - - // Set Nav Threshold to 0 m/s - _port->print(MTK_NAVTHRES_OFF); - - // set initial epoch code - _epoch = TIME_OF_DAY; - _time_offset = 0; - _offset_calculated = false; - idleTimeout = 1200; -} - -// Process bytes available from the stream -// -// The stream is assumed to contain only our custom message. If it -// contains other messages, and those messages contain the preamble bytes, -// it is possible for this code to become de-synchronised. Without -// buffering the entire message and re-processing it from the top, -// this is unavoidable. -// -// The lack of a standard header length field makes it impossible to skip -// unrecognised messages. -// -bool -AP_GPS_MTK16::read(void) -{ - uint8_t data; - int16_t numc; - bool parsed = false; - - numc = _port->available(); - for (int16_t i = 0; i < numc; i++) { // Process bytes received - - // read the next byte - data = _port->read(); - -restart: - switch(_step) { - - // Message preamble, class, ID detection - // - // If we fail to match any of the expected bytes, we - // reset the state machine and re-consider the failed - // byte as the first byte of the preamble. This - // improves our chances of recovering from a mismatch - // and makes it less likely that we will be fooled by - // the preamble appearing as data in some other message. - // - case 0: - if(PREAMBLE1 == data) - _step++; - break; - case 1: - if (PREAMBLE2 == data) { - _step++; - break; - } - _step = 0; - goto restart; - case 2: - if (sizeof(_buffer) == data) { - _step++; - _ck_b = _ck_a = data; // reset the checksum accumulators - _payload_counter = 0; - } else { - _step = 0; // reset and wait for a message of the right class - goto restart; - } - break; - - // Receive message data - // - case 3: - _buffer.bytes[_payload_counter++] = data; - _ck_b += (_ck_a += data); - if (_payload_counter == sizeof(_buffer)) - _step++; - break; - - // Checksum and message processing - // - case 4: - _step++; - if (_ck_a != data) { - _step = 0; - } - break; - case 5: - _step = 0; - if (_ck_b != data) { - break; - } - - fix = ((_buffer.msg.fix_type == FIX_3D) || - (_buffer.msg.fix_type == FIX_3D_SBAS)); - latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise - longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise - altitude = _buffer.msg.altitude; - ground_speed = _buffer.msg.ground_speed; - ground_course = _buffer.msg.ground_course; - num_sats = _buffer.msg.satellites; - hdop = _buffer.msg.hdop; - date = _buffer.msg.utc_date; - - // time from gps is UTC, but convert here to msToD - int32_t time_utc = _buffer.msg.utc_time; - int32_t temp = (time_utc/10000000); - time_utc -= temp*10000000; - time = temp * 3600000; - temp = (time_utc/100000); - time_utc -= temp*100000; - time += temp * 60000 + time_utc; - - parsed = true; - -#ifdef FAKE_GPS_LOCK_TIME - if (millis() > FAKE_GPS_LOCK_TIME*1000) { - fix = true; - latitude = -35000000UL; - longitude = 149000000UL; - altitude = 584; - } -#endif - - /* Waiting on clarification of MAVLink protocol! - * if(!_offset_calculated && parsed) { - * int32_t tempd1 = date; - * int32_t day = tempd1/10000; - * tempd1 -= day * 10000; - * int32_t month = tempd1/100; - * int32_t year = tempd1 - month * 100; - * _time_offset = _calc_epoch_offset(day, month, year); - * _epoch = UNIX_EPOCH; - * _offset_calculated = TRUE; - * } - */ - - } - } - return parsed; -} - - -/* - detect a MTK16 GPS - */ -bool -AP_GPS_MTK16::_detect(uint8_t data) -{ - static uint8_t payload_counter; - static uint8_t step; - static uint8_t ck_a, ck_b; - - switch (step) { - case 1: - if (PREAMBLE2 == data) { - step++; - break; - } - step = 0; - case 0: - ck_b = ck_a = payload_counter = 0; - if (PREAMBLE1 == data) - step++; - break; - case 2: - if (data == sizeof(struct diyd_mtk_msg)) { - step++; - ck_b = ck_a = data; - } else { - step = 0; - } - break; - case 3: - ck_b += (ck_a += data); - if (++payload_counter == sizeof(struct diyd_mtk_msg)) - step++; - break; - case 4: - step++; - if (ck_a != data) { - step = 0; - } - break; - case 5: - step = 0; - if (ck_b == data) { - return true; - } - break; - } - return false; -} diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h deleted file mode 100644 index 3b5e25c06f..0000000000 --- a/libraries/AP_GPS/AP_GPS_MTK16.h +++ /dev/null @@ -1,72 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -// -// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. -// Code by Michael Smith, Jordi Munoz and Jose Julio, Craig Elder, DIYDrones.com -// -// This library is free software; you can redistribute it and / or -// modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation; either -// version 2.1 of the License, or (at your option) any later version. -// -// GPS configuration : Custom protocol per "Customize Function Specification, 3D Robotics, v1.6, v1.7, v1.8" -// -#ifndef __AP_GPS_MTK16_H__ -#define __AP_GPS_MTK16_H__ - -#include -#include "GPS.h" -#include "AP_GPS_MTK_Common.h" - -class AP_GPS_MTK16 : public GPS { -public: - virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); - virtual bool read(void); - static bool _detect(uint8_t ); - -private: - #pragma pack(push,1) - struct diyd_mtk_msg { - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_date; - uint32_t utc_time; - uint16_t hdop; - }; - #pragma pack(pop) - enum diyd_mtk_fix_type { - FIX_NONE = 1, - FIX_2D = 2, - FIX_3D = 3, - FIX_3D_SBAS = 7 - }; - - enum diyd_mtk_protocol_bytes { - PREAMBLE1 = 0xd0, - PREAMBLE2 = 0xdd, - }; - - // Packet checksum accumulators - uint8_t _ck_a; - uint8_t _ck_b; - - // State machine state - uint8_t _step; - uint8_t _payload_counter; - - // Time from UNIX Epoch offset - long _time_offset; - bool _offset_calculated; - - // Receive buffer - union { - diyd_mtk_msg msg; - uint8_t bytes[]; - } _buffer; -}; - -#endif // __AP_GPS_MTK16_H__ diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 48e5135d3e..34ba3a566b 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -10,6 +10,8 @@ // // GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.6, v1.7, v1.8, v1.9" // +// Note that this driver supports both the 1.6 and 1.9 protocol varients +// #include #include "AP_GPS_MTK19.h"