analoguedevices
15 years ago
3 changed files with 283 additions and 0 deletions
@ -0,0 +1,218 @@ |
|||||||
|
/*
|
||||||
|
GPS_MTK.cpp - Ublox GPS library for Arduino |
||||||
|
Code by Jordi Muñoz and Jose Julio. DIYDrones.com |
||||||
|
This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1) |
||||||
|
|
||||||
|
This library is free software; you can redistribute it and/or |
||||||
|
modify it under the terms of the GNU Lesser General Public |
||||||
|
License as published by the Free Software Foundation; either |
||||||
|
version 2.1 of the License, or (at your option) any later version. |
||||||
|
|
||||||
|
GPS configuration : Costum protocol |
||||||
|
Baud rate : 38400 |
||||||
|
|
||||||
|
Methods: |
||||||
|
Init() : GPS Initialization |
||||||
|
Read() : Call this funcion as often as you want to ensure you read the incomming gps data |
||||||
|
|
||||||
|
Properties: |
||||||
|
Lattitude : Lattitude * 10000000 (long value) |
||||||
|
Longitude : Longitude * 10000000 (long value) |
||||||
|
Altitude : Altitude * 100 (meters) (long value) |
||||||
|
Ground_speed : Speed (m/s) * 100 (long value) |
||||||
|
Ground_course : Course (degrees) * 100 (long value) |
||||||
|
NewData : 1 when a new data is received. |
||||||
|
You need to write a 0 to NewData when you read the data |
||||||
|
Fix : 1: GPS NO FIX, 2: 2D FIX, 3: 3D FIX. |
||||||
|
|
||||||
|
*/ |
||||||
|
|
||||||
|
#include "GPS_MTK.h" |
||||||
|
#include <avr/interrupt.h> |
||||||
|
#include "WProgram.h" |
||||||
|
|
||||||
|
|
||||||
|
// Constructors ////////////////////////////////////////////////////////////////
|
||||||
|
GPS_MTK_Class::GPS_MTK_Class() |
||||||
|
{ |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
|
void GPS_MTK_Class::Init(void) |
||||||
|
{ |
||||||
|
ck_a=0; |
||||||
|
ck_b=0; |
||||||
|
UBX_step=0; |
||||||
|
NewData=0; |
||||||
|
Fix=0; |
||||||
|
PrintErrors=0; |
||||||
|
GPS_timer=millis(); //Restarting timer...
|
||||||
|
// Initialize serial port
|
||||||
|
#if defined(__AVR_ATmega1280__) |
||||||
|
Serial1.begin(38400); // Serial port 1 on ATMega1280
|
||||||
|
#else |
||||||
|
Serial.begin(38400); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
// optimization : This code don´t wait for data, only proccess the data available
|
||||||
|
// We can call this function on the main loop (50Hz loop)
|
||||||
|
// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info.
|
||||||
|
void GPS_MTK_Class::Read(void) |
||||||
|
{ |
||||||
|
static unsigned long GPS_timer=0; |
||||||
|
byte data; |
||||||
|
int numc; |
||||||
|
|
||||||
|
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
|
||||||
|
numc = Serial1.available(); |
||||||
|
#else |
||||||
|
numc = Serial.available(); |
||||||
|
#endif |
||||||
|
if (numc > 0) |
||||||
|
for (int i=0;i<numc;i++) // Process bytes received
|
||||||
|
{ |
||||||
|
#if defined(__AVR_ATmega1280__) |
||||||
|
data = Serial1.read(); |
||||||
|
#else |
||||||
|
data = Serial.read(); |
||||||
|
#endif |
||||||
|
switch(UBX_step) //Normally we start from zero. This is a state machine
|
||||||
|
{ |
||||||
|
case 0:
|
||||||
|
if(data==0xB5) // UBX sync char 1
|
||||||
|
UBX_step++; //OH first data packet is correct, so jump to the next step
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
if(data==0x62) // UBX sync char 2
|
||||||
|
UBX_step++; //ooh! The second data packet is correct, jump to the step 2
|
||||||
|
else
|
||||||
|
UBX_step=0; //Nop, is not correct so restart to step zero and try again.
|
||||||
|
break; |
||||||
|
case 2: |
||||||
|
UBX_class=data; |
||||||
|
ubx_checksum(UBX_class); |
||||||
|
UBX_step++; |
||||||
|
break; |
||||||
|
case 3: |
||||||
|
UBX_id=data; |
||||||
|
UBX_step=4; |
||||||
|
UBX_payload_length_hi=26; |
||||||
|
UBX_payload_length_lo=0; |
||||||
|
UBX_payload_counter=0; |
||||||
|
|
||||||
|
ubx_checksum(UBX_id); |
||||||
|
|
||||||
|
break; |
||||||
|
case 4: |
||||||
|
if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length
|
||||||
|
{ |
||||||
|
UBX_buffer[UBX_payload_counter] = data; |
||||||
|
ubx_checksum(data); |
||||||
|
UBX_payload_counter++; |
||||||
|
if (UBX_payload_counter==UBX_payload_length_hi) |
||||||
|
UBX_step++; |
||||||
|
} |
||||||
|
break; |
||||||
|
case 5: |
||||||
|
UBX_ck_a=data; // First checksum byte
|
||||||
|
UBX_step++; |
||||||
|
break; |
||||||
|
case 6: |
||||||
|
UBX_ck_b=data; // Second checksum byte
|
||||||
|
|
||||||
|
// We end the GPS read...
|
||||||
|
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b)) // Verify the received checksum with the generated checksum..
|
||||||
|
parse_ubx_gps(); // Parse the new GPS packet
|
||||||
|
else |
||||||
|
{ |
||||||
|
if (PrintErrors) |
||||||
|
Serial.println("ERR:GPS_CHK!!"); |
||||||
|
} |
||||||
|
// Variable initialization
|
||||||
|
UBX_step=0; |
||||||
|
ck_a=0; |
||||||
|
ck_b=0; |
||||||
|
GPS_timer=millis(); //Restarting timer...
|
||||||
|
break; |
||||||
|
} |
||||||
|
} // End for...
|
||||||
|
// If we don´t receive GPS packets in 2 seconds => Bad FIX state
|
||||||
|
if ((millis() - GPS_timer)>2000) |
||||||
|
{ |
||||||
|
Fix = 0; |
||||||
|
if (PrintErrors) |
||||||
|
Serial.println("ERR:GPS_TIMEOUT!!"); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
*
|
||||||
|
****************************************************************/ |
||||||
|
// Private Methods //////////////////////////////////////////////////////////////
|
||||||
|
void GPS_MTK_Class::parse_ubx_gps(void) |
||||||
|
{ |
||||||
|
int j; |
||||||
|
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes..
|
||||||
|
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
|
||||||
|
if(UBX_class==0x01)
|
||||||
|
{ |
||||||
|
switch(UBX_id)//Checking the UBX ID
|
||||||
|
{ |
||||||
|
|
||||||
|
|
||||||
|
case 0x05: //ID Custom
|
||||||
|
j=0; |
||||||
|
Lattitude= join_4_bytes(&UBX_buffer[j]); // lon*10000000
|
||||||
|
j+=4; |
||||||
|
Longitude = join_4_bytes(&UBX_buffer[j]); // lat*10000000
|
||||||
|
j+=4; |
||||||
|
Altitude = join_4_bytes(&UBX_buffer[j]); // MSL
|
||||||
|
j+=4; |
||||||
|
Ground_Speed = join_4_bytes(&UBX_buffer[j]); |
||||||
|
j+=4; |
||||||
|
Ground_Course = join_4_bytes(&UBX_buffer[j]); |
||||||
|
j+=4; |
||||||
|
NumSats=UBX_buffer[j]; |
||||||
|
j++; |
||||||
|
Fix=UBX_buffer[j]; |
||||||
|
j++; |
||||||
|
Time = join_4_bytes(&UBX_buffer[j]); |
||||||
|
NewData=1; |
||||||
|
break; |
||||||
|
|
||||||
|
} |
||||||
|
}
|
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
*
|
||||||
|
****************************************************************/ |
||||||
|
// Join 4 bytes into a long
|
||||||
|
long GPS_MTK_Class::join_4_bytes(unsigned char Buffer[]) |
||||||
|
{ |
||||||
|
union long_union { |
||||||
|
int32_t dword; |
||||||
|
uint8_t byte[4]; |
||||||
|
} longUnion; |
||||||
|
|
||||||
|
longUnion.byte[3] = *Buffer; |
||||||
|
longUnion.byte[2] = *(Buffer+1); |
||||||
|
longUnion.byte[1] = *(Buffer+2); |
||||||
|
longUnion.byte[0] = *(Buffer+3); |
||||||
|
return(longUnion.dword); |
||||||
|
} |
||||||
|
|
||||||
|
/****************************************************************
|
||||||
|
*
|
||||||
|
****************************************************************/ |
||||||
|
// checksum algorithm
|
||||||
|
void GPS_MTK_Class::ubx_checksum(byte ubx_data) |
||||||
|
{ |
||||||
|
ck_a+=ubx_data; |
||||||
|
ck_b+=ck_a;
|
||||||
|
} |
||||||
|
|
||||||
|
GPS_MTK_Class GPS; |
||||||
@ -0,0 +1,49 @@ |
|||||||
|
#ifndef GPS_MTK_h |
||||||
|
#define GPS_MTK_h |
||||||
|
|
||||||
|
#include <inttypes.h> |
||||||
|
|
||||||
|
#define UBX_MAXPAYLOAD 60 |
||||||
|
|
||||||
|
class GPS_MTK_Class |
||||||
|
{ |
||||||
|
private: |
||||||
|
// Internal variables
|
||||||
|
uint8_t ck_a; // Packet checksum
|
||||||
|
uint8_t ck_b; |
||||||
|
uint8_t UBX_step; |
||||||
|
uint8_t UBX_class; |
||||||
|
uint8_t UBX_id; |
||||||
|
uint8_t UBX_payload_length_hi; |
||||||
|
uint8_t UBX_payload_length_lo; |
||||||
|
uint8_t UBX_payload_counter; |
||||||
|
uint8_t UBX_buffer[UBX_MAXPAYLOAD]; |
||||||
|
uint8_t UBX_ck_a; |
||||||
|
uint8_t UBX_ck_b; |
||||||
|
long GPS_timer; |
||||||
|
long UBX_ecefVZ; |
||||||
|
void parse_ubx_gps(); |
||||||
|
void ubx_checksum(unsigned char ubx_data); |
||||||
|
long join_4_bytes(unsigned char Buffer[]); |
||||||
|
|
||||||
|
public: |
||||||
|
// Methods
|
||||||
|
GPS_MTK_Class(); |
||||||
|
void Init(); |
||||||
|
void Read(); |
||||||
|
// Properties
|
||||||
|
long Time; //GPS Millisecond Time of Week
|
||||||
|
long Lattitude; // Geographic coordinates
|
||||||
|
long Longitude; |
||||||
|
long Altitude; |
||||||
|
long Ground_Speed; |
||||||
|
long Ground_Course; |
||||||
|
uint8_t NumSats; // Number of visible satelites
|
||||||
|
uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic)
|
||||||
|
uint8_t NewData; // 1:New GPS Data
|
||||||
|
uint8_t PrintErrors; // 1: To Print GPS Errors (for debug)
|
||||||
|
}; |
||||||
|
|
||||||
|
extern GPS_MTK_Class GPS; |
||||||
|
|
||||||
|
#endif |
||||||
@ -0,0 +1,16 @@ |
|||||||
|
GPS KEYWORD1 |
||||||
|
GPS_UBLOX KEYWORD1 |
||||||
|
Init KEYWORD2 |
||||||
|
Read KEYWORD2 |
||||||
|
Time KEYWORD2 |
||||||
|
Lattitude KEYWORD2 |
||||||
|
Longitude KEYWORD2 |
||||||
|
Altitude KEYWORD2 |
||||||
|
Ground_Speed KETWORD2 |
||||||
|
Ground_Course KEYWORD2 |
||||||
|
Speed_3d KEYWORD2 |
||||||
|
NumSats KEYWORD2 |
||||||
|
Fix KEYWORD2 |
||||||
|
NewData KEYWORD2 |
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in new issue