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
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
|
/**
* True North Technologies - Revolution 2X Digital compass
*
* More info: http://www.tntc.com/
*
* This is a digital compass which uses magnetometers to measure
* the strength of the earth's magnetic field. Based on these
* measurements it provides a compass heading using NMEA
* formatted output strings. I threw this into gpsd since it
* already has convienient NMEA parsing support. Also because
* I use the compass to suplement the heading provided by
* another gps unit. A gps heading is unreliable at slow
* speed or no speed.
*
*/
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <stdarg.h>
#include "gpsd.h"
#ifdef TNT_ENABLE
enum {
#include "packet_states.h"
};
static void tnt_add_checksum(char *sentence)
{
unsigned char sum = '\0';
char c, *p = sentence;
if (*p == '@') {
p++;
} else {
gpsd_report(1, "Bad TNT sentence: '%s'\n", sentence);
}
while ( ((c = *p) != '*') && (c != '\0')) {
sum ^= c;
p++;
}
*p++ = '*';
/*@i@*/sprintf(p, "%02X\r\n", sum);
}
static int tnt_send(int fd, const char *fmt, ... )
{
int status;
char buf[BUFSIZ];
va_list ap;
va_start(ap, fmt) ;
(void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
va_end(ap);
strcat(buf, "*");
tnt_add_checksum(buf);
status = (int)write(fd, buf, strlen(buf));
if (status == (int)strlen(buf)) {
gpsd_report(2, "=> GPS: %s\n", buf);
return status;
} else {
gpsd_report(2, "=> GPS: %s FAILED\n", buf);
return -1;
}
}
#define TNT_SNIFF_RETRIES 100
/*
* The True North compass won't start talking
* unless you ask it to. So to identify it we
* need to query for it's ID string.
*/
static int tnt_packet_sniff(struct gps_device_t *session)
{
unsigned int n, count = 0;
gpsd_report(5, "tnt_packet_sniff begins\n");
for (n = 0; n < TNT_SNIFF_RETRIES; n++)
{
count = 0;
(void)tnt_send(session->gpsdata.gps_fd, "@X?");
if (ioctl(session->gpsdata.gps_fd, FIONREAD, &count) < 0)
return BAD_PACKET;
if (count == 0) {
//int delay = 10000000000.0 / session->gpsdata.baudrate;
//gpsd_report(5, "usleep(%d)\n", delay);
//usleep(delay);
gpsd_report(5, "sleep(1)\n");
(void)sleep(1);
} else if (packet_get(session) >= 0) {
if((session->packet_type == NMEA_PACKET)&&(session->packet_state == NMEA_RECOGNIZED))
{
gpsd_report(5, "tnt_packet_sniff returns %d\n",session->packet_type);
return session->packet_type;
}
}
}
gpsd_report(5, "tnt_packet_sniff found no packet\n");
return BAD_PACKET;
}
static void tnt_initializer(struct gps_device_t *session)
{
// Send codes to start the flow of data
//tnt_send(session->gpsdata.gps_fd, "@BA?"); // Query current rate
//tnt_send(session->gpsdata.gps_fd, "@BA=8"); // Start HTM packet at 1Hz
/*
* Sending this twice seems to make it more reliable!!
* I think it gets the input on the unit synced up.
*/
(void)tnt_send(session->gpsdata.gps_fd, "@BA=26"); // Start HTM packet at 2400 per minute
(void)tnt_send(session->gpsdata.gps_fd, "@BA=26"); // Start HTM packet at 2400 per minute
}
static bool tnt_probe(struct gps_device_t *session)
{
unsigned int *ip;
/* The supported baud rates */
static unsigned int rates[] = {38400, 19200, 2400, 4800, 9600 };
gpsd_report(1, "Probing TrueNorth Compass\n");
/*
* Only block until we get at least one character, whatever the
* third arg of read(2) says.
*/
/*@ ignore @*/
memset(session->ttyset.c_cc,0,sizeof(session->ttyset.c_cc));
session->ttyset.c_cc[VMIN] = 1;
/*@ end @*/
session->ttyset.c_cflag &= ~(PARENB | PARODD | CRTSCTS);
session->ttyset.c_cflag |= CREAD | CLOCAL;
session->ttyset.c_iflag = session->ttyset.c_oflag = session->ttyset.c_lflag = (tcflag_t) 0;
session->baudindex = 0;
for (ip = rates; ip < rates + sizeof(rates)/sizeof(rates[0]); ip++)
if (ip == rates || *ip != rates[0])
{
gpsd_report(1, "hunting at speed %d\n", *ip);
gpsd_set_speed(session, *ip, 'N',1);
if (tnt_packet_sniff(session) != BAD_PACKET)
return true;
}
return false;
}
struct gps_type_t trueNorth = {
.typename = "True North", /* full name of type */
.trigger = " TNT1500",
.channels = 0, /* not an actual GPS at all */
.probe = tnt_probe, /* probe by sending ID query */
.initializer = tnt_initializer, /* probe for True North Digital Compass */
.get_packet = packet_get, /* how to get a packet */
.parse_packet = nmea_parse_input, /* how to interpret a packet */
.rtcm_writer = NULL, /* Don't send */
.speed_switcher = NULL, /* no speed switcher */
.mode_switcher = NULL, /* no mode switcher */
.rate_switcher = NULL, /* no wrapup */
.cycle_chars = -1, /* not relevant, no rate switch */
.wrapup = NULL, /* no wrapup */
.cycle = 20, /* updates per second */
};
#endif
|
