forked from kk7ds/dantracker
/
fakegps.c
82 lines (67 loc) · 1.52 KB
/
fakegps.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#define _GNU_SOURCE
#include <unistd.h>
#include <stdio.h>
#include <math.h>
struct pos {
double lat;
double lon;
double speed;
double course;
};
char *checksum(const char *str)
{
unsigned char cs = 0;
const char *ptr;
static char csstr[4];
for (ptr = str+1; *ptr; ptr++)
cs ^= *ptr;
sprintf(csstr, "*%02hhX", cs % 256);
return csstr;
}
void make_gga(FILE *fp, struct pos *pos)
{
char *str = NULL;
int lat_d = floor(pos->lat);
double lat_m = (pos->lat - lat_d) * 60.0;
int lon_d = floor(pos->lon);
double lon_m = (pos->lon - lon_d) * 60.0;
asprintf(&str,
"$GPGGA,000000,%02i%06.3f,%c,%03i%06.3f,%c,"
"1,04,0.9,001.1,M,50.0,M,,,",
lat_d, lat_m, lat_d > 0 ? 'N' : 'S',
lon_d, lon_m, lon_d > 0 ? 'E' : 'W');
fprintf(fp, "%s%s\r", str, checksum(str));
}
void make_rmc(FILE *fp, struct pos *pos)
{
char *str = NULL;
int lat_d = floor(pos->lat);
double lat_m = (pos->lat - lat_d) * 60.0;
int lon_d = floor(pos->lon);
double lon_m = (pos->lon - lon_d) * 60.0;
int cs = 0;
char *ptr;
asprintf(&str,
"$GPRMC,000000,A,%02i%06.3f,%c,%03i%06.3f,%c,"
"%05.1f,%05.1f,000000,003.0,W,",
lat_d, lat_m, lat_d > 0 ? 'N' : 'S',
lon_d, lon_m, lon_d > 0 ? 'E' : 'W',
pos->speed, pos->course);
for (ptr = str; *ptr; ptr++)
cs += *ptr;
fprintf(fp, "%s%s\r", str, checksum(str));
}
int main()
{
struct pos pos = {45.525, 122.9164, 55.0, 123.0};
int i = 0;
while (1) {
i++;
make_gga(stdout, &pos);
make_rmc(stdout, &pos);
fflush(NULL);
sleep(1);
if ((i > 16) && (i < 20))
pos.course += 10;
}
}