OpenCPN Partial API docs
Loading...
Searching...
No Matches
comm_decoder.cpp
1/***************************************************************************
2 *
3 * Project: OpenCPN
4 * Purpose:
5 * Author: David Register, Alec Leamas
6 *
7 ***************************************************************************
8 * Copyright (C) 2022 by David Register, Alec Leamas *
9 * *
10 * This program is free software; you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation; either version 2 of the License, or *
13 * (at your option) any later version. *
14 * *
15 * This program is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
19 * *
20 * You should have received a copy of the GNU General Public License *
21 * along with this program; if not, write to the *
22 * Free Software Foundation, Inc., *
23 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. *
24 **************************************************************************/
25
26#include <wx/wxprec.h>
27
28#ifndef WX_PRECOMP
29#include <wx/wx.h>
30#endif // precompiled headers
31
32#include <wx/log.h>
33#include <wx/math.h>
34#include <wx/string.h>
35
36#include "rapidjson/document.h"
37
38#include "model/comm_decoder.h"
39#include "model/comm_util.h"
40#include "model/comm_vars.h"
41#include "model/geodesic.h"
42#include "model/own_ship.h"
43
44bool CommDecoder::ParsePosition(const LATLONG& Position, double& lat,
45 double& lon) {
46 bool ll_valid = true;
47 double llt = Position.Latitude.Latitude;
48
49 if (!std::isnan(llt)) {
50 int lat_deg_int = (int)(llt / 100);
51 double lat_deg = lat_deg_int;
52 double lat_min = llt - (lat_deg * 100);
53
54 lat = lat_deg + (lat_min / 60.);
55 if (Position.Latitude.Northing == South) lat = -lat;
56 } else
57 ll_valid = false;
58
59 double lln = Position.Longitude.Longitude;
60 if (!std::isnan(lln)) {
61 int lon_deg_int = (int)(lln / 100);
62 double lon_deg = lon_deg_int;
63 double lon_min = lln - (lon_deg * 100);
64
65 lon = lon_deg + (lon_min / 60.);
66 if (Position.Longitude.Easting == West) lon = -lon;
67 } else
68 ll_valid = false;
69
70 return ll_valid;
71}
72
73bool CommDecoder::DecodeRMC(std::string s, NavData& temp_data) {
74 wxString sentence(s.c_str());
75 wxString sentence3 = ProcessNMEA4Tags(sentence);
76 m_NMEA0183 << sentence3;
77
78 if (!m_NMEA0183.PreParse()) return false;
79 if (!m_NMEA0183.Parse()) return false;
80
81 if (m_NMEA0183.Rmc.IsDataValid == NTrue) {
82 double tlat, tlon;
83 if (ParsePosition(m_NMEA0183.Rmc.Position, tlat, tlon)) {
84 temp_data.gLat = tlat;
85 temp_data.gLon = tlon;
86 } else
87 return false;
88
89 // FIXME (dave) if (!g_own_ship_sog_cog_calc )
90 {
91 if (!std::isnan(m_NMEA0183.Rmc.SpeedOverGroundKnots)) {
92 temp_data.gSog = m_NMEA0183.Rmc.SpeedOverGroundKnots;
93 }
94 if (!std::isnan(temp_data.gSog) && (temp_data.gSog > 0.05)) {
95 temp_data.gCog = m_NMEA0183.Rmc.TrackMadeGoodDegreesTrue;
96 } else {
97 temp_data.gCog = NAN;
98 }
99 }
100 // Any device sending VAR=0.0 can be assumed to not really know
101 // what the actual variation is, so in this case we use WMM if
102 // available
103 if ((!std::isnan(m_NMEA0183.Rmc.MagneticVariation)) &&
104 0.0 != m_NMEA0183.Rmc.MagneticVariation) {
105 if (m_NMEA0183.Rmc.MagneticVariationDirection == East)
106 temp_data.gVar = m_NMEA0183.Rmc.MagneticVariation;
107 else if (m_NMEA0183.Rmc.MagneticVariationDirection == West)
108 temp_data.gVar = -m_NMEA0183.Rmc.MagneticVariation;
109
110 g_bVAR_Rx = true;
111 }
112
113 gRmcTime = m_NMEA0183.Rmc.UTCTime;
114 gRmcDate = m_NMEA0183.Rmc.Date;
115 } else
116 return false;
117
118 return true;
119}
120
121bool CommDecoder::DecodeHDM(std::string s, NavData& temp_data) {
122 wxString sentence(s.c_str());
123 wxString sentence3 = ProcessNMEA4Tags(sentence);
124 m_NMEA0183 << sentence3;
125
126 if (!m_NMEA0183.PreParse()) return false;
127 if (!m_NMEA0183.Parse()) return false;
128
129 temp_data.gHdm = m_NMEA0183.Hdm.DegreesMagnetic;
130
131 return true;
132}
133
134bool CommDecoder::DecodeTHS(std::string s, NavData& temp_data) {
135 wxString sentence(s.c_str());
136 wxString sentence3 = ProcessNMEA4Tags(sentence);
137 m_NMEA0183 << sentence3;
138
139 if (!m_NMEA0183.PreParse()) return false;
140 if (!m_NMEA0183.Parse()) return false;
141
142 // Handle only valid data A = Autonomous
143 if (!(m_NMEA0183.Ths.ModeInd == "A")) return false;
144 temp_data.gHdt = m_NMEA0183.Ths.TrueHeading;
145
146 return true;
147}
148
149bool CommDecoder::DecodeHDT(std::string s, NavData& temp_data) {
150 wxString sentence(s.c_str());
151 wxString sentence3 = ProcessNMEA4Tags(sentence);
152 m_NMEA0183 << sentence3;
153
154 if (!m_NMEA0183.PreParse()) return false;
155 if (!m_NMEA0183.Parse()) return false;
156
157 temp_data.gHdt = m_NMEA0183.Hdt.DegreesTrue;
158
159 return true;
160}
161
162bool CommDecoder::DecodeHDG(std::string s, NavData& temp_data) {
163 wxString sentence(s.c_str());
164 wxString sentence3 = ProcessNMEA4Tags(sentence);
165 m_NMEA0183 << sentence3;
166
167 if (!m_NMEA0183.PreParse()) return false;
168 if (!m_NMEA0183.Parse()) return false;
169
170 temp_data.gHdm = m_NMEA0183.Hdg.MagneticSensorHeadingDegrees;
171
172 // Any device sending VAR=0.0 can be assumed to not really know
173 // what the actual variation is, so in this case we use WMM if
174 // available
175 if ((!std::isnan(m_NMEA0183.Hdg.MagneticVariationDegrees)) &&
176 0.0 != m_NMEA0183.Hdg.MagneticVariationDegrees) {
177 if (m_NMEA0183.Hdg.MagneticVariationDirection == East)
178 temp_data.gVar = m_NMEA0183.Hdg.MagneticVariationDegrees;
179 else if (m_NMEA0183.Hdg.MagneticVariationDirection == West)
180 temp_data.gVar = -m_NMEA0183.Hdg.MagneticVariationDegrees;
181
182 g_bVAR_Rx = true;
183 }
184
185 return true;
186}
187
188bool CommDecoder::DecodeVTG(std::string s, NavData& temp_data) {
189 wxString sentence(s.c_str());
190 wxString sentence3 = ProcessNMEA4Tags(sentence);
191 m_NMEA0183 << sentence3;
192
193 if (!m_NMEA0183.PreParse()) return false;
194 if (!m_NMEA0183.Parse()) return false;
195
196 // FIXME (dave)if (g_own_ship_sog_cog_calc) return false;
197
198 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots))
199 temp_data.gSog = m_NMEA0183.Vtg.SpeedKnots;
200
201 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots) &&
202 !std::isnan(m_NMEA0183.Vtg.TrackDegreesTrue)) {
203 temp_data.gCog = m_NMEA0183.Vtg.TrackDegreesTrue;
204 }
205
206 // If COG-T is not available but COG-M is, then
207 // create COG-T from COG-M and gVar
208 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots) &&
209 !std::isnan(m_NMEA0183.Vtg.TrackDegreesMagnetic)) {
210 // establish gVar, if not already set
211 if (std::isnan(gVar) && (g_UserVar != 0.0)) gVar = g_UserVar;
212
213 double cogt = m_NMEA0183.Vtg.TrackDegreesMagnetic + gVar;
214 if (!std::isnan(cogt)) {
215 if (cogt < 0)
216 cogt += 360.0;
217 else if (cogt >= 360)
218 cogt -= 360.0;
219 temp_data.gCog = cogt;
220 }
221 }
222 return true;
223}
224
225bool CommDecoder::DecodeGLL(std::string s, NavData& temp_data) {
226 wxString sentence(s.c_str());
227 wxString sentence3 = ProcessNMEA4Tags(sentence);
228 m_NMEA0183 << sentence3;
229
230 if (!m_NMEA0183.PreParse()) return false;
231 if (!m_NMEA0183.Parse()) return false;
232
233 if (m_NMEA0183.Gll.IsDataValid == NTrue) {
234 double tlat, tlon;
235 if (ParsePosition(m_NMEA0183.Gll.Position, tlat, tlon)) {
236 temp_data.gLat = tlat;
237 temp_data.gLon = tlon;
238 } else
239 return false;
240 } else
241 return false;
242
243 return true;
244}
245
246bool CommDecoder::DecodeGSV(std::string s, NavData& temp_data) {
247 wxString sentence(s.c_str());
248 wxString sentence3 = ProcessNMEA4Tags(sentence);
249 m_NMEA0183 << sentence3;
250
251 if (!m_NMEA0183.PreParse()) return false;
252 if (!m_NMEA0183.Parse()) return false;
253
254 if (m_NMEA0183.Gsv.MessageNumber == 1)
255 temp_data.n_satellites = m_NMEA0183.Gsv.SatsInView;
256
257 return true;
258}
259
260bool CommDecoder::DecodeGGA(std::string s, NavData& temp_data) {
261 wxString sentence(s.c_str());
262 wxString sentence3 = ProcessNMEA4Tags(sentence);
263 m_NMEA0183 << sentence3;
264
265 if (!m_NMEA0183.PreParse()) return false;
266 if (!m_NMEA0183.Parse()) return false;
267
268 if (m_NMEA0183.Gga.GPSQuality > 0) {
269 double tlat, tlon;
270 if (ParsePosition(m_NMEA0183.Gga.Position, tlat, tlon)) {
271 temp_data.gLat = tlat;
272 temp_data.gLon = tlon;
273 } else
274 return false;
275
276 temp_data.n_satellites = m_NMEA0183.Gga.NumberOfSatellitesInUse;
277
278 } else
279 return false;
280
281 return true;
282}
283
284//---------------------------------------------------------------------
285// NMEA2000 PGN Decode
286//---------------------------------------------------------------------
287
288bool CommDecoder::DecodePGN129026(std::vector<unsigned char> v,
289 NavData& temp_data) {
290 unsigned char SID;
291 tN2kHeadingReference ref;
292 double COG, SOG;
293
294 if (ParseN2kPGN129026(v, SID, ref, COG, SOG)) {
295 temp_data.gCog = COG;
296 temp_data.gSog = SOG;
297 temp_data.SID = SID;
298 return true;
299 }
300
301 return false;
302}
303
304bool CommDecoder::DecodePGN129029(std::vector<unsigned char> v,
305 NavData& temp_data) {
306 unsigned char SID;
307 uint16_t DaysSince1970;
308 double SecondsSinceMidnight;
309 double Latitude, Longitude, Altitude;
310 tN2kGNSStype GNSStype;
311 tN2kGNSSmethod GNSSmethod;
312 unsigned char nSatellites;
313 double HDOP, PDOP, GeoidalSeparation;
314 unsigned char nReferenceStations;
315 tN2kGNSStype ReferenceStationType;
316 uint16_t ReferenceSationID;
317 double AgeOfCorrection;
318
319 if (ParseN2kPGN129029(v, SID, DaysSince1970, SecondsSinceMidnight, Latitude,
320 Longitude, Altitude, GNSStype, GNSSmethod, nSatellites,
321 HDOP, PDOP, GeoidalSeparation, nReferenceStations,
322 ReferenceStationType, ReferenceSationID,
323 AgeOfCorrection)) {
324 temp_data.gLat = Latitude;
325 temp_data.gLon = Longitude;
326 temp_data.SID = SID;
327
328 // Some devices produce "0" satelites for PGN 129029, even with a vaild fix
329 // One supposes that PGN 129540 should be used instead
330 // Here we decide that if a fix is valid, nSatellites must be > 0 to be
331 // reported in this PGN 129029
332 if ((GNSSmethod == N2kGNSSm_GNSSfix) || (GNSSmethod == N2kGNSSm_DGNSS) ||
333 (GNSSmethod == N2kGNSSm_PreciseGNSS)) {
334 if (nSatellites > 0) temp_data.n_satellites = nSatellites;
335 }
336
337 return true;
338 }
339
340 return false;
341}
342
343bool CommDecoder::DecodePGN127250(std::vector<unsigned char> v,
344 NavData& temp_data) {
345 unsigned char SID;
346 double Heading, Deviation, Variation;
347 tN2kHeadingReference ref;
348
349 if (ParseN2kPGN127250(v, SID, Heading, Deviation, Variation, ref)) {
350 temp_data.gHdt = N2kDoubleNA;
351 temp_data.gHdm = N2kDoubleNA;
352 if (ref == tN2kHeadingReference::N2khr_true)
353 temp_data.gHdt = Heading;
354 else if (ref == tN2kHeadingReference::N2khr_magnetic)
355 temp_data.gHdm = Heading;
356
357 temp_data.gVar = Variation;
358 temp_data.SID = SID;
359 return true;
360 }
361
362 return false;
363}
364
365bool CommDecoder::DecodePGN129025(std::vector<unsigned char> v,
366 NavData& temp_data) {
367 double Latitude, Longitude;
368
369 if (ParseN2kPGN129025(v, Latitude, Longitude)) {
370 temp_data.gLat = Latitude;
371 temp_data.gLon = Longitude;
372 return true;
373 }
374
375 return false;
376}
377
378bool CommDecoder::DecodePGN129540(std::vector<unsigned char> v,
379 NavData& temp_data) {
380 unsigned char SID;
381 uint8_t NumberOfSVs;
382 ;
383 tN2kRangeResidualMode Mode;
384
385 if (ParseN2kPGN129540(v, SID, Mode, NumberOfSVs)) {
386 temp_data.n_satellites = NumberOfSVs;
387 temp_data.SID = SID;
388 return true;
389 }
390
391 return false;
392}
393
394bool CommDecoder::DecodeSignalK(std::string s, NavData& temp_data) {
395 rapidjson::Document root;
396
397 root.Parse(s);
398 if (root.HasParseError()) return false;
399
400 if (root.HasMember("updates") && root["updates"].IsArray()) {
401 for (rapidjson::Value::ConstValueIterator itr = root["updates"].Begin();
402 itr != root["updates"].End(); ++itr) {
403 handleUpdate(*itr, temp_data);
404 }
405 }
406
407 return true;
408}
409
410void CommDecoder::handleUpdate(const rapidjson::Value& update,
411 NavData& temp_data) {
412 wxString sfixtime = "";
413
414 if (update.HasMember("timestamp")) {
415 sfixtime = update["timestamp"].GetString();
416 }
417 if (update.HasMember("source") && update["source"].HasMember("src")) {
418 src_string = update["source"]["src"].GetString();
419 }
420
421 if (update.HasMember("values") && update["values"].IsArray()) {
422 for (rapidjson::Value::ConstValueIterator itr = update["values"].Begin();
423 itr != update["values"].End(); ++itr) {
424 updateItem(*itr, sfixtime, temp_data);
425 }
426 }
427}
428
429void CommDecoder::updateItem(const rapidjson::Value& item, wxString& sfixtime,
430 NavData& temp_data) {
431 bool bposValid = false;
432 if (item.HasMember("path") && item.HasMember("value")) {
433 const wxString& update_path = item["path"].GetString();
434
435 if (update_path == _T("navigation.gnss.methodQuality")) {
436 // Record statically the GNSS status for this source in a hashmap
437 if (src_string.size()) {
438 if (item["value"] == "no GPS") { // no GPS GNSS Fix
439 GNSS_quality_map[src_string] = 0;
440 } else {
441 GNSS_quality_map[src_string] = 1;
442 }
443 }
444 }
445
446 if (update_path == _T("navigation.position") && !item["value"].IsNull()) {
447 bposValid = updateNavigationPosition(item["value"], sfixtime, temp_data);
448
449 // if "gnss.methodQuality" is reported as "no GPS", then invalidate gLat
450 // This will flow upstream, eventually triggering the GPS watchdog
451 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
452 if (GNSS_quality_map[src_string] == 0) {
453 temp_data.gLat = NAN;
454 }
455 }
456
457 } else if (update_path == _T("navigation.speedOverGround") &&
458 /*bposValid &&*/ !item["value"].IsNull()) {
459 updateNavigationSpeedOverGround(item["value"], sfixtime, temp_data);
460
461 // If the tracked "methodQuality" exists for this source,
462 // and state was recorded as "no GPS", set SOG = 0
463 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
464 if (GNSS_quality_map[src_string] == 0) {
465 temp_data.gSog = 0;
466 }
467 }
468
469 } else if (update_path == _T("navigation.courseOverGroundTrue") &&
470 /*bposValid &&*/ !item["value"].IsNull()) {
471 updateNavigationCourseOverGround(item["value"], sfixtime, temp_data);
472 } else if (update_path == _T("navigation.courseOverGroundMagnetic")) {
473 } else if (update_path ==
474 _T("navigation.gnss.satellites")) // From GGA sats in use
475 {
476 updateGnssSatellites(item["value"], sfixtime, temp_data);
477 } else if (update_path ==
478 _T("navigation.gnss.satellitesInView")) // From GSV sats in view
479 {
480 updateGnssSatellites(item["value"], sfixtime, temp_data);
481 } else if (update_path == _T("navigation.headingTrue")) {
482 if (!item["value"].IsNull())
483 updateHeadingTrue(item["value"], sfixtime, temp_data);
484 } else if (update_path == _T("navigation.headingMagnetic")) {
485 if (!item["value"].IsNull())
486 updateHeadingMagnetic(item["value"], sfixtime, temp_data);
487 } else if (update_path == _T("navigation.magneticVariation")) {
488 if (!item["value"].IsNull())
489 updateMagneticVariance(item["value"], sfixtime, temp_data);
490 } else {
491 // wxLogMessage(wxString::Format(_T("** Signal K unhandled update: %s"),
492 // update_path));
493 }
494 }
495}
496
497bool CommDecoder::updateNavigationPosition(const rapidjson::Value& value,
498 const wxString& sfixtime,
499 NavData& temp_data) {
500 if ((value.HasMember("latitude") && value["latitude"].IsDouble()) &&
501 (value.HasMember("longitude") && value["longitude"].IsDouble())) {
502 // wxLogMessage(_T(" ***** Position Update"));
503 temp_data.gLat = value["latitude"].GetDouble();
504 temp_data.gLon = value["longitude"].GetDouble();
505 return true;
506 } else {
507 return false;
508 }
509}
510
511void CommDecoder::updateNavigationSpeedOverGround(const rapidjson::Value& value,
512 const wxString& sfixtime,
513 NavData& temp_data) {
514 double sog_ms = value.GetDouble();
515 double sog_knot = sog_ms * 1.9438444924406; // m/s to knots
516 // wxLogMessage(wxString::Format(_T(" ***** SOG: %f, %f"), sog_ms, sog_knot));
517 temp_data.gSog = sog_knot;
518}
519
520void CommDecoder::updateNavigationCourseOverGround(
521 const rapidjson::Value& value, const wxString& sfixtime,
522 NavData& temp_data) {
523 double cog_rad = value.GetDouble();
524 double cog_deg = GEODESIC_RAD2DEG(cog_rad);
525 // wxLogMessage(wxString::Format(_T(" ***** COG: %f, %f"), cog_rad, cog_deg));
526 temp_data.gCog = cog_deg;
527}
528
529void CommDecoder::updateGnssSatellites(const rapidjson::Value& value,
530 const wxString& sfixtime,
531 NavData& temp_data) {
532 if (value.IsInt()) {
533 if (value.GetInt() > 0) {
534 temp_data.n_satellites = value.GetInt();
535 }
536 } else if ((value.HasMember("count") && value["count"].IsInt())) {
537 temp_data.n_satellites = value["count"].GetInt();
538 }
539 // If "gnss.methodQuality" is "no GPS", then clear the satellite count
540 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
541 if (GNSS_quality_map[src_string] == 0) {
542 temp_data.n_satellites = 0;
543 }
544 }
545}
546
547void CommDecoder::updateHeadingTrue(const rapidjson::Value& value,
548 const wxString& sfixtime,
549 NavData& temp_data) {
550 temp_data.gHdt = GEODESIC_RAD2DEG(value.GetDouble());
551}
552
553void CommDecoder::updateHeadingMagnetic(const rapidjson::Value& value,
554 const wxString& sfixtime,
555 NavData& temp_data) {
556 temp_data.gHdm = GEODESIC_RAD2DEG(value.GetDouble());
557}
558
559void CommDecoder::updateMagneticVariance(const rapidjson::Value& value,
560 const wxString& sfixtime,
561 NavData& temp_data) {
562 temp_data.gVar = GEODESIC_RAD2DEG(value.GetDouble());
563}