sbf_blocks.hpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
31 #pragma once
32 
33 #include <cstdint>
34 
36 static const uint8_t NR_OF_LOGICALCHANNELS = 80;
38 static const uint8_t MAX_NB_INMARSATCHANNELS = 1;
40 static const uint8_t MAX_NR_OF_SIGNALS_PER_SATELLITE = 7;
42 static const uint8_t NR_OF_ANTENNAS = 3;
44 static const uint8_t MAXSB_NBRANTENNA = 4;
46 static const uint8_t MAXSB_CHANNELSATINFO =
49 static const uint16_t MAXSB_CHANNELSTATEINFO =
52 static const uint8_t MAXSB_MEASEPOCH_T1 =
55 static const uint16_t MAXSB_MEASEPOCH_T2 =
59 static const uint8_t MAXSB_NBVECTORINFO = 30;
60 
62 static const uint8_t SBF_SYNC_1 = 0x24;
64 static const uint8_t SBF_SYNC_2 = 0x40;
65 
66 // C++
67 #include <algorithm>
68 #include <type_traits>
69 // Boost
70 #include <boost/spirit/include/qi.hpp>
71 // ROSaic
72 #ifdef ROS2
74 #endif
75 #ifdef ROS1
77 #endif
79 
92 {
93  uint8_t antenna;
94  uint16_t tracking_status;
95  uint16_t pvt_status;
96  uint16_t pvt_info;
97 };
98 
104 {
105  uint8_t sv_id;
106  uint8_t freq_nr;
107  uint16_t az_rise_set;
108  uint16_t health_status;
109  int8_t elev;
110  uint8_t n2;
111  uint8_t rx_channel;
112 
113  std::vector<ChannelStateInfo> stateInfo;
114 };
115 
121 {
123 
124  uint8_t n;
125  uint8_t sb1_length;
126  uint8_t sb2_length;
127 
128  std::vector<ChannelSatInfo> satInfo;
129 };
130 
135 struct Dop
136 {
138 
139  uint8_t nr_sv;
140  double pdop;
141  double tdop;
142  double hdop;
143  double vdop;
144  float hpl;
145  float vpl;
146 };
147 
153 {
155 
156  std::string marker_name;
157  std::string marker_number;
158  std::string observer;
159  std::string agency;
160  std::string rx_serial_number;
161  std::string rx_name;
162  std::string rx_version;
163  std::string ant_serial_nbr;
164  std::string ant_type;
165  float delta_h; /* [m] */
166  float delta_e; /* [m] */
167  float delta_n; /* [m] */
168  std::string marker_type;
169  std::string gnss_fw_version;
170  std::string product_name;
171  double latitude;
172  double longitude;
173  float height;
174  std::string station_code;
175  uint8_t monument_idx;
176  uint8_t receiver_idx;
177  std::string country_code;
178 };
179 
185 {
187 
188  uint8_t n = 0;
189 
190  std::vector<uint16_t> indicators;
191 };
192 
196 struct AgcState
197 {
198  uint8_t frontend_id;
199  int8_t gain;
200  uint8_t sample_var;
201  uint8_t blanking_stat;
202 };
203 
209 {
211 
212  uint8_t cpu_load;
213  uint8_t ext_error;
214  uint32_t up_time;
215  uint32_t rx_status;
216  uint32_t rx_error;
217  uint8_t n;
218  uint8_t sb_length;
219  uint8_t cmd_count;
220  uint8_t temperature;
221 
222  std::vector<AgcState> agc_state;
223 };
224 
230 static const std::array<uint16_t, 256> CRC_LOOK_UP = {
231  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129,
232  0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252,
233  0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c,
234  0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
235  0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
236  0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
237  0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861,
238  0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
239  0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
240  0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5,
241  0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b,
242  0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
243  0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9,
244  0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
245  0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c,
246  0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
247  0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3,
248  0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
249  0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676,
250  0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
251  0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c,
252  0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16,
253  0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
254  0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
255  0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36,
256  0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
257 
258 namespace qi = boost::spirit::qi;
259 
264 template <typename Val>
265 void setDoNotUse(Val& s)
266 {
267  static_assert(
268  std::is_same<uint16_t, Val>::value || std::is_same<uint32_t, Val>::value ||
269  std::is_same<uint64_t, Val>::value || std::is_same<float, Val>::value ||
270  std::is_same<double, Val>::value);
271 
272  if (std::is_same<uint16_t, Val>::value)
273  {
274  s = 65535;
275  } else if (std::is_same<uint32_t, Val>::value)
276  {
277  s = 4294967295ul;
278  } else if (std::is_same<float, Val>::value)
279  {
280  s = std::numeric_limits<float>::quiet_NaN();
281  } else if (std::is_same<double, Val>::value)
282  {
283  s = std::numeric_limits<double>::quiet_NaN();
284  }
285  // TODO add more
286 }
287 
292 template <typename Val>
293 void doNotUseToNaN(Val& s)
294 {
295  static_assert(std::is_same<float, Val>::value ||
296  std::is_same<double, Val>::value);
297 
298  if constexpr (std::is_same<float, Val>::value)
299  {
300  if (s == -2e10f)
301  s = std::numeric_limits<float>::quiet_NaN();
302  } else if constexpr (std::is_same<double, Val>::value)
303  {
304  if (s == -2e10)
305  s = std::numeric_limits<double>::quiet_NaN();
306  }
307 }
308 
313 template <typename It, typename Val>
314 void qiLittleEndianParser(It& it, Val& val)
315 {
316  static_assert(
317  std::is_same<int8_t, Val>::value || std::is_same<uint8_t, Val>::value ||
318  std::is_same<int16_t, Val>::value || std::is_same<uint16_t, Val>::value ||
319  std::is_same<int32_t, Val>::value || std::is_same<uint32_t, Val>::value ||
320  std::is_same<int64_t, Val>::value || std::is_same<uint64_t, Val>::value ||
321  std::is_same<float, Val>::value || std::is_same<double, Val>::value);
322 
323  if constexpr (std::is_same<int8_t, Val>::value)
324  {
325  qi::parse(it, it + 1, qi::char_, val);
326  } else if constexpr (std::is_same<uint8_t, Val>::value)
327  {
328  qi::parse(it, it + 1, qi::byte_, val);
329  } else if constexpr ((std::is_same<int16_t, Val>::value) ||
330  (std::is_same<uint16_t, Val>::value))
331  {
332  qi::parse(it, it + 2, qi::little_word, val);
333  } else if constexpr ((std::is_same<int32_t, Val>::value) ||
334  (std::is_same<uint32_t, Val>::value))
335  {
336  qi::parse(it, it + 4, qi::little_dword, val);
337  } else if constexpr ((std::is_same<int64_t, Val>::value) ||
338  (std::is_same<uint64_t, Val>::value))
339  {
340  qi::parse(it, it + 8, qi::little_qword, val);
341  } else if constexpr (std::is_same<float, Val>::value)
342  {
343  qi::parse(it, it + 4, qi::little_bin_float, val);
344  doNotUseToNaN(val);
345  } else if constexpr (std::is_same<double, Val>::value)
346  {
347  qi::parse(it, it + 8, qi::little_bin_double, val);
348  doNotUseToNaN(val);
349  }
350 }
351 
356 template <typename It>
357 void qiCharsToStringParser(It& it, std::string& val, std::size_t num)
358 {
359  val.clear();
360  qi::parse(it, it + num, qi::repeat(num)[qi::char_], val);
361  // remove string termination characters '\0'
362  val.erase(std::remove(val.begin(), val.end(), '\0'), val.end());
363 }
364 
369 template <typename It, typename Hdr>
370 [[nodiscard]] bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header)
371 {
372  qiLittleEndianParser(it, block_header.sync_1);
373  if (block_header.sync_1 != SBF_SYNC_1)
374  {
375  node->log(log_level::ERROR, "Parse error: Wrong sync byte 1.");
376  return false;
377  }
378  qiLittleEndianParser(it, block_header.sync_2);
379  if (block_header.sync_2 != SBF_SYNC_2)
380  {
381  node->log(log_level::ERROR, "Parse error: Wrong sync byte 2.");
382  return false;
383  }
384  qiLittleEndianParser(it, block_header.crc);
385  uint16_t ID;
386  qiLittleEndianParser(it, ID);
387  block_header.id = ID & 8191; // lower 13 bits are id
388  block_header.revision = ID >> 13; // upper 3 bits are revision
389  qiLittleEndianParser(it, block_header.length);
390  qiLittleEndianParser(it, block_header.tow);
391  qiLittleEndianParser(it, block_header.wnc);
392  return true;
393 }
394 
399 template <typename It>
400 void ChannelStateInfoParser(It& it, ChannelStateInfo& msg, uint8_t sb2_length)
401 {
402  qiLittleEndianParser(it, msg.antenna);
403  ++it; // reserved
407  std::advance(it, sb2_length - 8); // skip padding
408 };
409 
414 template <typename It>
415 [[nodiscard]] bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it,
416  ChannelSatInfo& msg, uint8_t sb1_length,
417  uint8_t sb2_length)
418 {
419  qiLittleEndianParser(it, msg.sv_id);
420  qiLittleEndianParser(it, msg.freq_nr);
421  std::advance(it, 2); // reserved
424  qiLittleEndianParser(it, msg.elev);
425  qiLittleEndianParser(it, msg.n2);
426  if (msg.n2 > MAXSB_CHANNELSTATEINFO)
427  {
428  node->log(log_level::ERROR, "Parse error: Too many ChannelStateInfo " +
429  std::to_string(msg.n2));
430  return false;
431  }
433  ++it; // reserved
434  std::advance(it, sb1_length - 12); // skip padding
435  msg.stateInfo.resize(msg.n2);
436  for (auto& stateInfo : msg.stateInfo)
437  {
438  ChannelStateInfoParser(it, stateInfo, sb2_length);
439  }
440  return true;
441 };
442 
447 template <typename It>
448 [[nodiscard]] bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd,
449  ChannelStatus& msg)
450 {
451  if (!BlockHeaderParser(node, it, msg.block_header))
452  return false;
453  if (msg.block_header.id != 4013)
454  {
455  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
456  std::to_string(msg.block_header.id));
457  return false;
458  }
459  qiLittleEndianParser(it, msg.n);
460  if (msg.n > MAXSB_CHANNELSATINFO)
461  {
462  node->log(log_level::ERROR,
463  "Parse error: Too many ChannelSatInfo " + std::to_string(msg.n));
464  return false;
465  }
468  std::advance(it, 3); // reserved
469  msg.satInfo.resize(msg.n);
470  for (auto& satInfo : msg.satInfo)
471  {
472  if (!ChannelSatInfoParser(node, it, satInfo, msg.sb1_length, msg.sb2_length))
473  return false;
474  }
475  if (it > itEnd)
476  {
477  node->log(log_level::ERROR, "Parse error: iterator past end.");
478  return false;
479  }
480  return true;
481 };
482 
487 template <typename It>
488 [[nodiscard]] bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, Dop& msg)
489 {
490 
491  if (!BlockHeaderParser(node, it, msg.block_header))
492  return false;
493  if (msg.block_header.id != 4001)
494  {
495  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
496  std::to_string(msg.block_header.id));
497  return false;
498  }
499  qiLittleEndianParser(it, msg.nr_sv);
500  ++it; // reserved
501  uint16_t temp;
502  qiLittleEndianParser(it, temp);
503  msg.pdop = temp / 100.0;
504  qiLittleEndianParser(it, temp);
505  msg.tdop = temp / 100.0;
506  qiLittleEndianParser(it, temp);
507  msg.hdop = temp / 100.0;
508  qiLittleEndianParser(it, temp);
509  msg.vdop = temp / 100.0;
510  qiLittleEndianParser(it, msg.hpl);
511  qiLittleEndianParser(it, msg.vpl);
512  if (it > itEnd)
513  {
514  node->log(log_level::ERROR, "Parse error: iterator past end.");
515  return false;
516  }
517  return true;
518 };
519 
524 template <typename It>
526  uint8_t sb2_length)
527 {
528  qiLittleEndianParser(it, msg.type);
529  qiLittleEndianParser(it, msg.lock_time);
530  qiLittleEndianParser(it, msg.cn0);
531  qiLittleEndianParser(it, msg.offsets_msb);
532  qiLittleEndianParser(it, msg.carrier_msb);
533  qiLittleEndianParser(it, msg.obs_info);
534  qiLittleEndianParser(it, msg.code_offset_lsb);
535  qiLittleEndianParser(it, msg.carrier_lsb);
536  qiLittleEndianParser(it, msg.doppler_offset_lsb);
537  std::advance(it, sb2_length - 12); // skip padding
538 };
539 
544 template <typename It>
545 [[nodiscard]] bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it,
547  uint8_t sb1_length,
548  uint8_t sb2_length)
549 {
550  qiLittleEndianParser(it, msg.rx_channel);
551  qiLittleEndianParser(it, msg.type);
552  qiLittleEndianParser(it, msg.sv_id);
553  qiLittleEndianParser(it, msg.misc);
554  qiLittleEndianParser(it, msg.code_lsb);
555  qiLittleEndianParser(it, msg.doppler);
556  qiLittleEndianParser(it, msg.carrier_lsb);
557  qiLittleEndianParser(it, msg.carrier_msb);
558  qiLittleEndianParser(it, msg.cn0);
559  qiLittleEndianParser(it, msg.lock_time);
560  qiLittleEndianParser(it, msg.obs_info);
561  qiLittleEndianParser(it, msg.n2);
562  std::advance(it, sb1_length - 20); // skip padding
563  if (msg.n2 > MAXSB_MEASEPOCH_T2)
564  {
565  node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType2 " +
566  std::to_string(msg.n2));
567  return false;
568  }
569  msg.type2.resize(msg.n2);
570  for (auto& type2 : msg.type2)
571  {
572  MeasEpochChannelType2Parser(it, type2, sb2_length);
573  }
574  return true;
575 };
576 
581 template <typename It>
582 [[nodiscard]] bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd,
583  MeasEpochMsg& msg)
584 {
585  if (!BlockHeaderParser(node, it, msg.block_header))
586  return false;
587  if (msg.block_header.id != 4027)
588  {
589  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
590  std::to_string(msg.block_header.id));
591  return false;
592  }
593  qiLittleEndianParser(it, msg.n);
594  if (msg.n > MAXSB_MEASEPOCH_T1)
595  {
596  node->log(log_level::ERROR, "Parse error: Too many MeasEpochChannelType1 " +
597  std::to_string(msg.n));
598  return false;
599  }
600  qiLittleEndianParser(it, msg.sb1_length);
601  qiLittleEndianParser(it, msg.sb2_length);
602  qiLittleEndianParser(it, msg.common_flags);
603  if (msg.block_header.revision > 0)
604  qiLittleEndianParser(it, msg.cum_clk_jumps);
605  ++it; // reserved
606  msg.type1.resize(msg.n);
607  for (auto& type1 : msg.type1)
608  {
609  if (!MeasEpochChannelType1Parser(node, it, type1, msg.sb1_length,
610  msg.sb2_length))
611  return false;
612  }
613  if (it > itEnd)
614  {
615  node->log(log_level::ERROR, "Parse error: iterator past end.");
616  return false;
617  }
618  return true;
619 };
620 
625 template <typename It>
626 [[nodiscard]] bool GalAuthStatusParser(ROSaicNodeBase* node, It it, It itEnd,
627  GalAuthStatusMsg& msg)
628 {
629  if (!BlockHeaderParser(node, it, msg.block_header))
630  return false;
631  if (msg.block_header.id != 4245)
632  {
633  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
634  std::to_string(msg.block_header.id));
635  return false;
636  }
637  qiLittleEndianParser(it, msg.osnma_status);
638  qiLittleEndianParser(it, msg.trusted_time_delta);
639  qiLittleEndianParser(it, msg.gal_active_mask);
640  qiLittleEndianParser(it, msg.gal_authentic_mask);
641  qiLittleEndianParser(it, msg.gps_active_mask);
642  qiLittleEndianParser(it, msg.gps_authentic_mask);
643  if (it > itEnd)
644  {
645  node->log(log_level::ERROR, "Parse error: iterator past end.");
646  return false;
647  }
648  return true;
649 };
650 
655 template <typename It>
656 void RfBandParser(It& it, RfBandMsg& msg, uint8_t sb_length)
657 {
658  qiLittleEndianParser(it, msg.frequency);
659  qiLittleEndianParser(it, msg.bandwidth);
660  qiLittleEndianParser(it, msg.info);
661  std::advance(it, sb_length - 7); // skip padding
662 };
663 
668 template <typename It>
669 [[nodiscard]] bool RfStatusParser(ROSaicNodeBase* node, It it, It itEnd,
670  RfStatusMsg& msg)
671 {
672  if (!BlockHeaderParser(node, it, msg.block_header))
673  return false;
674  if (msg.block_header.id != 4092)
675  {
676  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
677  std::to_string(msg.block_header.id));
678  return false;
679  }
680  qiLittleEndianParser(it, msg.n);
681  qiLittleEndianParser(it, msg.sb_length);
682  qiLittleEndianParser(it, msg.flags);
683  std::advance(it, 3); // reserved
684  msg.rfband.resize(msg.n);
685  for (auto& rfband : msg.rfband)
686  {
687  RfBandParser(it, rfband, msg.sb_length);
688  }
689  if (it > itEnd)
690  {
691  node->log(log_level::ERROR, "Parse error: iterator past end.");
692  return false;
693  }
694  return true;
695 };
696 
701 template <typename It>
702 [[nodiscard]] bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd,
703  ReceiverSetup& msg)
704 {
705  if (!BlockHeaderParser(node, it, msg.block_header))
706  return false;
707  if (msg.block_header.id != 5902)
708  {
709  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
710  std::to_string(msg.block_header.id));
711  return false;
712  }
713  std::advance(it, 2); // reserved
714  qiCharsToStringParser(it, msg.marker_name, 60);
716  qiCharsToStringParser(it, msg.observer, 20);
717  qiCharsToStringParser(it, msg.agency, 40);
719  qiCharsToStringParser(it, msg.rx_name, 20);
720  qiCharsToStringParser(it, msg.rx_version, 20);
722  qiCharsToStringParser(it, msg.ant_type, 20);
723  qiLittleEndianParser(it, msg.delta_h);
724  qiLittleEndianParser(it, msg.delta_e);
725  qiLittleEndianParser(it, msg.delta_n);
726  if (msg.block_header.revision > 0)
727  qiCharsToStringParser(it, msg.marker_type, 20);
728  if (msg.block_header.revision > 1)
730  if (msg.block_header.revision > 2)
731  qiCharsToStringParser(it, msg.product_name, 40);
732  if (msg.block_header.revision > 3)
733  {
736  qiLittleEndianParser(it, msg.height);
737  qiCharsToStringParser(it, msg.station_code, 10);
741  } else
742  {
743  setDoNotUse(msg.latitude);
744  setDoNotUse(msg.longitude);
745  setDoNotUse(msg.height);
746  }
747  if (it > itEnd)
748  {
749  node->log(log_level::ERROR, "Parse error: iterator past end.");
750  return false;
751  }
752  return true;
753 };
754 
759 template <typename It>
760 [[nodiscard]] bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd,
761  ReceiverTimeMsg& msg)
762 {
763  if (!BlockHeaderParser(node, it, msg.block_header))
764  return false;
765  if (msg.block_header.id != 5914)
766  {
767  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
768  std::to_string(msg.block_header.id));
769  return false;
770  }
771  qiLittleEndianParser(it, msg.utc_year);
772  qiLittleEndianParser(it, msg.utc_month);
773  qiLittleEndianParser(it, msg.utc_day);
774  qiLittleEndianParser(it, msg.utc_hour);
775  qiLittleEndianParser(it, msg.utc_min);
776  qiLittleEndianParser(it, msg.utc_second);
777  qiLittleEndianParser(it, msg.delta_ls);
778  qiLittleEndianParser(it, msg.sync_level);
779  if (it > itEnd)
780  {
781  node->log(log_level::ERROR, "Parse error: iterator past end.");
782  return false;
783  }
784  return true;
785 };
786 
791 template <typename It>
792 [[nodiscard]] bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd,
793  PVTCartesianMsg& msg)
794 {
795  if (!BlockHeaderParser(node, it, msg.block_header))
796  return false;
797  if (msg.block_header.id != 4006)
798  {
799  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
800  std::to_string(msg.block_header.id));
801  return false;
802  }
803  qiLittleEndianParser(it, msg.mode);
804  qiLittleEndianParser(it, msg.error);
805  qiLittleEndianParser(it, msg.x);
806  qiLittleEndianParser(it, msg.y);
807  qiLittleEndianParser(it, msg.z);
808  qiLittleEndianParser(it, msg.undulation);
809  qiLittleEndianParser(it, msg.vx);
810  qiLittleEndianParser(it, msg.vy);
811  qiLittleEndianParser(it, msg.vz);
812  qiLittleEndianParser(it, msg.cog);
813  qiLittleEndianParser(it, msg.rx_clk_bias);
814  qiLittleEndianParser(it, msg.rx_clk_drift);
815  qiLittleEndianParser(it, msg.time_system);
816  qiLittleEndianParser(it, msg.datum);
817  qiLittleEndianParser(it, msg.nr_sv);
818  qiLittleEndianParser(it, msg.wa_corr_info);
819  qiLittleEndianParser(it, msg.reference_id);
820  qiLittleEndianParser(it, msg.mean_corr_age);
821  qiLittleEndianParser(it, msg.signal_info);
822  qiLittleEndianParser(it, msg.alert_flag);
823  if (msg.block_header.revision > 0)
824  {
825  qiLittleEndianParser(it, msg.nr_bases);
826  qiLittleEndianParser(it, msg.ppp_info);
827  }
828  if (msg.block_header.revision > 1)
829  {
830  qiLittleEndianParser(it, msg.latency);
831  qiLittleEndianParser(it, msg.h_accuracy);
832  qiLittleEndianParser(it, msg.v_accuracy);
833  qiLittleEndianParser(it, msg.misc);
834  }
835  if (it > itEnd)
836  {
837  node->log(log_level::ERROR, "Parse error: iterator past end.");
838  return false;
839  }
840  return true;
841 }
842 
847 template <typename It>
848 [[nodiscard]] bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd,
849  PVTGeodeticMsg& msg)
850 {
851  if (!BlockHeaderParser(node, it, msg.block_header))
852  return false;
853  if (msg.block_header.id != 4007)
854  {
855  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
856  std::to_string(msg.block_header.id));
857  return false;
858  }
859  qiLittleEndianParser(it, msg.mode);
860  qiLittleEndianParser(it, msg.error);
861  qiLittleEndianParser(it, msg.latitude);
862  qiLittleEndianParser(it, msg.longitude);
863  qiLittleEndianParser(it, msg.height);
864  qiLittleEndianParser(it, msg.undulation);
865  qiLittleEndianParser(it, msg.vn);
866  qiLittleEndianParser(it, msg.ve);
867  qiLittleEndianParser(it, msg.vu);
868  qiLittleEndianParser(it, msg.cog);
869  qiLittleEndianParser(it, msg.rx_clk_bias);
870  qiLittleEndianParser(it, msg.rx_clk_drift);
871  qiLittleEndianParser(it, msg.time_system);
872  qiLittleEndianParser(it, msg.datum);
873  qiLittleEndianParser(it, msg.nr_sv);
874  qiLittleEndianParser(it, msg.wa_corr_info);
875  qiLittleEndianParser(it, msg.reference_id);
876  qiLittleEndianParser(it, msg.mean_corr_age);
877  qiLittleEndianParser(it, msg.signal_info);
878  qiLittleEndianParser(it, msg.alert_flag);
879  if (msg.block_header.revision > 0)
880  {
881  qiLittleEndianParser(it, msg.nr_bases);
882  qiLittleEndianParser(it, msg.ppp_info);
883  }
884  if (msg.block_header.revision > 1)
885  {
886  qiLittleEndianParser(it, msg.latency);
887  qiLittleEndianParser(it, msg.h_accuracy);
888  qiLittleEndianParser(it, msg.v_accuracy);
889  qiLittleEndianParser(it, msg.misc);
890  }
891  if (it > itEnd)
892  {
893  node->log(log_level::ERROR, "Parse error: iterator past end.");
894  return false;
895  }
896  return true;
897 }
898 
903 template <typename It>
904 [[nodiscard]] bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd,
905  AttEulerMsg& msg, bool use_ros_axis_orientation)
906 {
907  if (!BlockHeaderParser(node, it, msg.block_header))
908  return false;
909  if (msg.block_header.id != 5938)
910  {
911  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
912  std::to_string(msg.block_header.id));
913  return false;
914  }
915  qiLittleEndianParser(it, msg.nr_sv);
916  qiLittleEndianParser(it, msg.error);
917  qiLittleEndianParser(it, msg.mode);
918  std::advance(it, 2); // reserved
919  qiLittleEndianParser(it, msg.heading);
920  qiLittleEndianParser(it, msg.pitch);
921  qiLittleEndianParser(it, msg.roll);
922  qiLittleEndianParser(it, msg.pitch_dot);
923  qiLittleEndianParser(it, msg.roll_dot);
924  qiLittleEndianParser(it, msg.heading_dot);
925  if (use_ros_axis_orientation)
926  {
927  msg.heading = -msg.heading + 90;
928  msg.pitch = -msg.pitch;
929  msg.pitch_dot = -msg.pitch_dot;
930  msg.heading_dot = -msg.heading_dot;
931  }
932  if (it > itEnd)
933  {
934  node->log(log_level::ERROR, "Parse error: iterator past end.");
935  return false;
936  }
937  return true;
938 };
939 
944 template <typename It>
945 [[nodiscard]] bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd,
946  AttCovEulerMsg& msg,
947  bool use_ros_axis_orientation)
948 {
949  if (!BlockHeaderParser(node, it, msg.block_header))
950  return false;
951  if (msg.block_header.id != 5939)
952  {
953  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
954  std::to_string(msg.block_header.id));
955  return false;
956  }
957  ++it; // reserved
958  qiLittleEndianParser(it, msg.error);
959  qiLittleEndianParser(it, msg.cov_headhead);
960  qiLittleEndianParser(it, msg.cov_pitchpitch);
961  qiLittleEndianParser(it, msg.cov_rollroll);
962  qiLittleEndianParser(it, msg.cov_headpitch);
963  qiLittleEndianParser(it, msg.cov_headroll);
964  qiLittleEndianParser(it, msg.cov_pitchroll);
965  if (use_ros_axis_orientation)
966  {
967  msg.cov_headroll = -msg.cov_headroll;
968  msg.cov_pitchroll = -msg.cov_pitchroll;
969  }
970  if (it > itEnd)
971  {
972  node->log(log_level::ERROR, "Parse error: iterator past end.");
973  return false;
974  }
975  return true;
976 };
977 
982 template <typename It>
983 void VectorInfoCartParser(It& it, VectorInfoCartMsg& msg, uint8_t sb_length)
984 {
985  qiLittleEndianParser(it, msg.nr_sv);
986  qiLittleEndianParser(it, msg.error);
987  qiLittleEndianParser(it, msg.mode);
988  qiLittleEndianParser(it, msg.misc);
989  qiLittleEndianParser(it, msg.delta_x);
990  qiLittleEndianParser(it, msg.delta_y);
991  qiLittleEndianParser(it, msg.delta_z);
992  qiLittleEndianParser(it, msg.delta_vx);
993  qiLittleEndianParser(it, msg.delta_vy);
994  qiLittleEndianParser(it, msg.delta_vz);
995  qiLittleEndianParser(it, msg.azimuth);
996  qiLittleEndianParser(it, msg.elevation);
997  qiLittleEndianParser(it, msg.reference_id);
998  qiLittleEndianParser(it, msg.corr_age);
999  qiLittleEndianParser(it, msg.signal_info);
1000  std::advance(it, sb_length - 52); // skip padding
1001 };
1002 
1007 template <typename It>
1008 [[nodiscard]] bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd,
1009  BaseVectorCartMsg& msg)
1010 {
1011  if (!BlockHeaderParser(node, it, msg.block_header))
1012  return false;
1013  if (msg.block_header.id != 4043)
1014  {
1015  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1016  std::to_string(msg.block_header.id));
1017  return false;
1018  }
1019  qiLittleEndianParser(it, msg.n);
1020  if (msg.n > MAXSB_NBVECTORINFO)
1021  {
1022  node->log(log_level::ERROR,
1023  "Parse error: Too many VectorInfoCart " + std::to_string(msg.n));
1024  return false;
1025  }
1026  qiLittleEndianParser(it, msg.sb_length);
1027  msg.vector_info_cart.resize(msg.n);
1028  for (auto& vector_info_cart : msg.vector_info_cart)
1029  {
1030  VectorInfoCartParser(it, vector_info_cart, msg.sb_length);
1031  }
1032  if (it > itEnd)
1033  {
1034  node->log(log_level::ERROR, "Parse error: iterator past end.");
1035  return false;
1036  }
1037  return true;
1038 };
1039 
1044 template <typename It>
1045 void VectorInfoGeodParser(It& it, VectorInfoGeodMsg& msg, uint8_t sb_length)
1046 {
1047  qiLittleEndianParser(it, msg.nr_sv);
1048  qiLittleEndianParser(it, msg.error);
1049  qiLittleEndianParser(it, msg.mode);
1050  qiLittleEndianParser(it, msg.misc);
1051  qiLittleEndianParser(it, msg.delta_east);
1052  qiLittleEndianParser(it, msg.delta_north);
1053  qiLittleEndianParser(it, msg.delta_up);
1054  qiLittleEndianParser(it, msg.delta_ve);
1055  qiLittleEndianParser(it, msg.delta_vn);
1056  qiLittleEndianParser(it, msg.delta_vu);
1057  qiLittleEndianParser(it, msg.azimuth);
1058  qiLittleEndianParser(it, msg.elevation);
1059  qiLittleEndianParser(it, msg.reference_id);
1060  qiLittleEndianParser(it, msg.corr_age);
1061  qiLittleEndianParser(it, msg.signal_info);
1062  std::advance(it, sb_length - 52); // skip padding
1063 };
1064 
1069 template <typename It>
1070 [[nodiscard]] bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd,
1071  BaseVectorGeodMsg& msg)
1072 {
1073  if (!BlockHeaderParser(node, it, msg.block_header))
1074  return false;
1075  if (msg.block_header.id != 4028)
1076  {
1077  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1078  std::to_string(msg.block_header.id));
1079  return false;
1080  }
1081  qiLittleEndianParser(it, msg.n);
1082  if (msg.n > MAXSB_NBVECTORINFO)
1083  {
1084  node->log(log_level::ERROR,
1085  "Parse error: Too many VectorInfoGeod " + std::to_string(msg.n));
1086  return false;
1087  }
1088  qiLittleEndianParser(it, msg.sb_length);
1089  msg.vector_info_geod.resize(msg.n);
1090  for (auto& vector_info_geod : msg.vector_info_geod)
1091  {
1092  VectorInfoGeodParser(it, vector_info_geod, msg.sb_length);
1093  }
1094  if (it > itEnd)
1095  {
1096  node->log(log_level::ERROR, "Parse error: iterator past end.");
1097  return false;
1098  }
1099  return true;
1100 };
1101 
1106 template <typename It>
1107 [[nodiscard]] bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd,
1108  INSNavCartMsg& msg,
1109  bool use_ros_axis_orientation)
1110 {
1111  if (!BlockHeaderParser(node, it, msg.block_header))
1112  return false;
1113  if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229))
1114  {
1115  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1116  std::to_string(msg.block_header.id));
1117  return false;
1118  }
1119  qiLittleEndianParser(it, msg.gnss_mode);
1120  qiLittleEndianParser(it, msg.error);
1121  qiLittleEndianParser(it, msg.info);
1122  qiLittleEndianParser(it, msg.gnss_age);
1123  qiLittleEndianParser(it, msg.x);
1124  qiLittleEndianParser(it, msg.y);
1125  qiLittleEndianParser(it, msg.z);
1126  qiLittleEndianParser(it, msg.accuracy);
1127  qiLittleEndianParser(it, msg.latency);
1128  qiLittleEndianParser(it, msg.datum);
1129  ++it; // reserved
1130  qiLittleEndianParser(it, msg.sb_list);
1131  if ((msg.sb_list & 1) != 0)
1132  {
1133  qiLittleEndianParser(it, msg.x_std_dev);
1134  qiLittleEndianParser(it, msg.y_std_dev);
1135  qiLittleEndianParser(it, msg.z_std_dev);
1136  } else
1137  {
1138  setDoNotUse(msg.x_std_dev);
1139  setDoNotUse(msg.y_std_dev);
1140  setDoNotUse(msg.z_std_dev);
1141  }
1142  if ((msg.sb_list & 2) != 0)
1143  {
1144  qiLittleEndianParser(it, msg.heading);
1145  qiLittleEndianParser(it, msg.pitch);
1146  qiLittleEndianParser(it, msg.roll);
1147  if (use_ros_axis_orientation)
1148  {
1149  msg.heading = -msg.heading + 90;
1150  msg.pitch = -msg.pitch;
1151  }
1152  } else
1153  {
1154  setDoNotUse(msg.heading);
1155  setDoNotUse(msg.pitch);
1156  setDoNotUse(msg.roll);
1157  }
1158  if ((msg.sb_list & 4) != 0)
1159  {
1160  qiLittleEndianParser(it, msg.heading_std_dev);
1161  qiLittleEndianParser(it, msg.pitch_std_dev);
1162  qiLittleEndianParser(it, msg.roll_std_dev);
1163  } else
1164  {
1165  setDoNotUse(msg.heading_std_dev);
1166  setDoNotUse(msg.pitch_std_dev);
1167  setDoNotUse(msg.roll_std_dev);
1168  }
1169  if ((msg.sb_list & 8) != 0)
1170  {
1171  qiLittleEndianParser(it, msg.vx);
1172  qiLittleEndianParser(it, msg.vy);
1173  qiLittleEndianParser(it, msg.vz);
1174  } else
1175  {
1176  setDoNotUse(msg.vx);
1177  setDoNotUse(msg.vy);
1178  setDoNotUse(msg.vz);
1179  }
1180  if ((msg.sb_list & 16) != 0)
1181  {
1182  qiLittleEndianParser(it, msg.vx_std_dev);
1183  qiLittleEndianParser(it, msg.vy_std_dev);
1184  qiLittleEndianParser(it, msg.vz_std_dev);
1185  } else
1186  {
1187  setDoNotUse(msg.vx_std_dev);
1188  setDoNotUse(msg.vy_std_dev);
1189  setDoNotUse(msg.vz_std_dev);
1190  }
1191  if ((msg.sb_list & 32) != 0)
1192  {
1193  qiLittleEndianParser(it, msg.xy_cov);
1194  qiLittleEndianParser(it, msg.xz_cov);
1195  qiLittleEndianParser(it, msg.yz_cov);
1196  } else
1197  {
1198  setDoNotUse(msg.xy_cov);
1199  setDoNotUse(msg.xz_cov);
1200  setDoNotUse(msg.yz_cov);
1201  }
1202  if ((msg.sb_list & 64) != 0)
1203  {
1204  qiLittleEndianParser(it, msg.heading_pitch_cov);
1205  qiLittleEndianParser(it, msg.heading_roll_cov);
1206  qiLittleEndianParser(it, msg.pitch_roll_cov);
1207  if (use_ros_axis_orientation)
1208  {
1209  msg.heading_roll_cov = -msg.heading_roll_cov;
1210  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1211  }
1212  } else
1213  {
1214  setDoNotUse(msg.heading_pitch_cov);
1215  setDoNotUse(msg.heading_roll_cov);
1216  setDoNotUse(msg.pitch_roll_cov);
1217  }
1218  if ((msg.sb_list & 128) != 0)
1219  {
1220  qiLittleEndianParser(it, msg.vx_vy_cov);
1221  qiLittleEndianParser(it, msg.vx_vz_cov);
1222  qiLittleEndianParser(it, msg.vy_vz_cov);
1223  } else
1224  {
1225  setDoNotUse(msg.vx_vy_cov);
1226  setDoNotUse(msg.vx_vz_cov);
1227  setDoNotUse(msg.vy_vz_cov);
1228  }
1229  if (it > itEnd)
1230  {
1231  node->log(log_level::ERROR, "Parse error: iterator past end.");
1232  return false;
1233  }
1234  return true;
1235 };
1236 
1241 template <typename It>
1242 [[nodiscard]] bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd,
1243  PosCovCartesianMsg& msg)
1244 {
1245  if (!BlockHeaderParser(node, it, msg.block_header))
1246  return false;
1247  if (msg.block_header.id != 5905)
1248  {
1249  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1250  std::to_string(msg.block_header.id));
1251  return false;
1252  }
1253  qiLittleEndianParser(it, msg.mode);
1254  qiLittleEndianParser(it, msg.error);
1255  qiLittleEndianParser(it, msg.cov_xx);
1256  qiLittleEndianParser(it, msg.cov_yy);
1257  qiLittleEndianParser(it, msg.cov_zz);
1258  qiLittleEndianParser(it, msg.cov_bb);
1259  qiLittleEndianParser(it, msg.cov_xy);
1260  qiLittleEndianParser(it, msg.cov_xz);
1261  qiLittleEndianParser(it, msg.cov_xb);
1262  qiLittleEndianParser(it, msg.cov_yz);
1263  qiLittleEndianParser(it, msg.cov_yb);
1264  qiLittleEndianParser(it, msg.cov_zb);
1265  if (it > itEnd)
1266  {
1267  node->log(log_level::ERROR, "Parse error: iterator past end.");
1268  return false;
1269  }
1270  return true;
1271 };
1272 
1277 template <typename It>
1278 [[nodiscard]] bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd,
1279  PosCovGeodeticMsg& msg)
1280 {
1281  if (!BlockHeaderParser(node, it, msg.block_header))
1282  return false;
1283  if (msg.block_header.id != 5906)
1284  {
1285  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1286  std::to_string(msg.block_header.id));
1287  return false;
1288  }
1289  qiLittleEndianParser(it, msg.mode);
1290  qiLittleEndianParser(it, msg.error);
1291  qiLittleEndianParser(it, msg.cov_latlat);
1292  qiLittleEndianParser(it, msg.cov_lonlon);
1293  qiLittleEndianParser(it, msg.cov_hgthgt);
1294  qiLittleEndianParser(it, msg.cov_bb);
1295  qiLittleEndianParser(it, msg.cov_latlon);
1296  qiLittleEndianParser(it, msg.cov_lathgt);
1297  qiLittleEndianParser(it, msg.cov_latb);
1298  qiLittleEndianParser(it, msg.cov_lonhgt);
1299  qiLittleEndianParser(it, msg.cov_lonb);
1300  qiLittleEndianParser(it, msg.cov_hb);
1301  if (it > itEnd)
1302  {
1303  node->log(log_level::ERROR, "Parse error: iterator past end.");
1304  return false;
1305  }
1306  return true;
1307 };
1308 
1313 template <typename It>
1314 [[nodiscard]] bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd,
1315  VelCovCartesianMsg& msg)
1316 {
1317  if (!BlockHeaderParser(node, it, msg.block_header))
1318  return false;
1319  if (msg.block_header.id != 5907)
1320  {
1321  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1322  std::to_string(msg.block_header.id));
1323  return false;
1324  }
1325  qiLittleEndianParser(it, msg.mode);
1326  qiLittleEndianParser(it, msg.error);
1327  qiLittleEndianParser(it, msg.cov_vxvx);
1328  qiLittleEndianParser(it, msg.cov_vyvy);
1329  qiLittleEndianParser(it, msg.cov_vzvz);
1330  qiLittleEndianParser(it, msg.cov_dtdt);
1331  qiLittleEndianParser(it, msg.cov_vxvy);
1332  qiLittleEndianParser(it, msg.cov_vxvz);
1333  qiLittleEndianParser(it, msg.cov_vxdt);
1334  qiLittleEndianParser(it, msg.cov_vyvz);
1335  qiLittleEndianParser(it, msg.cov_vydt);
1336  qiLittleEndianParser(it, msg.cov_vzdt);
1337  if (it > itEnd)
1338  {
1339  node->log(log_level::ERROR, "Parse error: iterator past end.");
1340  return false;
1341  }
1342  return true;
1343 };
1344 
1349 template <typename It>
1350 [[nodiscard]] bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd,
1351  VelCovGeodeticMsg& msg)
1352 {
1353  if (!BlockHeaderParser(node, it, msg.block_header))
1354  return false;
1355  if (msg.block_header.id != 5908)
1356  {
1357  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1358  std::to_string(msg.block_header.id));
1359  return false;
1360  }
1361  qiLittleEndianParser(it, msg.mode);
1362  qiLittleEndianParser(it, msg.error);
1363  qiLittleEndianParser(it, msg.cov_vnvn);
1364  qiLittleEndianParser(it, msg.cov_veve);
1365  qiLittleEndianParser(it, msg.cov_vuvu);
1366  qiLittleEndianParser(it, msg.cov_dtdt);
1367  qiLittleEndianParser(it, msg.cov_vnve);
1368  qiLittleEndianParser(it, msg.cov_vnvu);
1369  qiLittleEndianParser(it, msg.cov_vndt);
1370  qiLittleEndianParser(it, msg.cov_vevu);
1371  qiLittleEndianParser(it, msg.cov_vedt);
1372  qiLittleEndianParser(it, msg.cov_vudt);
1373  if (it > itEnd)
1374  {
1375  node->log(log_level::ERROR, "Parse error: iterator past end.");
1376  return false;
1377  }
1378  return true;
1379 };
1380 
1385 template <typename It>
1386 [[nodiscard]] bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd,
1387  QualityInd& msg)
1388 {
1389  if (!BlockHeaderParser(node, it, msg.block_header))
1390  return false;
1391  if (msg.block_header.id != 4082)
1392  {
1393  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1394  std::to_string(msg.block_header.id));
1395  return false;
1396  }
1397  qiLittleEndianParser(it, msg.n);
1398  if (msg.n > 40)
1399  {
1400  node->log(log_level::ERROR,
1401  "Parse error: Too many indicators " + std::to_string(msg.n));
1402  return false;
1403  }
1404  ++it; // reserved
1405  msg.indicators.resize(msg.n);
1406  std::vector<uint16_t> indicators;
1407  for (auto& indicators : msg.indicators)
1408  {
1409  qiLittleEndianParser(it, indicators);
1410  }
1411  if (it > itEnd)
1412  {
1413  node->log(log_level::ERROR, "Parse error: iterator past end.");
1414  return false;
1415  }
1416  return true;
1417 };
1418 
1423 template <typename It>
1424 void AgcStateParser(It it, AgcState& msg, uint8_t sb_length)
1425 {
1427  qiLittleEndianParser(it, msg.gain);
1430  std::advance(it, sb_length - 4); // skip padding
1431 };
1432 
1437 template <typename It>
1438 [[nodiscard]] bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd,
1439  ReceiverStatus& msg)
1440 {
1441  if (!BlockHeaderParser(node, it, msg.block_header))
1442  return false;
1443  if (msg.block_header.id != 4014)
1444  {
1445  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1446  std::to_string(msg.block_header.id));
1447  return false;
1448  }
1449  qiLittleEndianParser(it, msg.cpu_load);
1451  qiLittleEndianParser(it, msg.up_time);
1453  qiLittleEndianParser(it, msg.rx_error);
1454  qiLittleEndianParser(it, msg.n);
1455  if (msg.n > 18)
1456  {
1457  node->log(log_level::ERROR,
1458  "Parse error: Too many AGCState " + std::to_string(msg.n));
1459  return false;
1460  }
1464  msg.agc_state.resize(msg.n);
1465  for (auto& agc_state : msg.agc_state)
1466  {
1467  AgcStateParser(it, agc_state, msg.sb_length);
1468  }
1469  if (it > itEnd)
1470  {
1471  node->log(log_level::ERROR, "Parse error: iterator past end.");
1472  return false;
1473  }
1474  return true;
1475 };
1476 
1481 template <typename It>
1482 [[nodiscard]] bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd,
1483  ReceiverTimeMsg& msg)
1484 {
1485  if (!BlockHeaderParser(node, it, msg.block_header))
1486  return false;
1487  if (msg.block_header.id != 5914)
1488  {
1489  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1490  std::to_string(msg.block_header.id));
1491  return false;
1492  }
1493  qiLittleEndianParser(it, msg.utc_year);
1494  qiLittleEndianParser(it, msg.utc_month);
1495  qiLittleEndianParser(it, msg.utc_day);
1496  qiLittleEndianParser(it, msg.utc_hour);
1497  qiLittleEndianParser(it, msg.utc_min);
1498  qiLittleEndianParser(it, msg.utc_second);
1499  qiLittleEndianParser(it, msg.delta_ls);
1500  qiLittleEndianParser(it, msg.sync_level);
1501  if (it > itEnd)
1502  {
1503  node->log(log_level::ERROR, "Parse error: iterator past end.");
1504  return false;
1505  }
1506  return true;
1507 };
1508 
1513 template <typename It>
1514 [[nodiscard]] bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd,
1515  INSNavGeodMsg& msg,
1516  bool use_ros_axis_orientation)
1517 {
1518  if (!BlockHeaderParser(node, it, msg.block_header))
1519  return false;
1520  if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230))
1521  {
1522  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1523  std::to_string(msg.block_header.id));
1524  return false;
1525  }
1526  qiLittleEndianParser(it, msg.gnss_mode);
1527  qiLittleEndianParser(it, msg.error);
1528  qiLittleEndianParser(it, msg.info);
1529  qiLittleEndianParser(it, msg.gnss_age);
1530  qiLittleEndianParser(it, msg.latitude);
1531  qiLittleEndianParser(it, msg.longitude);
1532  qiLittleEndianParser(it, msg.height);
1533  qiLittleEndianParser(it, msg.undulation);
1534  qiLittleEndianParser(it, msg.accuracy);
1535  qiLittleEndianParser(it, msg.latency);
1536  qiLittleEndianParser(it, msg.datum);
1537  ++it; // reserved
1538  qiLittleEndianParser(it, msg.sb_list);
1539  if ((msg.sb_list & 1) != 0)
1540  {
1541  qiLittleEndianParser(it, msg.latitude_std_dev);
1542  qiLittleEndianParser(it, msg.longitude_std_dev);
1543  qiLittleEndianParser(it, msg.height_std_dev);
1544  } else
1545  {
1546  setDoNotUse(msg.latitude_std_dev);
1547  setDoNotUse(msg.longitude_std_dev);
1548  setDoNotUse(msg.height_std_dev);
1549  }
1550  if ((msg.sb_list & 2) != 0)
1551  {
1552  qiLittleEndianParser(it, msg.heading);
1553  qiLittleEndianParser(it, msg.pitch);
1554  qiLittleEndianParser(it, msg.roll);
1555  if (use_ros_axis_orientation)
1556  {
1557  msg.heading = -msg.heading + 90;
1558  msg.pitch = -msg.pitch;
1559  }
1560  } else
1561  {
1562  setDoNotUse(msg.heading);
1563  setDoNotUse(msg.pitch);
1564  setDoNotUse(msg.roll);
1565  }
1566  if ((msg.sb_list & 4) != 0)
1567  {
1568  qiLittleEndianParser(it, msg.heading_std_dev);
1569  qiLittleEndianParser(it, msg.pitch_std_dev);
1570  qiLittleEndianParser(it, msg.roll_std_dev);
1571  } else
1572  {
1573  setDoNotUse(msg.heading_std_dev);
1574  setDoNotUse(msg.pitch_std_dev);
1575  setDoNotUse(msg.roll_std_dev);
1576  }
1577  if ((msg.sb_list & 8) != 0)
1578  {
1579  qiLittleEndianParser(it, msg.ve);
1580  qiLittleEndianParser(it, msg.vn);
1581  qiLittleEndianParser(it, msg.vu);
1582  } else
1583  {
1584  setDoNotUse(msg.ve);
1585  setDoNotUse(msg.vn);
1586  setDoNotUse(msg.vu);
1587  }
1588  if ((msg.sb_list & 16) != 0)
1589  {
1590  qiLittleEndianParser(it, msg.ve_std_dev);
1591  qiLittleEndianParser(it, msg.vn_std_dev);
1592  qiLittleEndianParser(it, msg.vu_std_dev);
1593  } else
1594  {
1595  setDoNotUse(msg.ve_std_dev);
1596  setDoNotUse(msg.vn_std_dev);
1597  setDoNotUse(msg.vu_std_dev);
1598  }
1599  if ((msg.sb_list & 32) != 0)
1600  {
1601  qiLittleEndianParser(it, msg.latitude_longitude_cov);
1602  qiLittleEndianParser(it, msg.latitude_height_cov);
1603  qiLittleEndianParser(it, msg.longitude_height_cov);
1604  } else
1605  {
1606  setDoNotUse(msg.latitude_longitude_cov);
1607  setDoNotUse(msg.latitude_height_cov);
1608  setDoNotUse(msg.longitude_height_cov);
1609  }
1610  if ((msg.sb_list & 64) != 0)
1611  {
1612  qiLittleEndianParser(it, msg.heading_pitch_cov);
1613  qiLittleEndianParser(it, msg.heading_roll_cov);
1614  qiLittleEndianParser(it, msg.pitch_roll_cov);
1615  if (use_ros_axis_orientation)
1616  {
1617  msg.heading_roll_cov = -msg.heading_roll_cov;
1618  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1619  }
1620  } else
1621  {
1622  setDoNotUse(msg.heading_pitch_cov);
1623  setDoNotUse(msg.heading_roll_cov);
1624  setDoNotUse(msg.pitch_roll_cov);
1625  }
1626  if ((msg.sb_list & 128) != 0)
1627  {
1628  qiLittleEndianParser(it, msg.ve_vn_cov);
1629  qiLittleEndianParser(it, msg.ve_vu_cov);
1630  qiLittleEndianParser(it, msg.vn_vu_cov);
1631  } else
1632  {
1633  setDoNotUse(msg.ve_vn_cov);
1634  setDoNotUse(msg.ve_vu_cov);
1635  setDoNotUse(msg.vn_vu_cov);
1636  }
1637  if (it > itEnd)
1638  {
1639  node->log(log_level::ERROR, "Parse error: iterator past end.");
1640  return false;
1641  }
1642  return true;
1643 };
1644 
1649 template <typename It>
1650 [[nodiscard]] bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd,
1651  IMUSetupMsg& msg, bool use_ros_axis_orientation)
1652 {
1653  if (!BlockHeaderParser(node, it, msg.block_header))
1654  return false;
1655  if (msg.block_header.id != 4224)
1656  {
1657  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1658  std::to_string(msg.block_header.id));
1659  return false;
1660  }
1661  ++it; // reserved
1662  qiLittleEndianParser(it, msg.serial_port);
1663  qiLittleEndianParser(it, msg.ant_lever_arm_x);
1664  qiLittleEndianParser(it, msg.ant_lever_arm_y);
1665  qiLittleEndianParser(it, msg.ant_lever_arm_z);
1666  qiLittleEndianParser(it, msg.theta_x);
1667  qiLittleEndianParser(it, msg.theta_y);
1668  qiLittleEndianParser(it, msg.theta_z);
1669  if (use_ros_axis_orientation)
1670  {
1671  msg.ant_lever_arm_y = -msg.ant_lever_arm_y;
1672  msg.ant_lever_arm_z = -msg.ant_lever_arm_z;
1673  msg.theta_x = parsing_utilities::wrapAngle180to180(msg.theta_x - 180.0f);
1674  }
1675  if (it > itEnd)
1676  {
1677  node->log(log_level::ERROR, "Parse error: iterator past end.");
1678  return false;
1679  }
1680  return true;
1681 };
1682 
1687 template <typename It>
1688 [[nodiscard]] bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd,
1689  VelSensorSetupMsg& msg,
1690  bool use_ros_axis_orientation)
1691 {
1692  if (!BlockHeaderParser(node, it, msg.block_header))
1693  return false;
1694  if (msg.block_header.id != 4244)
1695  {
1696  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1697  std::to_string(msg.block_header.id));
1698  return false;
1699  }
1700  ++it; // reserved
1701  qiLittleEndianParser(it, msg.port);
1702  qiLittleEndianParser(it, msg.lever_arm_x);
1703  qiLittleEndianParser(it, msg.lever_arm_y);
1704  qiLittleEndianParser(it, msg.lever_arm_z);
1705  if (use_ros_axis_orientation)
1706  {
1707  msg.lever_arm_y = -msg.lever_arm_y;
1708  msg.lever_arm_z = -msg.lever_arm_z;
1709  }
1710  if (it > itEnd)
1711  {
1712  node->log(log_level::ERROR, "Parse error: iterator past end.");
1713  return false;
1714  }
1715  return true;
1716 };
1717 
1722 template <typename It>
1723 [[nodiscard]] bool
1725  bool use_ros_axis_orientation, bool& hasImuMeas)
1726 {
1727  if (!BlockHeaderParser(node, it, msg.block_header))
1728  return false;
1729  if (msg.block_header.id != 4050)
1730  {
1731  node->log(log_level::ERROR, "Parse error: Wrong header ID " +
1732  std::to_string(msg.block_header.id));
1733  return false;
1734  }
1735  qiLittleEndianParser(it, msg.n);
1736  qiLittleEndianParser(it, msg.sb_length);
1737  if (msg.sb_length != 28)
1738  {
1739  node->log(log_level::ERROR,
1740  "Parse error: Wrong sb_length " + std::to_string(msg.sb_length));
1741  return false;
1742  }
1743 
1744  msg.acceleration_x = std::numeric_limits<double>::quiet_NaN();
1745  msg.acceleration_y = std::numeric_limits<double>::quiet_NaN();
1746  msg.acceleration_z = std::numeric_limits<double>::quiet_NaN();
1747 
1748  msg.angular_rate_x = std::numeric_limits<double>::quiet_NaN();
1749  msg.angular_rate_y = std::numeric_limits<double>::quiet_NaN();
1750  msg.angular_rate_z = std::numeric_limits<double>::quiet_NaN();
1751 
1752  msg.velocity_x = std::numeric_limits<double>::quiet_NaN();
1753  msg.velocity_y = std::numeric_limits<double>::quiet_NaN();
1754  msg.velocity_z = std::numeric_limits<double>::quiet_NaN();
1755 
1756  msg.std_dev_x = std::numeric_limits<double>::quiet_NaN();
1757  msg.std_dev_y = std::numeric_limits<double>::quiet_NaN();
1758  msg.std_dev_z = std::numeric_limits<double>::quiet_NaN();
1759 
1760  msg.sensor_temperature = std::numeric_limits<float>::quiet_NaN();
1761  msg.zero_velocity_flag = std::numeric_limits<double>::quiet_NaN();
1762 
1763  msg.source.resize(msg.n);
1764  msg.sensor_model.resize(msg.n);
1765  msg.type.resize(msg.n);
1766  msg.obs_info.resize(msg.n);
1767  bool hasAcc = false;
1768  bool hasOmega = false;
1769  hasImuMeas = false;
1770  for (size_t i = 0; i < msg.n; i++)
1771  {
1772  qiLittleEndianParser(it, msg.source[i]);
1773  qiLittleEndianParser(it, msg.sensor_model[i]);
1774  qiLittleEndianParser(it, msg.type[i]);
1775  qiLittleEndianParser(it, msg.obs_info[i]);
1776 
1777  switch (msg.type[i])
1778  {
1779  case 0:
1780  {
1781  qiLittleEndianParser(it, msg.acceleration_x);
1782  qiLittleEndianParser(it, msg.acceleration_y);
1783  qiLittleEndianParser(it, msg.acceleration_z);
1784  hasAcc = true;
1785  break;
1786  }
1787  case 1:
1788  {
1789  qiLittleEndianParser(it, msg.angular_rate_x);
1790  qiLittleEndianParser(it, msg.angular_rate_y);
1791  qiLittleEndianParser(it, msg.angular_rate_z);
1792  hasOmega = true;
1793  break;
1794  }
1795  case 3:
1796  {
1797  int16_t temp;
1798  qiLittleEndianParser(it, temp);
1799  if (temp != -32768)
1800  msg.sensor_temperature = temp / 100.0f;
1801  else
1802  msg.sensor_temperature = std::numeric_limits<float>::quiet_NaN();
1803  std::advance(it, 22); // reserved
1804  break;
1805  }
1806  case 4:
1807  {
1808  qiLittleEndianParser(it, msg.velocity_x);
1809  qiLittleEndianParser(it, msg.velocity_y);
1810  qiLittleEndianParser(it, msg.velocity_z);
1811  qiLittleEndianParser(it, msg.std_dev_x);
1812  qiLittleEndianParser(it, msg.std_dev_y);
1813  qiLittleEndianParser(it, msg.std_dev_z);
1814  if (use_ros_axis_orientation)
1815  {
1816  msg.velocity_y = -msg.velocity_y;
1817  msg.velocity_z = -msg.velocity_z;
1818  }
1819  break;
1820  }
1821  case 20:
1822  {
1823  qiLittleEndianParser(it, msg.zero_velocity_flag);
1824  std::advance(it, 16); // reserved
1825  break;
1826  }
1827  default:
1828  {
1829  node->log(
1831  "Unknown external sensor measurement type in SBF ExtSensorMeas: " +
1832  std::to_string(msg.type[i]));
1833  std::advance(it, 24);
1834  break;
1835  }
1836  }
1837  }
1838  if (it > itEnd)
1839  {
1840  node->log(log_level::ERROR, "Parse error: iterator past end.");
1841  return false;
1842  }
1843  hasImuMeas = hasAcc && hasOmega;
1844  return true;
1845 };
ReceiverSetup::delta_h
float delta_h
Definition: sbf_blocks.hpp:165
ExtSensorMeasParser
bool ExtSensorMeasParser(ROSaicNodeBase *node, It it, It itEnd, ExtSensorMeasMsg &msg, bool use_ros_axis_orientation, bool &hasImuMeas)
Qi based parser for the SBF block "ExtSensorMeas".
Definition: sbf_blocks.hpp:1724
QualityInd
Struct for the SBF block "QualityInd".
Definition: sbf_blocks.hpp:184
ChannelStatus::sb1_length
uint8_t sb1_length
Definition: sbf_blocks.hpp:125
ChannelStateInfoParser
void ChannelStateInfoParser(It &it, ChannelStateInfo &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "ChannelStateInfo".
Definition: sbf_blocks.hpp:400
QualityIndParser
bool QualityIndParser(ROSaicNodeBase *node, It it, It itEnd, QualityInd &msg)
Qi based parser for the SBF block "QualityInd".
Definition: sbf_blocks.hpp:1386
ReceiverSetup::agency
std::string agency
Definition: sbf_blocks.hpp:159
PosCovGeodeticParser
bool PosCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PosCovGeodeticMsg &msg)
Qi based parser for the SBF block "PosCovGeodetic".
Definition: sbf_blocks.hpp:1278
ChannelStatus::satInfo
std::vector< ChannelSatInfo > satInfo
Definition: sbf_blocks.hpp:128
ChannelStateInfo::pvt_info
uint16_t pvt_info
Definition: sbf_blocks.hpp:96
INSNavCartMsg
septentrio_gnss_driver::msg::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:151
MeasEpochChannelType1Parser
bool MeasEpochChannelType1Parser(ROSaicNodeBase *node, It &it, MeasEpochChannelType1Msg &msg, uint8_t sb1_length, uint8_t sb2_length)
Qi based parser for the SBF sub-block "MeasEpochChannelType1".
Definition: sbf_blocks.hpp:545
VectorInfoCartMsg
septentrio_gnss_driver::msg::VectorInfoCart VectorInfoCartMsg
Definition: typedefs.hpp:139
ChannelStatus::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:122
ReceiverSetup::delta_e
float delta_e
Definition: sbf_blocks.hpp:166
MeasEpochChannelType1Msg
septentrio_gnss_driver::msg::MeasEpochChannelType1 MeasEpochChannelType1Msg
Definition: typedefs.hpp:130
NR_OF_ANTENNAS
static const uint8_t NR_OF_ANTENNAS
Using maximum value of NR_OF_ANTENNAS for SBF definitions.
Definition: sbf_blocks.hpp:42
ChannelStateInfo
Struct for the SBF sub-block "ChannelStateInfo".
Definition: sbf_blocks.hpp:91
AttEulerMsg
septentrio_gnss_driver::msg::AttEuler AttEulerMsg
Definition: typedefs.hpp:133
ChannelSatInfo::rx_channel
uint8_t rx_channel
Definition: sbf_blocks.hpp:111
ReceiverSetup::rx_serial_number
std::string rx_serial_number
Definition: sbf_blocks.hpp:160
SBF_SYNC_2
static const uint8_t SBF_SYNC_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Definition: sbf_blocks.hpp:64
AgcState::frontend_id
uint8_t frontend_id
Definition: sbf_blocks.hpp:198
ROSaicNodeBase::log
void log(log_level::LogLevel logLevel, const std::string &s) const
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:286
MeasEpochChannelType2Msg
septentrio_gnss_driver::msg::MeasEpochChannelType2 MeasEpochChannelType2Msg
Definition: typedefs.hpp:131
s
XmlRpcServer s
MAXSB_MEASEPOCH_T2
static const uint16_t MAXSB_MEASEPOCH_T2
Max number of bytes that MeasEpochChannelType2 sub-block can consist of.
Definition: sbf_blocks.hpp:55
VelCovGeodeticMsg
septentrio_gnss_driver::msg::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:142
parsing_utilities::wrapAngle180to180
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Definition: parsing_utilities.hpp:122
ChannelStatusParser
bool ChannelStatusParser(ROSaicNodeBase *node, It it, It itEnd, ChannelStatus &msg)
Qi based parser for the SBF block "ChannelStatus".
Definition: sbf_blocks.hpp:448
ReceiverSetup::ant_type
std::string ant_type
Definition: sbf_blocks.hpp:164
BaseVectorGeodMsg
septentrio_gnss_driver::msg::BaseVectorGeod BaseVectorGeodMsg
Definition: typedefs.hpp:124
ReceiverSetup::monument_idx
uint8_t monument_idx
Definition: sbf_blocks.hpp:175
AgcStateParser
void AgcStateParser(It it, AgcState &msg, uint8_t sb_length)
Struct for the SBF sub-block "AGCState".
Definition: sbf_blocks.hpp:1424
PVTCartesianMsg
septentrio_gnss_driver::msg::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:134
RfStatusParser
bool RfStatusParser(ROSaicNodeBase *node, It it, It itEnd, RfStatusMsg &msg)
Qi based parser for the SBF block "RFStatus".
Definition: sbf_blocks.hpp:669
VectorInfoGeodParser
void VectorInfoGeodParser(It &it, VectorInfoGeodMsg &msg, uint8_t sb_length)
Qi based parser for the SBF sub-block "VectorInfoGeod".
Definition: sbf_blocks.hpp:1045
MAXSB_NBVECTORINFO
static const uint8_t MAXSB_NBVECTORINFO
Max number of vector info sub-blocks.
Definition: sbf_blocks.hpp:59
ChannelSatInfo::n2
uint8_t n2
Definition: sbf_blocks.hpp:110
ReceiverTimeParser
bool ReceiverTimeParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverTimeMsg &msg)
Struct for the SBF block "ReceiverTime".
Definition: sbf_blocks.hpp:1482
IMUSetupMsg
septentrio_gnss_driver::msg::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:153
ReceiverSetup::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:154
CRC_LOOK_UP
static const std::array< uint16_t, 256 > CRC_LOOK_UP
CRC look-up table for fast computation of the 16-bit CRC for SBF blocks.
Definition: sbf_blocks.hpp:230
MeasEpochParser
bool MeasEpochParser(ROSaicNodeBase *node, It it, It itEnd, MeasEpochMsg &msg)
Qi based parser for the SBF block "MeasEpoch".
Definition: sbf_blocks.hpp:582
RfBandParser
void RfBandParser(It &it, RfBandMsg &msg, uint8_t sb_length)
Qi based parser for the SBF sub-block "RFBand".
Definition: sbf_blocks.hpp:656
QualityInd::indicators
std::vector< uint16_t > indicators
Definition: sbf_blocks.hpp:190
ChannelStatus::sb2_length
uint8_t sb2_length
Definition: sbf_blocks.hpp:126
Dop::nr_sv
uint8_t nr_sv
Definition: sbf_blocks.hpp:139
SBF_SYNC_1
static const uint8_t SBF_SYNC_1
0x24 is ASCII for $ - 1st byte in each message
Definition: sbf_blocks.hpp:62
ChannelSatInfo::az_rise_set
uint16_t az_rise_set
Definition: sbf_blocks.hpp:107
AgcState
Struct for the SBF sub-block "AGCState".
Definition: sbf_blocks.hpp:196
ROSaicNodeBase
This class is the base class for abstraction.
Definition: typedefs.hpp:192
ReceiverSetup::ant_serial_nbr
std::string ant_serial_nbr
Definition: sbf_blocks.hpp:163
MAX_NB_INMARSATCHANNELS
static const uint8_t MAX_NB_INMARSATCHANNELS
Inmarsat is a British satellite telecommunications company.
Definition: sbf_blocks.hpp:38
ReceiverTimeMsg
septentrio_gnss_driver::msg::ReceiverTime ReceiverTimeMsg
Definition: typedefs.hpp:138
Dop::pdop
double pdop
Definition: sbf_blocks.hpp:140
VelCovCartesianParser
bool VelCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, VelCovCartesianMsg &msg)
Qi based parser for the SBF block "VelCovCartesian".
Definition: sbf_blocks.hpp:1314
ReceiverStatus::up_time
uint32_t up_time
Definition: sbf_blocks.hpp:214
Dop::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:137
GalAuthStatusParser
bool GalAuthStatusParser(ROSaicNodeBase *node, It it, It itEnd, GalAuthStatusMsg &msg)
Qi based parser for the SBF block "GALAuthStatus".
Definition: sbf_blocks.hpp:626
PosCovCartesianMsg
septentrio_gnss_driver::msg::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:136
Dop::tdop
double tdop
Definition: sbf_blocks.hpp:141
QualityInd::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:186
INSNavCartParser
bool INSNavCartParser(ROSaicNodeBase *node, It it, It itEnd, INSNavCartMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavCart".
Definition: sbf_blocks.hpp:1107
ReceiverSetup::longitude
double longitude
Definition: sbf_blocks.hpp:172
BlockHeaderParser
bool BlockHeaderParser(ROSaicNodeBase *node, It &it, Hdr &block_header)
Qi based parser for the SBF block "BlockHeader" plus receiver time stamp.
Definition: sbf_blocks.hpp:370
ReceiverSetup::rx_version
std::string rx_version
Definition: sbf_blocks.hpp:162
PVTCartesianParser
bool PVTCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PVTCartesianMsg &msg)
Qi based parser for the SBF block "PVTCartesian".
Definition: sbf_blocks.hpp:792
VelSensorSetupParser
bool VelSensorSetupParser(ROSaicNodeBase *node, It it, It itEnd, VelSensorSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "VelSensorSetup".
Definition: sbf_blocks.hpp:1688
INSNavGeodMsg
septentrio_gnss_driver::msg::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:152
MeasEpochMsg
septentrio_gnss_driver::msg::MeasEpoch MeasEpochMsg
Definition: typedefs.hpp:129
ReceiverStatus::agc_state
std::vector< AgcState > agc_state
Definition: sbf_blocks.hpp:222
AgcState::sample_var
uint8_t sample_var
Definition: sbf_blocks.hpp:200
parsing_utilities.hpp
Declares utility functions used when parsing messages.
GalAuthStatusMsg
septentrio_gnss_driver::msg::GALAuthStatus GalAuthStatusMsg
Definition: typedefs.hpp:126
ChannelStateInfo::pvt_status
uint16_t pvt_status
Definition: sbf_blocks.hpp:95
VelCovGeodeticParser
bool VelCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, VelCovGeodeticMsg &msg)
Qi based parser for the SBF block "VelCovGeodetic".
Definition: sbf_blocks.hpp:1350
ReceiverStatus::cpu_load
uint8_t cpu_load
Definition: sbf_blocks.hpp:212
ChannelSatInfo::health_status
uint16_t health_status
Definition: sbf_blocks.hpp:108
AgcState::blanking_stat
uint8_t blanking_stat
Definition: sbf_blocks.hpp:201
Dop
Definition: sbf_blocks.hpp:135
ReceiverStatusParser
bool ReceiverStatusParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverStatus &msg)
Struct for the SBF block "ReceiverStatus".
Definition: sbf_blocks.hpp:1438
MAXSB_CHANNELSATINFO
static const uint8_t MAXSB_CHANNELSATINFO
Max number of bytes that ChannelSatInfo sub-block can consist of.
Definition: sbf_blocks.hpp:46
ReceiverTimesParser
bool ReceiverTimesParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverTimeMsg &msg)
Struct for the SBF block "ReceiverTime".
Definition: sbf_blocks.hpp:760
Dop::vpl
float vpl
Definition: sbf_blocks.hpp:145
qiLittleEndianParser
void qiLittleEndianParser(It &it, Val &val)
Qi little endian parsers for numeric values.
Definition: sbf_blocks.hpp:314
PosCovGeodeticMsg
septentrio_gnss_driver::msg::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:137
QualityInd::n
uint8_t n
Definition: sbf_blocks.hpp:188
ReceiverSetup::product_name
std::string product_name
Definition: sbf_blocks.hpp:170
ReceiverSetup::country_code
std::string country_code
Definition: sbf_blocks.hpp:177
AttCovEulerMsg
septentrio_gnss_driver::msg::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:132
NR_OF_LOGICALCHANNELS
static const uint8_t NR_OF_LOGICALCHANNELS
Using maximum value of NR_OF_LOGICALCHANNELS for SBF definitions.
Definition: sbf_blocks.hpp:36
MAXSB_MEASEPOCH_T1
static const uint8_t MAXSB_MEASEPOCH_T1
Max number of bytes that MeasEpochChannelType1 sub-block can consist of.
Definition: sbf_blocks.hpp:52
ReceiverSetup::gnss_fw_version
std::string gnss_fw_version
Definition: sbf_blocks.hpp:169
ReceiverStatus::rx_error
uint32_t rx_error
Definition: sbf_blocks.hpp:216
ReceiverStatus::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:210
VelCovCartesianMsg
septentrio_gnss_driver::msg::VelCovCartesian VelCovCartesianMsg
Definition: typedefs.hpp:141
typedefs.hpp
ReceiverStatus::n
uint8_t n
Definition: sbf_blocks.hpp:217
ReceiverSetup::marker_name
std::string marker_name
Definition: sbf_blocks.hpp:156
IMUSetupParser
bool IMUSetupParser(ROSaicNodeBase *node, It it, It itEnd, IMUSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "IMUSetup".
Definition: sbf_blocks.hpp:1650
BlockHeaderMsg
septentrio_gnss_driver::msg::BlockHeader BlockHeaderMsg
Definition: typedefs.hpp:125
ReceiverStatus::rx_status
uint32_t rx_status
Definition: sbf_blocks.hpp:215
Dop::hdop
double hdop
Definition: sbf_blocks.hpp:142
ReceiverStatus::ext_error
uint8_t ext_error
Definition: sbf_blocks.hpp:213
BaseVectorCartMsg
septentrio_gnss_driver::msg::BaseVectorCart BaseVectorCartMsg
Definition: typedefs.hpp:123
ReceiverStatus::sb_length
uint8_t sb_length
Definition: sbf_blocks.hpp:218
ChannelSatInfo::freq_nr
uint8_t freq_nr
Definition: sbf_blocks.hpp:106
PVTGeodeticMsg
septentrio_gnss_driver::msg::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:135
MAXSB_NBRANTENNA
static const uint8_t MAXSB_NBRANTENNA
Maximum number of antennas that mosaic etc. can handle.
Definition: sbf_blocks.hpp:44
BaseVectorCartParser
bool BaseVectorCartParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorCartMsg &msg)
Qi based parser for the SBF block "BaseVectorCart".
Definition: sbf_blocks.hpp:1008
ChannelStatus
Struct for the SBF block "ChannelStatus".
Definition: sbf_blocks.hpp:120
log_level::ERROR
@ ERROR
Definition: typedefs.hpp:183
ReceiverStatus::cmd_count
uint8_t cmd_count
Definition: sbf_blocks.hpp:219
VectorInfoGeodMsg
septentrio_gnss_driver::msg::VectorInfoGeod VectorInfoGeodMsg
Definition: typedefs.hpp:140
MAX_NR_OF_SIGNALS_PER_SATELLITE
static const uint8_t MAX_NR_OF_SIGNALS_PER_SATELLITE
Using maximum value of MAX_NR_OF_SIGNALS_PER_SATELLITE for SBF definitions.
Definition: sbf_blocks.hpp:40
ReceiverSetup
Struct for the SBF block "ReceiverSetup".
Definition: sbf_blocks.hpp:152
ReceiverSetup::delta_n
float delta_n
Definition: sbf_blocks.hpp:167
ChannelSatInfo
Struct for the SBF sub-block "ChannelSatInfo".
Definition: sbf_blocks.hpp:103
BaseVectorGeodParser
bool BaseVectorGeodParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorGeodMsg &msg)
Qi based parser for the SBF block "BaseVectorGeod".
Definition: sbf_blocks.hpp:1070
setDoNotUse
void setDoNotUse(Val &s)
Sets scalar to Do-Not-Use value.
Definition: sbf_blocks.hpp:265
AgcState::gain
int8_t gain
Definition: sbf_blocks.hpp:199
ReceiverStatus::temperature
uint8_t temperature
Definition: sbf_blocks.hpp:220
log_level::DEBUG
@ DEBUG
Definition: typedefs.hpp:180
Dop::vdop
double vdop
Definition: sbf_blocks.hpp:143
MAXSB_CHANNELSTATEINFO
static const uint16_t MAXSB_CHANNELSTATEINFO
Max number of bytes that ChannelStateInfo sub-block can consist of.
Definition: sbf_blocks.hpp:49
ChannelStateInfo::antenna
uint8_t antenna
Definition: sbf_blocks.hpp:93
PosCovCartesianParser
bool PosCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PosCovCartesianMsg &msg)
Qi based parser for the SBF block "PosCovCartesian".
Definition: sbf_blocks.hpp:1242
ReceiverSetup::rx_name
std::string rx_name
Definition: sbf_blocks.hpp:161
ExtSensorMeasMsg
septentrio_gnss_driver::msg::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:155
ReceiverSetup::marker_type
std::string marker_type
Definition: sbf_blocks.hpp:168
MeasEpochChannelType2Parser
void MeasEpochChannelType2Parser(It &it, MeasEpochChannelType2Msg &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "MeasEpochChannelType2".
Definition: sbf_blocks.hpp:525
INSNavGeodParser
bool INSNavGeodParser(ROSaicNodeBase *node, It it, It itEnd, INSNavGeodMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavGeod".
Definition: sbf_blocks.hpp:1514
VelSensorSetupMsg
septentrio_gnss_driver::msg::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:154
typedefs_ros1.hpp
ChannelSatInfo::elev
int8_t elev
Definition: sbf_blocks.hpp:109
RfStatusMsg
septentrio_gnss_driver::msg::RFStatus RfStatusMsg
Definition: typedefs.hpp:127
ChannelStatus::n
uint8_t n
Definition: sbf_blocks.hpp:124
ReceiverSetup::receiver_idx
uint8_t receiver_idx
Definition: sbf_blocks.hpp:176
VectorInfoCartParser
void VectorInfoCartParser(It &it, VectorInfoCartMsg &msg, uint8_t sb_length)
Qi based parser for the SBF sub-block "VectorInfoCart".
Definition: sbf_blocks.hpp:983
ReceiverSetup::height
float height
Definition: sbf_blocks.hpp:173
ReceiverSetup::observer
std::string observer
Definition: sbf_blocks.hpp:158
doNotUseToNaN
void doNotUseToNaN(Val &s)
Sets scalar to Do-Not-Use value to NaN.
Definition: sbf_blocks.hpp:293
AttEulerParser
bool AttEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttEuler".
Definition: sbf_blocks.hpp:904
AttCovEulerParser
bool AttCovEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttCovEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttCovEuler".
Definition: sbf_blocks.hpp:945
ChannelSatInfo::stateInfo
std::vector< ChannelStateInfo > stateInfo
Definition: sbf_blocks.hpp:113
ReceiverSetupParser
bool ReceiverSetupParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverSetup &msg)
Qi based parser for the SBF block "ReceiverSetup".
Definition: sbf_blocks.hpp:702
ReceiverSetup::station_code
std::string station_code
Definition: sbf_blocks.hpp:174
ReceiverStatus
Struct for the SBF block "ReceiverStatus".
Definition: sbf_blocks.hpp:208
ReceiverSetup::latitude
double latitude
Definition: sbf_blocks.hpp:171
RfBandMsg
septentrio_gnss_driver::msg::RFBand RfBandMsg
Definition: typedefs.hpp:128
ReceiverSetup::marker_number
std::string marker_number
Definition: sbf_blocks.hpp:157
PVTGeodeticParser
bool PVTGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PVTGeodeticMsg &msg)
Qi based parser for the SBF block "PVTGeodetic".
Definition: sbf_blocks.hpp:848
Dop::hpl
float hpl
Definition: sbf_blocks.hpp:144
ChannelSatInfo::sv_id
uint8_t sv_id
Definition: sbf_blocks.hpp:105
qiCharsToStringParser
void qiCharsToStringParser(It &it, std::string &val, std::size_t num)
Qi parser for char array to string.
Definition: sbf_blocks.hpp:357
DOPParser
bool DOPParser(ROSaicNodeBase *node, It it, It itEnd, Dop &msg)
Qi based parser for the SBF block "DOP".
Definition: sbf_blocks.hpp:488
ChannelSatInfoParser
bool ChannelSatInfoParser(ROSaicNodeBase *node, It &it, ChannelSatInfo &msg, uint8_t sb1_length, uint8_t sb2_length)
Qi based parser or the SBF sub-block "ChannelSatInfo".
Definition: sbf_blocks.hpp:415
ChannelStateInfo::tracking_status
uint16_t tracking_status
Definition: sbf_blocks.hpp:94


septentrio_gnss_driver
Author(s): Tibor Dome, Thomas Emter
autogenerated on Tue Feb 25 2025 04:03:55