From 23aa2ea1cd9926abee2bced6260e838a8dd6817f Mon Sep 17 00:00:00 2001
From: sambas <sambas>
Date: Wed, 7 Apr 2010 08:02:44 +0000
Subject: [PATCH] OP-25: lat, lon and alt to float, lat and lon to desimal
 degrees format

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@435 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Modules/GPS/GPS.c         | 40 +++++++++++++---------
 flight/OpenPilot/Modules/GPS/inc/gpsinfo.h |  8 ++---
 flight/OpenPilot/System/openpilot.c        |  7 ++--
 3 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c
index f2a29c23f..b9b70d36d 100644
--- a/flight/OpenPilot/Modules/GPS/GPS.c
+++ b/flight/OpenPilot/Modules/GPS/GPS.c
@@ -284,7 +284,7 @@ void nmeaProcessGPGGA(char* packet)
     char *pEnd;
 
     uint8_t i=0;
-    long deg,desim;
+    long deg,min,desim;
 	double degrees, minutesfrac;
 
 	#ifdef NMEA_DEBUG_GGA
@@ -319,17 +319,21 @@ void nmeaProcessGPGGA(char* packet)
 	// next field: latitude
     // get latitude [ddmm.mmmmm]
 	tokens = strtok_r(NULL, delimiter, &last);
-	if(1) // 5 desimal output
+	if(strlen(tokens)==10)// 5 desimal output
 	{
 		deg=strtol (tokens,&pEnd,10);
+		min=deg%100;
+		deg=deg/100;
 		desim=strtol (pEnd+1,NULL,10);
-		GpsInfo.lat=deg*100000+desim;
+		GpsInfo.lat=deg+(min+desim/100000.0)/60.0; // to desimal degrees
 	}
-	else // 4 desimal output
+	else if(strlen(tokens)==9) // 4 desimal output
 	{
 		deg=strtol (tokens,&pEnd,10);
+		min=deg%100;
+		deg=deg/100;
 		desim=strtol (pEnd+1,NULL,10);
-		GpsInfo.lat=deg*10000+desim;
+		GpsInfo.lat=deg+(min+desim/10000.0)/60.0; // to desimal degrees
 	}
 	// convert to pure degrees [dd.dddd] format
     /*	minutesfrac = modf(GpsInfo.PosLLA.lat.f/100, &degrees);
@@ -340,22 +344,26 @@ void nmeaProcessGPGGA(char* packet)
     // next field: N/S indicator
 	// correct latitute for N/S
 	tokens = strtok_r(NULL, delimiter, &last);
-	//if(*tokens[3] == 'S') GpsInfo.PosLLA.lat.f = -GpsInfo.PosLLA.lat.f;
+	if(tokens[0] == 'S') GpsInfo.lat = -GpsInfo.lat;
 
 	// next field: longitude
-	// get longitude [ddmm.mmmmm]
+	// get longitude [dddmm.mmmmm]
 	tokens = strtok_r(NULL, delimiter, &last);
-	if(1) // 5 desimal output
+	if(strlen(tokens)==11)// 5 desimal output
 	{
 		deg=strtol (tokens,&pEnd,10);
+		min=deg%100;
+		deg=deg/100;
 		desim=strtol (pEnd+1,NULL,10);
-		GpsInfo.lon=deg*100000+desim;
+		GpsInfo.lon=deg+(min+desim/100000.0)/60.0; // to desimal degrees
 	}
-	else // 4 desimal output
+	else if(strlen(tokens)==10) // 4 desimal output
 	{
 		deg=strtol (tokens,&pEnd,10);
+		min=deg%100;
+		deg=deg/100;
 		desim=strtol (pEnd+1,NULL,10);
-		GpsInfo.lon=deg*10000+desim;
+		GpsInfo.lon=deg+(min+desim/10000.0)/60.0; // to desimal degrees
 	}
 
 	// convert to pure degrees [dd.dddd] format
@@ -367,15 +375,15 @@ void nmeaProcessGPGGA(char* packet)
     // next field: E/W indicator
 	// correct latitute for E/W
 	tokens = strtok_r(NULL, delimiter, &last);
-	//if(*tokens[5] == 'W') GpsInfo.PosLLA.lon.f = -GpsInfo.PosLLA.lon.f;
+	if(tokens[0] == 'W') GpsInfo.lon = -GpsInfo.lon;
 
     // next field: position fix status
 	// position fix status
 	// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
 	// check for good position fix
 	tokens = strtok_r(NULL, delimiter, &last);
-    //if(&tokens[6] != '0' || &tokens[6] != 0)
-	//	GpsInfo.PosLLA.updates++;
+    if((tokens[0] != '0') || (tokens[0] != 0))
+		GpsInfo.updates++;
 
     // next field: satellites used
     // get number of satellites used in GPS solution
@@ -389,9 +397,9 @@ void nmeaProcessGPGGA(char* packet)
 	// get altitude (in meters mm.m)
 	tokens = strtok_r(NULL, delimiter, &last);
 	//reuse variables for alt
-	deg=strtol (tokens,&pEnd,10);
+	deg=strtol (tokens,&pEnd,10); // always 0.1m resolution?
 	desim=strtol (pEnd+1,NULL,10);
-	GpsInfo.alt=deg*10+desim;
+	GpsInfo.alt=deg+desim/10.0;
 
 	// next field: altitude units, always 'M'
 	// next field: geoid seperation
diff --git a/flight/OpenPilot/Modules/GPS/inc/gpsinfo.h b/flight/OpenPilot/Modules/GPS/inc/gpsinfo.h
index 0194da4ce..c7ae63c8f 100644
--- a/flight/OpenPilot/Modules/GPS/inc/gpsinfo.h
+++ b/flight/OpenPilot/Modules/GPS/inc/gpsinfo.h
@@ -34,9 +34,9 @@ typedef struct struct_GpsInfo
 {
 	uint8_t numSVs;
 
-	signed long lat;
-	signed long lon;
-	signed int alt;
+	float lat;
+	float lon;
+	float alt;
 	char TimeOfFix[12];
 
 	// make these to values
@@ -45,6 +45,6 @@ typedef struct struct_GpsInfo
 	uint16_t updates;
 
 
-} GpsInfoType;
+} __attribute__((packed)) GpsInfoType;
 
 #endif // GPSINFO_H
diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index 781a43c72..46e5d9ca0 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -145,9 +145,10 @@ static void TaskTick(void *pvParameters)
 
 	for(;;)
 	{
-		/*PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"utc: %s alt:%u\r\n",GpsInfo.TimeOfFix,GpsInfo.alt);
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"lat: %u lon:%u\r\n",GpsInfo.lat,GpsInfo.lon);
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"SV: %u\r\n",GpsInfo.numSVs);
+		/*
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"utc: %s alt:%d\r\n",GpsInfo.TimeOfFix,(int)GpsInfo.alt);
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"lat: %d lon: %d\r\n",(long)(GpsInfo.lat*10000),(long)(GpsInfo.lon*10000));
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"SV: %d\r\n",GpsInfo.numSVs);
 		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"Heading: %s Speed: %s\r\n",GpsInfo.heading,GpsInfo.speed);
 		*/
 		PIOS_LED_Toggle(LED1);