Added support for Mediatek firmware Rev 1.9

libraries/AP_GPS/AP_GPS.h

@@ -8,7 +8,7 @@
 #include "AP_GPS_406.h"
 #include "AP_GPS_UBLOX.h"
 #include "AP_GPS_MTK.h"
-#include "AP_GPS_MTK16.h"
+#include "AP_GPS_MTK19.h"
 #include "AP_GPS_None.h"
 #include "AP_GPS_Auto.h"
 #include "AP_GPS_HIL.h"

libraries/AP_GPS/AP_GPS_Auto.cpp

@@ -89,9 +89,9 @@ AP_GPS_Auto::_detect(void)
 			Serial.print_P(PSTR(" ublox "));
 			return new AP_GPS_UBLOX(_port);
 		}
-		if (AP_GPS_MTK16::_detect(data)) {
+		if (AP_GPS_MTK19::_detect(data)) {
-			Serial.print_P(PSTR(" MTK16 "));
+			Serial.print_P(PSTR(" MTK19 "));
-			return new AP_GPS_MTK16(_port);
+			return new AP_GPS_MTK19(_port);
 		}
 		if (AP_GPS_MTK::_detect(data)) {
 			Serial.print_P(PSTR(" MTK "));

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -138,8 +138,7 @@ restart:
                 break;
             }
 
-            fix                         = ((_buffer.msg.fix_type == FIX_3D) ||
+            fix                 = _buffer.msg.fix_type == FIX_3D;
-                                           (_buffer.msg.fix_type == FIX_3D_SBAS));
             latitude            = _swapl(&_buffer.msg.latitude)  * 10;
             longitude           = _swapl(&_buffer.msg.longitude) * 10;
             altitude            = _swapl(&_buffer.msg.altitude);

libraries/AP_GPS/AP_GPS_MTK.h

@@ -41,9 +41,7 @@ private:
     enum diyd_mtk_fix_type {
         FIX_NONE = 1,
         FIX_2D = 2,
-        FIX_3D = 3,
+        FIX_3D = 3
-		FIX_2D_SBAS = 6,
-		FIX_3D_SBAS = 7
     };
 
     enum diyd_mtk_protocol_bytes {

libraries/AP_GPS/AP_GPS_MTK16.cpp

@@ -1,14 +1,14 @@
 // -*- 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, DIYDrones.com
+//	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.1"
+//	GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.6, v1.7, v1.8"
 //
 
 #include <FastSerial.h>

libraries/AP_GPS/AP_GPS_MTK16.h

@@ -1,14 +1,14 @@
 // -*- 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, DIYDrones.com
+//	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"
+//	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

libraries/AP_GPS/AP_GPS_MTK19.cpp

@@ -0,0 +1,273 @@
+// -*- 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 <FastSerial.h>
+#include "AP_GPS_MTK19.h"
+#include <stdint.h>
+#if defined(ARDUINO) && ARDUINO >= 100
+ #include "Arduino.h"
+#else
+ #include <wiring.h>
+#endif
+
+// Constructors ////////////////////////////////////////////////////////////////
+AP_GPS_MTK19::AP_GPS_MTK19(Stream *s) : GPS(s)
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void
+AP_GPS_MTK19::init(enum GPS_Engine_Setting nav_setting)
+{
+    _port->flush();
+
+	// set WAAS on
+    _port->print(WAAS_ON);
+	
+	// set SBAS on
+    _port->print(SBAS_ON);
+	
+	// Set Nav Threshold to 0 m/s
+    _port->print(MTK_NAVTHRES_OFF);
+	
+    // set 5Hz update rate
+    _port->print(MTK_OUTPUT_5HZ);
+	
+    // initialize serial port for binary protocol use
+    // XXX should assume binary, let GPS_AUTO handle dynamic config?
+    _port->print(MTK_SET_BINARY);
+
+    // 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_MTK19::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 (data == PREAMBLE1_V16) {
+				_mtk_type_step1 	= MTK_GPS_REVISION_V16;
+				_step++;
+			}	
+			if (data == PREAMBLE1_V19) {
+				_mtk_type_step1 	= MTK_GPS_REVISION_V19;
+                _step++;
+			}	
+			break;
+        case 1:
+			if ((_mtk_type_step1 == MTK_GPS_REVISION_V16) && (data == PREAMBLE2_V16)){
+                _step++;
+				_mtk_type_step2  	= MTK_GPS_REVISION_V16;
+                break;
+            }
+			if ((_mtk_type_step1 == MTK_GPS_REVISION_V19) && (data == PREAMBLE2_V19)){
+				_step++;
+				_mtk_type_step2  	= MTK_GPS_REVISION_V19;
+				break;
+			}
+			_mtk_type_step1 		= 0;
+			_mtk_type_step2 		= 0;
+			_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;
+			longitude           	= _buffer.msg.longitude;
+			if	(_mtk_type_step2 == MTK_GPS_REVISION_V16){
+				latitude			= _buffer.msg.latitude  * 10;  // V16, V17,V18 doc says *10e7 but device says otherwise
+				longitude			= _buffer.msg.longitude * 10;  // V16, V17,V18 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_MTK19::_detect(uint8_t data)
+{
+	static uint8_t payload_counter;
+	static uint8_t step;
+	static uint8_t mtk_type_step1, mtk_type_step2;
+	static uint8_t ck_a, ck_b;
+
+	switch (step) {
+        case 0:
+			ck_b = ck_a = payload_counter = 0;
+            if (data == PREAMBLE1_V16) {
+				mtk_type_step1 		= MTK_GPS_REVISION_V16;
+				step++;
+			}	
+			if (data == PREAMBLE1_V19) {
+				mtk_type_step1 		= MTK_GPS_REVISION_V19;
+                step++;
+			}	
+            mtk_type_step1 			= 0;
+			break;
+		case 1:
+            if ((mtk_type_step1 == MTK_GPS_REVISION_V16) && (PREAMBLE2_V16 == data)){
+                step++;
+				mtk_type_step2  == MTK_GPS_REVISION_V16;
+                break;
+            }
+			if ((mtk_type_step1 == MTK_GPS_REVISION_V19) && (PREAMBLE2_V19 == data)){
+				step++;
+				mtk_type_step2  == MTK_GPS_REVISION_V19;
+				break;
+			}
+			mtk_type_step1 			= 0;
+			mtk_type_step2 			= 0;
+			step 					= 0;
+        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) {
+				Serial.printf("wrong ck_a\n");
+                step 				= 0;
+            }
+            break;
+        case 5:
+            step 					= 0;
+            if (ck_b == data) {
+				return true;
+            }
+			Serial.printf("wrong ck_b\n");
+			break;
+	}
+    return false;
+}

libraries/AP_GPS/AP_GPS_MTK19.h

@@ -0,0 +1,83 @@
+// -*- 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, v1.9"
+//
+#ifndef AP_GPS_MTK19_h
+#define AP_GPS_MTK19_h
+
+#include "GPS.h"
+#include "AP_GPS_MTK_Common.h"
+
+#define MTK_GPS_REVISION_V16  16
+#define MTK_GPS_REVISION_V19  19
+
+
+class AP_GPS_MTK19 : public GPS {
+public:
+    AP_GPS_MTK19(Stream *s);
+    virtual void        init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
+    virtual bool        read(void);
+    static bool 		_detect(uint8_t );
+
+private:
+// XXX this is being ignored by the compiler #pragma pack(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_2D_SBAS = 6,
+        FIX_3D_SBAS = 7
+    };
+
+    enum diyd_mtk_protocol_bytes {
+	    PREAMBLE1 = 0xd0,
+        PREAMBLE2 = 0xdd,
+	    PREAMBLE1_V16 = 0xd0,
+        PREAMBLE2_V16 = 0xdd,
+        PREAMBLE1_V19 = 0xd1,
+        PREAMBLE2_V19 = 0xdd
+    };
+
+    // Packet checksum accumulators
+    uint8_t         _ck_a;
+    uint8_t         _ck_b;
+
+    // State machine state
+    uint8_t         _step;
+    uint8_t         _payload_counter;
+	uint8_t			_mtk_type_step1;
+	uint8_t 		_mtk_type_step2;
+
+    // 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_MTK19_H

libraries/AP_GPS/AP_GPS_MTK_Common.h

@@ -1,7 +1,7 @@
 // -*- 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, DIYDrones.com
+//	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

libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde

@@ -14,7 +14,7 @@
 FastSerialPort0(Serial);
 FastSerialPort1(Serial1);
 
-AP_GPS_MTK16 gps(&Serial1);
+AP_GPS_MTK19 gps(&Serial1);
 #define T6 1000000
 #define T7 10000000
 
@@ -24,7 +24,6 @@ void setup()
     Serial1.begin(38400);
     stderr = stdout;
     gps.print_errors = true;
-
     Serial.println("GPS MTK library test");
     gps.init();  // GPS Initialization
     delay(1000);