Ardupilot2/libraries/AP_GPS/AP_GPS_UBLOX.cpp

/*
	AP_GPS_UBLOX.cpp - Ublox GPS library for Arduino
	Code by Jordi Mu<EFBFBD>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 : 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
		update() : 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)
		new_data : 1 when a new data is received.
							You need to write a 0 to new_data when you read the data
		fix : 1: GPS FIX, 0: No fix (normal logic)
			
*/

#include "AP_GPS_UBLOX.h"

// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX()
{
}


// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void)
{
	ck_a 		= 0;
	ck_b 		= 0;
	step 		= 0;
	new_data 	= 0;
	fix 		= 0;
	print_errors = 0;
	
	// initialize serial port
	#if defined(__AVR_ATmega1280__)
		Serial1.begin(38400);				 // Serial port 1 on ATMega1280
	#else
		Serial.begin(38400); 
	#endif
}

// optimization : This code don<6F>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_gps() to parse and update the GPS info.
void AP_GPS_UBLOX::update(void)
{
	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(step){
			case 0:
				if(data == 0xB5)
					step++;
				break;
				
			case 1:	
				if(data == 0x62)
					step++;
				else 
					step = 0;
				break;
				
			case 2:
				msg_class = data;
				checksum(msg_class);
				step++;
				break;
				
			case 3:
				id = data;
				checksum(id);
				step++;
				break;
				
			case 4:
				payload_length_hi = data;
				checksum(payload_length_hi);
				step++;
				// We check if the payload lenght is valid...
				if (payload_length_hi >= MAXPAYLOAD){
					if (print_errors)
						Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!");					
					step = 0;	 // Bad data, so restart to step zero and try again.		 
					ck_a = 0;
					ck_b = 0;
				}
				break;
				
			case 5:
				payload_length_lo = data;
				checksum(payload_length_lo);
				step++;
				payload_counter = 0;
				break;
				
			case 6:				 // Payload data read...
				if (payload_counter < payload_length_hi){	// We stay in this state until we reach the payload_length
					buffer[payload_counter] = data;
					checksum(data);
					payload_counter++;
					if (payload_counter == payload_length_hi)
						step++;
				}
				break;
			case 7:
				GPS_ck_a = data;	 // First checksum byte
				step++;
				break;
				
			case 8:
				GPS_ck_b = data;	 // Second checksum byte	 
				// We end the GPS read...
				if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){	 // Verify the received checksum with the generated checksum.. 
					parse_gps();							 // Parse the new GPS packet
				}else{
					if (print_errors) Serial.println("ERR:GPS_CHK!!");
				}
				// Variable initialization
				step = 0;
				ck_a = 0;
				ck_b = 0;
				break;
			}
		}		// End for...
	}
}

// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::parse_gps(void)
{
	int j;
//Verifing if we are in msg_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 msg_class 1, to know more about classes check PAGE 60 of DataSheet.
	if(msg_class == 0x01){
		switch(id) {//Checking the UBX ID
		case 0x02: // ID NAV - POSLLH 
			j = 0;
			time = join_4_bytes(&buffer[j]); // ms Time of week
			j += 4;
			longitude = join_4_bytes(&buffer[j]); // lon * 10,000,000
			j += 4;
			lattitude = join_4_bytes(&buffer[j]); // lat * 10,000,000
			j += 4;
			//altitude = join_4_bytes(&buffer[j]);	// elipsoid heigth mm
			j += 4;
			altitude = (float)join_4_bytes(&buffer[j]) / 10;	// MSL heigth mm
			//j+=4;
			/*
			hacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
			j += 4;
			vacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
			j += 4;
			*/
			new_data = 1;
			break;

		case 0x03: //ID NAV - STATUS 
			//if(buffer[4] >= 0x03)
			if((buffer[4] >= 0x03) && (buffer[5] & 0x01))				
				fix = 1; // valid position				
			else
				fix = 0; // invalid position
			break;

		case 0x06: //ID NAV - SOL
			if((buffer[10] >= 0x03) && (buffer[11] & 0x01))
				fix = 1; // valid position
			else
				fix = 0; // invalid position				
			//ecefVZ = join_4_bytes(&buffer[36]);	// Vertical Speed in cm / s
			num_sats = buffer[47];										// Number of sats...		 
			break;

		case 0x12: // ID NAV - VELNED 
			j = 16;
			speed_3d = join_4_bytes(&buffer[j]); // cm / s
			j += 4;
			ground_speed = join_4_bytes(&buffer[j]); // Ground speed 2D cm / s
			j += 4;
			ground_course = join_4_bytes(&buffer[j]); // Heading 2D deg * 100000
			ground_course /= 1000;	// Rescale heading to deg * 100
			j += 4;
			break; 
		}
	}	 
}

 // Join 4 bytes into a long
long AP_GPS_UBLOX::join_4_bytes(unsigned char Buffer[])
{
	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 AP_GPS_UBLOX::checksum(byte data)
{
	ck_a += data;
	ck_b += ck_a; 
}