Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // NMEA parser, adapted by Michael Smith from TinyGPS v9:
18 : //
19 : // TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
20 : // Copyright (C) 2008-9 Mikal Hart
21 : // All rights reserved.
22 : //
23 :
24 : /// @file AP_GPS_NMEA.cpp
25 : /// @brief NMEA protocol parser
26 : ///
27 : /// This is a lightweight NMEA parser, derived originally from the
28 : /// TinyGPS parser by Mikal Hart.
29 : ///
30 :
31 : #include <AP_Common/AP_Common.h>
32 : #include <AP_Common/NMEA.h>
33 : #include <GCS_MAVLink/GCS.h>
34 : #include <AP_Logger/AP_Logger.h>
35 :
36 : #include <ctype.h>
37 : #include <stdint.h>
38 : #include <stdlib.h>
39 : #include <stdio.h>
40 :
41 : #include "AP_GPS_NMEA.h"
42 :
43 : #if AP_GPS_NMEA_ENABLED
44 : extern const AP_HAL::HAL& hal;
45 :
46 : #ifndef AP_GPS_NMEA_CONFIG_PERIOD_MS
47 : // how often we send board specific config commands
48 : #define AP_GPS_NMEA_CONFIG_PERIOD_MS 15000U
49 : #endif
50 :
51 : // a quiet nan for invalid values
52 : #define QNAN nanf("GPS")
53 :
54 : // Convenience macros //////////////////////////////////////////////////////////
55 : //
56 : #define DIGIT_TO_VAL(_x) (_x - '0')
57 : #define hexdigit(x) ((x)>9?'A'+((x)-10):'0'+(x))
58 :
59 3721 : bool AP_GPS_NMEA::read(void)
60 : {
61 : int16_t numc;
62 3721 : bool parsed = false;
63 :
64 3721 : send_config();
65 :
66 3721 : numc = port->available();
67 89040 : while (numc--) {
68 85319 : char c = port->read();
69 : #if AP_GPS_DEBUG_LOGGING_ENABLED
70 : log_data((const uint8_t *)&c, 1);
71 : #endif
72 85319 : if (_decode(c)) {
73 403 : parsed = true;
74 : }
75 : }
76 3721 : return parsed;
77 : }
78 :
79 : /*
80 : decode one character, return true if we have successfully completed a sentence, false otherwise
81 : */
82 85319 : bool AP_GPS_NMEA::_decode(char c)
83 : {
84 85319 : _sentence_length++;
85 :
86 85319 : switch (c) {
87 0 : case ';':
88 : // header separator for unicore
89 0 : if (!_is_unicore) {
90 0 : return false;
91 : }
92 : FALLTHROUGH;
93 : case ',': // term terminators
94 13926 : _parity ^= c;
95 13926 : if (_is_unicore) {
96 2 : _crc32 = crc_crc32(_crc32, (const uint8_t *)&c, 1);
97 : }
98 : FALLTHROUGH;
99 : case '\r':
100 : case '\n':
101 : case '*': {
102 18327 : if (_sentence_done) {
103 1502 : return false;
104 : }
105 16825 : bool valid_sentence = false;
106 16825 : if (_term_offset < sizeof(_term)) {
107 16825 : _term[_term_offset] = 0;
108 16825 : valid_sentence = _term_complete();
109 : }
110 16825 : ++_term_number;
111 16825 : _term_offset = 0;
112 16825 : _is_checksum_term = c == '*';
113 16825 : return valid_sentence;
114 : }
115 :
116 1461 : case '$': // sentence begin
117 : case '#': // unicore message begin
118 1461 : _is_unicore = (c == '#');
119 1461 : _term_number = _term_offset = 0;
120 1461 : _parity = 0;
121 1461 : _crc32 = 0;
122 1461 : _sentence_type = _GPS_SENTENCE_OTHER;
123 1461 : _is_checksum_term = false;
124 1461 : _sentence_length = 1;
125 1461 : _sentence_done = false;
126 1461 : _new_gps_yaw = QNAN;
127 1461 : return false;
128 : }
129 :
130 : // ordinary characters
131 65531 : if (_term_offset < sizeof(_term) - 1)
132 61605 : _term[_term_offset++] = c;
133 65531 : if (!_is_checksum_term) {
134 62633 : _parity ^= c;
135 62633 : if (_is_unicore) {
136 63 : _crc32 = crc_crc32(_crc32, (const uint8_t *)&c, 1);
137 : }
138 : }
139 :
140 65531 : return false;
141 : }
142 :
143 3456 : int32_t AP_GPS_NMEA::_parse_decimal_100(const char *p)
144 : {
145 3456 : char *endptr = nullptr;
146 3456 : long ret = 100 * strtol(p, &endptr, 10);
147 3456 : int sign = ret < 0 ? -1 : 1;
148 :
149 3456 : if (ret >= (long)INT32_MAX) {
150 0 : return INT32_MAX;
151 : }
152 3456 : if (ret <= (long)INT32_MIN) {
153 0 : return INT32_MIN;
154 : }
155 3456 : if (endptr == nullptr || *endptr != '.') {
156 4 : return ret;
157 : }
158 :
159 3452 : if (isdigit(endptr[1])) {
160 3452 : ret += sign * 10 * DIGIT_TO_VAL(endptr[1]);
161 3452 : if (isdigit(endptr[2])) {
162 3041 : ret += sign * DIGIT_TO_VAL(endptr[2]);
163 3041 : if (isdigit(endptr[3])) {
164 715 : ret += sign * (DIGIT_TO_VAL(endptr[3]) >= 5);
165 : }
166 : }
167 : }
168 3452 : return ret;
169 : }
170 :
171 : /*
172 : parse a NMEA latitude/longitude degree value. The result is in degrees*1e7
173 : */
174 1620 : uint32_t AP_GPS_NMEA::_parse_degrees()
175 : {
176 : char *p, *q;
177 1620 : uint8_t deg = 0, min = 0;
178 1620 : float frac_min = 0;
179 1620 : int32_t ret = 0;
180 :
181 : // scan for decimal point or end of field
182 8910 : for (p = _term; *p && isdigit(*p); p++)
183 : ;
184 1620 : q = _term;
185 :
186 : // convert degrees
187 5670 : while ((p - q) > 2 && *q) {
188 4050 : if (deg)
189 2430 : deg *= 10;
190 4050 : deg += DIGIT_TO_VAL(*q++);
191 : }
192 :
193 : // convert minutes
194 4860 : while (p > q && *q) {
195 3240 : if (min)
196 1207 : min *= 10;
197 3240 : min += DIGIT_TO_VAL(*q++);
198 : }
199 :
200 : // convert fractional minutes
201 1620 : if (*p == '.') {
202 1620 : q = p + 1;
203 1620 : float frac_scale = 0.1f;
204 9720 : while (*q && isdigit(*q)) {
205 8100 : frac_min += DIGIT_TO_VAL(*q) * frac_scale;
206 8100 : q++;
207 8100 : frac_scale *= 0.1f;
208 : }
209 : }
210 1620 : ret = (deg * (int32_t)10000000UL);
211 1620 : ret += (min * (int32_t)10000000UL / 60);
212 1620 : ret += (int32_t) (frac_min * (1.0e7f / 60.0f));
213 1620 : return ret;
214 : }
215 :
216 : /*
217 : see if we have a new set of NMEA messages
218 : */
219 1449 : bool AP_GPS_NMEA::_have_new_message()
220 : {
221 1449 : if (_last_RMC_ms == 0 ||
222 1446 : _last_GGA_ms == 0) {
223 9 : return false;
224 : }
225 1440 : uint32_t now = AP_HAL::millis();
226 1440 : if (now - _last_RMC_ms > 150 ||
227 425 : now - _last_GGA_ms > 150) {
228 1037 : return false;
229 : }
230 403 : if (_last_VTG_ms != 0 &&
231 348 : now - _last_VTG_ms > 150) {
232 0 : return false;
233 : }
234 :
235 : /*
236 : if we have seen a message with 3D velocity data messages then
237 : wait for them again. This is important as the
238 : have_vertical_velocity field will be overwritten by
239 : fill_3d_velocity()
240 : */
241 403 : if (_last_vvelocity_ms != 0 &&
242 0 : now - _last_vvelocity_ms > 150 &&
243 0 : now - _last_vvelocity_ms < 1000) {
244 : // waiting on a message with velocity
245 0 : return false;
246 : }
247 403 : if (_last_vaccuracy_ms != 0 &&
248 0 : now - _last_vaccuracy_ms > 150 &&
249 0 : now - _last_vaccuracy_ms < 1000) {
250 : // waiting on a message with velocity accuracy
251 0 : return false;
252 : }
253 :
254 : // prevent these messages being used again
255 403 : if (_last_VTG_ms != 0) {
256 348 : _last_VTG_ms = 1;
257 : }
258 :
259 403 : if (now - _last_yaw_ms > 300) {
260 : // we have lost GPS yaw
261 117 : state.have_gps_yaw = false;
262 : }
263 :
264 403 : if (now - _last_KSXT_pos_ms > 500) {
265 : // we have lost KSXT
266 403 : _last_KSXT_pos_ms = 0;
267 : }
268 :
269 : #if AP_GPS_NMEA_UNICORE_ENABLED
270 403 : if (now - _last_AGRICA_ms > 500) {
271 403 : if (_last_AGRICA_ms != 0) {
272 : // we have lost AGRICA
273 0 : state.have_gps_yaw = false;
274 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
275 : // the moving baseline offset must not outlive the yaw solution it
276 : // was calculated for
277 0 : state.mb_yaw_offset.zero();
278 : #endif
279 0 : state.have_vertical_velocity = false;
280 0 : state.have_speed_accuracy = false;
281 0 : state.have_horizontal_accuracy = false;
282 0 : state.have_vertical_accuracy = false;
283 0 : state.have_undulation = false;
284 0 : _last_AGRICA_ms = 0;
285 : }
286 : }
287 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
288 :
289 403 : _last_fix_ms = now;
290 :
291 403 : _last_GGA_ms = 1;
292 403 : _last_RMC_ms = 1;
293 403 : return true;
294 : }
295 :
296 : // Processes a just-completed term
297 : // Returns true if new sentence has just passed checksum test and is validated
298 16825 : bool AP_GPS_NMEA::_term_complete()
299 : {
300 : // handle the last term in a message
301 16825 : if (_is_checksum_term) {
302 1449 : _sentence_done = true;
303 1449 : const uint32_t crc = strtoul(_term, nullptr, 16);
304 1449 : const bool crc_ok = _is_unicore? (_crc32 == crc) : (_parity == crc);
305 1449 : if (crc_ok) {
306 1449 : uint32_t now = AP_HAL::millis();
307 1449 : switch (_sentence_type) {
308 407 : case _GPS_SENTENCE_RMC:
309 407 : _last_RMC_ms = now;
310 : //time = _new_time;
311 : //date = _new_date;
312 407 : if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
313 407 : state.location.lat = _new_latitude;
314 407 : state.location.lng = _new_longitude;
315 : }
316 407 : if (_last_3D_velocity_ms == 0 ||
317 0 : now - _last_3D_velocity_ms > 1000) {
318 407 : state.ground_speed = _new_speed*0.01f;
319 407 : state.ground_course = wrap_360(_new_course*0.01f);
320 : }
321 407 : if (state.status >= AP_GPS_FixType::FIX_3D) {
322 403 : make_gps_time(_new_date, _new_time * 10);
323 403 : if (_last_AGRICA_ms != 0) {
324 0 : state.time_week_ms = _last_itow_ms;
325 : }
326 : }
327 407 : set_uart_timestamp(_sentence_length);
328 407 : state.last_gps_time_ms = now;
329 407 : if (_last_vvelocity_ms == 0 ||
330 0 : now - _last_vvelocity_ms > 1000) {
331 407 : fill_3d_velocity();
332 : }
333 407 : break;
334 403 : case _GPS_SENTENCE_GGA:
335 403 : _last_GGA_ms = now;
336 403 : if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
337 403 : set_alt_amsl_cm(state, _new_altitude);
338 403 : state.location.lat = _new_latitude;
339 403 : state.location.lng = _new_longitude;
340 : }
341 403 : state.num_sats = _new_satellite_count;
342 403 : state.hdop = _new_hdop;
343 403 : switch(_new_quality_indicator) {
344 0 : case 0: // Fix not available or invalid
345 0 : state.status = AP_GPS_FixType::NONE;
346 0 : break;
347 348 : case 1: // GPS SPS Mode, fix valid
348 348 : state.status = AP_GPS_FixType::FIX_3D;
349 348 : break;
350 0 : case 2: // Differential GPS, SPS Mode, fix valid
351 0 : state.status = AP_GPS_FixType::DGPS;
352 0 : break;
353 0 : case 3: // GPS PPS Mode, fix valid
354 0 : state.status = AP_GPS_FixType::FIX_3D;
355 0 : break;
356 55 : case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
357 55 : state.status = AP_GPS_FixType::RTK_FIXED;
358 55 : break;
359 0 : case 5: // Float RTK. Satellite system used in RTK mode, floating integers
360 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
361 0 : break;
362 0 : case 6: // Estimated (dead reckoning) Mode
363 0 : state.status = AP_GPS_FixType::NONE;
364 0 : break;
365 0 : default://to maintain compatibility with MAV_GPS_INPUT and others
366 0 : state.status = AP_GPS_FixType::FIX_3D;
367 0 : break;
368 : }
369 403 : break;
370 351 : case _GPS_SENTENCE_VTG:
371 351 : _last_VTG_ms = now;
372 351 : if (_last_3D_velocity_ms == 0 ||
373 0 : now - _last_3D_velocity_ms > 1000) {
374 351 : state.ground_speed = _new_speed*0.01f;
375 351 : state.ground_course = wrap_360(_new_course*0.01f);
376 351 : if (_last_vvelocity_ms == 0 ||
377 0 : now - _last_vvelocity_ms > 1000) {
378 351 : fill_3d_velocity();
379 : }
380 : }
381 : // VTG has no fix indicator, can't change fix status
382 351 : break;
383 288 : case _GPS_SENTENCE_HDT:
384 : case _GPS_SENTENCE_THS:
385 288 : if (_last_AGRICA_ms != 0 || _expect_agrica) {
386 : // use AGRICA
387 : break;
388 : }
389 288 : if (isnan(_new_gps_yaw)) {
390 : // empty sentence
391 0 : break;
392 : }
393 288 : _last_yaw_ms = now;
394 288 : state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
395 288 : state.have_gps_yaw = true;
396 288 : state.gps_yaw_time_ms = now;
397 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
398 : // this yaw is the device-reported heading, not one calculated
399 : // from a moving baseline offset; clear any stale offset so no
400 : // attitude correction is applied to it
401 288 : state.mb_yaw_offset.zero();
402 : #endif
403 : // remember that we are setup to provide yaw. With
404 : // a NMEA GPS we can only tell if the GPS is
405 : // configured to provide yaw when it first sends a
406 : // HDT sentence.
407 288 : state.gps_yaw_configured = true;
408 288 : break;
409 0 : case _GPS_SENTENCE_PHD:
410 0 : if (_last_AGRICA_ms != 0) {
411 : // prefer AGRICA
412 0 : break;
413 : }
414 0 : if (_phd.msg_id == 12) {
415 0 : state.velocity.x = _phd.fields[0] * 0.01;
416 0 : state.velocity.y = _phd.fields[1] * 0.01;
417 0 : state.velocity.z = _phd.fields[2] * 0.01;
418 0 : state.have_vertical_velocity = true;
419 0 : _last_vvelocity_ms = now;
420 : // we prefer a true 3D velocity when available
421 0 : velocity_to_speed_course(state);
422 0 : _last_3D_velocity_ms = now;
423 0 : } else if (_phd.msg_id == 26) {
424 0 : state.horizontal_accuracy = MAX(_phd.fields[0],_phd.fields[1]) * 0.001;
425 0 : state.have_horizontal_accuracy = true;
426 0 : state.vertical_accuracy = _phd.fields[2] * 0.001;
427 0 : state.have_vertical_accuracy = true;
428 0 : state.speed_accuracy = MAX(_phd.fields[3],_phd.fields[4]) * 0.001;
429 0 : state.have_speed_accuracy = true;
430 0 : _last_vaccuracy_ms = now;
431 : }
432 0 : break;
433 0 : case _GPS_SENTENCE_KSXT:
434 0 : if (_last_AGRICA_ms != 0 || _expect_agrica) {
435 : // prefer AGRICA
436 : break;
437 : }
438 0 : state.location.lat = _ksxt.fields[2]*1.0e7;
439 0 : state.location.lng = _ksxt.fields[1]*1.0e7;
440 0 : set_alt_amsl_cm(state, _ksxt.fields[3]*1.0e2);
441 0 : _last_KSXT_pos_ms = now;
442 0 : if (_ksxt.fields[9] >= 1) {
443 : // we have 3D fix
444 0 : constexpr float kmh_to_mps = 1.0 / 3.6;
445 0 : state.velocity.y = _ksxt.fields[16] * kmh_to_mps;
446 0 : state.velocity.x = _ksxt.fields[17] * kmh_to_mps;
447 0 : state.velocity.z = _ksxt.fields[18] * -kmh_to_mps;
448 0 : state.have_vertical_velocity = true;
449 0 : _last_vvelocity_ms = now;
450 : // we prefer a true 3D velocity when available
451 0 : velocity_to_speed_course(state);
452 0 : _last_3D_velocity_ms = now;
453 : }
454 0 : if (is_equal(3.0f, float(_ksxt.fields[10]))) {
455 : // have good yaw (from RTK fixed moving baseline solution)
456 0 : _last_yaw_ms = now;
457 0 : state.gps_yaw = _ksxt.fields[4];
458 0 : state.have_gps_yaw = true;
459 0 : state.gps_yaw_time_ms = now;
460 0 : state.gps_yaw_configured = true;
461 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
462 : // this yaw is the receiver's own heading, not one calculated
463 : // from the configured moving baseline offset; clear any stale
464 : // offset so no attitude correction is applied to it
465 0 : state.mb_yaw_offset.zero();
466 : #endif
467 : }
468 0 : break;
469 : #if AP_GPS_NMEA_UNICORE_ENABLED
470 0 : case _GPS_SENTENCE_AGRICA: {
471 0 : const auto &ag = _agrica;
472 0 : _last_AGRICA_ms = now;
473 0 : _last_vvelocity_ms = now;
474 0 : _last_vaccuracy_ms = now;
475 0 : _last_3D_velocity_ms = now;
476 0 : state.location.lat = ag.lat*1.0e7;
477 0 : state.location.lng = ag.lng*1.0e7;
478 0 : state.undulation = -ag.undulation;
479 0 : state.have_undulation = true;
480 0 : set_alt_amsl_cm(state, ag.alt*1.0e2);
481 0 : state.velocity = ag.vel_NED;
482 0 : velocity_to_speed_course(state);
483 0 : state.speed_accuracy = ag.vel_stddev.length();
484 0 : state.horizontal_accuracy = ag.pos_stddev.xy().length();
485 0 : state.vertical_accuracy = ag.pos_stddev.z;
486 0 : state.have_vertical_velocity = true;
487 0 : state.have_speed_accuracy = true;
488 0 : state.have_horizontal_accuracy = true;
489 0 : state.have_vertical_accuracy = true;
490 0 : check_new_itow(ag.itow, _sentence_length);
491 0 : break;
492 : }
493 0 : case _GPS_SENTENCE_VERSIONA: {
494 0 : _have_unicore_versiona = true;
495 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
496 : "NMEA %s %s %s",
497 : _versiona.type,
498 : _versiona.version,
499 : _versiona.build_date);
500 0 : break;
501 : }
502 0 : case _GPS_SENTENCE_UNIHEADINGA: {
503 : #if GPS_MOVING_BASELINE
504 0 : const auto &ag = _agrica;
505 0 : const auto &uh = _uniheadinga;
506 0 : if (now - _last_AGRICA_ms > 500 || ag.heading_status != 4) {
507 : // we need heading_status from AGRICA
508 0 : state.have_gps_yaw = false;
509 0 : break;
510 : }
511 0 : const float dist = uh.baseline_length;
512 0 : const float bearing = uh.heading;
513 0 : const float alt_diff = dist*sinf(radians(-uh.pitch));
514 0 : state.relPosHeading = bearing;
515 0 : state.relPosLength = dist;
516 0 : state.relPosD = alt_diff;
517 0 : state.relposheading_ts = now;
518 0 : if (calculate_moving_base_yaw(bearing, dist, alt_diff)) {
519 0 : state.have_gps_yaw_accuracy = true;
520 0 : state.gps_yaw_accuracy = uh.heading_sd;
521 0 : _last_yaw_ms = now;
522 : }
523 0 : state.gps_yaw_configured = true;
524 : #endif // GPS_MOVING_BASELINE
525 0 : break;
526 : }
527 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
528 : }
529 : // see if we got a good message
530 1449 : return _have_new_message();
531 : }
532 : // we got a bad message, ignore it
533 0 : return false;
534 : }
535 :
536 : // the first term determines the sentence type
537 15376 : if (_term_number == 0) {
538 : /*
539 : special case for $PHD message
540 : */
541 1465 : if (strcmp(_term, "PHD") == 0) {
542 0 : _sentence_type = _GPS_SENTENCE_PHD;
543 0 : return false;
544 : }
545 1465 : if (strcmp(_term, "KSXT") == 0) {
546 0 : _sentence_type = _GPS_SENTENCE_KSXT;
547 0 : return false;
548 : }
549 : #if AP_GPS_NMEA_UNICORE_ENABLED
550 1465 : if (strcmp(_term, "AGRICA") == 0 && _expect_agrica) {
551 0 : _sentence_type = _GPS_SENTENCE_AGRICA;
552 0 : return false;
553 : }
554 1465 : if (strcmp(_term, "VERSIONA") == 0) {
555 0 : _sentence_type = _GPS_SENTENCE_VERSIONA;
556 0 : return false;
557 : }
558 1465 : if (strcmp(_term, "UNIHEADINGA") == 0 && _expect_agrica) {
559 0 : _sentence_type = _GPS_SENTENCE_UNIHEADINGA;
560 0 : return false;
561 : }
562 : #endif
563 : /*
564 : The first two letters of the NMEA term are the talker
565 : ID. The most common is 'GP' but there are a bunch of others
566 : that are valid. We accept any two characters here.
567 : */
568 1465 : if (_term[0] < 'A' || _term[0] > 'Z' ||
569 1449 : _term[1] < 'A' || _term[1] > 'Z') {
570 16 : _sentence_type = _GPS_SENTENCE_OTHER;
571 16 : return false;
572 : }
573 1449 : const char *term_type = &_term[2];
574 1449 : if (strcmp(term_type, "RMC") == 0) {
575 407 : _sentence_type = _GPS_SENTENCE_RMC;
576 1042 : } else if (strcmp(term_type, "GGA") == 0) {
577 403 : _sentence_type = _GPS_SENTENCE_GGA;
578 639 : } else if (strcmp(term_type, "HDT") == 0) {
579 288 : _sentence_type = _GPS_SENTENCE_HDT;
580 351 : } else if (strcmp(term_type, "THS") == 0) {
581 0 : _sentence_type = _GPS_SENTENCE_THS;
582 351 : } else if (strcmp(term_type, "VTG") == 0) {
583 351 : _sentence_type = _GPS_SENTENCE_VTG;
584 : } else {
585 0 : _sentence_type = _GPS_SENTENCE_OTHER;
586 : }
587 1449 : return false;
588 : }
589 :
590 : // 32 = RMC, 64 = GGA, 96 = VTG, 128 = HDT, 160 = THS
591 13911 : if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
592 12234 : switch (_sentence_type + _term_number) {
593 : // operational status
594 : //
595 407 : case _GPS_SENTENCE_RMC + 2: // validity (RMC)
596 407 : break;
597 403 : case _GPS_SENTENCE_GGA + 6: // Fix data (GGA)
598 403 : if (_term[0] > '0') {
599 403 : _new_quality_indicator = _term[0] - '0';
600 : } else {
601 0 : _new_quality_indicator = 0;
602 : }
603 403 : break;
604 403 : case _GPS_SENTENCE_GGA + 7: // satellite count (GGA)
605 403 : _new_satellite_count = atol(_term);
606 403 : break;
607 403 : case _GPS_SENTENCE_GGA + 8: // HDOP (GGA)
608 403 : _new_hdop = (uint16_t)_parse_decimal_100(_term);
609 403 : break;
610 :
611 : // time and date
612 : //
613 810 : case _GPS_SENTENCE_RMC + 1: // Time (RMC)
614 : case _GPS_SENTENCE_GGA + 1: // Time (GGA)
615 810 : _new_time = _parse_decimal_100(_term);
616 810 : break;
617 407 : case _GPS_SENTENCE_RMC + 9: // Date (GPRMC)
618 407 : _new_date = atol(_term);
619 407 : break;
620 :
621 : // location
622 : //
623 810 : case _GPS_SENTENCE_RMC + 3: // Latitude
624 : case _GPS_SENTENCE_GGA + 2:
625 810 : _new_latitude = _parse_degrees();
626 810 : break;
627 810 : case _GPS_SENTENCE_RMC + 4: // N/S
628 : case _GPS_SENTENCE_GGA + 3:
629 810 : if (_term[0] == 'S')
630 236 : _new_latitude = -_new_latitude;
631 810 : break;
632 810 : case _GPS_SENTENCE_RMC + 5: // Longitude
633 : case _GPS_SENTENCE_GGA + 4:
634 810 : _new_longitude = _parse_degrees();
635 810 : break;
636 810 : case _GPS_SENTENCE_RMC + 6: // E/W
637 : case _GPS_SENTENCE_GGA + 5:
638 810 : if (_term[0] == 'W')
639 574 : _new_longitude = -_new_longitude;
640 810 : break;
641 403 : case _GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
642 403 : _new_altitude = _parse_decimal_100(_term);
643 403 : break;
644 :
645 : // course and speed
646 : //
647 758 : case _GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
648 : case _GPS_SENTENCE_VTG + 5: // Speed (VTG)
649 758 : _new_speed = (_parse_decimal_100(_term) * 514) / 1000; // knots-> m/sec, approximates * 0.514
650 758 : break;
651 288 : case _GPS_SENTENCE_HDT + 1: // Course (HDT)
652 288 : _new_gps_yaw = _parse_decimal_100(_term);
653 288 : break;
654 0 : case _GPS_SENTENCE_THS + 1: // Course (THS)
655 0 : _new_gps_yaw = _parse_decimal_100(_term);
656 0 : break;
657 758 : case _GPS_SENTENCE_RMC + 8: // Course (GPRMC)
658 : case _GPS_SENTENCE_VTG + 1: // Course (VTG)
659 758 : _new_course = _parse_decimal_100(_term);
660 758 : break;
661 :
662 0 : case _GPS_SENTENCE_PHD + 1: // PHD class
663 0 : _phd.msg_class = atol(_term);
664 0 : break;
665 0 : case _GPS_SENTENCE_PHD + 2: // PHD message
666 0 : _phd.msg_id = atol(_term);
667 0 : break;
668 0 : case _GPS_SENTENCE_PHD + 5: // PHD message, itow
669 0 : _phd.itow = strtoul(_term, nullptr, 10);
670 0 : break;
671 0 : case _GPS_SENTENCE_PHD + 6 ... _GPS_SENTENCE_PHD + 11: // PHD message, fields
672 0 : _phd.fields[_term_number-6] = atol(_term);
673 0 : break;
674 0 : case _GPS_SENTENCE_KSXT + 1 ... _GPS_SENTENCE_KSXT + 22: // KSXT message, fields
675 0 : _ksxt.fields[_term_number-1] = atof(_term);
676 0 : break;
677 : #if AP_GPS_NMEA_UNICORE_ENABLED
678 0 : case _GPS_SENTENCE_AGRICA + 1 ... _GPS_SENTENCE_AGRICA + 65: // AGRICA message
679 0 : parse_agrica_field(_term_number, _term);
680 0 : break;
681 0 : case _GPS_SENTENCE_VERSIONA + 1 ... _GPS_SENTENCE_VERSIONA + 20:
682 0 : parse_versiona_field(_term_number, _term);
683 0 : break;
684 : #if GPS_MOVING_BASELINE
685 0 : case _GPS_SENTENCE_UNIHEADINGA + 1 ... _GPS_SENTENCE_UNIHEADINGA + 28: // UNIHEADINGA message
686 0 : parse_uniheadinga_field(_term_number, _term);
687 0 : break;
688 : #endif
689 : #endif
690 : }
691 : }
692 :
693 13911 : return false;
694 : }
695 :
696 : #if AP_GPS_NMEA_UNICORE_ENABLED
697 : /*
698 : parse an AGRICA message term
699 :
700 : Example:
701 : #AGRICA,82,GPS,FINE,2237,176366400,0,0,18,15;GNSS,232,22,11,22,0,59,8,1,5,8,12,0,0.0000,0.0000,0.0000,0.0000,0.0000,0.0000,296.4656,-26.5685,0.0000,0.005,0.000,0.000,-0.005,0.044,0.032,0.038,-35.33142715815,149.13181842030,609.1494,-4471799.0368,2672944.7758,-3668288.9857,1.3923,1.5128,3.2272,2.3026,2.1633,2.1586,0.00000000000,0.00000000000,0.0000,0.00000000000,0.00000000000,0.0000,176366400,0.000,66.175285,18.972784,0.000000,0.000000,5,0,0,0*9f704dad
702 : */
703 0 : void AP_GPS_NMEA::parse_agrica_field(uint16_t term_number, const char *term)
704 : {
705 0 : auto &ag = _agrica;
706 : // subtract 8 to align term numbers with reference manual
707 : // look for "Unicore Reference Command Manual" to find the specification
708 0 : const uint8_t hdr_align = 8;
709 0 : if (term_number < hdr_align) {
710 : // discard header;
711 0 : return;
712 : }
713 0 : term_number -= hdr_align;
714 : // useful for debugging
715 : //::printf("AGRICA[%u]=%s\n", unsigned(term_number), term);
716 0 : switch (term_number) {
717 0 : case 10:
718 0 : ag.rtk_status = atol(term);
719 0 : break;
720 0 : case 11:
721 0 : ag.heading_status = atol(term);
722 0 : break;
723 0 : case 25 ... 26:
724 0 : ag.vel_NED[term_number-25] = strtof(term, nullptr);
725 0 : break;
726 0 : case 27:
727 : // AGRIC gives velocity up
728 0 : ag.vel_NED.z = -strtof(term, nullptr);
729 0 : break;
730 0 : case 28 ... 30:
731 0 : ag.vel_stddev[term_number-28] = strtof(term, nullptr);
732 0 : break;
733 0 : case 31:
734 0 : ag.lat = atof(term);
735 0 : break;
736 0 : case 32:
737 0 : ag.lng = atof(term);
738 0 : break;
739 0 : case 33:
740 0 : ag.alt = strtof(term, nullptr);
741 0 : break;
742 0 : case 49:
743 0 : ag.itow = atol(term);
744 0 : break;
745 0 : case 37 ... 39:
746 0 : ag.pos_stddev[term_number-37] = strtof(term, nullptr);
747 0 : break;
748 0 : case 52:
749 0 : ag.undulation = strtof(term, nullptr);
750 0 : break;
751 : }
752 : }
753 :
754 : #if GPS_MOVING_BASELINE
755 : /*
756 : parse a UNIHEADINGA message term
757 :
758 : Example:
759 : #UNIHEADINGA,79,GPS,FINE,2242,167498200,0,0,18,22;SOL_COMPUTED,L1_INT,2.7889,296.7233,-25.7710,0.0000,0.1127,0.1812,"999",49,37,37,0,3,00,1,51*d50af0ea
760 : */
761 0 : void AP_GPS_NMEA::parse_uniheadinga_field(uint16_t term_number, const char *term)
762 : {
763 0 : const uint8_t hdr_align = 8;
764 0 : if (term_number < hdr_align) {
765 : // discard header;
766 0 : return;
767 : }
768 0 : term_number -= hdr_align;
769 : // useful for debugging
770 : // ::printf("UNIHEADINGA[%u]=%s\n", unsigned(term_number), term);
771 0 : auto &uh = _uniheadinga;
772 0 : switch (term_number) {
773 0 : case 4:
774 0 : uh.baseline_length = strtof(term, nullptr);
775 0 : break;
776 0 : case 5:
777 0 : uh.heading = strtof(term, nullptr);
778 0 : break;
779 0 : case 6:
780 0 : uh.pitch = strtof(term, nullptr);
781 0 : break;
782 0 : case 8:
783 0 : uh.heading_sd = strtof(term, nullptr);
784 0 : break;
785 : }
786 : }
787 : #endif // GPS_MOVING_BASELINE
788 :
789 : // parse VERSIONA fields
790 0 : void AP_GPS_NMEA::parse_versiona_field(uint16_t term_number, const char *term)
791 : {
792 : // printf useful for debugging
793 : // ::printf("VERSIONA[%u]='%s'\n", term_number, term);
794 0 : auto &v = _versiona;
795 : #pragma GCC diagnostic push
796 : #if defined(__GNUC__) && __GNUC__ >= 9
797 : #pragma GCC diagnostic ignored "-Wstringop-truncation"
798 : #endif
799 0 : switch (term_number) {
800 0 : case 10:
801 0 : strncpy(v.type, _term, sizeof(v.type)-1);
802 0 : break;
803 0 : case 11:
804 0 : strncpy(v.version, _term, sizeof(v.version)-1);
805 0 : break;
806 0 : case 15:
807 0 : strncpy(v.build_date, _term, sizeof(v.build_date)-1);
808 0 : break;
809 : }
810 : #pragma GCC diagnostic pop
811 0 : }
812 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
813 :
814 : /*
815 : detect a NMEA GPS. Adds one byte, and returns true if the stream
816 : matches a NMEA string
817 : */
818 : bool
819 299 : AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
820 : {
821 299 : switch (state.step) {
822 4 : case 0:
823 4 : state.ck = 0;
824 4 : if ('$' == data) {
825 4 : state.step++;
826 : }
827 4 : break;
828 287 : case 1:
829 287 : if ('*' == data) {
830 4 : state.step++;
831 : } else {
832 283 : state.ck ^= data;
833 : }
834 287 : break;
835 4 : case 2:
836 4 : if (hexdigit(state.ck>>4) == data) {
837 4 : state.step++;
838 : } else {
839 0 : state.step = 0;
840 : }
841 4 : break;
842 4 : case 3:
843 4 : if (hexdigit(state.ck&0xF) == data) {
844 4 : state.step = 0;
845 4 : return true;
846 : }
847 0 : state.step = 0;
848 0 : break;
849 : }
850 295 : return false;
851 : }
852 :
853 : /*
854 : send type specific config strings
855 : */
856 3721 : void AP_GPS_NMEA::send_config(void)
857 : {
858 3721 : const auto type = get_type();
859 3721 : _expect_agrica = (type == AP_GPS::GPS_TYPE_UNICORE_NMEA ||
860 : type == AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA);
861 3721 : if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
862 : // not doing auto-config
863 2868 : return;
864 : }
865 853 : uint32_t now_ms = AP_HAL::millis();
866 853 : if (now_ms - last_config_ms < AP_GPS_NMEA_CONFIG_PERIOD_MS) {
867 849 : return;
868 : }
869 4 : last_config_ms = now_ms;
870 4 : const uint16_t rate_ms = params.rate_ms;
871 : #if AP_GPS_NMEA_UNICORE_ENABLED
872 4 : const float rate_s = rate_ms * 0.001;
873 : #endif
874 4 : const uint8_t rate_hz = 1000U / rate_ms;
875 :
876 4 : switch (get_type()) {
877 : #if AP_GPS_NMEA_UNICORE_ENABLED
878 0 : case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
879 0 : port->printf("\r\nCONFIG HEADING FIXLENGTH\r\n" \
880 : "CONFIG UNDULATION AUTO\r\n" \
881 : "CONFIG\r\n" \
882 : "UNIHEADINGA %.3f\r\n",
883 : rate_s);
884 0 : state.gps_yaw_configured = true;
885 : FALLTHROUGH;
886 :
887 0 : case AP_GPS::GPS_TYPE_UNICORE_NMEA: {
888 0 : port->printf("\r\nAGRICA %.3f\r\n" \
889 : "MODE MOVINGBASE\r\n" \
890 : "GNGGA %.3f\r\n" \
891 : "GNRMC %.3f\r\n",
892 : rate_s, rate_s, rate_s);
893 0 : if (!_have_unicore_versiona) {
894 : // get version information for logging if we don't have it yet
895 0 : port->printf("VERSIONA\r\n");
896 0 : if (gps._save_config) {
897 : // save config changes for fast startup
898 0 : port->printf("SAVECONFIG\r\n");
899 : }
900 : }
901 0 : break;
902 : }
903 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
904 :
905 0 : case AP_GPS::GPS_TYPE_HEMI: {
906 0 : port->printf(
907 : "$JATT,NMEAHE,0\r\n" /* Prefix of GP on the HDT message */ \
908 : "$JASC,GPGGA,%u\r\n" /* GGA at 5Hz */ \
909 : "$JASC,GPRMC,%u\r\n" /* RMC at 5Hz */ \
910 : "$JASC,GPVTG,%u\r\n" /* VTG at 5Hz */ \
911 : "$JASC,GPHDT,%u\r\n" /* HDT at 5Hz */ \
912 : "$JMODE,SBASR,YES\r\n" /* Enable SBAS */,
913 : rate_hz, rate_hz, rate_hz, rate_hz);
914 0 : break;
915 : }
916 :
917 0 : case AP_GPS::GPS_TYPE_ALLYSTAR:
918 0 : nmea_printf(port, "$PHD,06,42,UUUUTTTT,BB,0,%u,55,0,%u,0,0,0",
919 : unsigned(rate_hz), unsigned(rate_ms));
920 0 : break;
921 :
922 4 : default:
923 4 : break;
924 : }
925 :
926 : #ifdef AP_GPS_NMEA_CUSTOM_CONFIG_STRING
927 : // allow for custom config strings, useful for peripherals
928 : port->printf("%s\r\n", AP_GPS_NMEA_CUSTOM_CONFIG_STRING);
929 : #endif
930 : }
931 :
932 : /*
933 : return health status
934 : */
935 615 : bool AP_GPS_NMEA::is_healthy(void) const
936 : {
937 615 : switch (get_type()) {
938 : #if AP_GPS_NMEA_UNICORE_ENABLED
939 0 : case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
940 : case AP_GPS::GPS_TYPE_UNICORE_NMEA:
941 : // we should be getting AGRICA messages
942 0 : return _last_AGRICA_ms != 0;
943 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
944 :
945 0 : case AP_GPS::GPS_TYPE_HEMI:
946 : // we should be getting HDR for yaw
947 0 : return _last_yaw_ms != 0;
948 :
949 0 : case AP_GPS::GPS_TYPE_ALLYSTAR:
950 : // we should get vertical velocity and accuracy from PHD
951 0 : return _last_vvelocity_ms != 0 && _last_vaccuracy_ms != 0;
952 :
953 615 : default:
954 615 : break;
955 : }
956 615 : return true;
957 : }
958 :
959 : // get the velocity lag
960 50641 : bool AP_GPS_NMEA::get_lag(float &lag_sec) const
961 : {
962 50641 : switch (get_type()) {
963 : #if AP_GPS_NMEA_UNICORE_ENABLED
964 0 : case AP_GPS::GPS_TYPE_UNICORE_MOVINGBASE_NMEA:
965 : case AP_GPS::GPS_TYPE_UNICORE_NMEA:
966 0 : lag_sec = 0.14;
967 0 : break;
968 : #endif // AP_GPS_NMEA_UNICORE_ENABLED
969 :
970 50641 : default:
971 50641 : lag_sec = 0.2;
972 50641 : break;
973 : }
974 50641 : return true;
975 : }
976 :
977 : #if HAL_LOGGING_ENABLED
978 1 : void AP_GPS_NMEA::Write_AP_Logger_Log_Startup_messages() const
979 : {
980 1 : AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages();
981 : #if AP_GPS_NMEA_UNICORE_ENABLED
982 1 : if (_have_unicore_versiona) {
983 0 : AP::logger().Write_MessageF("NMEA %u %s %s %s",
984 0 : state.instance+1,
985 0 : _versiona.type,
986 0 : _versiona.version,
987 0 : _versiona.build_date);
988 : }
989 : #endif
990 1 : }
991 : #endif
992 :
993 : #endif // AP_GPS_NMEA_ENABLED
|
