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

libraries/GPS_MTK/GPS_MTK.cpp

@@ -0,0 +1,218 @@
+/*
+	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 (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 * 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)
+		NewData : 1 when a new data is received.
+		          You need to write a 0 to NewData when you read the data
+		Fix : 1: GPS NO FIX, 2: 2D FIX, 3: 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)
+{
+	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
+	#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_MTK_Class::Read(void)
+{
+  static unsigned long GPS_timer=0;
+  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(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]); // lon*10000000
+      j+=4;
+      Longitude = join_4_bytes(&UBX_buffer[j]); // 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]);
+      j+=4;
+	  NumSats=UBX_buffer[j];
+	  j++;
+	  Fix=UBX_buffer[j];
+      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;

libraries/GPS_MTK/GPS_MTK.h

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

libraries/GPS_MTK/keywords.txt

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