diff --git a/libraries/GPS_UBLOX/GPS_UBLOX.cpp b/libraries/GPS_UBLOX/GPS_UBLOX.cpp index c255b13b05..b67a76df9e 100644 --- a/libraries/GPS_UBLOX/GPS_UBLOX.cpp +++ b/libraries/GPS_UBLOX/GPS_UBLOX.cpp @@ -1,34 +1,34 @@ /* 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 (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 - 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. + 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 + 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 * 10000000 (long value) - Longitude : Longitude * 10000000 (long value) - Altitude : Altitude * 100 (meters) (long value) - Ground_speed : Speed (m/s) * 100 (long value) + 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 + You need to write a 0 to NewData when you read the data Fix : 1: GPS FIX, 0: No Fix (normal logic) */ @@ -48,16 +48,16 @@ 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... + 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 + Serial1.begin(38400); // Serial port 1 on ATMega1280 #else Serial.begin(38400); #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. void GPS_UBLOX_Class::Read(void) { - static unsigned long GPS_timer=0; - byte data; - int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... + static unsigned long GPS_timer = 0; + byte data; + int numc; + + #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... numc = Serial1.available(); - #else + #else numc = Serial.available(); - #endif - if (numc > 0) - for (int i=0;i 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_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) + 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 - + 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) + 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... + 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) - { + if ((millis() - GPS_timer) > 2000) + { Fix = 0; if (PrintErrors) Serial.println("ERR:GPS_TIMEOUT!!"); - } + } } /**************************************************************** @@ -175,69 +175,71 @@ void GPS_UBLOX_Class::Read(void) // Private Methods ////////////////////////////////////////////////////////////// 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.. //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_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; + 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 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; + 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; - } - } + sacc = join_4_bytes(&UBX_buffer[j]) // Speed accuracy + j += 4; + headacc = join_4_bytes(&UBX_buffer[j]) // Heading accuracy + j += 4; + */ + break; + } + } } @@ -247,16 +249,16 @@ void GPS_UBLOX_Class::parse_ubx_gps(void) // Join 4 bytes into a long long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[]) { - union long_union { + union long_union { int32_t dword; - uint8_t byte[4]; + 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); + longUnion.byte[0] = *Buffer; + longUnion.byte[1] = *(Buffer + 1); + longUnion.byte[2] = *(Buffer + 2); + longUnion.byte[3] = *(Buffer + 3); + return(longUnion.dword); } /**************************************************************** @@ -265,8 +267,8 @@ long GPS_UBLOX_Class::join_4_bytes(unsigned char Buffer[]) // Ublox checksum algorithm void GPS_UBLOX_Class::ubx_checksum(byte ubx_data) { - ck_a+=ubx_data; - ck_b+=ck_a; + ck_a += ubx_data; + ck_b += ck_a; } GPS_UBLOX_Class GPS; \ No newline at end of file