Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GPS - removed GPS_MTK, GPT_NMEA and GPS_UBLOX. All functionality moved to AP_GPS many months ago.

This commit is contained in:
Randy Mackay 2011-12-30 22:25:00 +09:00
parent 3ea49e1415
commit a2edba69b1
15 changed files with 0 additions and 1103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
libraries/GPS_MTK/GPS_MTK.cpp
View File

@ -1,225 +0,0 @@
/*
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/2560 (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 * 10,000,000 (long value)
Longitude : Longitude * 10,000,000 (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 : 0: GPS NO FIX or 2D FIX, 1: 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)
{
delay(200);
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__) || defined(__AVR_ATmega2560__)
Serial1.begin(38400); // Serial port 1 on ATMega1280/2560
#else
Serial.begin(38400);
#endif
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
//Serial.println("sent config string");
}
// 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__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 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__) || defined(__AVR_ATmega2560__)
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]) * 10; // lon*10,000,000
j+=4;
Longitude = join_4_bytes(&UBX_buffer[j]) * 10; // 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]) / 10000; // Heading 2D
j+=4;
NumSats=UBX_buffer[j];
j++;
Fix=UBX_buffer[j];
if (Fix==3)
Fix = 1;
else
Fix = 0;
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;

49
libraries/GPS_MTK/GPS_MTK.h
View File

@ -1,49 +0,0 @@
#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

47
libraries/GPS_MTK/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -1,47 +0,0 @@
/*
Example of GPS MTK library.
Code by Jordi Mu<4D>oz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <GPS_MTK.h> // UBLOX GPS Library
#define T6 1000000
#define T7 10000000
void setup()
{
Serial.begin(38400);
Serial.println("GPS MTK library test");
GPS.Init(); // GPS Initialization
delay(1000);
}
void loop()
{
GPS.Read();
if (GPS.NewData) // New GPS data?
{
Serial.print("GPS:");
Serial.print(" Lat:");
Serial.print((float)GPS.Lattitude/T7, DEC);
Serial.print(" Lon:");
Serial.print((float)GPS.Longitude/T7, DEC);
Serial.print(" Alt:");
Serial.print((float)GPS.Altitude/100.0, DEC);
Serial.print(" GSP:");
Serial.print((float)GPS.Ground_Speed/100.0, DEC);
Serial.print(" COG:");
Serial.print(GPS.Ground_Course/100.0, DEC);
Serial.print(" SAT:");
Serial.print((int)GPS.NumSats);
Serial.print(" FIX:");
Serial.print((int)GPS.Fix);
Serial.print(" TIM:");
Serial.print(GPS.Time);
Serial.println();
GPS.NewData = 0; // We have readed the data
}
delay(20);
}

2
libraries/GPS_MTK/examples/GPS_MTK_test/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

16
libraries/GPS_MTK/keywords.txt
View File

@ -1,16 +0,0 @@
GPS KEYWORD1
GPS_MTK KEYWORD1
Init KEYWORD2
Read KEYWORD2
Time KEYWORD2
Lattitude KEYWORD2
Longitude KEYWORD2
Altitude KEYWORD2
Ground_Speed KEYWORD2
Ground_Course KEYWORD2
Speed_3d KEYWORD2
NumSats KEYWORD2
Fix KEYWORD2
NewData KEYWORD2

272
libraries/GPS_NMEA/GPS_NMEA.cpp
View File

@ -1,272 +0,0 @@
/*
GPS_NMEA.cpp - Generic NMEA 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/2560 (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 : NMEA protocol
Baud rate : 38400
NMEA Sentences :
$GPGGA : Global Positioning System Fix Data
$GPVTG : Ttack and Ground Speed
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 * 1000 (milimeters) (long value)
Ground_speed : Speed (m/s) * 100 (long value)
Ground_course : Course (degrees) * 100 (long value)
Type : 2 (This indicate that we are using the Generic NMEA library)
NewData : 1 when a new data is received.
You need to write a 0 to NewData when you read the data
Fix : >=1: GPS FIX, 0: No Fix (normal logic)
Quality : 0 = No Fix
1 = Bad (Num sats < 5)
2 = Poor
3 = Medium
4 = Good
NOTE : This code has been tested on a Locosys 20031 GPS receiver (MTK chipset)
*/
#include "GPS_NMEA.h"
#include <avr/interrupt.h>
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
GPS_NMEA_Class::GPS_NMEA_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void GPS_NMEA_Class::Init(void)
{
Type = 2;
GPS_checksum_calc = false;
bufferidx = 0;
NewData=0;
Fix=0;
Quality=0;
PrintErrors=0;
// Initialize serial port
#if defined(__AVR_ATmega1280__)|| defined(__AVR_ATmega2560__)
Serial1.begin(38400); // Serial port 1 on ATMega1280/2560
#else
Serial.begin(38400);
#endif
}
// This code don´t wait for data, only proccess the data available on serial port
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function call parse_nmea_gps() to parse and update the GPS info.
void GPS_NMEA_Class::Read(void)
{
char c;
int numc;
int i;
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 then Serial port 1...
numc = Serial1.available();
#else
numc = Serial.available();
#endif
if (numc > 0)
for (i=0;i<numc;i++){
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 then Serial port 1...
c = Serial1.read();
#else
c = Serial.read();
#endif
if (c == '$'){ // NMEA Start
bufferidx = 0;
buffer[bufferidx++] = c;
GPS_checksum = 0;
GPS_checksum_calc = true;
continue;
}
if (c == '\r'){ // NMEA End
buffer[bufferidx++] = 0;
parse_nmea_gps();
}
else {
if (bufferidx < (GPS_BUFFERSIZE-1)){
if (c == '*')
GPS_checksum_calc = false; // Checksum calculation end
buffer[bufferidx++] = c;
if (GPS_checksum_calc)
GPS_checksum ^= c; // XOR
}
else
bufferidx=0; // Buffer overflow : restart
}
}
}
/****************************************************************
*
****************************************************************/
// Private Methods //////////////////////////////////////////////////////////////
void GPS_NMEA_Class::parse_nmea_gps(void)
{
byte NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
NewData = 1; // New GPS Data
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
Time = parsenumber(parseptr,2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
//
aux_deg = parsedecimal(parseptr,2); // degrees
aux_min = parsenumber(parseptr+2,4); // minutes (sexagesimal) => Convert to decimal
Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5)
parseptr = strchr(parseptr, ',')+1;
//
if (*parseptr=='S')
Lattitude = -1*Lattitude; // South Lattitudes are negative
//
parseptr = strchr(parseptr, ',')+1;
// W Longitudes are Negative
aux_deg = parsedecimal(parseptr,3); // degrees
aux_min = parsenumber(parseptr+3,4); // minutes (sexagesimal)
Longitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000)
//Longitude = -1*Longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
//
if (*parseptr=='W')
Longitude = -1*Longitude; // West Longitudes are negative
//
parseptr = strchr(parseptr, ',')+1;
Fix = parsedecimal(parseptr,1);
parseptr = strchr(parseptr, ',')+1;
NumSats = parsedecimal(parseptr,2);
parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr,1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
Altitude = parsenumber(parseptr,1)*100; // Altitude in decimeters*100 = milimeters
if (Fix < 1)
Quality = 0; // No FIX
else if(NumSats<5)
Quality = 1; // Bad (Num sats < 5)
else if(HDOP>30)
Quality = 2; // Poor (HDOP > 30)
else if(HDOP>25)
Quality = 3; // Medium (HDOP > 25)
else
Quality = 4; // Good (HDOP < 25)
}
else
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
parseptr = strchr(buffer, ',')+1;
Ground_Course = parsenumber(parseptr,2); // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100)
//GPS_line = true;
}
else
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
else
{
bufferidx = 0;
if (PrintErrors)
Serial.println("GPSERR: Bad sentence!!");
}
}
/****************************************************************
*
****************************************************************/
// Parse hexadecimal numbers
byte GPS_NMEA_Class::parseHex(char c) {
if (c < '0')
return (0);
if (c <= '9')
return (c - '0');
if (c < 'A')
return (0);
if (c <= 'F')
return ((c - 'A')+10);
}
// Decimal number parser
long GPS_NMEA_Class::parsedecimal(char *str,byte num_car) {
long d = 0;
byte i;
i = num_car;
while ((str[0] != 0)&&(i>0)) {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
str++;
i--;
}
return d;
}
// Function to parse fixed point numbers (numdec=number of decimals)
long GPS_NMEA_Class::parsenumber(char *str,byte numdec) {
long d = 0;
byte ndec = 0;
while (str[0] != 0) {
if (str[0] == '.'){
ndec = 1;
}
else {
if ((str[0] > '9') || (str[0] < '0'))
return d;
d *= 10;
d += str[0] - '0';
if (ndec > 0)
ndec++;
if (ndec > numdec) // we reach the number of decimals...
return d;
}
str++;
}
return d;
}
GPS_NMEA_Class GPS;

46
libraries/GPS_NMEA/GPS_NMEA.h
View File

@ -1,46 +0,0 @@
#ifndef GPS_NMEA_h
#define GPS_NMEA_h
#include <inttypes.h>
#define GPS_BUFFERSIZE 120
class GPS_NMEA_Class
{
private:
// Internal variables
uint8_t GPS_checksum;
uint8_t GPS_checksum_calc;
char buffer[GPS_BUFFERSIZE];
int bufferidx;
void parse_nmea_gps(void);
uint8_t parseHex(char c);
long parsedecimal(char *str,uint8_t num_car);
long parsenumber(char *str,uint8_t numdec);
public:
// Methods
GPS_NMEA_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 Speed_3d; //Speed (3-D)
long Ground_Course;
uint8_t Type; // Type of GPS (library used)
uint8_t NumSats; // Number of visible satelites
uint8_t Fix; // >=1:GPS FIX 0:No FIX (normal logic)
uint8_t Quality; // GPS Signal quality
uint8_t NewData; // 1:New GPS Data
uint8_t PrintErrors; // 1: To Print GPS Errors (for debug)
int HDOP; // HDOP
};
extern GPS_NMEA_Class GPS;
#endif

42
libraries/GPS_NMEA/examples/GPS_NMEA_test/GPS_NMEA_test.pde
View File

@ -1,42 +0,0 @@
/*
Example of GPS NMEA library.
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <GPS_NMEA.h> // NMEA GPS Library
void setup()
{
Serial.begin(57600);
Serial.println("GPS NMEA library test");
GPS.Init(); // GPS Initialization
delay(1000);
}
void loop()
{
GPS.Read();
if (GPS.NewData) // New GPS data?
{
Serial.print("GPS:");
Serial.print(" Time:");
Serial.print(GPS.Time);
Serial.print(" Fix:");
Serial.print((int)GPS.Fix);
Serial.print(" Lat:");
Serial.print(GPS.Lattitude);
Serial.print(" Lon:");
Serial.print(GPS.Longitude);
Serial.print(" Alt:");
Serial.print(GPS.Altitude/1000.0);
Serial.print(" Speed:");
Serial.print(GPS.Ground_Speed/100.0);
Serial.print(" Course:");
Serial.print(GPS.Ground_Course/100.0);
Serial.println();
GPS.NewData = 0; // We have readed the data
}
delay(20);
}

2
libraries/GPS_NMEA/examples/GPS_NMEA_test/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

18
libraries/GPS_NMEA/keywords.txt
View File

@ -1,18 +0,0 @@
GPS KEYWORD1
GPS_NMEA KEYWORD1
Init KEYWORD2
Read KEYWORD2
Type KEYWORD2
Time KEYWORD2
Lattitude KEYWORD2
Longitude KEYWORD2
Altitude KEYWORD2
Ground_Speed KETWORD2
Ground_Course KEYWORD2
Speed_3d KEYWORD2
NumSats KEYWORD2
Fix KEYWORD2
NewData KEYWORD2
Quality KEYWORD2

274
libraries/GPS_UBLOX/GPS_UBLOX.cpp
View File

@ -1,274 +0,0 @@
/*
GPS_UBLOX.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/2560 (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 : Ublox protocol
Baud rate : 38400
Active messages :
NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV - STATUS Receiver Navigation Status
or
NAV - SOL Navigation Solution Information
Methods:
Init() : GPS Initialization
Read() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
Lattitude : Lattitude * 10,000,000 (long value)
Longitude : Longitude * 10,000,000 (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 FIX, 0: No Fix (normal logic)
*/
#include "GPS_UBLOX.h"
#include <avr/interrupt.h>
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
GPS_UBLOX_Class::GPS_UBLOX_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void GPS_UBLOX_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__) || defined(__AVR_ATmega2560__)
Serial1.begin(38400); // Serial port 1 on ATMega1280/2560
#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_UBLOX_Class::Read(void)
{
static unsigned long GPS_timer = 0;
byte data;
int numc;
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // If AtMega1280/2560 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__) || defined(__AVR_ATmega2560__)
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_checksum(UBX_id);
UBX_step++;
break;
case 4:
UBX_payload_length_hi = data;
ubx_checksum(UBX_payload_length_hi);
UBX_step++;
// We check if the payload lenght is valid...
if (UBX_payload_length_hi >= UBX_MAXPAYLOAD)
{
if (PrintErrors)
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
UBX_step = 0; // Bad data, so restart to step zero and try again.
ck_a = 0;
ck_b = 0;
}
break;
case 5:
UBX_payload_length_lo = data;
ubx_checksum(UBX_payload_length_lo);
UBX_step++;
UBX_payload_counter = 0;
break;
case 6: // Payload data read...
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 7:
UBX_ck_a = data; // First checksum byte
UBX_step++;
break;
case 8:
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_UBLOX_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 0x02: // ID NAV - POSLLH
j = 0;
Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week
j += 4;
Longitude = join_4_bytes(&UBX_buffer[j]); // lon * 10000000
j += 4;
Lattitude = join_4_bytes(&UBX_buffer[j]); // lat * 10000000
j += 4;
//Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm
j += 4;
Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm
Altitude /= 10.;
// Rescale altitude to cm
//j+=4;
/*
hacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
j += 4;
vacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
j += 4;
*/
NewData = 1;
break;
case 0x03: //ID NAV - STATUS
//if(UBX_buffer[4] >= 0x03)
if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01))
Fix = 1; // valid position
else
Fix = 0; // invalid position
break;
case 0x06: //ID NAV - SOL
if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01))
Fix = 1; // valid position
else
Fix = 0; // invalid position
UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]); // Vertical Speed in cm / s
NumSats = UBX_buffer[47]; // Number of sats...
break;
case 0x12: // ID NAV - VELNED
j = 16;
Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm / s
j += 4;
Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm / s
j += 4;
Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg * 100000
Ground_Course /= 1000; // Rescale heading to deg * 100
j += 4;
/*
sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy
j += 4;
headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy
j += 4;
*/
break;
}
}
}
/****************************************************************
*
****************************************************************/
// Join 4 bytes into a long
long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[])
{
union long_union {
int32_t dword;
uint8_t byte[4];
} longUnion;
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword);
}
/****************************************************************
*
****************************************************************/
// Ublox checksum algorithm
void GPS_UBLOX_Class::ubx_checksum(byte ubx_data)
{
ck_a += ubx_data;
ck_b += ck_a;
}
GPS_UBLOX_Class GPS;

50
libraries/GPS_UBLOX/GPS_UBLOX.h
View File

@ -1,50 +0,0 @@
#ifndef GPS_UBLOX_h
#define GPS_UBLOX_h
#include <inttypes.h>
#define UBX_MAXPAYLOAD 60
class GPS_UBLOX_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_UBLOX_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 Speed_3d; //Speed (3-D)
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_UBLOX_Class GPS;
#endif

42
libraries/GPS_UBLOX/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -1,42 +0,0 @@
/*
Example of GPS UBLOX library.
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
Works with Ardupilot Mega Hardware (GPS on Serial Port1)
and with standard ATMega168 and ATMega328 on Serial Port 0
*/
#include <GPS_UBLOX.h> // UBLOX GPS Library
void setup()
{
Serial.begin(57600);
Serial.println("GPS UBLOX library test");
GPS.Init(); // GPS Initialization
delay(1000);
}
void loop()
{
GPS.Read();
if (GPS.NewData) // New GPS data?
{
Serial.print("GPS:");
Serial.print(" Time:");
Serial.print(GPS.Time);
Serial.print(" Fix:");
Serial.print((int)GPS.Fix);
Serial.print(" Lat:");
Serial.print(GPS.Lattitude);
Serial.print(" Lon:");
Serial.print(GPS.Longitude);
Serial.print(" Alt:");
Serial.print(GPS.Altitude/1000.0);
Serial.print(" Speed:");
Serial.print(GPS.Ground_Speed/100.0);
Serial.print(" Course:");
Serial.print(GPS.Ground_Course/100000.0);
Serial.println();
GPS.NewData = 0; // We have readed the data
}
delay(20);
}

2
libraries/GPS_UBLOX/examples/GPS_UBLOX_test/Makefile
View File

@ -1,2 +0,0 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

16
libraries/GPS_UBLOX/keywords.txt
View File

@ -1,16 +0,0 @@
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