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
00031
00032
00033
00034
00035
00036 #include "GPS_UBLOX.h"
00037
00038 #include <avr/interrupt.h>
00039 #include "WProgram.h"
00040
00041
00042
00043 GPS_UBLOX_Class::GPS_UBLOX_Class()
00044 {
00045 }
00046
00047
00048
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();
00058
00059 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00060 Serial1.begin(38400);
00061 #else
00062 Serial.begin(38400);
00063 #endif
00064 }
00065
00066
00067
00068
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++)
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)
00089 {
00090 case 0:
00091 if(data == 0xB5)
00092 UBX_step++;
00093 break;
00094 case 1:
00095 if(data == 0x62)
00096 UBX_step++;
00097 else
00098 UBX_step = 0;
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
00115 if (UBX_payload_length_hi >= UBX_MAXPAYLOAD)
00116 {
00117 if (PrintErrors)
00118 Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
00119 UBX_step = 0;
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:
00131 if (UBX_payload_counter < UBX_payload_length_hi)
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;
00142 UBX_step++;
00143 break;
00144 case 8:
00145 UBX_ck_b = data;
00146
00147
00148 if((ck_a == UBX_ck_a) && (ck_b == UBX_ck_b))
00149 parse_ubx_gps();
00150 else
00151 {
00152 if (PrintErrors)
00153 Serial.println("ERR:GPS_CHK!!");
00154 }
00155
00156 UBX_step = 0;
00157 ck_a = 0;
00158 ck_b = 0;
00159 GPS_timer = millis();
00160 break;
00161 }
00162 }
00163
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
00176 void GPS_UBLOX_Class::parse_ubx_gps(void)
00177 {
00178 int j;
00179
00180
00181 if(UBX_class == 0x01)
00182 {
00183 switch(UBX_id)
00184 {
00185 case 0x02:
00186 j = 0;
00187 Time = join_4_bytes(&UBX_buffer[j]);
00188 j += 4;
00189 Longitude = join_4_bytes(&UBX_buffer[j]);
00190 j += 4;
00191 Lattitude = join_4_bytes(&UBX_buffer[j]);
00192 j += 4;
00193
00194 j += 4;
00195 Altitude = (float)join_4_bytes(&UBX_buffer[j]);
00196 Altitude /= 10.;
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206 NewData = 1;
00207 break;
00208 case 0x03:
00209
00210 if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01))
00211 Fix = 1;
00212 else
00213 Fix = 0;
00214 break;
00215
00216 case 0x06:
00217 if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01))
00218 Fix = 1;
00219 else
00220 Fix = 0;
00221 UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]);
00222 NumSats = UBX_buffer[47];
00223 break;
00224
00225 case 0x12:
00226 j = 16;
00227 Speed_3d = join_4_bytes(&UBX_buffer[j]);
00228 j += 4;
00229 Ground_Speed = join_4_bytes(&UBX_buffer[j]);
00230 j += 4;
00231 Ground_Course = join_4_bytes(&UBX_buffer[j]);
00232 Ground_Course /= 1000;
00233 j += 4;
00234
00235
00236
00237
00238
00239
00240 break;
00241 }
00242 }
00243 }
00244
00245
00246
00247
00248
00249
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
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;