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::DecodeHDT(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 temp_data.gHdt = m_NMEA0183.Hdt.DegreesTrue;
143
144 return true;
145}
146
147bool CommDecoder::DecodeHDG(std::string s, NavData& temp_data) {
148 wxString sentence(s.c_str());
149 wxString sentence3 = ProcessNMEA4Tags(sentence);
150 m_NMEA0183 << sentence3;
151
152 if (!m_NMEA0183.PreParse()) return false;
153 if (!m_NMEA0183.Parse()) return false;
154
155 temp_data.gHdm = m_NMEA0183.Hdg.MagneticSensorHeadingDegrees;
156
157 // Any device sending VAR=0.0 can be assumed to not really know
158 // what the actual variation is, so in this case we use WMM if
159 // available
160 if ((!std::isnan(m_NMEA0183.Hdg.MagneticVariationDegrees)) &&
161 0.0 != m_NMEA0183.Hdg.MagneticVariationDegrees) {
162 if (m_NMEA0183.Hdg.MagneticVariationDirection == East)
163 temp_data.gVar = m_NMEA0183.Hdg.MagneticVariationDegrees;
164 else if (m_NMEA0183.Hdg.MagneticVariationDirection == West)
165 temp_data.gVar = -m_NMEA0183.Hdg.MagneticVariationDegrees;
166
167 g_bVAR_Rx = true;
168 }
169
170 return true;
171}
172
173bool CommDecoder::DecodeVTG(std::string s, NavData& temp_data) {
174 wxString sentence(s.c_str());
175 wxString sentence3 = ProcessNMEA4Tags(sentence);
176 m_NMEA0183 << sentence3;
177
178 if (!m_NMEA0183.PreParse()) return false;
179 if (!m_NMEA0183.Parse()) return false;
180
181 // FIXME (dave)if (g_own_ship_sog_cog_calc) return false;
182
183 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots))
184 temp_data.gSog = m_NMEA0183.Vtg.SpeedKnots;
185
186 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots) &&
187 !std::isnan(m_NMEA0183.Vtg.TrackDegreesTrue)) {
188 temp_data.gCog = m_NMEA0183.Vtg.TrackDegreesTrue;
189 }
190
191 // If COG-T is not available but COG-M is, then
192 // create COG-T from COG-M and gVar
193 if (!std::isnan(m_NMEA0183.Vtg.SpeedKnots) &&
194 !std::isnan(m_NMEA0183.Vtg.TrackDegreesMagnetic)) {
195 // establish gVar, if not already set
196 if (std::isnan(gVar) && (g_UserVar != 0.0)) gVar = g_UserVar;
197
198 double cogt = m_NMEA0183.Vtg.TrackDegreesMagnetic + gVar;
199 if (!std::isnan(cogt)) {
200 if (cogt < 0)
201 cogt += 360.0;
202 else if (cogt >= 360)
203 cogt -= 360.0;
204 temp_data.gCog = cogt;
205 }
206 }
207 return true;
208}
209
210bool CommDecoder::DecodeGLL(std::string s, NavData& temp_data) {
211 wxString sentence(s.c_str());
212 wxString sentence3 = ProcessNMEA4Tags(sentence);
213 m_NMEA0183 << sentence3;
214
215 if (!m_NMEA0183.PreParse()) return false;
216 if (!m_NMEA0183.Parse()) return false;
217
218 if (m_NMEA0183.Gll.IsDataValid == NTrue) {
219 double tlat, tlon;
220 if (ParsePosition(m_NMEA0183.Gll.Position, tlat, tlon)) {
221 temp_data.gLat = tlat;
222 temp_data.gLon = tlon;
223 } else
224 return false;
225 } else
226 return false;
227
228 return true;
229}
230
231bool CommDecoder::DecodeGSV(std::string s, NavData& temp_data) {
232 wxString sentence(s.c_str());
233 wxString sentence3 = ProcessNMEA4Tags(sentence);
234 m_NMEA0183 << sentence3;
235
236 if (!m_NMEA0183.PreParse()) return false;
237 if (!m_NMEA0183.Parse()) return false;
238
239 if (m_NMEA0183.Gsv.MessageNumber == 1)
240 temp_data.n_satellites = m_NMEA0183.Gsv.SatsInView;
241
242 return true;
243}
244
245bool CommDecoder::DecodeGGA(std::string s, NavData& temp_data) {
246 wxString sentence(s.c_str());
247 wxString sentence3 = ProcessNMEA4Tags(sentence);
248 m_NMEA0183 << sentence3;
249
250 if (!m_NMEA0183.PreParse()) return false;
251 if (!m_NMEA0183.Parse()) return false;
252
253 if (m_NMEA0183.Gga.GPSQuality > 0) {
254 double tlat, tlon;
255 if (ParsePosition(m_NMEA0183.Gga.Position, tlat, tlon)) {
256 temp_data.gLat = tlat;
257 temp_data.gLon = tlon;
258 } else
259 return false;
260
261 temp_data.n_satellites = m_NMEA0183.Gga.NumberOfSatellitesInUse;
262
263 } else
264 return false;
265
266 return true;
267}
268
269//---------------------------------------------------------------------
270// NMEA2000 PGN Decode
271//---------------------------------------------------------------------
272
273bool CommDecoder::DecodePGN129026(std::vector<unsigned char> v,
274 NavData& temp_data) {
275 unsigned char SID;
276 tN2kHeadingReference ref;
277 double COG, SOG;
278
279 if (ParseN2kPGN129026(v, SID, ref, COG, SOG)) {
280 temp_data.gCog = COG;
281 temp_data.gSog = SOG;
282 temp_data.SID = SID;
283 return true;
284 }
285
286 return false;
287}
288
289bool CommDecoder::DecodePGN129029(std::vector<unsigned char> v,
290 NavData& temp_data) {
291 unsigned char SID;
292 uint16_t DaysSince1970;
293 double SecondsSinceMidnight;
294 double Latitude, Longitude, Altitude;
295 tN2kGNSStype GNSStype;
296 tN2kGNSSmethod GNSSmethod;
297 unsigned char nSatellites;
298 double HDOP, PDOP, GeoidalSeparation;
299 unsigned char nReferenceStations;
300 tN2kGNSStype ReferenceStationType;
301 uint16_t ReferenceSationID;
302 double AgeOfCorrection;
303
304 if (ParseN2kPGN129029(v, SID, DaysSince1970, SecondsSinceMidnight, Latitude,
305 Longitude, Altitude, GNSStype, GNSSmethod, nSatellites,
306 HDOP, PDOP, GeoidalSeparation, nReferenceStations,
307 ReferenceStationType, ReferenceSationID,
308 AgeOfCorrection)) {
309 temp_data.gLat = Latitude;
310 temp_data.gLon = Longitude;
311 temp_data.SID = SID;
312
313 // Some devices produce "0" satelites for PGN 129029, even with a vaild fix
314 // One supposes that PGN 129540 should be used instead
315 // Here we decide that if a fix is valid, nSatellites must be > 0 to be
316 // reported in this PGN 129029
317 if ((GNSSmethod == N2kGNSSm_GNSSfix) || (GNSSmethod == N2kGNSSm_DGNSS) ||
318 (GNSSmethod == N2kGNSSm_PreciseGNSS)) {
319 if (nSatellites > 0) temp_data.n_satellites = nSatellites;
320 }
321
322 return true;
323 }
324
325 return false;
326}
327
328bool CommDecoder::DecodePGN127250(std::vector<unsigned char> v,
329 NavData& temp_data) {
330 unsigned char SID;
331 double Heading, Deviation, Variation;
332 tN2kHeadingReference ref;
333
334 if (ParseN2kPGN127250(v, SID, Heading, Deviation, Variation, ref)) {
335 temp_data.gHdt = N2kDoubleNA;
336 temp_data.gHdm = N2kDoubleNA;
337 if (ref == tN2kHeadingReference::N2khr_true)
338 temp_data.gHdt = Heading;
339 else if (ref == tN2kHeadingReference::N2khr_magnetic)
340 temp_data.gHdm = Heading;
341
342 temp_data.gVar = Variation;
343 temp_data.SID = SID;
344 return true;
345 }
346
347 return false;
348}
349
350bool CommDecoder::DecodePGN129025(std::vector<unsigned char> v,
351 NavData& temp_data) {
352 double Latitude, Longitude;
353
354 if (ParseN2kPGN129025(v, Latitude, Longitude)) {
355 temp_data.gLat = Latitude;
356 temp_data.gLon = Longitude;
357 return true;
358 }
359
360 return false;
361}
362
363bool CommDecoder::DecodePGN129540(std::vector<unsigned char> v,
364 NavData& temp_data) {
365 unsigned char SID;
366 uint8_t NumberOfSVs;
367 ;
368 tN2kRangeResidualMode Mode;
369
370 if (ParseN2kPGN129540(v, SID, Mode, NumberOfSVs)) {
371 temp_data.n_satellites = NumberOfSVs;
372 temp_data.SID = SID;
373 return true;
374 }
375
376 return false;
377}
378
379bool CommDecoder::DecodeSignalK(std::string s, NavData& temp_data) {
380 rapidjson::Document root;
381
382 root.Parse(s);
383 if (root.HasParseError()) return false;
384
385 if (root.HasMember("updates") && root["updates"].IsArray()) {
386 for (rapidjson::Value::ConstValueIterator itr = root["updates"].Begin();
387 itr != root["updates"].End(); ++itr) {
388 handleUpdate(*itr, temp_data);
389 }
390 }
391
392 return true;
393}
394
395void CommDecoder::handleUpdate(const rapidjson::Value& update,
396 NavData& temp_data) {
397 wxString sfixtime = "";
398
399 if (update.HasMember("timestamp")) {
400 sfixtime = update["timestamp"].GetString();
401 }
402 if (update.HasMember("source") && update["source"].HasMember("src")) {
403 src_string = update["source"]["src"].GetString();
404 }
405
406 if (update.HasMember("values") && update["values"].IsArray()) {
407 for (rapidjson::Value::ConstValueIterator itr = update["values"].Begin();
408 itr != update["values"].End(); ++itr) {
409 updateItem(*itr, sfixtime, temp_data);
410 }
411 }
412}
413
414void CommDecoder::updateItem(const rapidjson::Value& item, wxString& sfixtime,
415 NavData& temp_data) {
416 bool bposValid = false;
417 if (item.HasMember("path") && item.HasMember("value")) {
418 const wxString& update_path = item["path"].GetString();
419
420 if (update_path == _T("navigation.gnss.methodQuality")) {
421 // Record statically the GNSS status for this source in a hashmap
422 if (src_string.size()) {
423 if (item["value"] == "no GPS") { // no GPS GNSS Fix
424 GNSS_quality_map[src_string] = 0;
425 } else {
426 GNSS_quality_map[src_string] = 1;
427 }
428 }
429 }
430
431 if (update_path == _T("navigation.position") && !item["value"].IsNull()) {
432 bposValid = updateNavigationPosition(item["value"], sfixtime, temp_data);
433
434 // if "gnss.methodQuality" is reported as "no GPS", then invalidate gLat
435 // This will flow upstream, eventually triggering the GPS watchdog
436 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
437 if (GNSS_quality_map[src_string] == 0) {
438 temp_data.gLat = NAN;
439 }
440 }
441
442 } else if (update_path == _T("navigation.speedOverGround") &&
443 /*bposValid &&*/ !item["value"].IsNull()) {
444 updateNavigationSpeedOverGround(item["value"], sfixtime, temp_data);
445
446 // If the tracked "methodQuality" exists for this source,
447 // and state was recorded as "no GPS", set SOG = 0
448 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
449 if (GNSS_quality_map[src_string] == 0) {
450 temp_data.gSog = 0;
451 }
452 }
453
454 } else if (update_path == _T("navigation.courseOverGroundTrue") &&
455 /*bposValid &&*/ !item["value"].IsNull()) {
456 updateNavigationCourseOverGround(item["value"], sfixtime, temp_data);
457 } else if (update_path == _T("navigation.courseOverGroundMagnetic")) {
458 } else if (update_path ==
459 _T("navigation.gnss.satellites")) // From GGA sats in use
460 {
461 updateGnssSatellites(item["value"], sfixtime, temp_data);
462 } else if (update_path ==
463 _T("navigation.gnss.satellitesInView")) // From GSV sats in view
464 {
465 updateGnssSatellites(item["value"], sfixtime, temp_data);
466 } else if (update_path == _T("navigation.headingTrue")) {
467 if (!item["value"].IsNull())
468 updateHeadingTrue(item["value"], sfixtime, temp_data);
469 } else if (update_path == _T("navigation.headingMagnetic")) {
470 if (!item["value"].IsNull())
471 updateHeadingMagnetic(item["value"], sfixtime, temp_data);
472 } else if (update_path == _T("navigation.magneticVariation")) {
473 if (!item["value"].IsNull())
474 updateMagneticVariance(item["value"], sfixtime, temp_data);
475 } else {
476 // wxLogMessage(wxString::Format(_T("** Signal K unhandled update: %s"),
477 // update_path));
478 }
479 }
480}
481
482bool CommDecoder::updateNavigationPosition(const rapidjson::Value& value,
483 const wxString& sfixtime,
484 NavData& temp_data) {
485 if ((value.HasMember("latitude") && value["latitude"].IsDouble()) &&
486 (value.HasMember("longitude") && value["longitude"].IsDouble())) {
487 // wxLogMessage(_T(" ***** Position Update"));
488 temp_data.gLat = value["latitude"].GetDouble();
489 temp_data.gLon = value["longitude"].GetDouble();
490 return true;
491 } else {
492 return false;
493 }
494}
495
496void CommDecoder::updateNavigationSpeedOverGround(const rapidjson::Value& value,
497 const wxString& sfixtime,
498 NavData& temp_data) {
499 double sog_ms = value.GetDouble();
500 double sog_knot = sog_ms * 1.9438444924406; // m/s to knots
501 // wxLogMessage(wxString::Format(_T(" ***** SOG: %f, %f"), sog_ms, sog_knot));
502 temp_data.gSog = sog_knot;
503}
504
505void CommDecoder::updateNavigationCourseOverGround(
506 const rapidjson::Value& value, const wxString& sfixtime,
507 NavData& temp_data) {
508 double cog_rad = value.GetDouble();
509 double cog_deg = GEODESIC_RAD2DEG(cog_rad);
510 // wxLogMessage(wxString::Format(_T(" ***** COG: %f, %f"), cog_rad, cog_deg));
511 temp_data.gCog = cog_deg;
512}
513
514void CommDecoder::updateGnssSatellites(const rapidjson::Value& value,
515 const wxString& sfixtime,
516 NavData& temp_data) {
517 if (value.IsInt()) {
518 if (value.GetInt() > 0) {
519 temp_data.n_satellites = value.GetInt();
520 }
521 } else if ((value.HasMember("count") && value["count"].IsInt())) {
522 temp_data.n_satellites = value["count"].GetInt();
523 }
524 // If "gnss.methodQuality" is "no GPS", then clear the satellite count
525 if (GNSS_quality_map.find(src_string) != GNSS_quality_map.end()) {
526 if (GNSS_quality_map[src_string] == 0) {
527 temp_data.n_satellites = 0;
528 }
529 }
530}
531
532void CommDecoder::updateHeadingTrue(const rapidjson::Value& value,
533 const wxString& sfixtime,
534 NavData& temp_data) {
535 temp_data.gHdt = GEODESIC_RAD2DEG(value.GetDouble());
536}
537
538void CommDecoder::updateHeadingMagnetic(const rapidjson::Value& value,
539 const wxString& sfixtime,
540 NavData& temp_data) {
541 temp_data.gHdm = GEODESIC_RAD2DEG(value.GetDouble());
542}
543
544void CommDecoder::updateMagneticVariance(const rapidjson::Value& value,
545 const wxString& sfixtime,
546 NavData& temp_data) {
547 temp_data.gVar = GEODESIC_RAD2DEG(value.GetDouble());
548}