• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/AP_GPS/AP_GPS_MTK16.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 //  DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
00004 //      Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
00005 //
00006 //      This library is free software; you can redistribute it and / or
00007 //      modify it under the terms of the GNU Lesser General Public
00008 //      License as published by the Free Software Foundation; either
00009 //      version 2.1 of the License, or (at your option) any later version.
00010 //
00011 //      GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
00012 //
00013 
00014 #include "AP_GPS_MTK16.h"
00015 #include <stdint.h>
00016 
00017 // Constructors ////////////////////////////////////////////////////////////////
00018 AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
00019 {
00020 }
00021 
00022 // Public Methods //////////////////////////////////////////////////////////////
00023 void 
00024 AP_GPS_MTK16::init(void)
00025 {       
00026         _port->flush();
00027 
00028         // initialize serial port for binary protocol use
00029         // XXX should assume binary, let GPS_AUTO handle dynamic config?
00030         _port->print(MTK_SET_BINARY);
00031 
00032         // set 4Hz update rate
00033         _port->print(MTK_OUTPUT_4HZ);
00034 }
00035 
00036 // Process bytes available from the stream
00037 //
00038 // The stream is assumed to contain only our custom message.  If it
00039 // contains other messages, and those messages contain the preamble bytes,
00040 // it is possible for this code to become de-synchronised.  Without
00041 // buffering the entire message and re-processing it from the top,
00042 // this is unavoidable.
00043 //
00044 // The lack of a standard header length field makes it impossible to skip
00045 // unrecognised messages.
00046 //
00047 bool
00048 AP_GPS_MTK16::read(void)
00049 {
00050         uint8_t data;
00051         int     numc;
00052         bool    parsed = false;
00053 
00054         numc = _port->available();
00055         for (int i = 0; i < numc; i++) {        // Process bytes received
00056 
00057                 // read the next byte
00058                 data = _port->read();
00059 
00060 restart:                
00061                 switch(_step){
00062 
00063                         // Message preamble, class, ID detection
00064                         //
00065                         // If we fail to match any of the expected bytes, we
00066                         // reset the state machine and re-consider the failed
00067                         // byte as the first byte of the preamble.  This 
00068                         // improves our chances of recovering from a mismatch
00069                         // and makes it less likely that we will be fooled by
00070                         // the preamble appearing as data in some other message.
00071                         //
00072                 case 0:
00073                         if(PREAMBLE1 == data)
00074                                 _step++;
00075                         break;
00076                 case 1:
00077                         if (PREAMBLE2 == data) {
00078                                 _step++;
00079                                 break;
00080                         }
00081                         _step = 0;
00082                         goto restart;
00083                 case 2:
00084                         if (sizeof(_buffer) == data) {
00085                                 _step++;
00086                                 _ck_b = _ck_a = data;                           // reset the checksum accumulators
00087                                 _payload_counter = 0;
00088                         } else {
00089                                 _step = 0;                                                      // reset and wait for a message of the right class
00090                                 goto restart;
00091                         }
00092                         break;
00093 
00094                         // Receive message data
00095                         //
00096                 case 3:
00097                         _buffer.bytes[_payload_counter++] = data;
00098                         _ck_b += (_ck_a += data);
00099                         if (_payload_counter == sizeof(_buffer))
00100                                 _step++;
00101                         break;
00102 
00103                         // Checksum and message processing
00104                         //
00105                 case 4:
00106                         _step++;
00107                         if (_ck_a != data) {
00108                                 _step = 0;
00109                         }
00110                         break;
00111                 case 5:
00112                         _step = 0;
00113                         if (_ck_b != data) {
00114                                 break;
00115                         }
00116 
00117                         fix                             = _buffer.msg.fix_type == FIX_3D;
00118                         latitude                = _buffer.msg.latitude  * 10;   // XXX doc says *10e7 but device says otherwise
00119                         longitude               = _buffer.msg.longitude * 10;   // XXX doc says *10e7 but device says otherwise
00120                         altitude                = _buffer.msg.altitude;
00121                         ground_speed    = _buffer.msg.ground_speed;
00122                         ground_course   = _buffer.msg.ground_course;
00123                         num_sats                = _buffer.msg.satellites;
00124                         hdop                    = _buffer.msg.hdop;
00125                         
00126                         // XXX docs say this is UTC, but our clients expect msToW
00127                         time                    = _swapl(&_buffer.msg.utc_time);
00128 
00129                         parsed = true;
00130                 }
00131         } 
00132         return parsed;
00133 }

Generated for ArduPilot Libraries by doxygen