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

AP_GPS_SIRF.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 //  SiRF Binary GPS driver for ArduPilot and ArduPilotMega.
00004 //      Code by Michael Smith.
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 
00012 #include "AP_GPS_SIRF.h"
00013 #include <stdint.h>
00014 
00015 // Initialisation messages
00016 //
00017 // Turn off all messages except for 0x29.
00018 //
00019 // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
00020 //
00021 static uint8_t  init_messages[] = {
00022         0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
00023         0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
00024 };
00025 
00026 // Constructors ////////////////////////////////////////////////////////////////
00027 AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
00028 {
00029 }
00030 
00031 // Public Methods //////////////////////////////////////////////////////////////
00032 void 
00033 AP_GPS_SIRF::init(void)
00034 {       
00035         _port->flush();
00036 
00037         // For modules that default to something other than SiRF binary,
00038         // the module-specific subclass should take care of switching to binary mode
00039         // before calling us.
00040 
00041         // send SiRF binary setup messages
00042         _port->write(init_messages, sizeof(init_messages));
00043 }
00044 
00045 // Process bytes available from the stream
00046 //
00047 // The stream is assumed to contain only messages we recognise.  If it
00048 // contains other messages, and those messages contain the preamble
00049 // bytes, it is possible for this code to fail to synchronise to the
00050 // stream immediately.  Without buffering the entire message and
00051 // re-processing it from the top, this is unavoidable. The parser
00052 // attempts to avoid this when possible.
00053 //
00054 bool
00055 AP_GPS_SIRF::read(void)
00056 {
00057         uint8_t         data;
00058         int             numc;
00059         bool            parsed = false;
00060 
00061         numc = _port->available();
00062         while(numc--) {
00063 
00064                 // read the next byte
00065                 data = _port->read();
00066 
00067                 switch(_step){
00068 
00069                         // Message preamble detection
00070                         //
00071                         // If we fail to match any of the expected bytes, we reset
00072                         // the state machine and re-consider the failed byte as
00073                         // the first byte of the preamble.  This improves our
00074                         // chances of recovering from a mismatch and makes it less
00075                         // likely that we will be fooled by the preamble appearing
00076                         // as data in some other message.
00077                         //
00078                 case 1:
00079                         if (PREAMBLE2 == data) {
00080                                 _step++;
00081                                 break;
00082                         }
00083                         _step = 0;
00084                         // FALLTHROUGH
00085                 case 0:
00086                         if(PREAMBLE1 == data)
00087                                 _step++;
00088                         break;
00089 
00090                         // Message length
00091                         //
00092                         // We always collect the length so that we can avoid being
00093                         // fooled by preamble bytes in messages.
00094                         //
00095                 case 2:
00096                         _step++;
00097                         _payload_length = (uint16_t)data << 8;
00098                         break;
00099                 case 3:
00100                         _step++;
00101                         _payload_length |= data;
00102                         _payload_counter = 0;
00103                         _checksum = 0;
00104                         break;
00105 
00106                         // Message header processing
00107                         //
00108                         // We sniff the message ID to determine whether we are going
00109                         // to gather the message bytes or just discard them.
00110                         //
00111                 case 4:
00112                         _step++;
00113                         _accumulate(data);
00114                         _payload_length--;
00115                         _gather = false;
00116                         switch(data) {
00117                         case MSG_GEONAV:
00118                                 if (_payload_length == sizeof(sirf_geonav)) {
00119                                         _gather = true;
00120                                         _msg_id = data;
00121                                 }
00122                                 break;
00123                         }
00124                         break;
00125 
00126                         // Receive message data
00127                         //
00128                         // Note that we are effectively guaranteed by the protocol
00129                         // that the checksum and postamble cannot be mistaken for
00130                         // the preamble, so if we are discarding bytes in this
00131                         // message when the payload is done we return directly
00132                         // to the preamble detector rather than bothering with
00133                         // the checksum logic.
00134                         //
00135                 case 5:
00136                         if (_gather) {                                          // gather data if requested
00137                                 _accumulate(data);
00138                                 _buffer.bytes[_payload_counter] = data;
00139                                 if (++_payload_counter == _payload_length)
00140                                         _step++;
00141                         } else {
00142                                 if (++_payload_counter == _payload_length)
00143                                         _step = 0;
00144                         }
00145                         break;
00146 
00147                         // Checksum and message processing
00148                         //
00149                 case 6:
00150                         _step++;
00151                         if ((_checksum >> 8) != data) {
00152                                 _error("GPS_SIRF: checksum error\n");
00153                                 _step = 0;
00154                         }
00155                         break;
00156                 case 7:
00157                         _step = 0;
00158                         if ((_checksum & 0xff) != data) {
00159                                 _error("GPS_SIRF: checksum error\n");
00160                                 break;
00161                         }
00162                         if (_gather) {
00163                                 parsed = _parse_gps();                                   // Parse the new GPS packet
00164                         }
00165                 }
00166         } 
00167         return(parsed);
00168 }
00169 
00170 bool
00171 AP_GPS_SIRF::_parse_gps(void)
00172 {
00173         switch(_msg_id) {
00174         case MSG_GEONAV:
00175                 time                    = _swapl(&_buffer.nav.time);
00176                 //fix                           = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
00177                 fix                             = (0 == _buffer.nav.fix_invalid);
00178                 latitude                = _swapl(&_buffer.nav.latitude);
00179                 longitude               = _swapl(&_buffer.nav.longitude);
00180                 altitude                = _swapl(&_buffer.nav.altitude_msl);
00181                 ground_speed    = _swapi(&_buffer.nav.ground_speed);
00182                 // at low speeds, ground course wanders wildly; suppress changes if we are not moving
00183                 if (ground_speed > 50)
00184                         ground_course   = _swapi(&_buffer.nav.ground_course);
00185                 num_sats                = _buffer.nav.satellites;
00186 
00187                 return true;
00188         }
00189         return false;
00190 }
00191 
00192 void
00193 AP_GPS_SIRF::_accumulate(uint8_t val)
00194 {
00195         _checksum = (_checksum + val) & 0x7fff;
00196 }

Generated for ArduPilot Libraries by doxygen