diff options
| -rw-r--r-- | navit/vehicle/demo/vehicle_demo.c | 41 |
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; } |