Commit 0ad6f2f1 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

+ Garmin 18x USB support

parent 5865b307
......@@ -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)
......
/*
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;
}
/*
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
/*
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
/*
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);
}
/*
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);
}
This diff is collapsed.
/*
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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment