/src/gpsd/gpsd-3.27.6~dev/drivers/driver_geostar.c
Line | Count | Source |
1 | | /* |
2 | | * This is the gpsd driver for GeoStar Navigation receivers |
3 | | * operating in binary mode. |
4 | | * |
5 | | * Tested with GeoS-1M GPS/GLONASS receiver. |
6 | | * |
7 | | * By Viktar Palstsiuk, viktar.palstsiuk@promwad.com |
8 | | * |
9 | | * This file is Copyright by the GPSD project |
10 | | * SPDX-License-Identifier: BSD-2-clause |
11 | | */ |
12 | | |
13 | | #include "../include/gpsd_config.h" // must be before all includes |
14 | | |
15 | | #include <math.h> |
16 | | #include <stdbool.h> |
17 | | #include <stdio.h> |
18 | | #include <string.h> |
19 | | |
20 | | #include "../include/bits.h" |
21 | | #include "../include/gpsd.h" |
22 | | #include "../include/strfuncs.h" |
23 | | #include "../include/timespec.h" |
24 | | |
25 | | #ifdef GEOSTAR_ENABLE |
26 | 140 | #define GEOSTAR_CHANNELS 24 |
27 | | |
28 | 12 | #define JAN_2008 0x47798280 // 1199145600 = 2008 - 1970 in seconds |
29 | | |
30 | 152 | #define OFFSET(n) ((n)*4+4) |
31 | | |
32 | 1.60k | static int decode_channel_id (uint32_t ch_id) { |
33 | 1.60k | int num = 0; |
34 | 1.60k | num = (int)(ch_id & 0x1F); // SV ID |
35 | 1.60k | if (0 == (ch_id & (1<<30))) { |
36 | 1.44k | num += GLONASS_PRN_OFFSET; // GLONASS SV |
37 | 1.44k | } else if (0 == num) { |
38 | 42 | num = 32; // GPS SV |
39 | 42 | } |
40 | 1.60k | return num; |
41 | 1.60k | } |
42 | | |
43 | | /* write to geostar |
44 | | * |
45 | | * id - message is |
46 | | * data - 32 bit words of message content |
47 | | * len - number of 32 bit words in data |
48 | | * |
49 | | * return: 0 == OK |
50 | | * negative on error |
51 | | */ |
52 | | static int geostar_write(struct gps_device_t *session, |
53 | | unsigned int id, const unsigned char *data, |
54 | | size_t len) |
55 | 505 | { |
56 | 505 | int i; |
57 | 505 | unsigned long cs = 0; |
58 | 505 | char buf2[64]; |
59 | | |
60 | 505 | if (sizeof(session->msgbuf) < ((len * 4) + 12)) { |
61 | |
|
62 | 0 | GPSD_LOG(LOG_ERROR, &session->context->errout, |
63 | 0 | "geostar_write() write too long %zu, %s\n", len, |
64 | 0 | gps_hexdump(buf2, sizeof(buf2), data, len * 4)); |
65 | 0 | return -1; |
66 | 0 | } |
67 | 505 | putbyte(session->msgbuf, 0, 'P'); |
68 | 505 | putbyte(session->msgbuf, 1, 'S'); |
69 | 505 | putbyte(session->msgbuf, 2, 'G'); |
70 | 505 | putbyte(session->msgbuf, 3, 'G'); |
71 | | |
72 | 505 | putbe16(session->msgbuf, 4, id); |
73 | 505 | putbe16(session->msgbuf, 6, len); |
74 | | |
75 | | // Copy content |
76 | 505 | memcpy(session->msgbuf + 8, data, len * 4); |
77 | | |
78 | 505 | len += 2; // PSGG + id + len |
79 | | |
80 | | // Calculate checksum |
81 | 2.06k | for (i = 0; (size_t)i < len; i++) { |
82 | 1.55k | cs ^= getleu32(session->msgbuf, i * 4); |
83 | 1.55k | } |
84 | | |
85 | 505 | putle32(session->msgbuf, len * 4, cs); |
86 | | |
87 | 505 | len += 1; // Checksum |
88 | | |
89 | 505 | session->msgbuflen = len * 4; |
90 | | |
91 | 505 | GPSD_LOG(LOG_PROG, &session->context->errout, |
92 | 505 | "Sent GeoStar packet id 0x%x (%s)\n", id, |
93 | 505 | gps_hexdump(buf2, sizeof(buf2), |
94 | 505 | (const unsigned char*)session->msgbuf, |
95 | 505 | session->msgbuflen)); |
96 | 505 | if (gpsd_write(session, session->msgbuf, session->msgbuflen) != |
97 | 505 | (ssize_t)session->msgbuflen) { |
98 | 505 | return -1; |
99 | 505 | } |
100 | | |
101 | 0 | return 0; |
102 | 505 | } |
103 | | |
104 | | /* geostar_detect() |
105 | | * |
106 | | * see if it looks like a GeoStar device is listening and |
107 | | * return 1 if found, 0 if not |
108 | | */ |
109 | | static bool geostar_detect(struct gps_device_t *session) |
110 | 0 | { |
111 | 0 | unsigned char buf[1 * 4]; |
112 | 0 | bool ret = false; |
113 | 0 | int myfd; |
114 | |
|
115 | 0 | myfd = session->gpsdata.gps_fd; |
116 | | |
117 | | // request firmware revision and look for a valid response |
118 | 0 | putbe32(buf, 0, 0); |
119 | 0 | if (0 == geostar_write(session, 0xc1, buf, 1)) { |
120 | 0 | unsigned int n; |
121 | 0 | struct timespec to; |
122 | | |
123 | | // FIXME: this holds the main loop from running... |
124 | 0 | for (n = 0; n < 3; n++) { |
125 | | // wait one second |
126 | 0 | to.tv_sec = 1; |
127 | 0 | to.tv_nsec = 0; |
128 | 0 | if (!nanowait(myfd, &to)) { |
129 | 0 | break; |
130 | 0 | } |
131 | 0 | if (0 <= packet_get1(session)) { |
132 | 0 | if (session->lexer.type == GEOSTAR_PACKET) { |
133 | 0 | GPSD_LOG(LOG_RAW, &session->context->errout, |
134 | 0 | "geostar_detect found\n"); |
135 | 0 | ret = true; |
136 | 0 | break; |
137 | 0 | } |
138 | 0 | } |
139 | 0 | } |
140 | 0 | } |
141 | |
|
142 | 0 | return ret; |
143 | 0 | } |
144 | | |
145 | | static gps_mask_t geostar_analyze(struct gps_device_t *session) |
146 | 447 | { |
147 | 447 | int i; |
148 | 447 | gps_mask_t mask = 0; |
149 | 447 | unsigned int id; |
150 | 447 | uint16_t uw1, uw2; |
151 | 447 | uint32_t ul1, ul2, ul3, ul4, ul5; |
152 | 447 | double d1, d2, d3, d4, d5; |
153 | 447 | char buf[sizeof(session->lexer.outbuffer)]; |
154 | 447 | char buf2[sizeof(session->lexer.outbuffer) * 3]; |
155 | | |
156 | 447 | if (GEOSTAR_PACKET != session->lexer.type) { |
157 | 0 | GPSD_LOG(LOG_INF, &session->context->errout, |
158 | 0 | "geostar_analyze packet type %d\n", |
159 | 0 | session->lexer.type); |
160 | 0 | return 0; |
161 | 0 | } |
162 | | |
163 | 447 | if (12 > session->lexer.outbuflen || |
164 | 447 | 'P' != session->lexer.outbuffer[0]) { |
165 | 112 | GPSD_LOG(LOG_WARN, &session->context->errout, |
166 | 112 | "geostar_analyze invalid packet\n"); |
167 | 112 | return 0; |
168 | 112 | } |
169 | 335 | if (sizeof(buf) <= session->lexer.outbuflen) { |
170 | 0 | GPSD_LOG(LOG_WARN, &session->context->errout, |
171 | 0 | "geostar_analyze overlong packet %zd\n", |
172 | 0 | session->lexer.outbuflen); |
173 | 0 | return 0; |
174 | 0 | } |
175 | | |
176 | | // put data part of message in buf |
177 | | |
178 | 335 | memset(buf, 0, sizeof(buf)); |
179 | 335 | memcpy(buf, session->lexer.outbuffer, session->lexer.outbuflen); |
180 | | |
181 | 335 | id = (unsigned int)getleu16(session->lexer.outbuffer, OFFSET(0)); |
182 | | |
183 | 335 | GPSD_LOG(LOG_DATA, &session->context->errout, |
184 | 335 | "GeoStar packet id 0x%02x length %zd: %s\n", |
185 | 335 | id, session->lexer.outbuflen, |
186 | 335 | gps_hexdump(buf2, sizeof(buf2), (unsigned char*)buf, |
187 | 335 | session->lexer.outbuflen)); |
188 | | |
189 | 335 | session->cycle_end_reliable = true; |
190 | | |
191 | 335 | switch (id) { |
192 | 1 | case 0x10: |
193 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, "Raw measurements\n"); |
194 | 1 | break; |
195 | 1 | case 0x11: |
196 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, "GPS sub-frame data\n"); |
197 | 1 | break; |
198 | 2 | case 0x12: |
199 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
200 | 2 | "GLONASS sub-frame data\n"); |
201 | 2 | break; |
202 | 1 | case 0x13: |
203 | 1 | d1 = getled64(buf, OFFSET(1)); |
204 | 1 | d2 = getled64(buf, OFFSET(3)); |
205 | 1 | d3 = getled64(buf, OFFSET(5)); |
206 | 1 | d4 = getled64(buf, OFFSET(29)); // GPS time |
207 | 1 | d5 = getled64(buf, OFFSET(31)); // GLONASS time |
208 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
209 | 1 | "ECEF coordinates %g %g %g %f %f\n", d1, d2, d3, d4, d5); |
210 | 1 | break; |
211 | 12 | case 0x20: |
212 | 12 | d1 = getled64(buf, OFFSET(1)); /* time */ |
213 | | |
214 | 12 | DTOTS(&session->newdata.time, d1); |
215 | 12 | session->newdata.time.tv_sec += JAN_2008; |
216 | | |
217 | 12 | session->newdata.latitude = getled64(buf, OFFSET(3)) * RAD_2_DEG; |
218 | 12 | session->newdata.longitude = getled64(buf, OFFSET(5)) * RAD_2_DEG; |
219 | | // altitude above ellipsoid |
220 | 12 | session->newdata.altHAE = getled64(buf, OFFSET(7)); |
221 | 12 | session->newdata.geoid_sep = getled64(buf, OFFSET(9)); |
222 | 12 | session->gpsdata.satellites_used = (int)getles32(buf, OFFSET(11)); |
223 | 12 | session->gpsdata.dop.gdop = getled64(buf, OFFSET(13)); |
224 | 12 | session->gpsdata.dop.pdop = getled64(buf, OFFSET(15)); |
225 | 12 | session->gpsdata.dop.tdop = getled64(buf, OFFSET(17)); |
226 | 12 | session->gpsdata.dop.hdop = getled64(buf, OFFSET(19)); |
227 | 12 | session->gpsdata.dop.vdop = getled64(buf, OFFSET(21)); |
228 | 12 | session->newdata.speed = getled64(buf, OFFSET(31)); |
229 | 12 | session->newdata.track = getled64(buf, OFFSET(33)) * RAD_2_DEG; |
230 | | |
231 | 12 | ul1 = getleu32(buf, OFFSET(29)); // status |
232 | | |
233 | 12 | if (0 != ul1) { |
234 | 1 | session->newdata.status = STATUS_UNK; |
235 | 1 | mask |= STATUS_SET; |
236 | 11 | } else if (STATUS_GPS > session->newdata.status) { |
237 | | // Don't step on previously set status |
238 | 11 | session->newdata.status = STATUS_GPS; |
239 | 11 | mask |= STATUS_SET; |
240 | 11 | } |
241 | 12 | mask |= TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET | |
242 | 12 | SPEED_SET | STATUS_SET | TRACK_SET | DOP_SET | USED_IS | |
243 | 12 | REPORT_IS; |
244 | | |
245 | 12 | GPSD_LOG(LOG_INF, &session->context->errout, |
246 | 12 | "Geographic coordinates %f %g %g %g %g %g\n", |
247 | 12 | d1, |
248 | 12 | session->newdata.latitude, |
249 | 12 | session->newdata.longitude, |
250 | 12 | session->newdata.altHAE, |
251 | 12 | session->newdata.speed, |
252 | 12 | session->newdata.track); |
253 | 12 | GPSD_LOG(LOG_INF, &session->context->errout, |
254 | 12 | "Dilution of precision %g %g %g %g %g\n", |
255 | 12 | session->gpsdata.dop.gdop, |
256 | 12 | session->gpsdata.dop.pdop, |
257 | 12 | session->gpsdata.dop.tdop, |
258 | 12 | session->gpsdata.dop.hdop, |
259 | 12 | session->gpsdata.dop.vdop); |
260 | 12 | break; |
261 | 4 | case 0x21: |
262 | 4 | ul1 = getleu32(buf, OFFSET(1)); |
263 | 4 | ul2 = getleu32(buf, OFFSET(2)); |
264 | 4 | uw1 = getleu16(buf, OFFSET(3)); |
265 | 4 | uw2 = getleu16(buf, OFFSET(3) + 2); |
266 | 4 | GPSD_LOG(LOG_INF, &session->context->errout, |
267 | 4 | "Current receiver telemetry %x %d %d %d\n", |
268 | 4 | ul1, ul2, uw1, uw2); |
269 | 4 | if (ul1 & (1<<3)) { |
270 | 2 | session->newdata.mode = MODE_2D; |
271 | 2 | } else { |
272 | 2 | session->newdata.mode = MODE_3D; |
273 | 2 | } |
274 | 4 | if (ul1 & (1<<2)) { |
275 | 2 | session->newdata.status = STATUS_GPS; |
276 | 2 | } else { |
277 | 2 | session->newdata.status = STATUS_UNK; |
278 | 2 | session->newdata.mode = MODE_NO_FIX; |
279 | 2 | } |
280 | | |
281 | 4 | mask |= MODE_SET | STATUS_SET; |
282 | 4 | break; |
283 | 75 | case 0x22: |
284 | 75 | ul1 = getleu32(buf, OFFSET(1)); |
285 | 75 | if (GEOSTAR_CHANNELS < ul1) { |
286 | 65 | ul1 = GEOSTAR_CHANNELS; |
287 | 65 | } |
288 | 75 | GPSD_LOG(LOG_INF, &session->context->errout, "SVs in view %d\n", ul1); |
289 | 75 | session->gpsdata.satellites_visible = (int)ul1; |
290 | 1.68k | for(i = 0; (uint32_t)i < ul1; i++) { |
291 | 1.60k | int16_t s1, s2, s3; |
292 | 1.60k | ul2 = getleu32(buf, OFFSET(2) + i * 3 * 4); |
293 | 1.60k | s1 = getles16(buf, OFFSET(3) + i * 3 * 4); |
294 | 1.60k | s2 = getles16(buf, OFFSET(3) + 2 + i * 3 * 4); |
295 | 1.60k | s3 = getles16(buf, OFFSET(4) + 2 + i * 3 * 4); |
296 | 1.60k | GPSD_LOG(LOG_INF, &session->context->errout, |
297 | 1.60k | "ID %d Az %g El %g SNR %g\n", |
298 | 1.60k | decode_channel_id(ul2), s1 * 0.001 * RAD_2_DEG, |
299 | 1.60k | s2 * 0.001 * RAD_2_DEG, s3 * 0.1); |
300 | 1.60k | session->gpsdata.skyview[i].PRN = (short)decode_channel_id(ul2); |
301 | 1.60k | session->gpsdata.skyview[i].azimuth = |
302 | 1.60k | (short)round((double)s1 * 0.001 * RAD_2_DEG); |
303 | 1.60k | session->gpsdata.skyview[i].elevation = |
304 | 1.60k | (short)round((double)s2 * 0.001 * RAD_2_DEG); |
305 | 1.60k | session->gpsdata.skyview[i].ss = (double)s3*0.1; |
306 | 1.60k | session->gpsdata.skyview[i].used = (bool)(ul2 & (1<<27)); |
307 | 1.60k | } |
308 | 75 | session->gpsdata.skyview_time.tv_sec = 0; |
309 | 75 | session->gpsdata.skyview_time.tv_nsec = 0; |
310 | 75 | mask |= SATELLITE_SET | USED_IS; |
311 | 75 | break; |
312 | 1 | case 0x3e: |
313 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
314 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
315 | 1 | ul3 = getleu32(buf, OFFSET(3)); |
316 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
317 | 1 | "Receiver power-up message %d %d %d\n", ul1, ul2, ul3); |
318 | 1 | break; |
319 | 1 | case 0x3f: |
320 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
321 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
322 | 1 | GPSD_LOG(LOG_WARN, &session->context->errout, |
323 | 1 | "Negative acknowledge %x %d\n", ul1, ul2); |
324 | 1 | break; |
325 | 1 | case 0x40: |
326 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
327 | 1 | "Response to Set initial parameters\n"); |
328 | 1 | break; |
329 | 1 | case 0x41: |
330 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
331 | 1 | "Response to Set serial ports parameters\n"); |
332 | 1 | break; |
333 | 2 | case 0x42: |
334 | 2 | ul1 = getleu32(buf, OFFSET(1)); |
335 | 2 | ul2 = getleu32(buf, OFFSET(2)); |
336 | 2 | ul3 = getleu32(buf, OFFSET(3)); |
337 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
338 | 2 | "Response to Set receiver operation mode %d %d %d\n", |
339 | 2 | ul1, ul2, ul3); |
340 | 2 | break; |
341 | 1 | case 0x43: |
342 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
343 | 1 | "Response to Set navigation task solution parameters\n"); |
344 | 1 | break; |
345 | 2 | case 0x44: |
346 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
347 | 2 | "Response to Set output data rate\n"); |
348 | 2 | break; |
349 | 2 | case 0x46: |
350 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
351 | 2 | "Response to Assign data protocol to communication port\n"); |
352 | 2 | break; |
353 | 1 | case 0x48: |
354 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
355 | 1 | "Response to Set GPS almanac\n"); |
356 | 1 | break; |
357 | 1 | case 0x49: |
358 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
359 | 1 | "Response to Set GLONASS almanac\n"); |
360 | 1 | break; |
361 | 1 | case 0x4a: |
362 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
363 | 1 | "Response to Set GPS ephemeris\n"); |
364 | 1 | break; |
365 | 1 | case 0x4b: |
366 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
367 | 1 | "Response to Set GLONASS ephemeris\n"); |
368 | 1 | break; |
369 | 1 | case 0x4c: |
370 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
371 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
372 | 1 | ul3 = getleu32(buf, OFFSET(3)); |
373 | 1 | ul4 = getleu32(buf, OFFSET(4)); |
374 | 1 | ul5 = getleu32(buf, OFFSET(5)); |
375 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
376 | 1 | "Response to Set PPS parameters %d %d %d %d %d\n", |
377 | 1 | ul1, ul2, ul3, ul4, ul5); |
378 | 1 | break; |
379 | 1 | case 0x4d: |
380 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
381 | 1 | "Response to Enable/disable SV in position fix\n"); |
382 | 1 | break; |
383 | 1 | case 0x4e: |
384 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
385 | 1 | "Response to Enable/disable NMEA messages\n"); |
386 | 1 | break; |
387 | 1 | case 0x4f: |
388 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
389 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
390 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
391 | 1 | "Response to Enable/disable binary messages %x %x\n", |
392 | 1 | ul1, ul2); |
393 | 1 | break; |
394 | 1 | case 0x80: |
395 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
396 | 1 | "Response to Query initial parameters\n"); |
397 | 1 | break; |
398 | 2 | case 0x81: |
399 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
400 | 2 | "Response to Query serial ports parameters\n"); |
401 | 2 | break; |
402 | 1 | case 0x82: |
403 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
404 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
405 | 1 | ul3 = getleu32(buf, OFFSET(3)); |
406 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
407 | 1 | "Response to Query receiver operation mode %d %d %d\n", |
408 | 1 | ul1, ul2, ul3); |
409 | 1 | break; |
410 | 1 | case 0x83: |
411 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
412 | 1 | "Response to Query navigation task solution parameters\n"); |
413 | 1 | break; |
414 | 1 | case 0x84: |
415 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
416 | 1 | "Response to Query output data rate\n"); |
417 | 1 | break; |
418 | 1 | case 0x86: |
419 | 1 | session->driver.geostar.physical_port = |
420 | 1 | (unsigned int)getleu32(buf, OFFSET(1)); |
421 | | |
422 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
423 | 1 | "Response to Query data protocol assignment to " |
424 | 1 | "communication port\n"); |
425 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
426 | 1 | "Connected to physical port %d\n", |
427 | 1 | session->driver.geostar.physical_port); |
428 | 1 | break; |
429 | 1 | case 0x88: |
430 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
431 | 1 | "Response to Query GPS almanac\n"); |
432 | 1 | break; |
433 | 1 | case 0x89: |
434 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
435 | 1 | "Response to Query GLONASS almanac\n"); |
436 | 1 | break; |
437 | 1 | case 0x8a: |
438 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
439 | 1 | "Response to Query GPS ephemerides\n"); |
440 | 1 | break; |
441 | 1 | case 0x8b: |
442 | 1 | d1 = getled64(buf, OFFSET(23)); |
443 | 1 | d2 = getled64(buf, OFFSET(25)); |
444 | 1 | d3 = getled64(buf, OFFSET(27)); |
445 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
446 | 1 | "Response to Query GLONASS ephemerides %g %g %g\n", |
447 | 1 | d1, d2, d3); |
448 | 1 | break; |
449 | 1 | case 0x8c: |
450 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
451 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
452 | 1 | ul3 = getleu32(buf, OFFSET(3)); |
453 | 1 | ul4 = getleu32(buf, OFFSET(4)); |
454 | 1 | ul5 = getleu32(buf, OFFSET(5)); |
455 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
456 | 1 | "Response to Query PPS parameters %d %d %d %d %d\n", |
457 | 1 | ul1, ul2, ul3, ul4, ul5); |
458 | 1 | break; |
459 | 1 | case 0x8d: |
460 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
461 | 1 | "Response to Query enable/disable status of " |
462 | 1 | "the SV in position fix\n"); |
463 | 1 | break; |
464 | 1 | case 0x8e: |
465 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
466 | 1 | "Response to Query enable NMEA messages\n"); |
467 | 1 | break; |
468 | 1 | case 0x8f: |
469 | 1 | ul1 = getleu32(buf, OFFSET(1)); |
470 | 1 | ul2 = getleu32(buf, OFFSET(2)); |
471 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
472 | 1 | "Response to Query enable binary messages %x %x\n", |
473 | 1 | ul1, ul2); |
474 | 1 | break; |
475 | 1 | case 0xc0: |
476 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
477 | 1 | "Response to Change operation mode command\n"); |
478 | 1 | break; |
479 | 1 | case 0xc1: |
480 | 1 | ul4 = getleu32(buf, OFFSET(1)); |
481 | 1 | ul1 = getleu32(buf, OFFSET(2)); |
482 | 1 | ul2 = getleu32(buf, OFFSET(3)); |
483 | 1 | ul3 = getleu32(buf, OFFSET(4)); |
484 | 1 | (void)snprintf(session->subtype, sizeof(session->subtype), |
485 | 1 | "%u.%u %u.%u.%u %x %c-%u\n", |
486 | 1 | ul4>>16, ul4&0xFFFF, ul1>>9, (ul1>>5)&0xF, |
487 | 1 | ul1&0x1F, ul2, ul3>>24, ul3&0x00FFFFFF); |
488 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
489 | 1 | "Response to Request FW version command: %s\n", |
490 | 1 | session->subtype); |
491 | 1 | mask |= DEVICEID_SET; |
492 | 1 | break; |
493 | 1 | case 0xc2: |
494 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
495 | 1 | "Response to Restart receiver command\n"); |
496 | 1 | break; |
497 | 1 | case 0xc3: |
498 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
499 | 1 | "Response to Store parameters to Flash command\n"); |
500 | 1 | break; |
501 | 1 | case 0xd0: |
502 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
503 | 1 | "Response to Erase Flash sector command\n"); |
504 | 1 | break; |
505 | 1 | case 0xd1: |
506 | 1 | GPSD_LOG(LOG_INF, &session->context->errout, |
507 | 1 | "Response to Write data to Flash command\n"); |
508 | 1 | break; |
509 | 2 | case 0xd2: |
510 | 2 | GPSD_LOG(LOG_INF, &session->context->errout, |
511 | 2 | "Response to Store Serial Number command\n"); |
512 | 2 | break; |
513 | 197 | default: |
514 | 197 | GPSD_LOG(LOG_WARN, &session->context->errout, |
515 | 197 | "Unhandled GeoStar packet type 0x%02x\n", id); |
516 | 197 | break; |
517 | 335 | } |
518 | | |
519 | 335 | return mask; |
520 | 335 | } |
521 | | |
522 | | static gps_mask_t geostar_parse_input(struct gps_device_t *session) |
523 | 553 | { |
524 | 553 | if (GEOSTAR_PACKET == session->lexer.type) { |
525 | 447 | return geostar_analyze(session); |
526 | 447 | } // else |
527 | 106 | return 0; |
528 | 553 | } |
529 | | |
530 | | // not used by the daemon, it's for gpsctl and friends |
531 | | static ssize_t geostar_control_send(struct gps_device_t *session, |
532 | | char *buf, size_t buflen) |
533 | 0 | { |
534 | 0 | return (ssize_t)geostar_write(session, |
535 | 0 | (unsigned int)buf[0], |
536 | 0 | (unsigned char *)buf + 1, |
537 | 0 | (buflen - 1) / 4); |
538 | 0 | } |
539 | | |
540 | | |
541 | | static void geostar_init_query(struct gps_device_t *session) |
542 | 65 | { |
543 | 65 | unsigned char buf[4]; |
544 | | |
545 | | // 0xC1 request content is ignored, init for Coverity |
546 | 65 | memset(buf, 0, sizeof(buf)); |
547 | | |
548 | | // Request Firmware Version |
549 | 65 | (void)geostar_write(session, 0xc1, buf, 1); |
550 | 65 | } |
551 | | |
552 | | static void geostar_event_hook(struct gps_device_t *session, event_t event) |
553 | 1.00k | { |
554 | 1.00k | unsigned char buf[2 * 4]; |
555 | | |
556 | 1.00k | if (session->context->readonly || |
557 | 774 | session->context->passive) { |
558 | 437 | return; |
559 | 437 | } |
560 | | |
561 | 567 | if (EVENT_IDENTIFIED == event || |
562 | 527 | EVENT_REACTIVATE == event) { |
563 | | // Select binary packets |
564 | 40 | putbe32(buf, 0, 0xffff0000); |
565 | 40 | putbe32(buf, 4, 0); |
566 | 40 | (void)geostar_write(session, 0x4f, buf, 2); |
567 | | |
568 | | // Poll Ports params |
569 | 40 | putbe32(buf, 0, 1); |
570 | 40 | (void)geostar_write(session, 0x81, buf, 1); |
571 | 40 | putbe32(buf, 0, 0); |
572 | 40 | (void)geostar_write(session, 0x81, buf, 1); |
573 | | // Poll Init params |
574 | 40 | (void)geostar_write(session, 0x80, buf, 1); |
575 | | // Poll Mode |
576 | 40 | (void)geostar_write(session, 0x82, buf, 1); |
577 | | // Poll Solution params |
578 | 40 | (void)geostar_write(session, 0x83, buf, 1); |
579 | | // Poll Output rate |
580 | 40 | (void)geostar_write(session, 0x84, buf, 1); |
581 | | // Poll Protocols assignment |
582 | 40 | (void)geostar_write(session, 0x86, buf, 1); |
583 | | // Poll PPS params |
584 | 40 | (void)geostar_write(session, 0x8c, buf, 1); |
585 | | // Poll NMEA packets selected |
586 | 40 | (void)geostar_write(session, 0x8e, buf, 1); |
587 | | // Poll binary packets selected |
588 | 40 | (void)geostar_write(session, 0x8f, buf, 1); |
589 | 40 | } |
590 | | |
591 | 567 | if (EVENT_DEACTIVATE == event) { |
592 | | // Perform cold restart. Seem brutal?? |
593 | 0 | putbe32(buf, 0, 3); |
594 | 0 | (void)geostar_write(session, 0xc2, buf, 1); |
595 | 0 | } |
596 | 567 | } |
597 | | |
598 | | static bool geostar_speed_switch(struct gps_device_t *session, |
599 | | speed_t speed, char parity, int stopbits) |
600 | 0 | { |
601 | 0 | unsigned char buf[4 * 4]; |
602 | 0 | int iparity; |
603 | |
|
604 | 0 | switch (parity) { |
605 | 0 | case 'E': |
606 | 0 | case 2: |
607 | 0 | parity = (char)2; |
608 | 0 | break; |
609 | 0 | case 'O': |
610 | 0 | case 1: |
611 | 0 | parity = (char)1; |
612 | 0 | break; |
613 | 0 | case 'N': |
614 | 0 | case 0: |
615 | 0 | default: |
616 | 0 | parity = (char)0; |
617 | 0 | break; |
618 | 0 | } |
619 | 0 | iparity = parity; |
620 | |
|
621 | 0 | putbe32(buf, 0, session->driver.geostar.physical_port); |
622 | 0 | putbe32(buf, 4, speed); |
623 | 0 | putbe32(buf, 8, stopbits); |
624 | 0 | putbe32(buf, 12, iparity); |
625 | 0 | (void)geostar_write(session, 0x41, buf, 4); |
626 | |
|
627 | 0 | return true; // it would be nice to error-check this |
628 | 0 | } |
629 | | |
630 | | static void geostar_mode(struct gps_device_t *session, int mode) |
631 | 0 | { |
632 | 0 | if (MODE_NMEA == mode) { |
633 | 0 | unsigned char buf[1 * 4]; |
634 | | // Switch to NMEA mode |
635 | 0 | putbe32(buf, 0, 1); |
636 | 0 | (void)geostar_write(session, 0x46, buf, 1); |
637 | 0 | } else if (MODE_BINARY == mode) { |
638 | | // Switch to binary mode |
639 | 0 | (void)nmea_send(session, "$GPSGG,SWPROT"); |
640 | 0 | } else { |
641 | 0 | GPSD_LOG(LOG_ERROR, &session->context->errout, |
642 | 0 | "unknown mode %i requested\n", mode); |
643 | 0 | } |
644 | 0 | } |
645 | | |
646 | | static double geostar_time_offset(struct gps_device_t *session UNUSED) |
647 | 0 | { |
648 | 0 | return 0.31; |
649 | 0 | } |
650 | | |
651 | | // this is everything we export |
652 | | // *INDENT-OFF* |
653 | | const struct gps_type_t driver_geostar = |
654 | | { |
655 | | .type_name = "GeoStar", // full name of type |
656 | | .packet_type = GEOSTAR_PACKET, // associated lexer packet type |
657 | | .flags = DRIVER_STICKY, // remember this |
658 | | .trigger = NULL, // no trigger |
659 | | .channels = GEOSTAR_CHANNELS, // consumer-grade GPS/GLONASS |
660 | | .probe_detect = geostar_detect, // probe for device |
661 | | .get_packet = packet_get1, // use the generic packet getter |
662 | | .parse_packet = geostar_parse_input, // parse message packets |
663 | | .rtcm_writer = NULL, // no DGPS corrections |
664 | | .init_query = geostar_init_query, // non-perturbing initial query |
665 | | .event_hook = geostar_event_hook, // fire on various lifetime events |
666 | | .speed_switcher = geostar_speed_switch, // change baud rate |
667 | | .mode_switcher = geostar_mode, // there is a mode switcher |
668 | | .rate_switcher = NULL, // no rate switcher |
669 | | .min_cycle.tv_sec = 1, // not relevant, no rate switch |
670 | | .min_cycle.tv_nsec = 0, // not relevant, no rate switch |
671 | | .control_send = geostar_control_send, // how to send commands |
672 | | .time_offset = geostar_time_offset, |
673 | | }; |
674 | | // *INDENT-ON* |
675 | | |
676 | | #endif // GEOSTAR_ENABLE |
677 | | |
678 | | // vim: set expandtab shiftwidth=4 |