Add:vehicle_demo:Emit synthesized nmea data

git-svn-id: http://svn.code.sf.net/p/navit/code/trunk/navit@5413 ffa7fe5e-494d-0410-b361-a75ebd5db220

1 files changed, 41 insertions, 0 deletions

diff --git a/navit/vehicle/demo/vehicle_demo.c b/navit/vehicle/demo/vehicle_demo.c
index 6c9f1b225..24319a6aa 100644
--- a/navit/vehicle/demo/vehicle_demo.c
+++ b/navit/vehicle/demo/vehicle_demo.c
@@ -19,6 +19,7 @@
 
 #include <glib.h>
 #include <string.h>
+#include <math.h>
 #include "config.h"
 #include "debug.h"
 #include "coord.h"
@@ -47,6 +48,7 @@ struct vehicle_priv {
 	struct callback *timer_callback;
 	struct event_timeout *timer;
 	char *timep;
+	char *nmea;
 
 };
 
@@ -60,10 +62,25 @@ vehicle_demo_destroy(struct vehicle_priv *priv)
 	g_free(priv);
 }
 
+static void
+nmea_chksum(char *nmea)
+{
+	int i;
+	if (nmea && strlen(nmea) > 3) {
+		unsigned char csum=0;
+		for (i = 1 ; i < strlen(nmea)-4 ; i++) 
+			csum^=(unsigned char)(nmea[i]);
+		sprintf(nmea+strlen(nmea)-3,"%02X\n",csum);
+	}
+}
+
 static int
 vehicle_demo_position_attr_get(struct vehicle_priv *priv,
 			       enum attr_type type, struct attr *attr)
 {
+	char ns='N',ew='E',*timep,*rmc,*gga;
+	int hr,min,sec,year,mon,day;
+	double lat,lng;
 	switch (type) {
 	case attr_position_speed:
 		attr->u.numd = &priv->speed;
@@ -85,6 +102,30 @@ vehicle_demo_position_attr_get(struct vehicle_priv *priv,
         case attr_position_sats_used:
                 attr->u.num = 9;
                 break;
+	case attr_position_nmea:
+		lat=priv->geo.lat;
+		if (lat < 0) {
+			lat=-lat;
+			ns='S';
+		}
+		lng=priv->geo.lng;
+		if (lng < 0) {
+			lng=-lng;
+			ew='W';
+		}
+		timep=current_to_iso8601();
+		sscanf(timep,"%d-%d-%dT%d:%d:%d",&year,&mon,&day,&hr,&min,&sec);
+		g_free(timep);
+		gga=g_strdup_printf("$GPGGA,%02d%02d%02d,%02.0f%07.4f,%c,%03.0f%07.4f,%c,1,08,2.5,0,M,,,,0000*  \n",hr,min,sec,floor(lat),(lat-floor(lat))*60.0,ns,floor(lng),(lng-floor(lng))*60,ew);
+		nmea_chksum(gga);
+		rmc=g_strdup_printf("$GPRMC,%02d%02d%02d,A,%02.0f%07.4f,%c,%03.0f%07.4f,%c,%3.1f,%3.1f,%02d%02d%02d,,*  \n",hr,min,sec,floor(lat),(lat-floor(lat))*60.0,ns,floor(lng),(lng-floor(lng))*60,ew,priv->speed/1.852,(double)priv->direction,day,mon,year%100);
+		nmea_chksum(rmc);
+		g_free(priv->nmea);
+		priv->nmea=g_strdup_printf("%s%s",gga,rmc);
+		g_free(gga);
+		g_free(rmc);
+		attr->u.str=priv->nmea;
+		break;
 	default:
 		return 0;
 	}