Cleanup.

Teach AP_GPS about FastSerial (in the few places it needs to know) and about Stream everywhere else.

Do some minor code cleanup.

Tested with Mega and uBlox.  Some issues (e.g. reporting 0 satelites) remain.



git-svn-id: https://arducopter.googlecode.com/svn/trunk@404 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_GPS/AP_GPS_406.cpp

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 /*
 	GPS_406.cpp - 406 GPS library for Arduino
 	Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com
@@ -16,7 +17,7 @@
 		update() : Call this funcion as often as you want to ensure you read the incomming gps data
 		
 	Properties:
-		Lattitude : Lattitude * 10,000,000 (long value)
+		Latitude : Latitude * 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)
@@ -27,58 +28,42 @@
 			
 */
 
+#include "../FastSerial/FastSerial.h"	// because we need to change baud rates... ugh.
 #include "AP_GPS_406.h"
 #include "WProgram.h"
+#include <Stream.h>
 
 uint8_t AP_GPS_406::buffer[MAXPAYLOAD] = {0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x35,0x37,0x36,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x37,0x0D,0x0A};
 
-
 #define PAYLOAD_LENGTH 92
 #define BAUD_RATE 57600
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_406::AP_GPS_406()
+AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s)
 {
 }
 
-
 // Public Methods ////////////////////////////////////////////////////////////////////
 void AP_GPS_406::init(void)
 {
-	change_to_sirf_protocol();		//Changes to SIFR protocol and sets baud rate
+	change_to_sirf_protocol();		// Changes to SIRF protocol and sets baud rate
-	delay(100); //Waits fot the GPS to start_UP
+	delay(100);						// Waits fot the GPS to start_UP
-	configure_gps(); //Function to configure GPS, to output only the desired msg's
+	configure_gps(); 				// Function to configure GPS, to output only the desired msg's
-
-	step 		= 0;
-	new_data 	= 0;
-	fix 		= 0;
-	print_errors = 0;
-
 }
 
-// optimization : This code don´t wait for data, only proccess the data available
+// optimization : This code doesn´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_406::update(void)
 {
 	byte data;
 	int numc;
-	
+
-	#if defined(__AVR_ATmega1280__)		// If AtMega1280 then Serial port 1...
+	numc = _port->available();
-		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 = _port->read();
-			data = Serial1.read();
-//Serial.print(data,HEX);
-//Serial.println(" ");
-		#else
-			data = Serial.read();
-		#endif
 		
 		switch(step){
 			case 0:
@@ -143,57 +128,27 @@ AP_GPS_406::parse_gps(void)
 {
 	uint8_t j;
 
-	fix = (buffer[1] > 0) ? 0:1;
+	fix = buffer[1] > 0;
 
-	j = 22;
+	latitude     = _swapl(&buffer[22]);	// lat * 10, 000, 000
-	lattitude = join_4_bytes(&buffer[j]); // lat * 10, 000, 000
+	longitude    = _swapl(&buffer[26]);	// lon * 10, 000, 000
+	altitude     = _swapl(&buffer[34]);	// alt in meters * 100
+	ground_speed = _swapi(&buffer[39]);	// meters / second * 100
 
-	j = 26;
+	if (ground_speed >= 50) {			// Only updates data if we are really moving... 
-	longitude = join_4_bytes(&buffer[j]); // lon * 10, 000, 000
+		ground_course = (unsigned int)_swapi(&buffer[41]);	// meters / second * 100
-
-	j = 34;
-	altitude = join_4_bytes(&buffer[j]);	// alt in meters * 100
-	
-	j = 39;
-	ground_speed = join_2_bytes(&buffer[j]);	// meters / second * 100
-
-	if(ground_speed >= 50){
-		//Only updates data if we are really moving... 
-		j = 41;
-		ground_course 		= (unsigned int)join_2_bytes(&buffer[j]);	// meters / second * 100
 	}
 
-	j = 45;
+	//climb_rate = _swapi(&buffer[45]);	// meters / second * 100
-	//climb_rate = join_2_bytes(&buffer[j]);	// meters / second * 100
 
-	if(lattitude == 0){
+	if (latitude == 0) {
 		new_data = false;
-		fix = 0;
+		fix = false;
-	}else{
+	} else {
 		new_data = true;
 	}
 }
 
- // Join 4 bytes into a long
-int32_t
-AP_GPS_406::join_4_bytes(unsigned char Buffer[])
-{
-	longUnion.byte[3] = *Buffer;
-	longUnion.byte[2] = *(Buffer + 1);
-	longUnion.byte[1] = *(Buffer + 2);
-	longUnion.byte[0] = *(Buffer + 3);
-	return(longUnion.dword);
-}
-
-// Join 2 bytes into an int
-int16_t
-AP_GPS_406::join_2_bytes(unsigned char Buffer[])
-{
-	intUnion.byte[1] = *Buffer;
-	intUnion.byte[0] = *(Buffer + 1);
-	return(intUnion.word);
-}
-
 void 
 AP_GPS_406::configure_gps(void)
 {
@@ -201,82 +156,37 @@ AP_GPS_406::configure_gps(void)
 	const uint8_t gps_payload[] 	= {0x02, 0x04, 0x07, 0x09, 0x1B};
 	const uint8_t gps_checksum[] 	= {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
 	const uint8_t gps_ender[]		= {0xB0, 0xB3};
-	const uint8_t cero 				= 0x00;
 	
-	#if defined(__AVR_ATmega1280__)
 	for(int z = 0; z < 2; z++){
 		for(int x = 0; x < 5; x++){
-			for(int y = 0; y < 6; y++){
+			_port->write(gps_header, sizeof(gps_header));	// Prints the msg header, is the same header for all msg..
-				Serial1.print(gps_header[y]); // Prints the msg header, is the same header for all msg..	
+			_port->write(gps_payload[x]);					// Prints the payload, is not the same for every msg
-			}
+			for(int y = 0; y < 6; y++)						// Prints 6 zeros
-			Serial1.print(gps_payload[x]); // Prints the payload, is not the same for every msg
+				_port->write((uint8_t)0);
-			for(int y = 0; y < 6; y++){
+			_port->write(gps_checksum[x]);					// Print the Checksum
-				Serial1.print(cero); // Prints 6 zeros
+			_port->write(gps_ender[0]);						// Print the Ender of the string, is same on all msg's. 
-			}
+			_port->write(gps_ender[1]);						// ender	
-			Serial1.print(gps_checksum[x]); // Print the Checksum
-			Serial1.print(gps_ender[0]);	// Print the Ender of the string, is same on all msg's. 
-			Serial1.print(gps_ender[1]);	// ender	
 		}
 	}	
-	#else
-	for(int z = 0; z < 2; z++){
-		for(int x = 0; x < 5; x++){
-			for(int y = 0; y < 6; y++){
-				Serial.print(gps_header[y]); // Prints the msg header, is the same header for all msg..	
-			}
-			Serial.print(gps_payload[x]); // Prints the payload, is not the same for every msg
-			for(int y = 0; y < 6; y++){
-				Serial.print(cero); // Prints 6 zeros
-			}
-			Serial.print(gps_checksum[x]); // Print the Checksum
-			Serial.print(gps_ender[0]);	// Print the Ender of the string, is same on all msg's. 
-			Serial.print(gps_ender[1]);	// ender	
-		}
-	}	
-	#endif
 }
 
 void 
 AP_GPS_406::change_to_sirf_protocol(void)
 {
-	#if defined(__AVR_ATmega1280__)
+	FastSerial	*fs = (FastSerial *)_port;	// this is a bit grody...
-		Serial1.begin(4800);				 // Serial port 1 on ATMega1280	First try in 4800
+
-		delay(300);
+	fs->begin(4800);				// First try at 4800bps
-		for (byte x = 0; x <= 28; x++){
+	delay(300);
-			Serial1.print(buffer[x]); // Sending special bytes declared at the beginning 
+	_port->write(buffer, 28);		// Sending special bytes declared at the beginning 
-		}
+	delay(300);
-		delay(300);
 	
-		Serial1.begin(9600); // Then try in 9600 
+	fs->begin(9600);				// Then try at 9600bps
-		delay(300);
+	delay(300);
-		for (byte x = 0; x <= 28; x++){
+	_port->write(buffer, 28);
-			Serial1.print(buffer[x]);
+	delay(300);
-		}
-		delay(300);
-		
-		Serial1.begin(BAUD_RATE); // 
-		delay(300);
-		for (byte x = 0; x <= 28; x++){
-			Serial1.print(buffer[x]);
-		}
-		delay(300);
-		
-	#else
-		Serial.begin(4800);				 // Serial port (0) on AT168/328	First try in 4800
-		delay(300);
-		for (byte x = 0; x <= 28; x++){
-			Serial.print(buffer[x]); // Sending special bytes declared at the beginning 
-		}
-		delay(300);
 	
-		Serial.begin(9600); // Then try in 9600 
+	fs->begin(BAUD_RATE);			// Finally try at the configured bit rate (57600?)
-		delay(300);
+	delay(300);
-		for (byte x = 0; x <= 28; x++){
+	_port->write(buffer, 28);
-			Serial.print(buffer[x]);
-		}
-		
-		Serial.begin(BAUD_RATE); // Universal Sincronus Asyncronus Receiveing Transmiting
-		
-	#endif
 }
 

libraries/AP_GPS/AP_GPS_406.h

@@ -1,3 +1,5 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
 #ifndef AP_GPS_406_h
 #define AP_GPS_406_h
 
@@ -6,21 +8,19 @@
 
 class AP_GPS_406 : public GPS
 {
-  public:
+public:
     // Methods
-	AP_GPS_406();
+	AP_GPS_406(Stream *port);
 	void init();
 	void update();
 
-  private:
+private:
     // Internal variables
 	uint8_t step;
 	uint8_t payload_counter;
 	static uint8_t buffer[MAXPAYLOAD];
 	
 	void parse_gps();
-	int32_t join_4_bytes(uint8_t Buffer[]);
-	int16_t join_2_bytes(uint8_t Buffer[]);
 	void change_to_sirf_protocol(void);
 	void configure_gps(void);
 };

libraries/AP_GPS/AP_GPS_IMU.cpp

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 /*
 	GPS_MTK.cpp - Ublox GPS library for Arduino
 	Code by Jordi Muñoz and Jose Julio. DIYDrones.com
@@ -31,7 +32,7 @@
 
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_IMU::AP_GPS_IMU()
+AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
 {
 }
 
@@ -39,19 +40,7 @@ AP_GPS_IMU::AP_GPS_IMU()
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_GPS_IMU::init(void)
 {
-	ck_a			= 0;
+	// we expect the stream to already be open at the corret bitrate
-	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 doesn't wait for data. It only proccess the data available.
@@ -62,21 +51,13 @@ void AP_GPS_IMU::update(void)
 	byte data;
 	int numc = 0;
 	static byte message_num = 0;
-	
+
-	#if defined(__AVR_ATmega1280__)		// If AtMega1280 then Serial port 1...
+	numc = _port->available();
-		numc = Serial.available();
+
-	#else
-		numc = Serial.available();
-	#endif
-	
 	if (numc > 0){
 		for (int i=0;i<numc;i++){	// Process bytes received		
-	
+
-			#if defined(__AVR_ATmega1280__)
+			data = _port->read();
-				data = Serial.read();
-			#else
-				data = Serial.read();
-			#endif
 			
 			switch(step){		 //Normally we start from zero. This is a state machine
 				case 0:	
@@ -154,11 +135,10 @@ void AP_GPS_IMU::update(void)
 						} else if (message_num == 0x0a) {
 							//PERF_join_data();
 						} else {
-							Serial.print("Invalid message number = ");
+							_error("Invalid message number = %d\n", (int)message_num);
-							Serial.println(message_num, DEC);
 						}
 					} else {
-						Serial.println("XXX Checksum error");	//bad checksum
+						_error("XXX Checksum error\n");	//bad checksum
 						//imu_checksum_error_count++;
 					} 						 
 					// Variable initialization
@@ -178,50 +158,34 @@ void AP_GPS_IMU::update(void)
 
 void AP_GPS_IMU::join_data(void)
 {
-	int j = 0;
 	//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. 
 	//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
 
 	 //Storing IMU roll
-	intUnion.byte[0] = buffer[j++];
+	roll_sensor = *(int *)&buffer[0];
-	intUnion.byte[1] = buffer[j++];
-	roll_sensor = intUnion.word;
 
 	 //Storing IMU pitch
-	intUnion.byte[0] = buffer[j++];
+	pitch_sensor = *(int *)&buffer[2];
-	intUnion.byte[1] = buffer[j++];
-	pitch_sensor = intUnion.word;
 
 	 //Storing IMU heading (yaw)
-	intUnion.byte[0] = buffer[j++];
+	ground_course = *(int *)&buffer[4];
-	intUnion.byte[1] = buffer[j++];
-	ground_course = intUnion.word;
 	imu_ok = true;
 }
 
 void AP_GPS_IMU::join_data_xplane()
 {
-	int j = 0;
-
 	 //Storing IMU roll
-	intUnion.byte[0] = buffer[j++];
+	roll_sensor = *(int *)&buffer[0];
-	intUnion.byte[1] = buffer[j++];
+	
-	roll_sensor = intUnion.word;
 
 	 //Storing IMU pitch
-	intUnion.byte[0] = buffer[j++];
+	pitch_sensor = *(int *)&buffer[2];
-	intUnion.byte[1] = buffer[j++];
-	pitch_sensor = intUnion.word;
 
 	 //Storing IMU heading (yaw)
-	intUnion.byte[0] = buffer[j++];
+	ground_course = *(int *)&buffer[4];
-	intUnion.byte[1] = buffer[j++];
-	ground_course = (unsigned int)intUnion.word;
 	
 	 //Storing airspeed
-	intUnion.byte[0] = buffer[j++];
+	airspeed = *(int *)&buffer[6];
-	intUnion.byte[1] = buffer[j++];
-	airspeed = intUnion.word;
 
 	imu_ok = true;
 
@@ -229,50 +193,25 @@ void AP_GPS_IMU::join_data_xplane()
 
 void AP_GPS_IMU::GPS_join_data(void)
 {
-	//gps_messages_received++;
+	longitude = *(long *)&buffer[0];	// degrees * 10e7
-	int j = 0;				 
+	latitude  = *(long *)&buffer[4];
-
-	longitude = join_4_bytes(&buffer[j]);		// Lat and Lon * 10**7
-	j += 4;
-
-	lattitude = join_4_bytes(&buffer[j]);
-	j += 4;
 
 	//Storing GPS Height above the sea level
-	intUnion.byte[0] = buffer[j++];
+	altitude = (long)*(int *)&buffer[8] * 10;
-	intUnion.byte[1] = buffer[j++];
-	altitude = (long)intUnion.word * 10;		 //	Altitude in meters * 100 
 
 	//Storing Speed
-	intUnion.byte[0] = buffer[j++];
+	speed_3d = ground_speed = (float)*(int *)&buffer[10];
-	intUnion.byte[1] = buffer[j++];
-	speed_3d = ground_speed = (float)intUnion.word;			// Speed in M/S * 100
 	
 	//We skip the gps ground course because we use yaw value from the IMU for ground course
-	j += 2;
+	time = *(long *)&buffer[14];
-	time = join_4_bytes(&buffer[j]);		//	Time of Week in milliseconds
 
-	j += 4;
+	imu_health = buffer[15];
-	imu_health = buffer[j++];
 	
-	new_data 	= 1;
+	new_data 	= true;
-	fix 		= 1;
+	fix 		= true;
 }
 
 
-/****************************************************************
- * 
- ****************************************************************/
- // Join 4 bytes into a long
-long AP_GPS_IMU::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);
-}
-
 
 /****************************************************************
  * 

libraries/AP_GPS/AP_GPS_IMU.h

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 #ifndef AP_GPS_IMU_h
 #define AP_GPS_IMU_h
 
@@ -7,8 +8,9 @@
 class AP_GPS_IMU : public GPS
 {
   public:
+
     // Methods
-	AP_GPS_IMU();
+	AP_GPS_IMU(Stream *s);
 	void init();
 	void update();
 	
@@ -37,7 +39,6 @@ class AP_GPS_IMU : public GPS
 	void join_data_xplane();
 	void GPS_join_data();
 	void checksum(unsigned char data);
-	long join_4_bytes(unsigned char Buffer[]);
 };
 
-#endif
+#endif

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 /*
 	GPS_MTK.cpp - Ublox GPS library for Arduino
 	Code by Jordi Munoz and Jose Julio. DIYDrones.com
@@ -31,7 +32,7 @@
 #include "WProgram.h"
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_MTK::AP_GPS_MTK()
+AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
 {
 }
 
@@ -39,45 +40,23 @@ AP_GPS_MTK::AP_GPS_MTK()
 // Public Methods //////////////////////////////////////////////////////////////
 void AP_GPS_MTK::init(void)
 {	
-	ck_a 		= 0;
-	ck_b 		= 0;
-	step 		= 0;
-	new_data 	= 0;
-	fix 		= 0;
-	print_errors = 0;
-	
 	// initialize serial port for binary protocol use
-	#if defined(__AVR_ATmega1280__)
+	_port->print(MTK_SET_BINARY);
-		Serial1.begin(38400);				 // Serial port 1 on ATMega1280
+	_port->print(MTK_OUTPUT_4HZ);
-		Serial1.print(MTK_SET_BINARY);
-		Serial1.print(MTK_OUTPUT_4HZ);
-	#else
-		Serial.begin(38400); 
-		Serial.print(MTK_SET_BINARY);
-		Serial.print(MTK_OUTPUT_4HZ);
-	#endif
 }
 
-// optimization : This code don¥t wait for data, only proccess the data available
+// optimization : This code doesn'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_MTK::update(void)
 {
 	byte data;
 	int numc;
-	
+
-	#if defined(__AVR_ATmega1280__)		// If AtMega1280 then Serial port 1...
+	numc = _port->available();
-		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 = _port->read();
-			data = Serial1.read();
-		#else
-			data = Serial.read();
-		#endif
 		
 		switch(step){
 			case 0:
@@ -126,8 +105,7 @@ void AP_GPS_MTK::update(void)
 				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)
+					_error("ERR:GPS_CHK!!\n");
-						Serial.println("ERR:GPS_CHK!!");
 				}
 				// Variable initialization
 				step = 0;
@@ -143,50 +121,26 @@ void AP_GPS_MTK::update(void)
 void 
 AP_GPS_MTK::parse_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(msg_class == 0x01) {
 		switch(id){
 			//Checking the UBX ID
 			case 0x05: // ID Custom
-				j = 0;
+				latitude      = _swapl(&buffer[0]);
-				lattitude= join_4_bytes(&buffer[j]) * 10; // lon * 10000000
+				longitude     = _swapl(&buffer[4]);
-				j += 4;
+				altitude      = _swapl(&buffer[8]);
-				longitude = join_4_bytes(&buffer[j]) * 10; // lat * 10000000
+				speed_3d      = ground_speed = _swapl(&buffer[12]);
-				j += 4;
+				ground_course = _swapl(&buffer[16]) / 10000;
-				altitude = join_4_bytes(&buffer[j]);	// MSL
+				num_sats      = buffer[20];
-				j += 4;
+				fix           = buffer[21] == 3;
-				speed_3d = ground_speed = join_4_bytes(&buffer[j]);
+				time          = _swapl(&buffer[22]);
-				j += 4;
+				new_data      = true;
-				ground_course = join_4_bytes(&buffer[j]) / 10000;
-				j += 4;
-				num_sats = buffer[j];
-				j++;
-				fix = buffer[j];
-				if (fix==3)
-					fix = 1;
-				else
-					fix = 0;
-				j++;
-				time = join_4_bytes(&buffer[j]);
-				new_data = 1;
 				break;
 		}
 	}
 }
 
- // Join 4 bytes into a long
-long 
-AP_GPS_MTK::join_4_bytes(unsigned char buffer[])
-{
-	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
 AP_GPS_MTK::checksum(byte data)

libraries/AP_GPS/AP_GPS_MTK.h

@@ -1,31 +1,32 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 #ifndef AP_GPS_MTK_h
 #define AP_GPS_MTK_h
 
 #include <GPS.h>
 #define MAXPAYLOAD 32
 
-#define MTK_SET_BINARY      "$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define MTK_SET_BINARY	"$PGCMD,16,0,0,0,0,0*6A\r\n"
-#define MTK_SET_NMEA        "$PGCMD,16,1,1,1,1,1*6B\r\n"
+#define MTK_SET_NMEA	"$PGCMD,16,1,1,1,1,1*6B\r\n"
 
-#define MTK_OUTPUT_1HZ      "$PMTK220,1000*1F\r\n"
+#define MTK_OUTPUT_1HZ	"$PMTK220,1000*1F\r\n"
-#define MTK_OUTPUT_2HZ      "$PMTK220,500*2B\r\n"
+#define MTK_OUTPUT_2HZ	"$PMTK220,500*2B\r\n"
-#define MTK_OUTPUT_4HZ      "$PMTK220,250*29\r\n"
+#define MTK_OUTPUT_4HZ	"$PMTK220,250*29\r\n"
-#define MTK_OTUPUT_5HZ      "$PMTK220,200*2C\r\n"
+#define MTK_OTUPUT_5HZ	"$PMTK220,200*2C\r\n"
-#define MTK_OUTPUT_10HZ     "$PMTK220,100*2F\r\n"
+#define MTK_OUTPUT_10HZ	"$PMTK220,100*2F\r\n"
 
 #define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
 
-#define SBAS_ON             "$PMTK313,1*2E\r\n"
+#define SBAS_ON			"$PMTK313,1*2E\r\n"
-#define SBAS_OFF            "$PMTK313,0*2F\r\n"
+#define SBAS_OFF		"$PMTK313,0*2F\r\n"
 
-#define WAAS_ON             "$PSRF151,1*3F\r\n"
+#define WAAS_ON			"$PSRF151,1*3F\r\n"
-#define WAAS_OFF            "$PSRF151,0*3E\r\n"
+#define WAAS_OFF		"$PSRF151,0*3E\r\n"
 
 class AP_GPS_MTK : public GPS
 {
   public:
     // Methods
-	AP_GPS_MTK();
+	AP_GPS_MTK(Stream *s);
 	void init();
 	void update();
 
@@ -46,6 +47,5 @@ class AP_GPS_MTK : public GPS
 
 	void parse_gps();
 	void checksum(unsigned char data);
-	long join_4_bytes(unsigned char Buffer[]);
 };
 #endif

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 /*
 	GPS_NMEA.cpp - Generic NMEA GPS library for Arduino
 	Code by Jordi Muñoz and Jose Julio. DIYDrones.com
@@ -19,7 +20,7 @@
 		update() : Call this funcion as often as you want to ensure you read the incomming gps data
 		
 	Properties:
-		lattitude : lattitude * 10000000 (long value)
+		latitude : latitude * 10000000 (long value)
 		longitude : longitude * 10000000 (long value)
 		altitude :	altitude * 1000 (milimeters) (long value)
 		ground_speed : Speed (m / s) * 100 (long value)
@@ -40,7 +41,7 @@
 #include "WProgram.h"
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_NMEA::AP_GPS_NMEA()
+AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s)
 {
 }
 
@@ -49,18 +50,6 @@ void
 AP_GPS_NMEA::init(void)
 {
 	//Type = 2;
-	GPS_checksum_calc = false;
-	bufferidx = 0;
-	new_data = 0;
-	fix = 0;
-	quality = 0;
-	print_errors = 0;	
-	// Initialize serial port
-	#if defined(__AVR_ATmega1280__)
-		Serial1.begin(38400);				 // Serial port 1 on ATMega1280
-	#else
-		Serial.begin(38400);
-	#endif
 }
 
 // This code don´t wait for data, only proccess the data available on serial port
@@ -72,20 +61,12 @@ AP_GPS_NMEA::update(void)
 	char c;
 	int numc;
 	int i;
- 
+
-	#if defined(__AVR_ATmega1280__)		// If AtMega1280 then Serial port 1...
+	numc = _port->available();
-		numc = Serial1.available();		
-	#else
-		numc = Serial.available();
-	#endif
 	
 	if (numc > 0){
 		for (i = 0; i < numc; i++){
-			#if defined(__AVR_ATmega1280__)		// If AtMega1280 then Serial port 1...
+			c = _port->read();
-				c = Serial1.read();	 
-			#else
-				c = Serial.read();
-			#endif
 			if (c == '$'){											// NMEA Start
 				bufferidx = 0;
 				buffer[bufferidx++] = c;
@@ -138,10 +119,10 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 				//
 				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)
+				latitude = 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
+					latitude = -1 * latitude;							// South latitudes are negative
 				parseptr = strchr(parseptr, ', ')+1;
 				// W longitudes are Negative
 				aux_deg = parsedecimal(parseptr, 3);			// degrees
@@ -170,8 +151,7 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 				else
 					quality = 4;			// Good (HDOP < 25)
 			} else {
-				if (print_errors)
+				_error("GPSERR: Checksum error!!\n");
-					Serial.println("GPSERR: Checksum error!!");
 			}
 		}
 	} else if (strncmp(buffer,"$GPVTG",6)==0){				// Check if sentence begins with $GPVTG
@@ -190,14 +170,12 @@ AP_GPS_NMEA::parse_nmea_gps(void)
 				ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100)
 				//GPS_line = true;
 			} else {
-				if (print_errors)
+				_error("GPSERR: Checksum error!!\n");
-					Serial.println("GPSERR: Checksum error!!");
 			}
 		}
 	} else {
 		bufferidx = 0;
-		if (print_errors)
+		_error("GPSERR: Bad sentence!!\n");
-			Serial.println("GPSERR: Bad sentence!!");
 	}
 }
 

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 #ifndef AP_GPS_NMEA_h
 #define AP_GPS_NMEA_h
 
@@ -8,7 +9,7 @@ class AP_GPS_NMEA : public GPS
 {
   public:
     // Methods
-	AP_GPS_NMEA();
+	AP_GPS_NMEA(Stream *s);
 	void init();
 	void update();
 
@@ -30,4 +31,4 @@ class AP_GPS_NMEA : public GPS
 
 };
 
-#endif
+#endif

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 /*
 	GPS_UBLOX.cpp - Ublox GPS library for Arduino
 	Code by Jordi Muñoz and Jose Julio. DIYDrones.com
@@ -37,7 +38,7 @@
 #include "WProgram.h"
 
 // Constructors ////////////////////////////////////////////////////////////////
-AP_GPS_UBLOX::AP_GPS_UBLOX()
+AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
 {
 }
 
@@ -45,42 +46,21 @@ 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´t wait for data, only proccess the data available
+// optimization : This code doesn'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 = _port->available();
-		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 = _port->read();
-			data = Serial1.read();
-		#else
-			data = Serial.read();
-		#endif
 		
 		switch(step){
 			case 0:
@@ -113,8 +93,7 @@ void AP_GPS_UBLOX::update(void)
 				step++;
 				// We check if the payload lenght is valid...
 				if (payload_length_hi >= MAXPAYLOAD){
-					if (print_errors)
+					_error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n");
-						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;
@@ -148,7 +127,7 @@ void AP_GPS_UBLOX::update(void)
 				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!!");
+					_error("ERR:GPS_CHK!!\n");
 				}
 				// Variable initialization
 				step = 0;
@@ -164,73 +143,41 @@ void AP_GPS_UBLOX::update(void)
 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 = *(long *)&buffer[0];				// ms Time of week
-			time = join_4_bytes(&buffer[j]); // ms Time of week
+			longitude = *(long *)&buffer[4];		// lon * 10,000,000
-			j += 4;
+			latitude = *(long *)&buffer[8];			// lat * 10,000,000
-			longitude = join_4_bytes(&buffer[j]); // lon * 10,000,000
+			//altitude = *(long *)&buffer[12];		// elipsoid heigth mm
-			j += 4;
+			altitude = *(long *)&buffer[16] / 10;	// MSL heigth mm
-			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;
+			hacc = (float)*(long *)&buffer[20];
-			j += 4;
+			vacc = (float)*(long *)&buffer[24];
-			vacc = (float)join_4_bytes(&buffer[j]) / (float)1000;
-			j += 4;
 			*/
-			new_data = 1;
+			new_data = true;
 			break;
 
 		case 0x03: //ID NAV - STATUS 
-			//if(buffer[4] >= 0x03)
+			fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
-			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 = ((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 = *(long *)&buffer[16];			// cm / s
-			speed_3d = join_4_bytes(&buffer[j]); // cm / s
+			ground_speed = *(long *)&buffer[20];		// Ground speed 2D cm / s
-			j += 4;
+			ground_course = *(long *)&buffer[24] / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
-			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)
 {

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -1,3 +1,4 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
 #ifndef AP_GPS_UBLOX_h
 #define AP_GPS_UBLOX_h
 
@@ -8,7 +9,7 @@ class AP_GPS_UBLOX : public GPS
 {
   public:
     // Methods
-	AP_GPS_UBLOX();
+	AP_GPS_UBLOX(Stream *s);
 	void init();
 	void update();
 
@@ -32,4 +33,4 @@ class AP_GPS_UBLOX : public GPS
 	long join_4_bytes(unsigned char Buffer[]);
 };
 
-#endif
+#endif

libraries/AP_GPS/GPS.h

@@ -1,38 +1,116 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file	GPS.h
+/// @brief	Interface definition for the various GPS drivers.
+
 #ifndef GPS_h
 #define GPS_h
 
 #include <inttypes.h>
+#include <Stream.h>
 
+/// @class	GPS
+/// @brief	Abstract base class for GPS receiver drivers.
 class GPS
 {
-	public:
+public:
-	// Methods
+
-	virtual void init();
+	/// Constructor
-	virtual void update();
+	///
+	/// @note The stream is expected to be set up and configured for the
+	///       correct bitrate before ::init is called.
+	///
+	/// @param	port	Stream connected to the GPS module.
+	///
+	GPS(Stream *port) : _port(port) {};
+
+	/// Startup initialisation.
+	///
+	/// This routine performs any one-off initialisation required to set the
+	/// GPS up for use.
+	///
+	virtual void	init();
+
+	/// Update GPS state based on possible bytes received from the module.
+	///
+	/// This routine must be called periodically to process incoming data.
+	///
+	virtual void	update();
 
 	// Properties
-	long time;					//GPS Millisecond Time of Week
+	long	time;			///< GPS time in milliseconds from the start of the week
-	long lattitude;		 // Geographic coordinates
+	long	latitude;		///< latitude in degrees * 10,000,000
-	long longitude;
+	long	longitude;		///< longitude in degrees * 10,000,000
-	long altitude;
+	long	altitude;		///< altitude in cm
-	long ground_speed;
+	long	ground_speed;	///< ground speed in cm/sec
-	long ground_course;
+	long	ground_course;	///< ground course in 100ths of a degree
-	long speed_3d;
+	long	speed_3d;		///< 3D speed in cm/sec (not always available)
-	uint8_t num_sats;			// Number of visible satelites
+	uint8_t num_sats;		///< Number of visible satelites
-	uint8_t fix;				// 1:GPS FIX	 0:No FIX (normal logic)
+	bool	fix;			///< true if we have a position fix
-	uint8_t new_data;		// 1:New GPS Data
+	
-	uint8_t print_errors; // 1: To Print GPS Errors (for debug)
+	/// Set to true when new data arrives.  A client may set this
-	long GPS_timer;
+	/// to false in order to avoid processing data they have
+	/// already seen.
+	bool	new_data;
 
-	union long_union {
+	bool	print_errors; 	///< if true, errors will be printed to stderr
-		int32_t dword;
-		uint8_t	byte[4];
-	} longUnion;
 
-	union int_union {
+protected:
-		int16_t word;
+	Stream	*_port;			///< stream port the GPS is attached to
-		uint8_t	byte[2];
+
-	} intUnion;
+	/// perform an endian swap on a long
+	///
+	/// @param	bytes		pointer to a buffer containing bytes representing a
+	///						long in the wrong byte order
+	/// @returns			endian-swapped value
+	///
+	long	_swapl(const uint8_t *bytes);
+
+	/// perform an endian swap on an int
+	///
+	/// @param	bytes		pointer to a buffer containing bytes representing an
+	///						int in the wrong byte order
+	///	@returns			endian-swapped value
+	int		_swapi(const uint8_t *bytes);
+
+	/// emit an error message
+	///
+	/// based on the value of print_errors, emits the printf-formatted message
+	/// in msg,... to stderr
+	///
+	/// @param	fmt			printf-like format string
+	///
+	void	_error(const char *msg, ...);
 };
 
+inline long
+GPS::_swapl(const uint8_t *bytes)
+{
+	union {
+		long	v;
+		uint8_t b[4];
+	} u;
+
+	u.b[0] = bytes[3];
+	u.b[1] = bytes[2];
+	u.b[2] = bytes[1];
+	u.b[3] = bytes[0];
+
+	return(u.v);
+}
+
+inline int16_t
+GPS::_swapi(const uint8_t *bytes)
+{
+	union {
+		int16_t	v;
+		uint8_t b[2];
+	} u;
+
+	u.b[0] = bytes[1];
+	u.b[1] = bytes[0];
+
+	return(u.v);
+}
+
 #endif

libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde

@@ -1,23 +1,28 @@
 /*
-	Example of GPS MTK library.
+	Example of GPS 406 library.
 	Code by Jordi Munoz 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 <AP_GPS_406.h> // UBLOX GPS Library
+#include <FastSerial.h>
+#include <AP_GPS_406.h>
+#include <stdio.h>
 
-AP_GPS_406 gps;
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+
+AP_GPS_406 gps(&Serial1);
 #define T6 1000000
 #define T7 10000000
 
 void setup()
 {
 	Serial.begin(38400);
-	Serial.println("Switching to 57600 Baud");
+	Serial1.begin(57600);
-	delay(500);
+	stderr = stdout;
-	Serial.begin(57600);
+	gps.print_errors = true;
+
 	Serial.println("GPS 406 library test");
 	gps.init();	 // GPS Initialization
 	delay(1000);
@@ -29,7 +34,7 @@ void loop()
 	if (gps.new_data){
 		Serial.print("gps:");
 		Serial.print(" Lat:");
-		Serial.print((float)gps.lattitude / T7, DEC);
+		Serial.print((float)gps.latitude / T7, DEC);
 		Serial.print(" Lon:");
 		Serial.print((float)gps.longitude / T7, DEC);
 		Serial.print(" Alt:");

libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde

@@ -3,18 +3,26 @@
 	Code by Jordi Munoz 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 <AP_GPS_MTK.h> // UBLOX GPS Library
+#include <FastSerial.h>
+#include <AP_GPS_MTK.h>
+#include <stdio.h>
 
-AP_GPS_MTK gps;
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+
+AP_GPS_MTK gps(&Serial);
 #define T6 1000000
 #define T7 10000000
 
 void setup()
 {
 	Serial.begin(38400);
+	Serial1.begin(57600);
+	stderr = stdout;
+	gps.print_errors = true;
+
 	Serial.println("GPS MTK library test");
 	gps.init();	 // GPS Initialization
 	delay(1000);
@@ -26,7 +34,7 @@ void loop()
 	if (gps.new_data){
 		Serial.print("gps:");
 		Serial.print(" Lat:");
-		Serial.print((float)gps.lattitude / T7, DEC);
+		Serial.print((float)gps.latitude / T7, DEC);
 		Serial.print(" Lon:");
 		Serial.print((float)gps.longitude / T7, DEC);
 		Serial.print(" Alt:");

libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde

@@ -1,20 +1,28 @@
 /*
-	Example of GPS MTK library.
+	Example of GPS NMEA library.
 	Code by Jordi Munoz 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 <AP_GPS_NMEA.h> // UBLOX GPS Library
+#include <FastSerial.h>
+#include <AP_GPS_NMEA.h>
+#include <stdio.h>
 
-AP_GPS_NMEA gps;
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+
+AP_GPS_NMEA gps(&Serial1);
 #define T6 1000000
 #define T7 10000000
 
 void setup()
 {
-	Serial.begin(57600);
+	Serial.begin(38400);
+	Serial1.begin(38400);
+	stderr = stdout;
+	gps.print_errors = true;
+
 	Serial.println("GPS NMEA library test");
 	gps.init();	 // GPS Initialization
 	delay(1000);
@@ -27,7 +35,7 @@ void loop()
 	if (gps.new_data){
 		Serial.print("gps:");
 		Serial.print(" Lat:");
-		Serial.print((float)gps.lattitude / T7, DEC);
+		Serial.print((float)gps.latitude / T7, DEC);
 		Serial.print(" Lon:");
 		Serial.print((float)gps.longitude / T7, DEC);
 		Serial.print(" Alt:");

libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde

@@ -1,20 +1,28 @@
 /*
-	Example of GPS MTK library.
+	Example of GPS UBlox library.
 	Code by Jordi Munoz 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 <AP_GPS_UBLOX.h> // UBLOX GPS Library
+#include <FastSerial.h>
+#include <AP_GPS_UBLOX.h>
+#include <stdio.h>
 
-AP_GPS_UBLOX gps;
+FastSerialPort0(Serial);
+FastSerialPort1(Serial1);
+
+AP_GPS_UBLOX gps(&Serial1);
 #define T6 1000000
 #define T7 10000000
 
 void setup()
 {
 	Serial.begin(38400);
+	Serial1.begin(38400);
+	stderr = stdout;
+	gps.print_errors = true;
+
 	Serial.println("GPS UBLOX library test");
 	gps.init();	 // GPS Initialization
 	delay(1000);
@@ -26,7 +34,7 @@ void loop()
 	if (gps.new_data){
 		Serial.print("gps:");
 		Serial.print(" Lat:");
-		Serial.print((float)gps.lattitude / T7, DEC);
+		Serial.print((float)gps.latitude / T7, DEC);
 		Serial.print(" Lon:");
 		Serial.print((float)gps.longitude / T7, DEC);
 		Serial.print(" Alt:");