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

AP_GPS_UBLOX.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 //  u-blox UBX 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 
00012 #include "AP_GPS_UBLOX.h"
00013 #include <stdint.h>
00014 
00015 // Constructors ////////////////////////////////////////////////////////////////
00016 
00017 AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
00018 {
00019 }
00020 
00021 // Public Methods //////////////////////////////////////////////////////////////
00022 
00023 void
00024 AP_GPS_UBLOX::init(void)
00025 {
00026         // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the 
00027         // right reporting configuration.
00028 
00029         _port->flush();
00030 }
00031 
00032 // Process bytes available from the stream
00033 //
00034 // The stream is assumed to contain only messages we recognise.  If it
00035 // contains other messages, and those messages contain the preamble
00036 // bytes, it is possible for this code to fail to synchronise to the
00037 // stream immediately.  Without buffering the entire message and
00038 // re-processing it from the top, this is unavoidable. The parser
00039 // attempts to avoid this when possible.
00040 //
00041 bool
00042 AP_GPS_UBLOX::read(void)
00043 {
00044         uint8_t         data;
00045         int             numc;
00046         bool            parsed = false;
00047 
00048         numc = _port->available();
00049         for (int i = 0; i < numc; i++){ // Process bytes received
00050 
00051                 // read the next byte
00052                 data = _port->read();
00053 
00054                 switch(_step){
00055 
00056                         // Message preamble detection
00057                         //
00058                         // If we fail to match any of the expected bytes, we reset
00059                         // the state machine and re-consider the failed byte as
00060                         // the first byte of the preamble.  This improves our
00061                         // chances of recovering from a mismatch and makes it less
00062                         // likely that we will be fooled by the preamble appearing
00063                         // as data in some other message.
00064                         //
00065                 case 1:
00066                         if (PREAMBLE2 == data) {
00067                                 _step++;
00068                                 break;
00069                         }
00070                         _step = 0;
00071                         // FALLTHROUGH
00072                 case 0:
00073                         if(PREAMBLE1 == data)
00074                                 _step++;
00075                         break;
00076 
00077                         // Message header processing
00078                         //
00079                         // We sniff the class and message ID to decide whether we
00080                         // are going to gather the message bytes or just discard
00081                         // them.
00082                         //
00083                         // We always collect the length so that we can avoid being
00084                         // fooled by preamble bytes in messages.
00085                         //
00086                 case 2:
00087                         _step++;
00088                         if (CLASS_NAV == data) {
00089                                 _gather = true;                                 // class is interesting, maybe gather
00090                                 _ck_b = _ck_a = data;                   // reset the checksum accumulators
00091                         } else {
00092                                 _gather = false;                                // class is not interesting, discard
00093                         }
00094                         break;
00095                 case 3:
00096                         _step++;
00097                         _ck_b += (_ck_a += data);                       // checksum byte
00098                         _msg_id = data;
00099                         if (_gather) {                                          // if class was interesting
00100                                 switch(data) {
00101                                 case MSG_POSLLH:                                // message is interesting
00102                                         _expect = sizeof(ubx_nav_posllh);
00103                                         break;
00104                                 case MSG_STATUS:
00105                                         _expect = sizeof(ubx_nav_status);
00106                                         break;
00107                                 case MSG_SOL:
00108                                         _expect = sizeof(ubx_nav_solution);
00109                                         break;
00110                                 case MSG_VELNED:
00111                                         _expect = sizeof(ubx_nav_velned);
00112                                         break;
00113                                 default:
00114                                         _gather = false;                        // message is not interesting
00115                                 }
00116                         }
00117                         break;
00118                 case 4:
00119                         _step++;
00120                         _ck_b += (_ck_a += data);                       // checksum byte
00121                         _payload_length = data;                         // payload length low byte
00122                         break;
00123                 case 5:
00124                         _step++;
00125                         _ck_b += (_ck_a += data);                       // checksum byte
00126                         _payload_length += (uint16_t)data;      // payload length high byte
00127                         _payload_counter = 0;                           // prepare to receive payload
00128                         if (_payload_length != _expect)
00129                                 _gather = false;
00130                         break;
00131 
00132                         // Receive message data
00133                         //
00134                 case 6:
00135                         _ck_b += (_ck_a += data);                       // checksum byte
00136                         if (_gather)                                            // gather data if requested
00137                                 _buffer.bytes[_payload_counter] = data;
00138                         if (++_payload_counter == _payload_length)
00139                                 _step++;
00140                         break;
00141 
00142                         // Checksum and message processing
00143                         //
00144                 case 7:
00145                         _step++;
00146                         if (_ck_a != data)
00147                                 _step = 0;                                              // bad checksum
00148                         break;
00149                 case 8:
00150                         _step = 0;
00151                         if (_ck_b != data)
00152                                 break;                                                  // bad checksum
00153 
00154                         if (_gather) {
00155                                 parsed = _parse_gps();                  // Parse the new GPS packet
00156                         }
00157                 }
00158         } 
00159         return parsed;
00160 }
00161 
00162 // Private Methods /////////////////////////////////////////////////////////////
00163 
00164 bool
00165 AP_GPS_UBLOX::_parse_gps(void)
00166 {
00167         switch (_msg_id) {
00168         case MSG_POSLLH:
00169                 time            = _buffer.posllh.time;
00170                 longitude       = _buffer.posllh.longitude;
00171                 latitude        = _buffer.posllh.latitude;
00172                 altitude        = _buffer.posllh.altitude_msl / 10;
00173                 break;
00174         case MSG_STATUS:
00175                 fix                     = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
00176                 break;
00177         case MSG_SOL:
00178                 fix                     = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
00179                 num_sats        = _buffer.solution.satellites;
00180                 break;
00181         case MSG_VELNED:
00182                 speed_3d        = _buffer.velned.speed_3d;                              // cm/s
00183                 ground_speed = _buffer.velned.speed_2d;                         // cm/s
00184                 ground_course = _buffer.velned.heading_2d / 1000;       // Heading 2D deg * 100000 rescaled to deg * 100
00185                 break;
00186         default:
00187                 return false;
00188         }
00189         return true;
00190 }

Generated for ArduPilot Libraries by doxygen