Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "GPS_MTK.h"
00031 #include <avr/interrupt.h>
00032 #include "WProgram.h"
00033
00034
00035
00036 GPS_MTK_Class::GPS_MTK_Class()
00037 {
00038 }
00039
00040
00041
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();
00052
00053 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00054 Serial1.begin(38400);
00055 #else
00056 Serial.begin(38400);
00057 #endif
00058 Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
00059
00060 }
00061
00062
00063
00064
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++)
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)
00085 {
00086 case 0:
00087 if(data==0xB5)
00088 UBX_step++;
00089 break;
00090 case 1:
00091 if(data==0x62)
00092 UBX_step++;
00093 else
00094 UBX_step=0;
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)
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;
00123 UBX_step++;
00124 break;
00125 case 6:
00126 UBX_ck_b=data;
00127
00128
00129 if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b))
00130 parse_ubx_gps();
00131 else
00132 {
00133 if (PrintErrors)
00134 Serial.println("ERR:GPS_CHK!!");
00135 }
00136
00137 UBX_step=0;
00138 ck_a=0;
00139 ck_b=0;
00140 GPS_timer=millis();
00141 break;
00142 }
00143 }
00144
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
00157 void GPS_MTK_Class::parse_ubx_gps(void)
00158 {
00159 int j;
00160
00161
00162 if(UBX_class==0x01)
00163 {
00164 switch(UBX_id)
00165 {
00166
00167
00168 case 0x05:
00169 j=0;
00170 Lattitude= join_4_bytes(&UBX_buffer[j]) * 10;
00171 j+=4;
00172 Longitude = join_4_bytes(&UBX_buffer[j]) * 10;
00173 j+=4;
00174 Altitude = join_4_bytes(&UBX_buffer[j]);
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;
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
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
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;