Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
E
elphel-apps-gps
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Board
Labels
Milestones
Wiki
Wiki
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Commits
Issue Boards
Open sidebar
Elphel
elphel-apps-gps
Commits
0ad6f2f1
Commit
0ad6f2f1
authored
Aug 10, 2017
by
Oleg Dzhimiev
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
+ Garmin 18x USB support
parent
5865b307
Changes
8
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
8 changed files
with
1448 additions
and
6 deletions
+1448
-6
Makefile
src/Makefile
+12
-6
exifgen.c
src/exifgen.c
+156
-0
exifgen.h
src/exifgen.h
+21
-0
garmin.h
src/garmin.h
+147
-0
garminusb2exif.c
src/garminusb2exif.c
+373
-0
garminusb2nmea.c
src/garminusb2nmea.c
+242
-0
nmeagen.c
src/nmeagen.c
+434
-0
nmeagen.h
src/nmeagen.h
+63
-0
No files found.
src/Makefile
View file @
0ad6f2f1
#TODO: pass $(LN) from bitbake
#TODO: pass $(LN) from bitbake
LN
=
ln
WWW_PAGES
=
/www/pages/
BINDIR
=
/usr/bin/
...
...
@@ -9,15 +9,15 @@ 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
=
nmea
gen.c exifgen.c garminusb2nmea.c garminusb2exif.c nmea
2exif.c log_imu.c
OBJS
=
nmea
gen.o exifgen.o garminusb2nmea.o garminusb2exif.o nmea
2exif.o
PHPSCRIPTS
=
start_gps_compass.php
PHPWEB
=
logger_launcher.php imu_setup.php read_imu_log.php
CFLAGS
+=
-Wall
-I
$(STAGING_DIR_HOST)
/usr/include-uapi
LDLIBS
+=
-lm
all
:
$(PROGS)
log_imu
:
log_imu.o
...
...
@@ -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)
...
...
@@ -37,7 +43,7 @@ install: $(PROGS)
$(LN)
-sf
$(VARHTML)
$(DESTDIR)$(WWW_PAGES)
var
$(INSTALL)
$(OWN)
-d
$(DESTDIR)$(CONFDIR)
$(LN)
-sf
$(CONFDIR)
$(DESTDIR)$(WWW_PAGES)
etc
clean
:
rm
-rf
$(PROGS)
*
.o
*
~
...
...
src/exifgen.c
0 → 100644
View file @
0ad6f2f1
/*
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
View file @
0ad6f2f1
/*
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
View file @
0ad6f2f1
/*
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
View file @
0ad6f2f1
/*
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
0 → 100644
View file @
0ad6f2f1
/*
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.