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

/home/jgoppert/Projects/ap/libraries/AP_GPS/AP_GPS_MTK.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_MTK.h"
00015 #include <stdint.h>
00016 
00017 // Constructors ////////////////////////////////////////////////////////////////
00018 AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
00019 {
00020 }
00021 
00022 // Public Methods //////////////////////////////////////////////////////////////
00023 void 
00024 AP_GPS_MTK::init(void)
00025 {       
00026         _port->flush();
00027         // initialize serial port for binary protocol use
00028         // XXX should assume binary, let GPS_AUTO handle dynamic config?
00029         _port->print(MTK_SET_BINARY);
00030 
00031         // set 4Hz update rate
00032         _port->print(MTK_OUTPUT_4HZ);
00033 }
00034 
00035 // Process bytes available from the stream
00036 //
00037 // The stream is assumed to contain only our custom message.  If it
00038 // contains other messages, and those messages contain the preamble bytes,
00039 // it is possible for this code to become de-synchronised.  Without
00040 // buffering the entire message and re-processing it from the top,
00041 // this is unavoidable.
00042 //
00043 // The lack of a standard header length field makes it impossible to skip
00044 // unrecognised messages.
00045 //
00046 bool
00047 AP_GPS_MTK::read(void)
00048 {
00049         uint8_t data;
00050         int             numc;
00051         bool    parsed = false;
00052 
00053         numc = _port->available();
00054         for (int i = 0; i < numc; i++){ // Process bytes received
00055 
00056                 // read the next byte
00057                 data = _port->read();
00058 
00059 restart:                
00060                 switch(_step){
00061 
00062                         // Message preamble, class, ID detection
00063                         //
00064                         // If we fail to match any of the expected bytes, we
00065                         // reset the state machine and re-consider the failed
00066                         // byte as the first byte of the preamble.  This 
00067                         // improves our chances of recovering from a mismatch
00068                         // and makes it less likely that we will be fooled by
00069                         // the preamble appearing as data in some other message.
00070                         //
00071                 case 0:
00072                         if(PREAMBLE1 == data)
00073                                 _step++;
00074                         break;
00075                 case 1:
00076                         if (PREAMBLE2 == data) {
00077                                 _step++;
00078                                 break;
00079                         }
00080                         _step = 0;
00081                         goto restart;
00082                 case 2:
00083                         if (MESSAGE_CLASS == data) {
00084                                 _step++;
00085                                 _ck_b = _ck_a = data;                                   // reset the checksum accumulators
00086                         } else {
00087                                 _step = 0;                                                      // reset and wait for a message of the right class
00088                                 goto restart;
00089                         }
00090                         break;
00091                 case 3:
00092                         if (MESSAGE_ID == data) {
00093                                 _step++;
00094                                 _ck_b += (_ck_a += data);
00095                                 _payload_counter = 0;
00096                         } else {
00097                                 _step = 0;
00098                                 goto restart;
00099                         }
00100                         break;
00101 
00102                         // Receive message data
00103                         //
00104                 case 4:
00105                         _buffer.bytes[_payload_counter++] = data;
00106                         _ck_b += (_ck_a += data);
00107                         if (_payload_counter == sizeof(_buffer))
00108                                 _step++;
00109                         break;
00110 
00111                         // Checksum and message processing
00112                         //
00113                 case 5:
00114                         _step++;
00115                         if (_ck_a != data) {
00116                                 _error("GPS_MTK: checksum error\n");
00117                                 _step = 0;
00118                         }
00119                         break;
00120                 case 6:
00121                         _step = 0;
00122                         if (_ck_b != data) {
00123                                 _error("GPS_MTK: checksum error\n");
00124                                 break;
00125                         }
00126 
00127                         fix                             = (_buffer.msg.fix_type == FIX_3D);
00128                         latitude                = _swapl(&_buffer.msg.latitude)  * 10;
00129                         longitude               = _swapl(&_buffer.msg.longitude) * 10;
00130                         altitude                = _swapl(&_buffer.msg.altitude);
00131                         ground_speed    = _swapl(&_buffer.msg.ground_speed);
00132                         ground_course   = _swapl(&_buffer.msg.ground_course) / 10000;
00133                         num_sats                = _buffer.msg.satellites;
00134                         
00135                         // XXX docs say this is UTC, but our clients expect msToW
00136                         time                    = _swapl(&_buffer.msg.utc_time);
00137 
00138                         parsed = true;
00139                 }
00140         }
00141         return parsed;
00142 }

Generated for ArduPilot Libraries by doxygen