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