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