+ Garmin 18x USB support

src/Makefile

@@ -9,9 +9,9 @@ INSTDOCS =  0644
 INSTOWNER = root
 INSTGROUP = root
 
-PROGS     = nmea2exif log_imu
-SRCS =      nmea2exif.c log_imu.c
-OBJS =      nmea2exif.o
+PROGS     = nmea2exif log_imu garminusb2nmea garminusb2exif
+SRCS =      nmeagen.c exifgen.c garminusb2nmea.c garminusb2exif.c nmea2exif.c log_imu.c
+OBJS =      nmeagen.o exifgen.o garminusb2nmea.o garminusb2exif.o nmea2exif.o
 PHPSCRIPTS= start_gps_compass.php
 PHPWEB=     logger_launcher.php imu_setup.php read_imu_log.php
 
@@ -26,6 +26,12 @@ log_imu: log_imu.o
 nmea2exif: nmea2exif.o
 	$(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@
 
+garminusb2nmea: garminusb2nmea.o nmeagen.o
+	$(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@
+
+garminusb2exif: garminusb2exif.o exifgen.o
+	$(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@
+
 install:	$(PROGS)
 	$(INSTALL) $(OWN) -d $(DESTDIR)
 	$(INSTALL) -d $(DESTDIR)$(BINDIR)

src/exifgen.c

+/*
+exifgen.c
+   Garmin protocol to Exif GPS data conevrter
+   exif_getutc() function is based on nmea_getutc(), found in
+   nmeagen.c:
+	Garmin protocol to NMEA 0183 converter
+	Copyright (C) 2004 Manuel Kasper <mk@neon1.net>.
+	All rights reserved.
+
+*/
+
+#include "garmin.h"
+#include "nmeagen.h"
+#include "exifgen.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+void exif_init_meta(struct meta_GPSInfo_t *meta) {
+   meta->GPSLatitudeRef=       'N';
+   meta->GPSLatitude_deg_nom=   __cpu_to_be32((int) 0);
+   meta->GPSLatitude_deg_denom= __cpu_to_be32((int) 1);
+   meta->GPSLatitude_min_nom=   __cpu_to_be32((int) 0);
+   meta->GPSLatitude_min_denom= __cpu_to_be32((int) EXIF_GPS_MIN_DENOM);
+   meta->GPSLongitudeRef=      'W';
+   meta->GPSLongitude_deg_nom=  __cpu_to_be32((int) 0);
+   meta->GPSLongitude_deg_denom=__cpu_to_be32((int) 1);
+   meta->GPSLongitude_min_nom=  __cpu_to_be32((int) 0);
+   meta->GPSLongitude_min_denom=__cpu_to_be32((int) EXIF_GPS_MIN_DENOM);
+   meta->GPSAltitudeRef=        0; //byte, not ascii 0 - above sea level, 1 - below
+   meta->GPSAltitude_nom=       __cpu_to_be32((int) 0);   //in meters
+   meta->GPSAltitude_denom=     __cpu_to_be32((int) EXIF_GPS_METERS_DENOM);
+   meta->GPSTimeStamp_hrs_nom=  __cpu_to_be32((int) 0);
+   meta->GPSTimeStamp_hrs_denom=__cpu_to_be32((int) 1);
+   meta->GPSTimeStamp_min_nom=  __cpu_to_be32((int) 0);
+   meta->GPSTimeStamp_min_denom=__cpu_to_be32((int) 1);
+   meta->GPSTimeStamp_sec_nom=  __cpu_to_be32((int) 0);
+   meta->GPSMeasureMode=        '0';
+   meta->GPSTimeStamp_sec_denom=__cpu_to_be32((int) EXIF_GPS_TIMESEC_DENOM);
+// meta->GPSDateStamp[10]; //1 less than defined - no '\0';
+
+}
+
+void exif_getutc(D800_Pvt_Data_Type *pvt,  struct meta_GPSInfo_t *meta) { // char *utctime, char *utcdate) {
+   int      tmp = 0, dtmp=0;
+   int      frac_sec;
+   /* UTC time of position fix 
+      Reminder:
+         pvt->tow = seconds (including fractions) since the start of the week.
+         pvt->wn_days = days since 31-DEC-1989 for the start of the current week
+           (neither is adjusted for leap seconds)
+         pvt->leap_scnds = leap second adjustment required.
+   */
+   tmp = (pvt->tow + 0.5/EXIF_GPS_TIMESEC_DENOM);
+   frac_sec = ((pvt->tow + 0.5/EXIF_GPS_TIMESEC_DENOM) - tmp)*EXIF_GPS_TIMESEC_DENOM +0.5;
+
+   dtmp = pvt->wn_days;
+
+   /* 
+      If the result is 604800, it's really the first sample
+      of the new week, so zero out tmp and increment dtmp 
+      by a week ( 7 days ).
+   */
+   if (tmp >= 604800)   
+   {
+      dtmp += 7;
+      tmp = 0;
+   }
+   /*
+      At this point we have tmp = seconds since the start
+      of the week, and dtmp = the first day of the week.
+      We now need to correct for leap seconds.  This may actually
+      result in reversing the previous adjustment but the code
+      required to combine the two operations wouldn't be clear.
+   */
+   tmp -= pvt->leap_scnds;
+   if (tmp < 0)
+   {
+      tmp+= 604800;
+      dtmp -= 7;
+   }
+
+   /*
+      Now we have tmp = seconds since the start if the week, 
+      and dtmp = the first day of the week, all corrected for
+      rounding and leap seconds.
+
+      We now convert dtmp to today's day number and tmp to
+      seconds since midnignt. 
+   */
+
+   dtmp += (tmp / 86400);
+   tmp %= 86400;
+// calculate time
+   int h, m, s;
+   h = tmp / 3600;
+   m = (tmp - h*3600) / 60;
+   s = ((tmp - h*3600 - m*60) * EXIF_GPS_TIMESEC_DENOM)+frac_sec;
+
+   meta->GPSTimeStamp_hrs_nom=  __cpu_to_be32((int) h);
+   meta->GPSTimeStamp_min_nom=  __cpu_to_be32((int) m);
+   meta->GPSTimeStamp_sec_nom=  __cpu_to_be32((int) s);
+
+      /* Garmin format: number of days since December 31, 1989 */
+   unsigned long jd = dtmp + 2447892;
+   unsigned long w, x, a, b, c, d, e, f;
+   unsigned long day, month, year;
+
+   w = (unsigned long)((jd - 1867216.25)/36524.25);
+   x = w/4;
+   a = jd + 1 + w - x;
+   b = a + 1524;
+   c = (unsigned long)((b - 122.1)/365.25);
+   d = (unsigned long)(365.25 * c);
+   e = (unsigned long)((b-d)/30.6001);
+   f = (unsigned long)(30.6001 * e);
+
+   day = b - d - f;
+   month = e - 1;
+   if (month > 12)                month -= 12;
+   year = c - 4716;
+   if (month == 1 || month == 2)  year++;
+//   year -= 2000;
+   sprintf(meta->GPSDateStamp, "%04ld:%02ld:%02ld", year, month,day);
+}
+
+
+int exif_gen(D800_Pvt_Data_Type *pvt, cpo_sat_data *sat, struct meta_GPSInfo_t *meta) {
+   int deg,m_scaled;
+   exif_getutc( pvt, meta);
+   meta->GPSMeasureMode= (pvt->fix >= 4)?'3':'2'; // '0' is not allowed
+   /// latitude degrees, minutes/10000
+   m_scaled=rad2deg(pvt->lat)*60*EXIF_GPS_MIN_DENOM +0.5;
+   meta->GPSLatitudeRef= (m_scaled >=0)?'N':'S';
+   if (m_scaled<0) m_scaled=-m_scaled;
+   deg     = m_scaled/(60*EXIF_GPS_MIN_DENOM);
+   m_scaled -= deg*(60*EXIF_GPS_MIN_DENOM);
+   meta->GPSLatitude_deg_nom=   __cpu_to_be32(deg);
+   meta->GPSLatitude_min_nom=   __cpu_to_be32(m_scaled);
+   /// longitude degrees, minutes/10000
+   m_scaled=rad2deg(pvt->lon)*60*EXIF_GPS_MIN_DENOM +0.5;
+   meta->GPSLongitudeRef= (m_scaled >=0)?'E':'W';
+   if (m_scaled<0) m_scaled=-m_scaled;
+   deg     = m_scaled/(60*EXIF_GPS_MIN_DENOM);
+   m_scaled -= deg*(60*EXIF_GPS_MIN_DENOM);
+   meta->GPSLongitude_deg_nom=   __cpu_to_be32(deg);
+   meta->GPSLongitude_min_nom=   __cpu_to_be32(m_scaled);
+   ///altitude
+   m_scaled= (pvt->msl_hght + pvt->alt)*EXIF_GPS_METERS_DENOM +0.5;
+   meta->GPSAltitudeRef= (m_scaled >=0)? 0 : 1;
+   if (m_scaled<0) m_scaled=-m_scaled;
+   meta->GPSAltitude_nom=       __cpu_to_be32(m_scaled);   //in meters
+   return 0;
+}
+

src/exifgen.h

+/*
+exifgen.h
+*/
+#ifndef EXIFGEN_H
+#define EXIFGEN_H
+//#include <asm/elphel/exifa.h>
+#include <elphel/exifa.h>
+#include <asm/byteorder.h> //endians
+
+#include "garmin.h"
+
+#define KNOTS_TO_KMH	1.852
+#define G_PI			3.14159265358979324
+#define rad2deg(x)		((x) * 180.0 / G_PI)
+#define EXIF_GPS_MIN_DENOM     10000 //4 digits after decimal point
+#define EXIF_GPS_METERS_DENOM     10 //1 digit after decimal point
+#define EXIF_GPS_TIMESEC_DENOM  1000 //3 digits after decimal point
+void exif_init_meta(struct meta_GPSInfo_t *meta);
+int exif_gen(D800_Pvt_Data_Type *pvt, cpo_sat_data *sat, struct meta_GPSInfo_t *meta);
+
+#endif

src/garmin.h

+/*
+	Garmin protocol to NMEA 0183 converter
+	Copyright (C) 2004 Manuel Kasper <mk@neon1.net>.
+	All rights reserved.
+
+	Input:
+		- D800_Pvt_Data_Type (PID 51)
+		- satellite data record (PID 114)
+
+	Available output sentences:
+		GPGGA, GPRMC, GPGLL, GPGSA, GPGSV
+
+	Known caveats:
+		- DOP (Dilution of Precision) information not available
+		  (Garmin protocol includes EPE only)
+		- DGPS information in GPGGA sentence not returned
+		- speed and course over ground are calculated from the
+		  north/east velocity and may not be accurate
+		- magnetic variation information not available
+		- Garmin 16-bit SNR scale unknown
+
+	---------------------------------------------------------------------------
+	
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+	
+	1. Redistributions of source code must retain the above copyright notice,
+	   this list of conditions and the following disclaimer.
+	
+	2. Redistributions in binary form must reproduce the above copyright
+	   notice, this list of conditions and the following disclaimer in the
+	   documentation and/or other materials provided with the distribution.
+	
+	THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+	INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+	AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+	AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+	OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+	SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+	INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+	CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+	ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+	POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef GARMIN_H
+#define GARMIN_H
+
+#ifdef _WIN32
+typedef unsigned char u_int8_t;
+typedef unsigned short u_int16_t;
+typedef short int16_t;
+typedef unsigned long u_int32_t;
+typedef long int32_t;
+#else
+#include <sys/types.h>
+#endif
+
+#pragma pack(push, 1)
+
+typedef struct {
+    u_int8_t	tag;
+	u_int16_t	data;
+} Protocol_Data_Type;
+
+typedef struct {
+	float	alt;
+	float	epe;
+	float	eph;
+	float	epv;
+	int16_t	fix;
+	double	tow;
+	double	lat;
+	double	lon;
+	float	east;
+	float	north;
+	float	up;
+	float	msl_hght;
+	int16_t	leap_scnds;
+	int32_t	wn_days;
+} D800_Pvt_Data_Type;
+
+typedef struct {
+	u_int8_t	svid;
+	u_int16_t	snr;
+	u_int8_t	elev;
+	u_int16_t	azmth;
+	u_int8_t	status;
+} cpo_sat_data;
+
+typedef struct {
+	u_int32_t	cycles;
+	double		pr;
+	u_int16_t	phase;
+	u_int8_t	slp_dtct;
+	u_int8_t	snr_dbhz;
+	u_int8_t	svid;
+	u_int8_t	valid;
+} cpo_rcv_sv_data;
+
+typedef struct {
+	double			rcvr_tow;
+	u_int16_t		rcvr_wn;
+	cpo_rcv_sv_data	sv[12];
+} cpo_rcv_data;
+
+#pragma pack(pop)
+
+enum {
+	Pid_Command_Data = 10,
+	Pid_Xfer_Cmplt = 12,
+	Pid_Date_Time_Data = 14,
+	Pid_Position_Data = 17,
+	Pid_Prx_Wpt_Data = 19,
+	Pid_Records = 27,
+	Pid_Rte_Hdr = 29,
+	Pid_Rte_Wpt_Data = 30,
+	Pid_Almanac_Data = 31,
+	Pid_Trk_Data = 34,
+	Pid_Wpt_Data = 35,
+	Pid_Pvt_Data = 51,
+	Pid_RMR_Data = 52,
+	Pid_Rte_Link_Data = 98,
+	Pid_Trk_Hdr = 99,
+	Pid_SatData_Record = 114,
+	Pid_FlightBook_Record = 134,
+	Pid_Lap = 149
+};
+
+enum {
+	Cmnd_Abort_Transfer = 0, /* abort current transfer */
+	Cmnd_Transfer_Alm = 1, /* transfer almanac */
+	Cmnd_Transfer_Posn = 2, /* transfer position */
+	Cmnd_Transfer_Prx = 3, /* transfer proximity waypoints */
+	Cmnd_Transfer_Rte = 4, /* transfer routes */
+	Cmnd_Transfer_Time = 5, /* transfer time */
+	Cmnd_Transfer_Trk = 6, /* transfer track log */
+	Cmnd_Transfer_Wpt = 7, /* transfer waypoints */
+	Cmnd_Turn_Off_Pwr = 8, /* turn off power */
+	Cmnd_Start_Pvt_Data = 49, /* start transmitting PVT data */
+	Cmnd_Stop_Pvt_Data = 50, /* stop transmitting PVT data */
+	Cmnd_FlightBook_Transfer = 92, /* start transferring flight records */
+	Cmnd_Start_RMR = 110, /* start transmitting Receiver Measurement Records */
+	Cmnd_Stop_RMR = 111, /* start transmitting Receiver Measurement Records */
+	Cmnd_Transfer_Laps = 117 /* transfer laps */
+};
+
+#endif

src/garminusb2exif.c

+/*
+ Garmin USB data source for Elphel Model 353 camera.
+ Modified from
+	Garmin USB NMEA converter
+	Copyright (C) 2004 Manuel Kasper <mk@neon1.net>.
+	All rights reserved.
+
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+
+	1. Redistributions of source code must retain the above copyright notice,
+	   this list of conditions and the following disclaimer.
+
+	2. Redistributions in binary form must reproduce the above copyright
+	   notice, this list of conditions and the following disclaimer in the
+	   documentation and/or other materials provided with the distribution.
+
+	THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+	INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+	AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+	AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+	OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+	SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+	INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+	CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+	ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+	POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include "garmin.h"
+#include "nmeagen.h"
+#include "exifgen.h"
+//#include <asm/elphel/exifa.h>
+#include <elphel/x393_devices.h>
+#include <elphel/c313a.h>
+
+#define GARMIN_HEADER_SIZE  12
+#define GARMIN_MAX_PKTSIZE  512
+#define PKTBUF_SIZE         4097
+#define D(x)
+//#define D(x) x
+#define D1(x)
+//#define D1(x) x
+
+#pragma pack(push, 1)
+typedef struct {
+  u_int8_t    mPacketType;
+  u_int8_t    mReserved1;
+  u_int16_t   mReserved2;
+  u_int16_t   mPacketId;
+  u_int16_t   mReserved3;
+  u_int32_t   mDataSize;
+  u_int8_t    mData[1];
+} Packet_t; /// 13 bytes
+#pragma pack(pop)
+
+//#define EXIF_DEV "/dev/exif_meta"
+const char *exif_paths[SENSOR_PORTS] = {
+        DEV393_PATH(DEV393_EXIF_META0),
+        DEV393_PATH(DEV393_EXIF_META1),
+        DEV393_PATH(DEV393_EXIF_META2),
+        DEV393_PATH(DEV393_EXIF_META3)};
+
+int gps_fd;
+char pktbuf[PKTBUF_SIZE];
+int pktbuf_head = 0, pktbuf_tail = 0;
+
+D800_Pvt_Data_Type    lastpvt;         /// 64 bytes
+cpo_sat_data          lastsatdata[12]; /// 84 bytes
+int satdata_valid = 0;
+
+/**
+ * @brief return currenf used buffer size
+ * @return current buffer size
+ */
+int pktbuf_size() {
+  return (PKTBUF_SIZE - pktbuf_head + pktbuf_tail) % PKTBUF_SIZE;
+}
+
+/**
+ * @brief Read byte from buffer, keep buffer pointers
+ * @param buf buffer to read data to (will be limited to buffer data if requested more)
+ * @param n   number of bytes to read
+ * @return number of bytes actually read
+ */
+int pktbuf_peek(char *buf, int n) {
+
+  int i;
+  int mypktbuf_head = pktbuf_head;
+
+  for (i = 0; (i < n) && (mypktbuf_head != pktbuf_tail); i++) {
+    buf[i] = pktbuf[mypktbuf_head];
+    mypktbuf_head++;
+    if (mypktbuf_head == PKTBUF_SIZE){
+      mypktbuf_head = 0;
+    }
+  }
+
+  return i;
+
+}
+
+/**
+ * @brief Read byte from buffer, advance buffer head
+ * @param buf buffer to read data to (will be limited to buffer data if requested more)
+ * @param n   number of bytes to read
+ * @return number of bytes actually read
+ */
+int pktbuf_deq(char *buf, int n) {
+
+  int i;
+
+  //fprintf(stderr, "pktbuf_deq:pktbuf_size()=%d\n",pktbuf_size());
+  for (i = 0; (i < n) && (pktbuf_head != pktbuf_tail); i++) {
+    buf[i] = pktbuf[pktbuf_head];
+    pktbuf_head++;
+    if (pktbuf_head == PKTBUF_SIZE){
+      pktbuf_head = 0;
+    }
+  }
+
+  return i;
+
+}
+
+/**
+ * @brief Put bytes to buffer, advance buffer tail
+ * @param buf buffer to write data from
+ * @param n   number of bytes to enqueue
+ * @return number of bytes actually written
+ */
+int pktbuf_enq(char *buf, int n) {
+
+  int i;
+
+  //fprintf(stderr, "pktbuf_enq:pktbuf_size()=%d\n",pktbuf_size());
+  if (pktbuf_size() + n >= PKTBUF_SIZE){
+    return 0;
+  }
+
+  for (i = 0; i < n; i++) {
+    pktbuf[pktbuf_tail] = buf[i];
+    pktbuf_tail++;
+    if (pktbuf_tail == PKTBUF_SIZE){
+      pktbuf_tail = 0;
+    }
+  }
+
+  return i;
+
+}
+
+/**
+ * @brief Send packet to GPS
+ * @param pack packet data, variable size
+ * @return  0 - OK, 1 - error
+ */
+int sendpacket(Packet_t *pack) {
+
+  int nwr;
+
+  nwr = write(gps_fd, pack, GARMIN_HEADER_SIZE + pack->mDataSize);
+  if (nwr == -1) {
+    perror("GPS write error");
+    return 1;
+  }
+  return 0;
+
+}
+
+/**
+ * @brief Receive packet from the GPS. Wait if not yet available
+ * @param  none
+ * @return  pointer to a packet or NULL if error (buffer overflow)
+ */
+Packet_t* recvpacket(void) {
+
+  Packet_t *pkt;
+  char tmp[64];
+  int nr;
+
+  D1(fprintf(stderr, "+1"));
+  pkt = (Packet_t*)malloc(GARMIN_MAX_PKTSIZE); /// 512 bytes
+  if (pkt == NULL) {
+    perror("malloc failed");
+    return NULL;
+  }
+
+chkbuf:
+  /* complete packet in buffer? */
+  if (pktbuf_size() >= GARMIN_HEADER_SIZE) { /// 12 bytes
+
+    Packet_t bufpkt;
+
+    pktbuf_peek((char*)&bufpkt, GARMIN_HEADER_SIZE);
+    // should a packet be checked for consistency? at least that the pktlen<= GARMIN_MAX_PKTSIZE?
+
+    int pktlen = GARMIN_HEADER_SIZE + bufpkt.mDataSize;
+    if (pktbuf_size() >= pktlen) {
+      pktbuf_deq((char*)pkt, pktlen);
+      return pkt;
+    }
+
+  }
+
+  /* not enough data - read some */
+  nr = read(gps_fd, tmp, 64);
+  if (nr == -1) {
+    perror("GPS read error");
+    D1(fprintf(stderr, "-1"));
+    free(pkt);
+    return NULL;
+  }
+
+  if (pktbuf_enq(tmp, nr) == 0) {
+    fprintf(stderr, "Input buffer full!\n");
+    //got here during overnight run - gets stuck in the loop
+    // probably - lost sync, need restart
+    D1(fprintf(stderr, "-2"));
+    free(pkt);
+    return NULL;
+  }
+
+  goto chkbuf;
+
+}
+
+/*
+  garmin_pvton()
+  turn on position records
+
+  receiver measurement records could also be enabled with
+  command 110 (instead of 49), but we don't need them at present
+*/
+
+void garmin_pvton(void) {
+
+  D1(fprintf(stderr, "+2"));
+  Packet_t *pvtpack = (Packet_t*)malloc(14);
+
+  pvtpack->mPacketType = 20;
+  pvtpack->mReserved1 = 0;
+  pvtpack->mReserved2 = 0;
+  pvtpack->mPacketId = 10;
+  pvtpack->mReserved3 = 0;
+  pvtpack->mDataSize = 2;
+  pvtpack->mData[0] = 49;
+  pvtpack->mData[1] = 0;
+
+  sendpacket(pvtpack);
+
+}
+
+void garmin_privcmd(int fd) {
+
+  u_int32_t privcmd[4];
+  privcmd[0] = 0x01106E4B;
+  privcmd[1] = 2;
+  privcmd[2] = 4;
+  privcmd[3] = 0;
+  write(fd, privcmd, 16);
+
+}
+
+int main(int argc, char *argv[]) {
+
+  //int fd_exif;
+  int chn, good_chns;
+  int fd_exif[SENSOR_PORTS];
+
+  struct meta_GPSInfo_t meta;
+  //char nmeabuf[256];
+  //u_int32_t privcmd[4];
+  //FILE *nmeaout;
+
+  struct termios termio;
+
+  if (argc < 2) {
+    printf("Usage: %s gpsdev\n", argv[0]);
+    return 1;
+  }
+
+  gps_fd = open(argv[1], O_RDWR);
+  if (gps_fd == -1) {
+    perror("Cannot open GPS device");
+    return 1;
+  }
+
+  tcgetattr(gps_fd, &termio);
+  cfmakeraw(&termio);
+  tcsetattr(gps_fd, TCIOFLUSH, &termio);
+  garmin_privcmd(gps_fd);
+  garmin_pvton();
+  pktbuf_head = 0;
+  pktbuf_tail = 0;
+
+  good_chns=0;
+  for (chn =0; chn < SENSOR_PORTS; chn++){
+    if (exif_paths[chn]) { // make them null if inactive
+      fd_exif[chn] = open(exif_paths[chn], O_RDWR);
+      if (fd_exif[chn] < 0) {
+        fprintf(stderr,"Can not open device file %s\n",exif_paths[chn]);
+      } else {
+        good_chns++;
+      }
+    }
+  }
+  if (!good_chns){
+    fprintf(stderr,"Could not open any of EXIF channel device files, aborting\n");
+    close(gps_fd);
+    exit(-1);
+  }
+
+  /*
+  fd_exif = open(EXIF_DEV, O_RDWR);
+  if (fd_exif<=0) {
+    fprintf(stderr,"Can not open device file "EXIF_DEV);
+    close(gps_fd);
+    exit (-1);
+  }
+  */
+
+  exif_init_meta(&meta);
+
+  while (1) {
+
+    Packet_t *pkt = recvpacket();
+
+    if (pkt) {
+
+      D(printf("Packet ID: %d, head=%d, tail=%d\n", pkt->mPacketId, pktbuf_head, pktbuf_tail));
+
+      if (pkt->mPacketId == Pid_Pvt_Data) {
+        memcpy(&lastpvt, pkt->mData, sizeof(lastpvt));
+
+        if (exif_gen(&lastpvt, satdata_valid ? lastsatdata : NULL, &meta) >=0) {
+
+          for (chn =0; chn < SENSOR_PORTS; chn++) {
+            if (fd_exif[chn]>=0){
+              // position file pointer at the beginning of the data field for GPSLatitudeRef
+              lseek (fd_exif[chn],Exif_GPSInfo_GPSLatitudeRef,SEEK_END);
+              write(fd_exif[chn], &meta, sizeof(meta));
+            }
+          }
+
+        }
+
+      } else if (pkt->mPacketId == Pid_SatData_Record) {
+        memcpy(lastsatdata, pkt->mData, sizeof(lastsatdata));
+        satdata_valid = 1;
+      }
+
+      D1(fprintf(stderr, "-3"));
+      free(pkt);
+
+    } else { // lost GPS/sync, trying to restart GPS/buffer
+      fprintf(stderr,"Lost GPS or sync, trying to restart communication and packet buffer. (pktbuf_head=%d,pktbuf_tail=%d)\n",pktbuf_head,pktbuf_tail);
+      tcsetattr(gps_fd, TCIOFLUSH, &termio);
+      garmin_privcmd(gps_fd);
+      garmin_pvton();
+      pktbuf_head = 0;
+      pktbuf_tail = 0;
+    }
+  }
+  close(gps_fd);
+}

src/garminusb2nmea.c

+/*
+	Garmin USB NMEA converter
+	Copyright (C) 2004 Manuel Kasper <mk@neon1.net>.
+	All rights reserved.
+		
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+	
+	1. Redistributions of source code must retain the above copyright notice,
+	   this list of conditions and the following disclaimer.
+	
+	2. Redistributions in binary form must reproduce the above copyright
+	   notice, this list of conditions and the following disclaimer in the
+	   documentation and/or other materials provided with the distribution.
+	
+	THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+	INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+	AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+	AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+	OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+	SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+	INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+	CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+	ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+	POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <termios.h>
+#include "garmin.h"
+#include "nmeagen.h"
+
+#define GARMIN_HEADER_SIZE	12
+#define GARMIN_MAX_PKTSIZE	512
+#define PKTBUF_SIZE			4097
+
+#pragma pack(push, 1)
+typedef struct {
+	u_int8_t	mPacketType;
+	u_int8_t	mReserved1;
+	u_int16_t	mReserved2;
+	u_int16_t	mPacketId;
+	u_int16_t	mReserved3;
+	u_int32_t	mDataSize;
+	u_int8_t	mData[1];
+} Packet_t;
+#pragma pack(pop)
+
+int gps_fd;
+char pktbuf[PKTBUF_SIZE];
+int	pktbuf_head = 0, pktbuf_tail = 0;
+
+D800_Pvt_Data_Type	lastpvt;
+cpo_sat_data		lastsatdata[12];
+int	satdata_valid = 0;
+
+int pktbuf_size() {
+	return (PKTBUF_SIZE - pktbuf_head + pktbuf_tail) % PKTBUF_SIZE;
+}
+
+int pktbuf_peek(char *buf, int n) {
+	int i;
+	int mypktbuf_head = pktbuf_head;
+	for (i = 0; (i < n) && (mypktbuf_head != pktbuf_tail); i++) {
+		buf[i] = pktbuf[mypktbuf_head];
+		mypktbuf_head++;
+		if (mypktbuf_head == PKTBUF_SIZE)
+			mypktbuf_head = 0;
+	}
+	return i;
+}
+
+int pktbuf_deq(char *buf, int n) {
+	int i;
+	for (i = 0; (i < n) && (pktbuf_head != pktbuf_tail); i++) {
+		buf[i] = pktbuf[pktbuf_head];
+		pktbuf_head++;
+		if (pktbuf_head == PKTBUF_SIZE)
+			pktbuf_head = 0;
+	}
+	return i;
+}
+
+int pktbuf_enq(char *buf, int n) {
+	
+	int i;
+	
+	if (pktbuf_size() + n >= PKTBUF_SIZE)
+		return 0;
+	
+	for (i = 0; i < n; i++) {
+		pktbuf[pktbuf_tail] = buf[i];
+		pktbuf_tail++;
+		if (pktbuf_tail == PKTBUF_SIZE)
+			pktbuf_tail = 0;
+	}
+	
+	return i;
+}
+
+int sendpacket(Packet_t *pack) {
+	int nwr;
+	nwr = write(gps_fd, pack, GARMIN_HEADER_SIZE + pack->mDataSize);
+	if (nwr == -1) {
+		perror("GPS write error");
+		return 1;
+	}
+	return 0;
+}
+
+Packet_t* recvpacket(void) {
+	Packet_t *pkt;
+	char tmp[64];
+	int nr;
+	
+	pkt = (Packet_t*)malloc(GARMIN_MAX_PKTSIZE);
+	if (pkt == NULL) {
+		perror("malloc failed");
+		return NULL;
+	}
+
+chkbuf:	
+	/* complete packet in buffer? */
+	if (pktbuf_size() >= GARMIN_HEADER_SIZE) {
+		Packet_t bufpkt;
+		pktbuf_peek((char*)&bufpkt, GARMIN_HEADER_SIZE);
+		int pktlen = GARMIN_HEADER_SIZE + bufpkt.mDataSize;
+		if (pktbuf_size() >= pktlen) {
+			pktbuf_deq((char*)pkt, pktlen);
+			return pkt;
+		}
+	}
+	
+	/* not enough data - read some */
+	nr = read(gps_fd, tmp, 64);
+	if (nr == -1) {
+		perror("GPS read error");
+		free(pkt);
+		return NULL;
+	}
+	if (pktbuf_enq(tmp, nr) == 0)
+		fprintf(stderr, "Input buffer full!");
+	
+	goto chkbuf;
+}
+
+/*
+	garmin_pvton()
+	turn on position records
+  
+	receiver measurement records could also be enabled with 
+	command 110 (instead of 49), but we don't need them at present
+*/
+void garmin_pvton(void) {
+	Packet_t *pvtpack = (Packet_t*)malloc(14);
+
+	pvtpack->mPacketType = 20;
+	pvtpack->mReserved1 = 0;
+	pvtpack->mReserved2 = 0;
+	pvtpack->mPacketId = 10;
+	pvtpack->mReserved3 = 0;
+	pvtpack->mDataSize = 2;
+	pvtpack->mData[0] = 49;
+	pvtpack->mData[1] = 0;
+	
+	sendpacket(pvtpack);
+}
+
+int main(int argc, char *argv[]) {
+	
+	char nmeabuf[256];
+	u_int32_t privcmd[4];
+	FILE *nmeaout;
+	struct termios termio;
+
+	if (argc < 2) {
+		printf("Usage: %s gpsdev [nmeaoutdev]\n", argv[0]);
+		return 1;
+	}
+
+	gps_fd = open(argv[1], O_RDWR);
+	if (gps_fd == -1) {
+		perror("Cannot open GPS device");
+		return 1;
+	}
+	
+	tcgetattr(gps_fd, &termio);
+	cfmakeraw(&termio);
+	tcsetattr(gps_fd, TCIOFLUSH, &termio);
+	
+	privcmd[0] = 0x01106E4B;
+	privcmd[1] = 2;
+	privcmd[2] = 4;
+	privcmd[3] = 0;
+	write(gps_fd, privcmd, 16);
+	
+	if (argc == 3) {
+		nmeaout = fopen(argv[2], "w");
+		if (!nmeaout) {
+			perror("Cannot open output file");
+			return 1;
+		}
+	} else
+		nmeaout = stdout;
+
+	garmin_pvton();
+	
+	while (1) {
+		Packet_t *pkt = recvpacket();
+		printf("Packet ID: %d\n", pkt->mPacketId);
+		if (pkt->mPacketId == Pid_Pvt_Data) {
+			memcpy(&lastpvt, pkt->mData, sizeof(lastpvt));
+			
+			if (nmea_gpgga(&lastpvt, satdata_valid ? lastsatdata : NULL, nmeabuf) == 0)
+				fprintf(nmeaout, "%s", nmeabuf);
+			if (nmea_gprmc(&lastpvt, nmeabuf) == 0)
+				fprintf(nmeaout, "%s", nmeabuf);
+			if (nmea_gpgll(&lastpvt, nmeabuf) == 0)
+				fprintf(nmeaout, "%s", nmeabuf);
+			if (nmea_gpgsa(&lastpvt, satdata_valid ? lastsatdata : NULL, nmeabuf) == 0)
+				fprintf(nmeaout, "%s", nmeabuf);
+			
+			fflush(nmeaout);
+			
+		} else if (pkt->mPacketId == Pid_SatData_Record) {
+			memcpy(lastsatdata, pkt->mData, sizeof(lastsatdata));
+			satdata_valid = 1;
+			if (nmea_gpgsv(lastsatdata, nmeabuf) == 0) {
+				fprintf(nmeaout, "%s", nmeabuf);
+				fflush(nmeaout);
+			}
+		}
+		free(pkt);
+	}
+	
+	close(gps_fd);
+}

src/nmeagen.h

+/*
+	Garmin protocol to NMEA 0183 converter
+	Copyright (C) 2004 Manuel Kasper <mk@neon1.net>.
+	All rights reserved.
+
+	Input:
+		- D800_Pvt_Data_Type (PID 51)
+		- satellite data record (PID 114)
+
+	Available output sentences:
+		GPGGA, GPRMC, GPGLL, GPGSA, GPGSV
+
+	Known caveats:
+		- DOP (Dilution of Precision) information not available
+		  (Garmin protocol includes EPE only)
+		- DGPS information in GPGGA sentence not returned
+		- speed and course over ground are calculated from the
+		  north/east velocity and may not be accurate
+		- magnetic variation information not available
+		- Garmin 16-bit SNR scale unknown
+
+	---------------------------------------------------------------------------
+	
+	Redistribution and use in source and binary forms, with or without
+	modification, are permitted provided that the following conditions are met:
+	
+	1. Redistributions of source code must retain the above copyright notice,
+	   this list of conditions and the following disclaimer.
+	
+	2. Redistributions in binary form must reproduce the above copyright
+	   notice, this list of conditions and the following disclaimer in the
+	   documentation and/or other materials provided with the distribution.
+	
+	THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+	INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+	AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+	AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+	OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+	SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+	INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+	CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+	ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+	POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef NMEAGEN_H
+#define NMEAGEN_H
+#include "garmin.h"
+
+#define KNOTS_TO_KMH	1.852
+#define G_PI			3.14159265358979324
+#define rad2deg(x)		((x) * 180.0 / G_PI)
+
+void nmea_getutc(D800_Pvt_Data_Type *pvt, char *utctime, char *utcdate);
+void nmea_fmtlat(double lat, char *latstr);
+void nmea_fmtlon(double lon, char *lonstr);
+int nmea_gpgga(D800_Pvt_Data_Type *pvt, cpo_sat_data *sat, char *nmeastc);
+int nmea_gprmc(D800_Pvt_Data_Type *pvt, char *nmeastc);
+int nmea_gpgll(D800_Pvt_Data_Type *pvt, char *nmeastc);
+int nmea_gpgsa(D800_Pvt_Data_Type *pvt, cpo_sat_data *sat, char *nmeastc);
+int nmea_gpgsv(cpo_sat_data *sat, char *nmeastc);
+unsigned char nmea_cksum(char *str);
+
+#endif