From eaa9d97939d65b8aff72a66a0ae34135e94b469d Mon Sep 17 00:00:00 2001
From: vassilis <vassilis>
Date: Thu, 10 Jun 2010 01:28:06 +0000
Subject: [PATCH] Flight/GPS Renamed GPSObject to PositionActual and
 implemented field for GPS status (NoGPS, NoFix, Fix2D and Fix3D)

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@734 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Makefile                     |  2 +-
 flight/OpenPilot/Modules/GPS/GPS.c            | 75 +++++++++++----
 flight/OpenPilot/UAVObjects/inc/gpsobject.h   | 92 -------------------
 .../OpenPilot/UAVObjects/inc/positionactual.h | 92 +++++++++++++++++++
 .../{gpsobject.c => positionactual.c}         | 20 ++--
 flight/OpenPilot/UAVObjects/uavobjectsinit.c  |  4 +-
 6 files changed, 160 insertions(+), 125 deletions(-)
 delete mode 100644 flight/OpenPilot/UAVObjects/inc/gpsobject.h
 create mode 100644 flight/OpenPilot/UAVObjects/inc/positionactual.h
 rename flight/OpenPilot/UAVObjects/{gpsobject.c => positionactual.c} (79%)

diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile
index 03ea388cd..832dd66f1 100644
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@@ -131,7 +131,7 @@ SRC += $(OPUAVOBJ)/exampleobject1.c
 SRC += $(OPUAVOBJ)/exampleobject2.c
 SRC += $(OPUAVOBJ)/examplesettings.c
 SRC += $(OPUAVOBJ)/objectpersistence.c
-SRC += $(OPUAVOBJ)/gpsobject.c
+SRC += $(OPUAVOBJ)/positionactual.c
 SRC += $(OPUAVOBJ)/gcstelemetrystats.c
 SRC += $(OPUAVOBJ)/flighttelemetrystats.c
 SRC += $(OPUAVOBJ)/systemstats.c
diff --git a/flight/OpenPilot/Modules/GPS/GPS.c b/flight/OpenPilot/Modules/GPS/GPS.c
index 2df32769a..b9b6d2a59 100644
--- a/flight/OpenPilot/Modules/GPS/GPS.c
+++ b/flight/OpenPilot/Modules/GPS/GPS.c
@@ -26,7 +26,7 @@
 #include "openpilot.h"
 #include "buffer.h"
 #include "GPS.h"
-#include "gpsobject.h"
+#include "positionactual.h"
 
 // constants/macros/typdefs
 #define NMEA_BUFFERSIZE		128
@@ -41,6 +41,8 @@
 #define NMEA_GPRMC		6	// Recommended minimum specific GPS data
 #define NMEA_UNKNOWN	0xFF// Packet received but not known
 
+#define GPS_TIMEOUT_MS 500
+
 // Debugging
 
 //#define GPSDEBUG
@@ -79,6 +81,9 @@ static xTaskHandle gpsTaskHandle;
 cBuffer gpsRxBuffer;
 static char gpsRxData[512];
 char NmeaPacket[NMEA_BUFFERSIZE];
+static uint32_t numUpdates;
+static uint32_t numErrors;
+static uint32_t timeOfLastUpdateMs;
 
 /**
  * Initialise the gps module
@@ -88,6 +93,12 @@ char NmeaPacket[NMEA_BUFFERSIZE];
 int32_t GPSInitialize(void)
 {
 	signed portBASE_TYPE xReturn;
+
+	// Init vars
+	numUpdates = 0;
+	numErrors = 0;
+	timeOfLastUpdateMs = 0;
+
 	// TODO: Get gps settings object
 	gpsPort = COM_USART2;
 
@@ -109,6 +120,8 @@ static void gpsTask(void* parameters)
 	int32_t gpsRxOverflow=0;
 	char c;
 	portTickType xDelay = 100 / portTICK_RATE_MS;
+	PositionActualData GpsData;
+	uint32_t timeNowMs;
 
 	// Loop forever
 	while(1)
@@ -126,6 +139,15 @@ static void gpsTask(void* parameters)
 			}
 			nmeaProcess(&gpsRxBuffer);
 		}
+		// Check for GPS timeout
+		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
+		if ( (timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS )
+		{
+			PositionActualGet(&GpsData);
+			GpsData.Status = POSITIONACTUAL_STATUS_NOGPS;
+			PositionActualSet(&GpsData);
+		}
+		// Block task until next update
 		vTaskDelay(xDelay);
 	}
 }
@@ -145,8 +167,8 @@ char nmeaChecksum(char* gps_buffer)
 {
 	char checksum=0;
 	char checksum_received=0;
-	GpsObjectData GpsData;
-	GpsObjectGet(&GpsData);
+	PositionActualData GpsData;
+	PositionActualGet(&GpsData);
 
 	for(int x=0; x<NMEA_BUFFERSIZE; x++)
 	{
@@ -165,14 +187,13 @@ char nmeaChecksum(char* gps_buffer)
 	//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
 	if(checksum == checksum_received)
 	{
-		GpsData.Updates++;
-		GpsObjectSet(&GpsData);
+		++numUpdates;
+		timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
 		return 1;
 	}
 	else
 	{
-		GpsData.Failures++;
-		GpsObjectSet(&GpsData);
+		++numErrors;
 		return 0;
 	}
 }
@@ -300,7 +321,7 @@ uint8_t nmeaProcess(cBuffer* rxBuffer)
  */
 void nmeaProcessGPGGA(char* packet)
 {
-	GpsObjectData GpsData;
+	PositionActualData GpsData;
 
 	char *tokens;
     char *delimiter = ",";
@@ -324,7 +345,7 @@ void nmeaProcessGPGGA(char* packet)
 		return;
 	}
 
-	GpsObjectGet(&GpsData);
+	PositionActualGet(&GpsData);
 	// tokenizer for nmea sentence
 	//GPGGA header
 	tokens = strsep(&packet, delimiter);
@@ -417,7 +438,7 @@ void nmeaProcessGPGGA(char* packet)
 	// next field: DGPS age
 	// next field: DGPS station ID
 	// next field: checksum
-	GpsObjectSet(&GpsData);
+	PositionActualSet(&GpsData);
 }
 
 /**
@@ -426,7 +447,7 @@ void nmeaProcessGPGGA(char* packet)
  */
 void nmeaProcessGPRMC(char* packet)
 {
-	GpsObjectData GpsData;
+	PositionActualData GpsData;
 
 	char *tokens;
     char *delimiter = ",";
@@ -450,7 +471,7 @@ void nmeaProcessGPRMC(char* packet)
 		return;
 	}
 
-	GpsObjectGet(&GpsData);
+	PositionActualGet(&GpsData);
 	// tokenizer for nmea sentence
 	//GPRMC header
 	tokens = strsep(&packet, delimiter);
@@ -517,7 +538,7 @@ void nmeaProcessGPRMC(char* packet)
 	tokens = strsep(&packet, delimiter);
 	deg=strtol (tokens,&pEnd,10);
 	desim=strtol (pEnd+1,NULL,10);
-	GpsData.GroundSpeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
+	GpsData.Groundspeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
 
 	// next field: True course
 	// get True course
@@ -533,7 +554,7 @@ void nmeaProcessGPRMC(char* packet)
 	// next field: Magnetic variation
 	// next field: E or W
 	// next field: checksum
-	GpsObjectSet(&GpsData);
+	PositionActualSet(&GpsData);
 }
 
 
@@ -543,7 +564,7 @@ void nmeaProcessGPRMC(char* packet)
  */
 void nmeaProcessGPVTG(char* packet)
 {
-	GpsObjectData GpsData;
+	PositionActualData GpsData;
 
 	char *tokens;
     char *delimiter = ",";
@@ -562,7 +583,7 @@ void nmeaProcessGPVTG(char* packet)
 		// checksum not valid
 		return;
 	}
-	GpsObjectGet(&GpsData);
+	PositionActualGet(&GpsData);
 	// tokenizer for nmea sentence
 
 	//GPVTG header
@@ -590,7 +611,7 @@ void nmeaProcessGPVTG(char* packet)
 	tokens = strsep(&packet, delimiter);
 	// next field: 'K'
 	// next field: checksum
-	GpsObjectSet(&GpsData);
+	PositionActualSet(&GpsData);
 
 }
 
@@ -600,12 +621,13 @@ void nmeaProcessGPVTG(char* packet)
  */
 void nmeaProcessGPGSA(char* packet)
 {
-	GpsObjectData GpsData;
+	PositionActualData GpsData;
 
 	char *tokens;
     char *delimiter = ",";
     char *pEnd;
     long value,desim;
+    int mode;
 
 	#ifdef NMEA_DEBUG_GSA
 		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
@@ -621,7 +643,7 @@ void nmeaProcessGPGSA(char* packet)
 		// checksum not valid
 		return;
 	}
-	GpsObjectGet(&GpsData);
+	PositionActualGet(&GpsData);
 	// tokenizer for nmea sentence
 
 	//GPGSA header
@@ -633,6 +655,19 @@ void nmeaProcessGPGSA(char* packet)
     // next field: Mode
     // Mode: 1=Fix not available, 2=2D, 3=3D
 	tokens = strsep(&packet, delimiter);
+	mode = atoi(tokens);
+	if (mode == 1)
+	{
+		GpsData.Status = POSITIONACTUAL_STATUS_NOFIX;
+	}
+	else if (mode == 2)
+	{
+		GpsData.Status = POSITIONACTUAL_STATUS_FIX2D;
+	}
+	else if (mode == 3)
+	{
+		GpsData.Status = POSITIONACTUAL_STATUS_FIX3D;
+	}
 
     // next field: 3-14 IDs of SVs used in position fix (null for unused fields)
 	tokens = strsep(&packet, delimiter);
@@ -664,7 +699,7 @@ void nmeaProcessGPGSA(char* packet)
 	desim=strtol (pEnd+1,NULL,10);
 	GpsData.VDOP=value+desim/100.0;
 	// next field: checksum
-	GpsObjectSet(&GpsData);
+	PositionActualSet(&GpsData);
 
 }
 
diff --git a/flight/OpenPilot/UAVObjects/inc/gpsobject.h b/flight/OpenPilot/UAVObjects/inc/gpsobject.h
deleted file mode 100644
index a1335455e..000000000
--- a/flight/OpenPilot/UAVObjects/inc/gpsobject.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       gpsobject.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Implementation of the GpsObject object. This file has been 
- *             automatically generated by the UAVObjectGenerator.
- * 
- * @note       Object definition file: gpsobject.xml. 
- *             This is an automatically generated file.
- *             DO NOT modify manually.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef GPSOBJECT_H
-#define GPSOBJECT_H
-
-// Object constants
-#define GPSOBJECT_OBJID 3887395742U
-#define GPSOBJECT_NAME "GpsObject"
-#define GPSOBJECT_METANAME "GpsObjectMeta"
-#define GPSOBJECT_ISSINGLEINST 1
-#define GPSOBJECT_ISSETTINGS 0
-#define GPSOBJECT_NUMBYTES sizeof(GpsObjectData)
-
-// Object access macros
-#define GpsObjectGet(dataOut) UAVObjGetData(GpsObjectHandle(), dataOut)
-#define GpsObjectSet(dataIn) UAVObjSetData(GpsObjectHandle(), dataIn)
-#define GpsObjectInstGet(instId, dataOut) UAVObjGetInstanceData(GpsObjectHandle(), instId, dataOut)
-#define GpsObjectInstSet(instId, dataIn) UAVObjSetInstanceData(GpsObjectHandle(), instId, dataIn)
-#define GpsObjectConnectQueue(queue) UAVObjConnectQueue(GpsObjectHandle(), queue, EV_MASK_ALL_UPDATES)
-#define GpsObjectConnectCallback(cb) UAVObjConnectCallback(GpsObjectHandle(), cb, EV_MASK_ALL_UPDATES)
-#define GpsObjectCreateInstance() UAVObjCreateInstance(GpsObjectHandle())
-#define GpsObjectRequestUpdate() UAVObjRequestUpdate(GpsObjectHandle())
-#define GpsObjectRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(GpsObjectHandle(), instId)
-#define GpsObjectUpdated() UAVObjUpdated(GpsObjectHandle())
-#define GpsObjectInstUpdated(instId) UAVObjUpdated(GpsObjectHandle(), instId)
-#define GpsObjectGetMetadata(dataOut) UAVObjGetMetadata(GpsObjectHandle(), dataOut)
-#define GpsObjectSetMetadata(dataIn) UAVObjSetMetadata(GpsObjectHandle(), dataIn)
-
-// Object data
-typedef struct {
-    float Latitude;
-    float Longitude;
-    float Altitude;
-    float Heading;
-    float GroundSpeed;
-    int8_t Satellites;
-    uint16_t Updates;
-    uint16_t Failures;
-    float PDOP;
-    float HDOP;
-    float VDOP;
-
-} __attribute__((packed)) GpsObjectData;
-
-// Field information
-// Field Latitude information
-// Field Longitude information
-// Field Altitude information
-// Field Heading information
-// Field GroundSpeed information
-// Field Satellites information
-// Field Updates information
-// Field Failures information
-// Field PDOP information
-// Field HDOP information
-// Field VDOP information
-
-
-// Generic interface functions
-int32_t GpsObjectInitialize();
-UAVObjHandle GpsObjectHandle();
-
-#endif // GPSOBJECT_H
diff --git a/flight/OpenPilot/UAVObjects/inc/positionactual.h b/flight/OpenPilot/UAVObjects/inc/positionactual.h
new file mode 100644
index 000000000..6b01caef3
--- /dev/null
+++ b/flight/OpenPilot/UAVObjects/inc/positionactual.h
@@ -0,0 +1,92 @@
+/**
+ ******************************************************************************
+ *
+ * @file       positionactual.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the PositionActual object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: positionactual.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef POSITIONACTUAL_H
+#define POSITIONACTUAL_H
+
+// Object constants
+#define POSITIONACTUAL_OBJID 981132812U
+#define POSITIONACTUAL_NAME "PositionActual"
+#define POSITIONACTUAL_METANAME "PositionActualMeta"
+#define POSITIONACTUAL_ISSINGLEINST 1
+#define POSITIONACTUAL_ISSETTINGS 0
+#define POSITIONACTUAL_NUMBYTES sizeof(PositionActualData)
+
+// Object access macros
+#define PositionActualGet(dataOut) UAVObjGetData(PositionActualHandle(), dataOut)
+#define PositionActualSet(dataIn) UAVObjSetData(PositionActualHandle(), dataIn)
+#define PositionActualInstGet(instId, dataOut) UAVObjGetInstanceData(PositionActualHandle(), instId, dataOut)
+#define PositionActualInstSet(instId, dataIn) UAVObjSetInstanceData(PositionActualHandle(), instId, dataIn)
+#define PositionActualConnectQueue(queue) UAVObjConnectQueue(PositionActualHandle(), queue, EV_MASK_ALL_UPDATES)
+#define PositionActualConnectCallback(cb) UAVObjConnectCallback(PositionActualHandle(), cb, EV_MASK_ALL_UPDATES)
+#define PositionActualCreateInstance() UAVObjCreateInstance(PositionActualHandle())
+#define PositionActualRequestUpdate() UAVObjRequestUpdate(PositionActualHandle())
+#define PositionActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(PositionActualHandle(), instId)
+#define PositionActualUpdated() UAVObjUpdated(PositionActualHandle())
+#define PositionActualInstUpdated(instId) UAVObjUpdated(PositionActualHandle(), instId)
+#define PositionActualGetMetadata(dataOut) UAVObjGetMetadata(PositionActualHandle(), dataOut)
+#define PositionActualSetMetadata(dataIn) UAVObjSetMetadata(PositionActualHandle(), dataIn)
+
+// Object data
+typedef struct {
+    uint8_t Status;
+    float Latitude;
+    float Longitude;
+    float Altitude;
+    float Heading;
+    float Groundspeed;
+    int8_t Satellites;
+    float PDOP;
+    float HDOP;
+    float VDOP;
+
+} __attribute__((packed)) PositionActualData;
+
+// Field information
+// Field Status information
+/* Enumeration options for field Status */
+typedef enum { POSITIONACTUAL_STATUS_NOGPS=0, POSITIONACTUAL_STATUS_NOFIX=1, POSITIONACTUAL_STATUS_FIX2D=2, POSITIONACTUAL_STATUS_FIX3D=3,  } PositionActualStatusOptions;
+// Field Latitude information
+// Field Longitude information
+// Field Altitude information
+// Field Heading information
+// Field Groundspeed information
+// Field Satellites information
+// Field PDOP information
+// Field HDOP information
+// Field VDOP information
+
+
+// Generic interface functions
+int32_t PositionActualInitialize();
+UAVObjHandle PositionActualHandle();
+
+#endif // POSITIONACTUAL_H
diff --git a/flight/OpenPilot/UAVObjects/gpsobject.c b/flight/OpenPilot/UAVObjects/positionactual.c
similarity index 79%
rename from flight/OpenPilot/UAVObjects/gpsobject.c
rename to flight/OpenPilot/UAVObjects/positionactual.c
index cd6d8f0f5..f5d991e77 100644
--- a/flight/OpenPilot/UAVObjects/gpsobject.c
+++ b/flight/OpenPilot/UAVObjects/positionactual.c
@@ -1,12 +1,12 @@
 /**
  ******************************************************************************
  *
- * @file       gpsobject.c
+ * @file       positionactual.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Implementation of the GpsObject object. This file has been 
+ * @brief      Implementation of the PositionActual object. This file has been 
  *             automatically generated by the UAVObjectGenerator.
  * 
- * @note       Object definition file: gpsobject.xml. 
+ * @note       Object definition file: positionactual.xml. 
  *             This is an automatically generated file.
  *             DO NOT modify manually.
  *
@@ -30,7 +30,7 @@
  */
 
 #include "openpilot.h"
-#include "gpsobject.h"
+#include "positionactual.h"
 
 // Private variables
 static UAVObjHandle handle;
@@ -43,11 +43,11 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId);
  * \return 0 Success
  * \return -1 Failure
  */
-int32_t GpsObjectInitialize()
+int32_t PositionActualInitialize()
 {
 	// Register object with the object manager
-	handle = UAVObjRegister(GPSOBJECT_OBJID, GPSOBJECT_NAME, GPSOBJECT_METANAME, 0,
-			GPSOBJECT_ISSINGLEINST, GPSOBJECT_ISSETTINGS, GPSOBJECT_NUMBYTES, &setDefaults);
+	handle = UAVObjRegister(POSITIONACTUAL_OBJID, POSITIONACTUAL_NAME, POSITIONACTUAL_METANAME, 0,
+			POSITIONACTUAL_ISSINGLEINST, POSITIONACTUAL_ISSETTINGS, POSITIONACTUAL_NUMBYTES, &setDefaults);
 
 	// Done
 	if (handle != 0)
@@ -67,12 +67,12 @@ int32_t GpsObjectInitialize()
  */
 static void setDefaults(UAVObjHandle obj, uint16_t instId)
 {
-	GpsObjectData data;
+	PositionActualData data;
 	UAVObjMetadata metadata;
 
 	// Initialize object fields to their default values
 	UAVObjGetInstanceData(obj, instId, &data);
-	memset(&data, 0, sizeof(GpsObjectData));
+	memset(&data, 0, sizeof(PositionActualData));
 
 	UAVObjSetInstanceData(obj, instId, &data);
 
@@ -93,7 +93,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
 /**
  * Get object handle
  */
-UAVObjHandle GpsObjectHandle()
+UAVObjHandle PositionActualHandle()
 {
 	return handle;
 }
diff --git a/flight/OpenPilot/UAVObjects/uavobjectsinit.c b/flight/OpenPilot/UAVObjects/uavobjectsinit.c
index 645d19482..059d1bb03 100644
--- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c
+++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c
@@ -40,10 +40,10 @@
 #include "examplesettings.h"
 #include "flighttelemetrystats.h"
 #include "gcstelemetrystats.h"
-#include "gpsobject.h"
 #include "manualcontrolcommand.h"
 #include "manualcontrolsettings.h"
 #include "objectpersistence.h"
+#include "positionactual.h"
 #include "stabilizationsettings.h"
 #include "systemalarms.h"
 #include "systemsettings.h"
@@ -69,10 +69,10 @@ void UAVObjectsInitializeAll()
     ExampleSettingsInitialize();
     FlightTelemetryStatsInitialize();
     GCSTelemetryStatsInitialize();
-    GpsObjectInitialize();
     ManualControlCommandInitialize();
     ManualControlSettingsInitialize();
     ObjectPersistenceInitialize();
+    PositionActualInitialize();
     StabilizationSettingsInitialize();
     SystemAlarmsInitialize();
     SystemSettingsInitialize();