Loading src/Makefile +12 −6 Original line number Diff line number Diff line Loading @@ -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 Loading @@ -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) Loading src/exifgen.c 0 → 100644 +156 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +21 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +147 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +373 −0 Original line number Diff line number Diff line /* 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); } Loading
src/Makefile +12 −6 Original line number Diff line number Diff line Loading @@ -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 Loading @@ -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) Loading
src/exifgen.c 0 → 100644 +156 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +21 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +147 −0 Original line number Diff line number Diff line /* 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 0 → 100644 +373 −0 Original line number Diff line number Diff line /* 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); }
