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

GPS_MTK.cpp

Go to the documentation of this file.
00001 /*
00002         GPS_MTK.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 : Costum protocol
00012         Baud rate : 38400
00013 
00014         Methods:
00015                 Init() : GPS Initialization
00016                 Read() : Call this funcion as often as you want to ensure you read the incomming gps data
00017                 
00018         Properties:
00019                 Lattitude : Lattitude * 10,000,000 (long value)
00020                 Longitude : Longitude * 10,000,000 (long value)
00021                 Altitude :  Altitude * 100 (meters) (long value)
00022                 Ground_speed : Speed (m/s) * 100 (long value)
00023                 Ground_course : Course (degrees) * 100 (long value)
00024                 NewData : 1 when a new data is received.
00025                           You need to write a 0 to NewData when you read the data
00026                 Fix : 0: GPS NO FIX or 2D FIX, 1: 3D FIX.
00027                         
00028 */
00029 
00030 #include "GPS_MTK.h"
00031 #include <avr/interrupt.h>
00032 #include "WProgram.h"
00033 
00034 
00035 // Constructors ////////////////////////////////////////////////////////////////
00036 GPS_MTK_Class::GPS_MTK_Class()
00037 {
00038 }
00039 
00040 
00041 // Public Methods //////////////////////////////////////////////////////////////
00042 void GPS_MTK_Class::Init(void)
00043 {
00044         delay(200);
00045         ck_a=0;
00046         ck_b=0;
00047         UBX_step=0;
00048         NewData=0;
00049         Fix=0;
00050         PrintErrors=0;
00051         GPS_timer=millis();   //Restarting timer...
00052         // Initialize serial port
00053         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00054                 Serial1.begin(38400);         // Serial port 1 on ATMega1280/2560
00055         #else
00056                 Serial.begin(38400);
00057         #endif
00058         Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
00059         //Serial.println("sent config string");
00060 }
00061 
00062 // optimization : This code donīt wait for data, only proccess the data available
00063 // We can call this function on the main loop (50Hz loop)
00064 // If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
00065 void GPS_MTK_Class::Read(void)
00066 {
00067   static unsigned long GPS_timer=0;
00068   byte data;
00069   int numc;
00070   
00071   #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)    // If AtMega1280/2560 then Serial port 1...
00072         numc = Serial1.available();
00073   #else
00074         numc = Serial.available();
00075   #endif
00076   if (numc > 0)
00077     for (int i=0;i<numc;i++)  // Process bytes received
00078       {
00079           #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00080         data = Serial1.read();
00081       #else
00082                 data = Serial.read();
00083           #endif
00084       switch(UBX_step)     //Normally we start from zero. This is a state machine
00085       {
00086       case 0:  
00087         if(data==0xB5)  // UBX sync char 1
00088           UBX_step++;   //OH first data packet is correct, so jump to the next step
00089         break; 
00090       case 1:  
00091         if(data==0x62)  // UBX sync char 2
00092           UBX_step++;   //ooh! The second data packet is correct, jump to the step 2
00093         else 
00094           UBX_step=0;   //Nop, is not correct so restart to step zero and try again.     
00095         break;
00096       case 2:
00097         UBX_class=data;
00098         ubx_checksum(UBX_class);
00099         UBX_step++;
00100         break;
00101       case 3:
00102         UBX_id=data;
00103         UBX_step=4;
00104         UBX_payload_length_hi=26;
00105                 UBX_payload_length_lo=0;
00106                 UBX_payload_counter=0;
00107 
00108         ubx_checksum(UBX_id);
00109         
00110         break;
00111       case 4:
00112         if (UBX_payload_counter < UBX_payload_length_hi)  // We stay in this state until we reach the payload_length
00113         {
00114           UBX_buffer[UBX_payload_counter] = data;
00115           ubx_checksum(data);
00116           UBX_payload_counter++;
00117           if (UBX_payload_counter==UBX_payload_length_hi)
00118             UBX_step++;
00119         }
00120         break;
00121       case 5:
00122         UBX_ck_a=data;   // First checksum byte
00123         UBX_step++;
00124         break;
00125       case 6:
00126         UBX_ck_b=data;   // Second checksum byte
00127        
00128           // We end the GPS read...
00129         if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b))   // Verify the received checksum with the generated checksum.. 
00130                         parse_ubx_gps();               // Parse the new GPS packet
00131         else
00132                   {
00133                   if (PrintErrors)
00134                         Serial.println("ERR:GPS_CHK!!");
00135                   }
00136         // Variable initialization
00137         UBX_step=0;
00138         ck_a=0;
00139         ck_b=0;
00140         GPS_timer=millis(); //Restarting timer...
00141         break;
00142           }
00143     }    // End for...
00144   // If we donīt receive GPS packets in 2 seconds => Bad FIX state
00145   if ((millis() - GPS_timer)>2000)
00146     {
00147         Fix = 0;
00148         if (PrintErrors)
00149           Serial.println("ERR:GPS_TIMEOUT!!");
00150     }
00151 }
00152 
00153 /****************************************************************
00154  * 
00155  ****************************************************************/
00156 // Private Methods //////////////////////////////////////////////////////////////
00157 void GPS_MTK_Class::parse_ubx_gps(void)
00158 {
00159   int j;
00160 //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. 
00161 //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
00162   if(UBX_class==0x01) 
00163   {
00164     switch(UBX_id)//Checking the UBX ID
00165     {
00166 
00167 
00168     case 0x05: //ID Custom
00169       j=0;
00170       Lattitude= join_4_bytes(&UBX_buffer[j]) * 10; // lon*10,000,000
00171       j+=4;
00172       Longitude = join_4_bytes(&UBX_buffer[j]) * 10; // lat*10000000
00173       j+=4;
00174       Altitude = join_4_bytes(&UBX_buffer[j]);  // MSL
00175       j+=4;
00176           Ground_Speed = join_4_bytes(&UBX_buffer[j]);
00177       j+=4;
00178           Ground_Course = join_4_bytes(&UBX_buffer[j]) / 10000; // Heading 2D
00179       j+=4;
00180           NumSats=UBX_buffer[j];
00181           j++;
00182           Fix=UBX_buffer[j];
00183           if (Fix==3)
00184                 Fix = 1;
00185           else
00186                 Fix = 0;
00187       j++;
00188           Time = join_4_bytes(&UBX_buffer[j]);
00189       NewData=1;
00190       break;
00191 
00192       }
00193     }   
00194 }
00195 
00196 
00197 /****************************************************************
00198  * 
00199  ****************************************************************/
00200  // Join 4 bytes into a long
00201 long GPS_MTK_Class::join_4_bytes(unsigned char Buffer[])
00202 {
00203   union long_union {
00204         int32_t dword;
00205         uint8_t  byte[4];
00206 } longUnion;
00207 
00208   longUnion.byte[3] = *Buffer;
00209   longUnion.byte[2] = *(Buffer+1);
00210   longUnion.byte[1] = *(Buffer+2);
00211   longUnion.byte[0] = *(Buffer+3);
00212   return(longUnion.dword);
00213 }
00214 
00215 /****************************************************************
00216  * 
00217  ****************************************************************/
00218 // checksum algorithm
00219 void GPS_MTK_Class::ubx_checksum(byte ubx_data)
00220 {
00221   ck_a+=ubx_data;
00222   ck_b+=ck_a; 
00223 }
00224 
00225 GPS_MTK_Class GPS;

Generated for ArduPilot Libraries by doxygen