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

cleanup 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@316 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-08-26 02:19:30 +00:00
parent 673c547d91
commit 884098b74d
1 changed files with 179 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

356
libraries/GPS_UBLOX/GPS_UBLOX.cpp
View File

@ -1,34 +1,34 @@
/* /*
GPS_UBLOX.cpp - Ublox GPS library for Arduino GPS_UBLOX.cpp - Ublox GPS library for Arduino
Code by Jordi Muñoz and Jose Julio. DIYDrones.com 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 code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1)
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
GPS configuration : Ublox protocol GPS configuration : Ublox protocol
Baud rate : 38400 Baud rate : 38400
Active messages : Active messages :
NAV-POSLLH Geodetic Position Solution, PAGE 66 of datasheet NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV-VELNED Velocity Solution in NED, PAGE 71 of datasheet NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV-STATUS Receiver Navigation Status NAV - STATUS Receiver Navigation Status
or or
NAV-SOL Navigation Solution Information NAV - SOL Navigation Solution Information
Methods: Methods:
Init() : GPS Initialization Init() : GPS Initialization
Read() : Call this funcion as often as you want to ensure you read the incomming gps data Read() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
Lattitude : Lattitude * 10000000 (long value) Lattitude : Lattitude * 10,000,000 (long value)
Longitude : Longitude * 10000000 (long value) Longitude : Longitude * 10,000,000 (long value)
Altitude : Altitude * 100 (meters) (long value) Altitude : Altitude * 100 (meters) (long value)
Ground_speed : Speed (m/s) * 100 (long value) Ground_speed : Speed (m / s) * 100 (long value)
Ground_course : Course (degrees) * 100 (long value) Ground_course : Course (degrees) * 100 (long value)
NewData : 1 when a new data is received. NewData : 1 when a new data is received.
You need to write a 0 to NewData when you read the data You need to write a 0 to NewData when you read the data
Fix : 1: GPS FIX, 0: No Fix (normal logic) Fix : 1: GPS FIX, 0: No Fix (normal logic)
*/ */
@ -48,16 +48,16 @@ GPS_UBLOX_Class::GPS_UBLOX_Class()
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void GPS_UBLOX_Class::Init(void) void GPS_UBLOX_Class::Init(void)
{ {
ck_a=0; ck_a = 0;
ck_b=0; ck_b = 0;
UBX_step=0; UBX_step = 0;
NewData=0; NewData = 0;
Fix=0; Fix = 0;
PrintErrors=0; PrintErrors = 0;
GPS_timer=millis(); //Restarting timer... GPS_timer = millis(); // Restarting timer...
// Initialize serial port // Initialize serial port
#if defined(__AVR_ATmega1280__) #if defined(__AVR_ATmega1280__)
Serial1.begin(38400); // Serial port 1 on ATMega1280 Serial1.begin(38400); // Serial port 1 on ATMega1280
#else #else
Serial.begin(38400); Serial.begin(38400);
#endif #endif
@ -68,105 +68,105 @@ void GPS_UBLOX_Class::Init(void)
// If we get a complete packet this function calls parse_ubx_gps() to parse and update the GPS info. // 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) void GPS_UBLOX_Class::Read(void)
{ {
static unsigned long GPS_timer=0; static unsigned long GPS_timer = 0;
byte data; byte data;
int numc; int numc;
#if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1...
numc = Serial1.available(); numc = Serial1.available();
#else #else
numc = Serial.available(); numc = Serial.available();
#endif #endif
if (numc > 0) if (numc > 0)
for (int i=0;i<numc;i++) // Process bytes received for (int i = 0; i < numc; i++) // Process bytes received
{ {
#if defined(__AVR_ATmega1280__) #if defined(__AVR_ATmega1280__)
data = Serial1.read(); data = Serial1.read();
#else #else
data = Serial.read(); data = Serial.read();
#endif #endif
switch(UBX_step) //Normally we start from zero. This is a state machine switch(UBX_step) // Normally we start from zero. This is a state machine
{ {
case 0: case 0:
if(data==0xB5) // UBX sync char 1 if(data == 0xB5) // UBX sync char 1
UBX_step++; //OH first data packet is correct, so jump to the next step UBX_step++; // OH first data packet is correct, so jump to the next step
break; break;
case 1: case 1:
if(data==0x62) // UBX sync char 2 if(data == 0x62) // UBX sync char 2
UBX_step++; //ooh! The second data packet is correct, jump to the step 2 UBX_step++; // ooh! The second data packet is correct, jump to the step 2
else else
UBX_step=0; //Nop, is not correct so restart to step zero and try again. UBX_step = 0; // Nop, is not correct so restart to step zero and try again.
break; break;
case 2: case 2:
UBX_class=data; UBX_class = data;
ubx_checksum(UBX_class); ubx_checksum(UBX_class);
UBX_step++; UBX_step++;
break; break;
case 3: case 3:
UBX_id=data; UBX_id = data;
ubx_checksum(UBX_id); ubx_checksum(UBX_id);
UBX_step++; UBX_step++;
break; break;
case 4: case 4:
UBX_payload_length_hi=data; UBX_payload_length_hi = data;
ubx_checksum(UBX_payload_length_hi); ubx_checksum(UBX_payload_length_hi);
UBX_step++; UBX_step++;
// We check if the payload lenght is valid... // We check if the payload lenght is valid...
if (UBX_payload_length_hi>=UBX_MAXPAYLOAD) if (UBX_payload_length_hi >= UBX_MAXPAYLOAD)
{ {
if (PrintErrors) if (PrintErrors)
Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!"); Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");
UBX_step=0; //Bad data, so restart to step zero and try again. UBX_step = 0; // Bad data, so restart to step zero and try again.
ck_a=0; ck_a = 0;
ck_b=0; ck_b = 0;
} }
break; break;
case 5: case 5:
UBX_payload_length_lo=data; UBX_payload_length_lo = data;
ubx_checksum(UBX_payload_length_lo); ubx_checksum(UBX_payload_length_lo);
UBX_step++; UBX_step++;
UBX_payload_counter=0; UBX_payload_counter = 0;
break; break;
case 6: // Payload data read... case 6: // Payload data read...
if (UBX_payload_counter < UBX_payload_length_hi) // We stay in this state until we reach the payload_length 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_buffer[UBX_payload_counter] = data;
ubx_checksum(data); ubx_checksum(data);
UBX_payload_counter++; UBX_payload_counter++;
if (UBX_payload_counter==UBX_payload_length_hi) if (UBX_payload_counter == UBX_payload_length_hi)
UBX_step++; UBX_step++;
} }
break; break;
case 7: case 7:
UBX_ck_a=data; // First checksum byte UBX_ck_a = data; // First checksum byte
UBX_step++; UBX_step++;
break; break;
case 8: case 8:
UBX_ck_b=data; // Second checksum byte UBX_ck_b = data; // Second checksum byte
// We end the GPS read... // We end the GPS read...
if((ck_a==UBX_ck_a)&&(ck_b==UBX_ck_b)) // Verify the received checksum with the generated checksum.. 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 parse_ubx_gps(); // Parse the new GPS packet
else else
{ {
if (PrintErrors) if (PrintErrors)
Serial.println("ERR:GPS_CHK!!"); Serial.println("ERR:GPS_CHK!!");
} }
// Variable initialization // Variable initialization
UBX_step=0; UBX_step = 0;
ck_a=0; ck_a = 0;
ck_b=0; ck_b = 0;
GPS_timer=millis(); //Restarting timer... GPS_timer = millis(); // Restarting timer...
break; break;
} }
} // End for... } // End for...
// If we don´t receive GPS packets in 2 seconds => Bad FIX state // If we don´t receive GPS packets in 2 seconds => Bad FIX state
if ((millis() - GPS_timer)>2000) if ((millis() - GPS_timer) > 2000)
{ {
Fix = 0; Fix = 0;
if (PrintErrors) if (PrintErrors)
Serial.println("ERR:GPS_TIMEOUT!!"); Serial.println("ERR:GPS_TIMEOUT!!");
} }
} }
/**************************************************************** /****************************************************************
@ -175,69 +175,71 @@ void GPS_UBLOX_Class::Read(void)
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods //////////////////////////////////////////////////////////////
void GPS_UBLOX_Class::parse_ubx_gps(void) void GPS_UBLOX_Class::parse_ubx_gps(void)
{ {
int j; 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.. //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. //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) if(UBX_class == 0x01)
{ {
switch(UBX_id)//Checking the UBX ID switch(UBX_id) //Checking the UBX ID
{ {
case 0x02: //ID NAV-POSLLH case 0x02: // ID NAV - POSLLH
j=0; j = 0;
Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week Time = join_4_bytes(&UBX_buffer[j]); // ms Time of week
j+=4; j += 4;
Longitude = join_4_bytes(&UBX_buffer[j]); // lon*10000000 Longitude = join_4_bytes(&UBX_buffer[j]); // lon * 10000000
j+=4; j += 4;
Lattitude = join_4_bytes(&UBX_buffer[j]); // lat*10000000 Lattitude = join_4_bytes(&UBX_buffer[j]); // lat * 10000000
j+=4; j += 4;
//Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm //Altitude = join_4_bytes(&UBX_buffer[j]); // elipsoid heigth mm
j+=4; j += 4;
Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm Altitude = (float)join_4_bytes(&UBX_buffer[j]); // MSL heigth mm
Altitude /= 10.; // Rescale altitude to cm Altitude /= 10.;
//j+=4;
/* // Rescale altitude to cm
hacc = (float)join_4_bytes(&UBX_buffer[j])/(float)1000; //j+=4;
j+=4; /*
vacc = (float)join_4_bytes(&UBX_buffer[j])/(float)1000; hacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
j+=4; j += 4;
*/ vacc = (float)join_4_bytes(&UBX_buffer[j]) / (float)1000;
NewData=1; j += 4;
break; */
case 0x03://ID NAV-STATUS NewData = 1;
break;
case 0x03: //ID NAV - STATUS
//if(UBX_buffer[4] >= 0x03) //if(UBX_buffer[4] >= 0x03)
if((UBX_buffer[4] >= 0x03)&&(UBX_buffer[5]&0x01)) if((UBX_buffer[4] >= 0x03) && (UBX_buffer[5] & 0x01))
Fix=1; //valid position Fix = 1; // valid position
else else
Fix=0; //invalid position Fix = 0; // invalid position
break; break;
case 0x06://ID NAV-SOL case 0x06: //ID NAV - SOL
if((UBX_buffer[10] >= 0x03)&&(UBX_buffer[11]&0x01)) if((UBX_buffer[10] >= 0x03) && (UBX_buffer[11] & 0x01))
Fix=1; //valid position Fix = 1; // valid position
else else
Fix=0; //invalid position Fix = 0; // invalid position
UBX_ecefVZ=join_4_bytes(&UBX_buffer[36]); //Vertical Speed in cm/s UBX_ecefVZ = join_4_bytes(&UBX_buffer[36]); // Vertical Speed in cm / s
NumSats=UBX_buffer[47]; //Number of sats... NumSats = UBX_buffer[47]; // Number of sats...
break; break;
case 0x12:// ID NAV-VELNED case 0x12: // ID NAV - VELNED
j=16; j = 16;
Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm/s Speed_3d = join_4_bytes(&UBX_buffer[j]); // cm / s
j+=4; j += 4;
Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm/s Ground_Speed = join_4_bytes(&UBX_buffer[j]); // Ground speed 2D cm / s
j+=4; j += 4;
Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg*100000 Ground_Course = join_4_bytes(&UBX_buffer[j]); // Heading 2D deg * 100000
Ground_Course /= 1000; // Rescale heading to deg * 100 Ground_Course /= 1000; // Rescale heading to deg * 100
j+=4; j += 4;
/* /*
sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy
j+=4; j += 4;
headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy
j+=4; j += 4;
*/ */
break; break;
} }
} }
} }
@ -247,16 +249,16 @@ void GPS_UBLOX_Class::parse_ubx_gps(void)
// Join 4 bytes into a long // Join 4 bytes into a long
long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[]) long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[])
{ {
union long_union { union long_union {
int32_t dword; int32_t dword;
uint8_t byte[4]; uint8_t byte[4];
} longUnion; } longUnion;
longUnion.byte[0] = *Buffer; longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer+1); longUnion.byte[1] = *(Buffer + 1);
longUnion.byte[2] = *(Buffer+2); longUnion.byte[2] = *(Buffer + 2);
longUnion.byte[3] = *(Buffer+3); longUnion.byte[3] = *(Buffer + 3);
return(longUnion.dword); return(longUnion.dword);
} }
/**************************************************************** /****************************************************************
@ -265,8 +267,8 @@ long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[])
// Ublox checksum algorithm // Ublox checksum algorithm
void GPS_UBLOX_Class::ubx_checksum(byte ubx_data) void GPS_UBLOX_Class::ubx_checksum(byte ubx_data)
{ {
ck_a+=ubx_data; ck_a += ubx_data;
ck_b+=ck_a; ck_b += ck_a;
} }
GPS_UBLOX_Class GPS; GPS_UBLOX_Class GPS;