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

GPS_UBLOX.cpp

Go to the documentation of this file.
00001 /*
00002         GPS_UBLOX.cpp - Ublox GPS library for Arduino
00003         Code by Jordi Muņoz and Jose Julio. DIYDrones.com
00004         This code works with boards based on ATMega168 / 328 and ATMega1280/2560 (Serial port 1)
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 : Ublox protocol
00012         Baud rate : 38400
00013         Active messages : 
00014                 NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
00015                 NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
00016                 NAV - STATUS Receiver Navigation Status
00017                         or 
00018                 NAV - SOL Navigation Solution Information
00019 
00020         Methods:
00021                 Init() : GPS Initialization
00022                 Read() : Call this funcion as often as you want to ensure you read the incomming gps data
00023                 
00024         Properties:
00025                 Lattitude : Lattitude * 10,000,000 (long value)
00026                 Longitude : Longitude * 10,000,000 (long value)
00027                 Altitude :      Altitude * 100 (meters) (long value)
00028                 Ground_speed : Speed (m / s) * 100 (long value)
00029                 Ground_course : Course (degrees) * 100 (long value)
00030                 NewData : 1 when a new data is received.
00031                                                         You need to write a 0 to NewData when you read the data
00032                 Fix : 1: GPS FIX, 0: No Fix (normal logic)
00033                         
00034 */
00035 
00036 #include "GPS_UBLOX.h"
00037 
00038 #include <avr/interrupt.h>
00039 #include "WProgram.h"
00040 
00041 
00042 // Constructors ////////////////////////////////////////////////////////////////
00043 GPS_UBLOX_Class::GPS_UBLOX_Class()
00044 {
00045 }
00046 
00047 
00048 // Public Methods //////////////////////////////////////////////////////////////
00049 void GPS_UBLOX_Class::Init(void)
00050 {
00051         ck_a = 0;
00052         ck_b = 0;
00053         UBX_step = 0;
00054         NewData = 0;
00055         Fix = 0;
00056         PrintErrors = 0;
00057         GPS_timer = millis();    // Restarting timer...
00058         // Initialize serial port
00059         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00060                 Serial1.begin(38400);                            // Serial port 1 on ATMega1280/2560
00061         #else
00062                 Serial.begin(38400);
00063         #endif
00064 }
00065 
00066 // optimization : This code donīt wait for data, only proccess the data available
00067 // We can call this function on the main loop (50Hz loop)
00068 // If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
00069 void GPS_UBLOX_Class::Read(void)
00070 {
00071         static unsigned long GPS_timer = 0;
00072         byte data;
00073         int numc;
00074         
00075         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)          // If AtMega1280/2560 then Serial port 1...
00076         numc = Serial1.available();
00077         #else
00078         numc = Serial.available();
00079         #endif
00080         if (numc > 0)
00081                 for (int i = 0; i < numc; i++)  // Process bytes received
00082                         {
00083                 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00084                         data = Serial1.read();
00085                 #else
00086                         data = Serial.read();
00087                 #endif
00088                         switch(UBX_step)                 // Normally we start from zero. This is a state machine
00089                         {
00090                         case 0: 
00091                                 if(data == 0xB5)        // UBX sync char 1
00092                                         UBX_step++;      // OH first data packet is correct, so jump to the next step
00093                                 break; 
00094                         case 1: 
00095                                 if(data == 0x62)        // UBX sync char 2
00096                                         UBX_step++;      // ooh! The second data packet is correct, jump to the step 2
00097                                 else 
00098                                         UBX_step = 0;    // Nop, is not correct so restart to step zero and try again.           
00099                                 break;
00100                         case 2:
00101                                 UBX_class = data;
00102                                 ubx_checksum(UBX_class);
00103                                 UBX_step++;
00104                                 break;
00105                         case 3:
00106                                 UBX_id = data;
00107                                 ubx_checksum(UBX_id);
00108                                 UBX_step++;
00109                                 break;
00110                         case 4:
00111                                 UBX_payload_length_hi = data;
00112                                 ubx_checksum(UBX_payload_length_hi);
00113                                 UBX_step++;
00114                 // We check if the payload lenght is valid...
00115                 if (UBX_payload_length_hi >= UBX_MAXPAYLOAD)
00116                                 {
00117                         if (PrintErrors)
00118                         Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");          
00119                                         UBX_step = 0;    // Bad data, so restart to step zero and try again.             
00120                                         ck_a = 0;
00121                                         ck_b = 0;
00122                                 }
00123                                 break;
00124                         case 5:
00125                                 UBX_payload_length_lo = data;
00126                                 ubx_checksum(UBX_payload_length_lo);
00127                                 UBX_step++;
00128                 UBX_payload_counter = 0;
00129                                 break;
00130                         case 6:                          // Payload data read...
00131         if (UBX_payload_counter < UBX_payload_length_hi)        // We stay in this state until we reach the payload_length
00132                                 {
00133                                         UBX_buffer[UBX_payload_counter] = data;
00134                                         ubx_checksum(data);
00135                                         UBX_payload_counter++;
00136                                         if (UBX_payload_counter == UBX_payload_length_hi)
00137                                                 UBX_step++;
00138                                 }
00139                                 break;
00140                         case 7:
00141                                 UBX_ck_a = data;         // First checksum byte
00142                                 UBX_step++;
00143                                 break;
00144                         case 8:
00145                                 UBX_ck_b = data;         // Second checksum byte
00146                          
00147           // We end the GPS read...
00148                                 if((ck_a == UBX_ck_a) && (ck_b == UBX_ck_b))     // Verify the received checksum with the generated checksum.. 
00149                                 parse_ubx_gps();                                                         // Parse the new GPS packet
00150                                 else
00151                         {
00152                         if (PrintErrors)
00153                         Serial.println("ERR:GPS_CHK!!");
00154                         }
00155         // Variable initialization
00156                                 UBX_step = 0;
00157                                 ck_a = 0;
00158                                 ck_b = 0;
00159                                 GPS_timer = millis(); // Restarting timer...
00160                                 break;
00161                 }
00162                 }               // End for...
00163   // If we donīt receive GPS packets in 2 seconds => Bad FIX state
00164         if ((millis() - GPS_timer) > 2000)
00165                 {
00166         Fix = 0;
00167         if (PrintErrors)
00168           Serial.println("ERR:GPS_TIMEOUT!!");
00169                 }
00170 }
00171 
00172 /****************************************************************
00173  * 
00174  ****************************************************************/
00175 // Private Methods //////////////////////////////////////////////////////////////
00176 void GPS_UBLOX_Class::parse_ubx_gps(void)
00177 {
00178         int j;
00179 //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. 
00180 //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
00181         if(UBX_class == 0x01) 
00182         {
00183                 switch(UBX_id) //Checking the UBX ID
00184                 {
00185                 case 0x02: // ID NAV - POSLLH 
00186                         j = 0;
00187                         Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week
00188                         j += 4;
00189                         Longitude = join_4_bytes(&UBX_buffer[j]); // lon * 10000000
00190                         j += 4;
00191                         Lattitude = join_4_bytes(&UBX_buffer[j]); // lat * 10000000
00192                         j += 4;
00193                  //Altitude = join_4_bytes(&UBX_buffer[j]);  // elipsoid heigth mm
00194                         j += 4;
00195                         Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm
00196                         Altitude /= 10.;                                                                        
00197                         
00198                         // Rescale altitude to cm
00199                 //j+=4;
00200                 /*
00201                         hacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
00202                         j += 4;
00203                         vacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
00204                         j += 4;
00205                         */
00206                         NewData = 1;
00207                         break;
00208                 case 0x03: //ID NAV - STATUS 
00209       //if(UBX_buffer[4] >= 0x03)
00210                 if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01))                           
00211                                 Fix = 1; // valid position                              
00212                         else
00213                                 Fix = 0; // invalid position
00214                         break;
00215 
00216                 case 0x06: //ID NAV - SOL
00217                         if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01))
00218                                 Fix = 1; // valid position
00219                         else
00220                                 Fix = 0; // invalid position                            
00221                         UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]);     // Vertical Speed in cm / s
00222                         NumSats = UBX_buffer[47];                                                                               // Number of sats...             
00223                         break;
00224 
00225                 case 0x12: // ID NAV - VELNED 
00226                         j = 16;
00227                         Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm / s
00228                         j += 4;
00229                         Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm / s
00230                         j += 4;
00231                         Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg * 100000
00232                         Ground_Course /= 1000;  // Rescale heading to deg * 100
00233                         j += 4;
00234       /*
00235                         sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy
00236                         j += 4;
00237                         headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy
00238                         j += 4;
00239                         */
00240                         break; 
00241                         }
00242                 }        
00243 }
00244 
00245 
00246 /****************************************************************
00247  * 
00248  ****************************************************************/
00249  // Join 4 bytes into a long
00250 long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[])
00251 {
00252         union long_union {
00253         int32_t dword;
00254         uint8_t byte[4];
00255 } longUnion;
00256 
00257         longUnion.byte[0] = *Buffer;
00258         longUnion.byte[1] = *(Buffer + 1);
00259         longUnion.byte[2] = *(Buffer + 2);
00260         longUnion.byte[3] = *(Buffer + 3);
00261         return(longUnion.dword);
00262 }
00263 
00264 /****************************************************************
00265  * 
00266  ****************************************************************/
00267 // Ublox checksum algorithm
00268 void GPS_UBLOX_Class::ubx_checksum(byte ubx_data)
00269 {
00270         ck_a += ubx_data;
00271         ck_b += ck_a; 
00272 }
00273 
00274 GPS_UBLOX_Class GPS;

Generated for ArduPilot Libraries by doxygen