sbf_structs.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_BYTE_1 = 0x24;
64 static const uint8_t SBF_SYNC_BYTE_2 = 0x40;
65 
66 // C++
67 #include <algorithm>
68 // Boost
69 #include <boost/spirit/include/qi.hpp>
72 
84 {
85  uint8_t sync_1;
86  uint8_t sync_2;
87  uint16_t crc;
88  uint16_t id;
89  uint8_t revision;
90  uint16_t length;
91  uint32_t tow;
93  uint16_t wnc;
94 };
95 
101 {
102  uint8_t antenna;
103  uint16_t tracking_status;
104  uint16_t pvt_status;
105  uint16_t pvt_info;
106 };
107 
113 {
114  uint8_t sv_id;
115  uint8_t freq_nr;
116  uint16_t az_rise_set;
117  uint16_t health_status;
118  int8_t elev;
119  uint8_t n2;
120  uint8_t rx_channel;
121 
122  std::vector<ChannelStateInfo> stateInfo;
123 };
124 
130 {
132 
133  uint8_t n;
134  uint8_t sb1_length;
135  uint8_t sb2_length;
136 
137  std::vector<ChannelSatInfo> satInfo;
138 };
139 
144 struct DOP
145 {
147 
148  uint8_t nr_sv;
149  double pdop;
150  double tdop;
151  double hdop;
152  double vdop;
153  float hpl;
154  float vpl;
155 };
156 
162 {
164 
165  std::string marker_name;
166  std::string marker_number;
167  std::string observer;
168  std::string agency;
169  std::string rx_serial_number;
170  std::string rx_name;
171  std::string rx_version;
172  std::string ant_serial_nbr;
173  std::string ant_type;
174  float delta_h; /* [m] */
175  float delta_e; /* [m] */
176  float delta_n; /* [m] */
177  std::string marker_type;
178  std::string gnss_fw_version;
179  std::string product_name;
180  double latitude;
181  double longitude;
182  float height;
183  std::string station_code;
184  uint8_t monument_idx;
185  uint8_t receiver_idx;
186  std::string country_code;
187 };
188 
194 {
196 
197  uint8_t n = 0;
198 
199  std::vector<uint16_t> indicators;
200 };
201 
205 struct AgcState
206 {
207  uint8_t frontend_id;
208  int8_t gain;
209  uint8_t sample_var;
210  uint8_t blanking_stat;
211 };
212 
218 {
220 
221  uint8_t cpu_load;
222  uint8_t ext_error;
223  uint32_t up_time;
224  uint32_t rx_status;
225  uint32_t rx_error;
226  uint8_t n;
227  uint8_t sb_length;
228  uint8_t cmd_count;
229  uint8_t temperature;
230 
231  std::vector<AgcState> agc_state;
232 };
233 
239 static const std::array<uint16_t, 256> CRC_LOOK_UP = {
240  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129,
241  0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252,
242  0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c,
243  0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
244  0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
245  0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
246  0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861,
247  0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
248  0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
249  0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5,
250  0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b,
251  0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
252  0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9,
253  0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
254  0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c,
255  0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
256  0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3,
257  0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
258  0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676,
259  0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
260  0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c,
261  0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16,
262  0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
263  0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
264  0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36,
265  0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
266 
267 namespace qi = boost::spirit::qi;
268 
273 template <typename T>
274 bool validValue(T s)
275 {
276  static_assert(std::is_same<uint16_t, T>::value ||
277  std::is_same<uint32_t, T>::value ||
278  std::is_same<float, T>::value || std::is_same<double, T>::value);
279  if (std::is_same<uint16_t, T>::value)
280  {
281  return (s != static_cast<uint16_t>(65535));
282  } else if (std::is_same<uint32_t, T>::value)
283  {
284  return (s != 4294967295u);
285  } else if (std::is_same<float, T>::value)
286  {
287  return (s != -2e10f);
288  } else if (std::is_same<double, T>::value)
289  {
290  return (s != -2e10);
291  }
292 }
293 
298 template <typename T>
299 void setDoNotUse(T& s)
300 {
301  static_assert(std::is_same<float, T>::value || std::is_same<double, T>::value);
302  if (std::is_same<float, T>::value)
303  {
304  s = -2e10f;
305  } else if (std::is_same<double, T>::value)
306  {
307  s = -2e10;
308  }
309 }
310 
315 template <typename It, typename Val>
316 bool qiLittleEndianParser(It& it, Val& val)
317 {
318  static_assert(
319  std::is_same<int8_t, Val>::value || std::is_same<uint8_t, Val>::value ||
320  std::is_same<int16_t, Val>::value || std::is_same<uint16_t, Val>::value ||
321  std::is_same<int32_t, Val>::value || std::is_same<uint32_t, Val>::value ||
322  std::is_same<int64_t, Val>::value || std::is_same<uint64_t, Val>::value ||
323  std::is_same<float, Val>::value || std::is_same<double, Val>::value);
324 
325  if (std::is_same<int8_t, Val>::value)
326  {
327  return qi::parse(it, it + 1, qi::char_, val);
328  } else if (std::is_same<uint8_t, Val>::value)
329  {
330  return qi::parse(it, it + 1, qi::byte_, val);
331  } else if ((std::is_same<int16_t, Val>::value) ||
332  (std::is_same<uint16_t, Val>::value))
333  {
334  return qi::parse(it, it + 2, qi::little_word, val);
335  } else if ((std::is_same<int32_t, Val>::value) ||
336  (std::is_same<uint32_t, Val>::value))
337  {
338  return qi::parse(it, it + 4, qi::little_dword, val);
339  } else if ((std::is_same<int64_t, Val>::value) ||
340  (std::is_same<uint64_t, Val>::value))
341  {
342  return qi::parse(it, it + 8, qi::little_qword, val);
343  } else if (std::is_same<float, Val>::value)
344  {
345  return qi::parse(it, it + 4, qi::little_bin_float, val);
346  } else if (std::is_same<double, Val>::value)
347  {
348  return qi::parse(it, it + 8, qi::little_bin_double, val);
349  }
350 }
351 
356 template <typename It>
357 bool qiCharsToStringParser(It& it, std::string& val, std::size_t num)
358 {
359  bool success = false;
360  val.clear();
361  success = qi::parse(it, it + num, qi::repeat(num)[qi::char_], val);
362  // remove string termination characters '\0'
363  val.erase(std::remove(val.begin(), val.end(), '\0'), val.end());
364  return success;
365 }
366 
371 template <typename It, typename Hdr>
372 bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header)
373 {
374  qiLittleEndianParser(it, block_header.sync_1);
375  if (block_header.sync_1 != SBF_SYNC_BYTE_1)
376  {
377  node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 1.");
378  return false;
379  }
380  qiLittleEndianParser(it, block_header.sync_2);
381  if (block_header.sync_2 != SBF_SYNC_BYTE_2)
382  {
383  node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 2.");
384  return false;
385  }
386  qiLittleEndianParser(it, block_header.crc);
387  uint16_t ID;
388  qiLittleEndianParser(it, ID);
389  block_header.id = ID & 8191; // lower 13 bits are id
390  block_header.revision = ID >> 13; // upper 3 bits are revision
391  qiLittleEndianParser(it, block_header.length);
392  qiLittleEndianParser(it, block_header.tow);
393  qiLittleEndianParser(it, block_header.wnc);
394  return true;
395 }
396 
401 template <typename It>
402 void ChannelStateInfoParser(It& it, ChannelStateInfo& msg, uint8_t sb2_length)
403 {
404  qiLittleEndianParser(it, msg.antenna);
405  ++it; // reserved
409  std::advance(it, sb2_length - 8); // skip padding
410 };
411 
416 template <typename It>
418  uint8_t sb1_length, uint8_t sb2_length)
419 {
420  qiLittleEndianParser(it, msg.sv_id);
421  qiLittleEndianParser(it, msg.freq_nr);
422  std::advance(it, 2); // reserved
425  qiLittleEndianParser(it, msg.elev);
426  qiLittleEndianParser(it, msg.n2);
427  if (msg.n2 > MAXSB_CHANNELSTATEINFO)
428  {
429  node->log(LogLevel::ERROR, "Parse error: Too many ChannelStateInfo " +
430  std::to_string(msg.n2));
431  return false;
432  }
434  ++it; // reserved
435  std::advance(it, sb1_length - 12); // skip padding
436  msg.stateInfo.resize(msg.n2);
437  for (auto& stateInfo : msg.stateInfo)
438  {
439  ChannelStateInfoParser(it, stateInfo, sb2_length);
440  }
441  return true;
442 };
443 
448 template <typename It>
449 bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, 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(LogLevel::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(LogLevel::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(LogLevel::ERROR, "Parse error: iterator past end.");
478  return false;
479  }
480  return true;
481 };
482 
487 template <typename It>
488 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(LogLevel::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(LogLevel::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>
546  MeasEpochChannelType1Msg& msg, uint8_t sb1_length,
547  uint8_t sb2_length)
548 {
549  qiLittleEndianParser(it, msg.rx_channel);
550  qiLittleEndianParser(it, msg.type);
551  qiLittleEndianParser(it, msg.sv_id);
552  qiLittleEndianParser(it, msg.misc);
553  qiLittleEndianParser(it, msg.code_lsb);
554  qiLittleEndianParser(it, msg.doppler);
555  qiLittleEndianParser(it, msg.carrier_lsb);
556  qiLittleEndianParser(it, msg.carrier_msb);
557  qiLittleEndianParser(it, msg.cn0);
558  qiLittleEndianParser(it, msg.lock_time);
559  qiLittleEndianParser(it, msg.obs_info);
560  qiLittleEndianParser(it, msg.n2);
561  std::advance(it, sb1_length - 20); // skip padding
562  if (msg.n2 > MAXSB_MEASEPOCH_T2)
563  {
564  node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType2 " +
565  std::to_string(msg.n2));
566  return false;
567  }
568  msg.type2.resize(msg.n2);
569  for (auto& type2 : msg.type2)
570  {
571  MeasEpochChannelType2Parser(it, type2, sb2_length);
572  }
573  return true;
574 };
575 
580 template <typename It>
581 bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg)
582 {
583  if (!BlockHeaderParser(node, it, msg.block_header))
584  return false;
585  if (msg.block_header.id != 4027)
586  {
587  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
588  std::to_string(msg.block_header.id));
589  return false;
590  }
591  qiLittleEndianParser(it, msg.n);
592  if (msg.n > MAXSB_MEASEPOCH_T1)
593  {
594  node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType1 " +
595  std::to_string(msg.n));
596  return false;
597  }
598  qiLittleEndianParser(it, msg.sb1_length);
599  qiLittleEndianParser(it, msg.sb2_length);
600  qiLittleEndianParser(it, msg.common_flags);
601  if (msg.block_header.revision > 0)
602  qiLittleEndianParser(it, msg.cum_clk_jumps);
603  ++it; // reserved
604  msg.type1.resize(msg.n);
605  for (auto& type1 : msg.type1)
606  {
607  if (!MeasEpochChannelType1Parser(node, it, type1, msg.sb1_length,
608  msg.sb2_length))
609  return false;
610  }
611  if (it > itEnd)
612  {
613  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
614  return false;
615  }
616  return true;
617 };
618 
623 template <typename It>
624 bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& msg)
625 {
626  if (!BlockHeaderParser(node, it, msg.block_header))
627  return false;
628  if (msg.block_header.id != 5902)
629  {
630  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
631  std::to_string(msg.block_header.id));
632  return false;
633  }
634  std::advance(it, 2); // reserved
635  qiCharsToStringParser(it, msg.marker_name, 60);
637  qiCharsToStringParser(it, msg.observer, 20);
638  qiCharsToStringParser(it, msg.agency, 40);
640  qiCharsToStringParser(it, msg.rx_name, 20);
641  qiCharsToStringParser(it, msg.rx_version, 20);
643  qiCharsToStringParser(it, msg.ant_type, 20);
644  qiLittleEndianParser(it, msg.delta_h);
645  qiLittleEndianParser(it, msg.delta_e);
646  qiLittleEndianParser(it, msg.delta_n);
647  if (msg.block_header.revision > 0)
648  qiCharsToStringParser(it, msg.marker_type, 20);
649  if (msg.block_header.revision > 1)
651  if (msg.block_header.revision > 2)
652  qiCharsToStringParser(it, msg.product_name, 40);
653  if (msg.block_header.revision > 3)
654  {
657  qiLittleEndianParser(it, msg.height);
658  qiCharsToStringParser(it, msg.station_code, 10);
662  } else
663  {
664  setDoNotUse(msg.latitude);
665  setDoNotUse(msg.longitude);
666  setDoNotUse(msg.height);
667  }
668  if (it > itEnd)
669  {
670  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
671  return false;
672  }
673  return true;
674 };
675 
680 template <typename It>
681 bool ReceiverTimesParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg)
682 {
683  if (!BlockHeaderParser(node, it, msg.block_header))
684  return false;
685  if (msg.block_header.id != 5914)
686  {
687  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
688  std::to_string(msg.block_header.id));
689  return false;
690  }
691  qiLittleEndianParser(it, msg.utc_year);
692  qiLittleEndianParser(it, msg.utc_month);
693  qiLittleEndianParser(it, msg.utc_day);
694  qiLittleEndianParser(it, msg.utc_hour);
695  qiLittleEndianParser(it, msg.utc_min);
696  qiLittleEndianParser(it, msg.utc_second);
697  qiLittleEndianParser(it, msg.delta_ls);
698  qiLittleEndianParser(it, msg.sync_level);
699  if (it > itEnd)
700  {
701  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
702  return false;
703  }
704  return true;
705 };
706 
711 template <typename It>
712 bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& msg)
713 {
714  if (!BlockHeaderParser(node, it, msg.block_header))
715  return false;
716  if (msg.block_header.id != 4006)
717  {
718  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
719  std::to_string(msg.block_header.id));
720  return false;
721  }
722  qiLittleEndianParser(it, msg.mode);
723  qiLittleEndianParser(it, msg.error);
724  qiLittleEndianParser(it, msg.x);
725  qiLittleEndianParser(it, msg.y);
726  qiLittleEndianParser(it, msg.z);
727  qiLittleEndianParser(it, msg.undulation);
728  qiLittleEndianParser(it, msg.vx);
729  qiLittleEndianParser(it, msg.vy);
730  qiLittleEndianParser(it, msg.vz);
731  qiLittleEndianParser(it, msg.cog);
732  qiLittleEndianParser(it, msg.rx_clk_bias);
733  qiLittleEndianParser(it, msg.rx_clk_drift);
734  qiLittleEndianParser(it, msg.time_system);
735  qiLittleEndianParser(it, msg.datum);
736  qiLittleEndianParser(it, msg.nr_sv);
737  qiLittleEndianParser(it, msg.wa_corr_info);
738  qiLittleEndianParser(it, msg.reference_id);
739  qiLittleEndianParser(it, msg.mean_corr_age);
740  qiLittleEndianParser(it, msg.signal_info);
741  qiLittleEndianParser(it, msg.alert_flag);
742  if (msg.block_header.revision > 0)
743  {
744  qiLittleEndianParser(it, msg.nr_bases);
745  qiLittleEndianParser(it, msg.ppp_info);
746  }
747  if (msg.block_header.revision > 1)
748  {
749  qiLittleEndianParser(it, msg.latency);
750  qiLittleEndianParser(it, msg.h_accuracy);
751  qiLittleEndianParser(it, msg.v_accuracy);
752  qiLittleEndianParser(it, msg.misc);
753  }
754  if (it > itEnd)
755  {
756  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
757  return false;
758  }
759  return true;
760 }
761 
766 template <typename It>
767 bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& msg)
768 {
769  if (!BlockHeaderParser(node, it, msg.block_header))
770  return false;
771  if (msg.block_header.id != 4007)
772  {
773  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
774  std::to_string(msg.block_header.id));
775  return false;
776  }
777  qiLittleEndianParser(it, msg.mode);
778  qiLittleEndianParser(it, msg.error);
779  qiLittleEndianParser(it, msg.latitude);
780  qiLittleEndianParser(it, msg.longitude);
781  qiLittleEndianParser(it, msg.height);
782  qiLittleEndianParser(it, msg.undulation);
783  qiLittleEndianParser(it, msg.vn);
784  qiLittleEndianParser(it, msg.ve);
785  qiLittleEndianParser(it, msg.vu);
786  qiLittleEndianParser(it, msg.cog);
787  qiLittleEndianParser(it, msg.rx_clk_bias);
788  qiLittleEndianParser(it, msg.rx_clk_drift);
789  qiLittleEndianParser(it, msg.time_system);
790  qiLittleEndianParser(it, msg.datum);
791  qiLittleEndianParser(it, msg.nr_sv);
792  qiLittleEndianParser(it, msg.wa_corr_info);
793  qiLittleEndianParser(it, msg.reference_id);
794  qiLittleEndianParser(it, msg.mean_corr_age);
795  qiLittleEndianParser(it, msg.signal_info);
796  qiLittleEndianParser(it, msg.alert_flag);
797  if (msg.block_header.revision > 0)
798  {
799  qiLittleEndianParser(it, msg.nr_bases);
800  qiLittleEndianParser(it, msg.ppp_info);
801  }
802  if (msg.block_header.revision > 1)
803  {
804  qiLittleEndianParser(it, msg.latency);
805  qiLittleEndianParser(it, msg.h_accuracy);
806  qiLittleEndianParser(it, msg.v_accuracy);
807  qiLittleEndianParser(it, msg.misc);
808  }
809  if (it > itEnd)
810  {
811  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
812  return false;
813  }
814  return true;
815 }
816 
821 template <typename It>
822 bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg,
823  bool use_ros_axis_orientation)
824 {
825  if (!BlockHeaderParser(node, it, msg.block_header))
826  return false;
827  if (msg.block_header.id != 5938)
828  {
829  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
830  std::to_string(msg.block_header.id));
831  return false;
832  }
833  qiLittleEndianParser(it, msg.nr_sv);
834  qiLittleEndianParser(it, msg.error);
835  qiLittleEndianParser(it, msg.mode);
836  std::advance(it, 2); // reserved
837  qiLittleEndianParser(it, msg.heading);
838  qiLittleEndianParser(it, msg.pitch);
839  qiLittleEndianParser(it, msg.roll);
840  qiLittleEndianParser(it, msg.pitch_dot);
841  qiLittleEndianParser(it, msg.roll_dot);
842  qiLittleEndianParser(it, msg.heading_dot);
843  if (use_ros_axis_orientation)
844  {
845  if (validValue(msg.heading))
846  msg.heading = -msg.heading + 90;
847  if (validValue(msg.pitch))
848  msg.pitch = -msg.pitch;
849  if (validValue(msg.pitch_dot))
850  msg.pitch_dot = -msg.pitch_dot;
851  if (validValue(msg.heading_dot))
852  msg.heading_dot = -msg.heading_dot;
853  }
854  if (it > itEnd)
855  {
856  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
857  return false;
858  }
859  return true;
860 };
861 
866 template <typename It>
867 bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& msg,
868  bool use_ros_axis_orientation)
869 {
870  if (!BlockHeaderParser(node, it, msg.block_header))
871  return false;
872  if (msg.block_header.id != 5939)
873  {
874  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
875  std::to_string(msg.block_header.id));
876  return false;
877  }
878  ++it; // reserved
879  qiLittleEndianParser(it, msg.error);
880  qiLittleEndianParser(it, msg.cov_headhead);
881  qiLittleEndianParser(it, msg.cov_pitchpitch);
882  qiLittleEndianParser(it, msg.cov_rollroll);
883  qiLittleEndianParser(it, msg.cov_headpitch);
884  qiLittleEndianParser(it, msg.cov_headroll);
885  qiLittleEndianParser(it, msg.cov_pitchroll);
886  if (use_ros_axis_orientation)
887  {
888  if (validValue(msg.cov_headroll))
889  msg.cov_headroll = -msg.cov_headroll;
890  if (validValue(msg.cov_pitchroll))
891  msg.cov_pitchroll = -msg.cov_pitchroll;
892  }
893  if (it > itEnd)
894  {
895  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
896  return false;
897  }
898  return true;
899 };
900 
905 template <typename It>
906 void VectorInfoCartParser(It& it, VectorInfoCartMsg& msg, uint8_t sb_length)
907 {
908  qiLittleEndianParser(it, msg.nr_sv);
909  qiLittleEndianParser(it, msg.error);
910  qiLittleEndianParser(it, msg.mode);
911  qiLittleEndianParser(it, msg.misc);
912  qiLittleEndianParser(it, msg.delta_x);
913  qiLittleEndianParser(it, msg.delta_y);
914  qiLittleEndianParser(it, msg.delta_z);
915  qiLittleEndianParser(it, msg.delta_vx);
916  qiLittleEndianParser(it, msg.delta_vy);
917  qiLittleEndianParser(it, msg.delta_vz);
918  qiLittleEndianParser(it, msg.azimuth);
919  qiLittleEndianParser(it, msg.elevation);
920  qiLittleEndianParser(it, msg.reference_id);
921  qiLittleEndianParser(it, msg.corr_age);
922  qiLittleEndianParser(it, msg.signal_info);
923  std::advance(it, sb_length - 52); // skip padding
924 };
925 
930 template <typename It>
931 bool BaseVectorCartParser(ROSaicNodeBase* node, It it, It itEnd,
932  BaseVectorCartMsg& msg)
933 {
934  if (!BlockHeaderParser(node, it, msg.block_header))
935  return false;
936  if (msg.block_header.id != 4043)
937  {
938  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
939  std::to_string(msg.block_header.id));
940  return false;
941  }
942  qiLittleEndianParser(it, msg.n);
943  if (msg.n > MAXSB_NBVECTORINFO)
944  {
945  node->log(LogLevel::ERROR,
946  "Parse error: Too many VectorInfoCart " + std::to_string(msg.n));
947  return false;
948  }
949  qiLittleEndianParser(it, msg.sb_length);
950  msg.vector_info_cart.resize(msg.n);
951  for (auto& vector_info_cart : msg.vector_info_cart)
952  {
953  VectorInfoCartParser(it, vector_info_cart, msg.sb_length);
954  }
955  if (it > itEnd)
956  {
957  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
958  return false;
959  }
960  return true;
961 };
962 
967 template <typename It>
968 void VectorInfoGeodParser(It& it, VectorInfoGeodMsg& msg, uint8_t sb_length)
969 {
970  qiLittleEndianParser(it, msg.nr_sv);
971  qiLittleEndianParser(it, msg.error);
972  qiLittleEndianParser(it, msg.mode);
973  qiLittleEndianParser(it, msg.misc);
974  qiLittleEndianParser(it, msg.delta_east);
975  qiLittleEndianParser(it, msg.delta_north);
976  qiLittleEndianParser(it, msg.delta_up);
977  qiLittleEndianParser(it, msg.delta_ve);
978  qiLittleEndianParser(it, msg.delta_vn);
979  qiLittleEndianParser(it, msg.delta_vu);
980  qiLittleEndianParser(it, msg.azimuth);
981  qiLittleEndianParser(it, msg.elevation);
982  qiLittleEndianParser(it, msg.reference_id);
983  qiLittleEndianParser(it, msg.corr_age);
984  qiLittleEndianParser(it, msg.signal_info);
985  std::advance(it, sb_length - 52); // skip padding
986 };
987 
992 template <typename It>
993 bool BaseVectorGeodParser(ROSaicNodeBase* node, It it, It itEnd,
994  BaseVectorGeodMsg& msg)
995 {
996  if (!BlockHeaderParser(node, it, msg.block_header))
997  return false;
998  if (msg.block_header.id != 4028)
999  {
1000  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1001  std::to_string(msg.block_header.id));
1002  return false;
1003  }
1004  qiLittleEndianParser(it, msg.n);
1005  if (msg.n > MAXSB_NBVECTORINFO)
1006  {
1007  node->log(LogLevel::ERROR,
1008  "Parse error: Too many VectorInfoGeod " + std::to_string(msg.n));
1009  return false;
1010  }
1011  qiLittleEndianParser(it, msg.sb_length);
1012  msg.vector_info_geod.resize(msg.n);
1013  for (auto& vector_info_geod : msg.vector_info_geod)
1014  {
1015  VectorInfoGeodParser(it, vector_info_geod, msg.sb_length);
1016  }
1017  if (it > itEnd)
1018  {
1019  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1020  return false;
1021  }
1022  return true;
1023 };
1024 
1029 template <typename It>
1030 bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg,
1031  bool use_ros_axis_orientation)
1032 {
1033  if (!BlockHeaderParser(node, it, msg.block_header))
1034  return false;
1035  if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229))
1036  {
1037  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1038  std::to_string(msg.block_header.id));
1039  return false;
1040  }
1041  qiLittleEndianParser(it, msg.gnss_mode);
1042  qiLittleEndianParser(it, msg.error);
1043  qiLittleEndianParser(it, msg.info);
1044  qiLittleEndianParser(it, msg.gnss_age);
1045  qiLittleEndianParser(it, msg.x);
1046  qiLittleEndianParser(it, msg.y);
1047  qiLittleEndianParser(it, msg.z);
1048  qiLittleEndianParser(it, msg.accuracy);
1049  qiLittleEndianParser(it, msg.latency);
1050  qiLittleEndianParser(it, msg.datum);
1051  ++it; // reserved
1052  qiLittleEndianParser(it, msg.sb_list);
1053  if ((msg.sb_list & 1) != 0)
1054  {
1055  qiLittleEndianParser(it, msg.x_std_dev);
1056  qiLittleEndianParser(it, msg.y_std_dev);
1057  qiLittleEndianParser(it, msg.z_std_dev);
1058  } else
1059  {
1060  setDoNotUse(msg.x_std_dev);
1061  setDoNotUse(msg.y_std_dev);
1062  setDoNotUse(msg.z_std_dev);
1063  }
1064  if ((msg.sb_list & 2) != 0)
1065  {
1066  qiLittleEndianParser(it, msg.heading);
1067  qiLittleEndianParser(it, msg.pitch);
1068  qiLittleEndianParser(it, msg.roll);
1069  if (use_ros_axis_orientation)
1070  {
1071  if (validValue(msg.heading))
1072  msg.heading = -msg.heading + 90;
1073  if (validValue(msg.pitch))
1074  msg.pitch = -msg.pitch;
1075  }
1076  } else
1077  {
1078  setDoNotUse(msg.heading);
1079  setDoNotUse(msg.pitch);
1080  setDoNotUse(msg.roll);
1081  }
1082  if ((msg.sb_list & 4) != 0)
1083  {
1084  qiLittleEndianParser(it, msg.heading_std_dev);
1085  qiLittleEndianParser(it, msg.pitch_std_dev);
1086  qiLittleEndianParser(it, msg.roll_std_dev);
1087  } else
1088  {
1089  setDoNotUse(msg.heading_std_dev);
1090  setDoNotUse(msg.pitch_std_dev);
1091  setDoNotUse(msg.roll_std_dev);
1092  }
1093  if ((msg.sb_list & 8) != 0)
1094  {
1095  qiLittleEndianParser(it, msg.vx);
1096  qiLittleEndianParser(it, msg.vy);
1097  qiLittleEndianParser(it, msg.vz);
1098  } else
1099  {
1100  setDoNotUse(msg.vx);
1101  setDoNotUse(msg.vy);
1102  setDoNotUse(msg.vz);
1103  }
1104  if ((msg.sb_list & 16) != 0)
1105  {
1106  qiLittleEndianParser(it, msg.vx_std_dev);
1107  qiLittleEndianParser(it, msg.vy_std_dev);
1108  qiLittleEndianParser(it, msg.vz_std_dev);
1109  } else
1110  {
1111  setDoNotUse(msg.vx_std_dev);
1112  setDoNotUse(msg.vy_std_dev);
1113  setDoNotUse(msg.vz_std_dev);
1114  }
1115  if ((msg.sb_list & 32) != 0)
1116  {
1117  qiLittleEndianParser(it, msg.xy_cov);
1118  qiLittleEndianParser(it, msg.xz_cov);
1119  qiLittleEndianParser(it, msg.yz_cov);
1120  } else
1121  {
1122  setDoNotUse(msg.xy_cov);
1123  setDoNotUse(msg.xz_cov);
1124  setDoNotUse(msg.yz_cov);
1125  }
1126  if ((msg.sb_list & 64) != 0)
1127  {
1128  qiLittleEndianParser(it, msg.heading_pitch_cov);
1129  qiLittleEndianParser(it, msg.heading_roll_cov);
1130  qiLittleEndianParser(it, msg.pitch_roll_cov);
1131  if (use_ros_axis_orientation)
1132  {
1133  if (validValue(msg.heading_roll_cov))
1134  msg.heading_roll_cov = -msg.heading_roll_cov;
1135  if (validValue(msg.pitch_roll_cov))
1136  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1137  }
1138  } else
1139  {
1140  setDoNotUse(msg.heading_pitch_cov);
1141  setDoNotUse(msg.heading_roll_cov);
1142  setDoNotUse(msg.pitch_roll_cov);
1143  }
1144  if ((msg.sb_list & 128) != 0)
1145  {
1146  qiLittleEndianParser(it, msg.vx_vy_cov);
1147  qiLittleEndianParser(it, msg.vx_vz_cov);
1148  qiLittleEndianParser(it, msg.vy_vz_cov);
1149  } else
1150  {
1151  setDoNotUse(msg.vx_vy_cov);
1152  setDoNotUse(msg.vx_vz_cov);
1153  setDoNotUse(msg.vy_vz_cov);
1154  }
1155  if (it > itEnd)
1156  {
1157  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1158  return false;
1159  }
1160  return true;
1161 };
1162 
1167 template <typename It>
1168 bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd,
1169  PosCovCartesianMsg& msg)
1170 {
1171  if (!BlockHeaderParser(node, it, msg.block_header))
1172  return false;
1173  if (msg.block_header.id != 5905)
1174  {
1175  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1176  std::to_string(msg.block_header.id));
1177  return false;
1178  }
1179  qiLittleEndianParser(it, msg.mode);
1180  qiLittleEndianParser(it, msg.error);
1181  qiLittleEndianParser(it, msg.cov_xx);
1182  qiLittleEndianParser(it, msg.cov_yy);
1183  qiLittleEndianParser(it, msg.cov_zz);
1184  qiLittleEndianParser(it, msg.cov_bb);
1185  qiLittleEndianParser(it, msg.cov_xy);
1186  qiLittleEndianParser(it, msg.cov_xz);
1187  qiLittleEndianParser(it, msg.cov_xb);
1188  qiLittleEndianParser(it, msg.cov_yz);
1189  qiLittleEndianParser(it, msg.cov_yb);
1190  qiLittleEndianParser(it, msg.cov_zb);
1191  if (it > itEnd)
1192  {
1193  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1194  return false;
1195  }
1196  return true;
1197 };
1198 
1203 template <typename It>
1204 bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd,
1205  PosCovGeodeticMsg& msg)
1206 {
1207  if (!BlockHeaderParser(node, it, msg.block_header))
1208  return false;
1209  if (msg.block_header.id != 5906)
1210  {
1211  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1212  std::to_string(msg.block_header.id));
1213  return false;
1214  }
1215  qiLittleEndianParser(it, msg.mode);
1216  qiLittleEndianParser(it, msg.error);
1217  qiLittleEndianParser(it, msg.cov_latlat);
1218  qiLittleEndianParser(it, msg.cov_lonlon);
1219  qiLittleEndianParser(it, msg.cov_hgthgt);
1220  qiLittleEndianParser(it, msg.cov_bb);
1221  qiLittleEndianParser(it, msg.cov_latlon);
1222  qiLittleEndianParser(it, msg.cov_lathgt);
1223  qiLittleEndianParser(it, msg.cov_latb);
1224  qiLittleEndianParser(it, msg.cov_lonhgt);
1225  qiLittleEndianParser(it, msg.cov_lonb);
1226  qiLittleEndianParser(it, msg.cov_hb);
1227  if (it > itEnd)
1228  {
1229  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1230  return false;
1231  }
1232  return true;
1233 };
1234 
1239 template <typename It>
1240 bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd,
1241  VelCovCartesianMsg& msg)
1242 {
1243  if (!BlockHeaderParser(node, it, msg.block_header))
1244  return false;
1245  if (msg.block_header.id != 5907)
1246  {
1247  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1248  std::to_string(msg.block_header.id));
1249  return false;
1250  }
1251  qiLittleEndianParser(it, msg.mode);
1252  qiLittleEndianParser(it, msg.error);
1253  qiLittleEndianParser(it, msg.cov_vxvx);
1254  qiLittleEndianParser(it, msg.cov_vyvy);
1255  qiLittleEndianParser(it, msg.cov_vzvz);
1256  qiLittleEndianParser(it, msg.cov_dtdt);
1257  qiLittleEndianParser(it, msg.cov_vxvy);
1258  qiLittleEndianParser(it, msg.cov_vxvz);
1259  qiLittleEndianParser(it, msg.cov_vxdt);
1260  qiLittleEndianParser(it, msg.cov_vyvz);
1261  qiLittleEndianParser(it, msg.cov_vydt);
1262  qiLittleEndianParser(it, msg.cov_vzdt);
1263  if (it > itEnd)
1264  {
1265  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1266  return false;
1267  }
1268  return true;
1269 };
1270 
1275 template <typename It>
1276 bool VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd,
1277  VelCovGeodeticMsg& msg)
1278 {
1279  if (!BlockHeaderParser(node, it, msg.block_header))
1280  return false;
1281  if (msg.block_header.id != 5908)
1282  {
1283  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1284  std::to_string(msg.block_header.id));
1285  return false;
1286  }
1287  qiLittleEndianParser(it, msg.mode);
1288  qiLittleEndianParser(it, msg.error);
1289  qiLittleEndianParser(it, msg.cov_vnvn);
1290  qiLittleEndianParser(it, msg.cov_veve);
1291  qiLittleEndianParser(it, msg.cov_vuvu);
1292  qiLittleEndianParser(it, msg.cov_dtdt);
1293  qiLittleEndianParser(it, msg.cov_vnve);
1294  qiLittleEndianParser(it, msg.cov_vnvu);
1295  qiLittleEndianParser(it, msg.cov_vndt);
1296  qiLittleEndianParser(it, msg.cov_vevu);
1297  qiLittleEndianParser(it, msg.cov_vedt);
1298  qiLittleEndianParser(it, msg.cov_vudt);
1299  if (it > itEnd)
1300  {
1301  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1302  return false;
1303  }
1304  return true;
1305 };
1306 
1311 template <typename It>
1312 bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg)
1313 {
1314  if (!BlockHeaderParser(node, it, msg.block_header))
1315  return false;
1316  if (msg.block_header.id != 4082)
1317  {
1318  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1319  std::to_string(msg.block_header.id));
1320  return false;
1321  }
1322  qiLittleEndianParser(it, msg.n);
1323  if (msg.n > 40)
1324  {
1325  node->log(LogLevel::ERROR,
1326  "Parse error: Too many indicators " + std::to_string(msg.n));
1327  return false;
1328  }
1329  ++it; // reserved
1330  msg.indicators.resize(msg.n);
1331  std::vector<uint16_t> indicators;
1332  for (auto& indicators : msg.indicators)
1333  {
1334  qiLittleEndianParser(it, indicators);
1335  }
1336  if (it > itEnd)
1337  {
1338  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1339  return false;
1340  }
1341  return true;
1342 };
1343 
1348 template <typename It>
1349 void AgcStateParser(It it, AgcState& msg, uint8_t sb_length)
1350 {
1352  qiLittleEndianParser(it, msg.gain);
1355  std::advance(it, sb_length - 4); // skip padding
1356 };
1357 
1362 template <typename It>
1363 bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& msg)
1364 {
1365  if (!BlockHeaderParser(node, it, msg.block_header))
1366  return false;
1367  if (msg.block_header.id != 4014)
1368  {
1369  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1370  std::to_string(msg.block_header.id));
1371  return false;
1372  }
1373  qiLittleEndianParser(it, msg.cpu_load);
1375  qiLittleEndianParser(it, msg.up_time);
1377  qiLittleEndianParser(it, msg.rx_error);
1378  qiLittleEndianParser(it, msg.n);
1379  if (msg.n > 18)
1380  {
1381  node->log(LogLevel::ERROR,
1382  "Parse error: Too many AGCState " + std::to_string(msg.n));
1383  return false;
1384  }
1388  msg.agc_state.resize(msg.n);
1389  for (auto& agc_state : msg.agc_state)
1390  {
1391  AgcStateParser(it, agc_state, msg.sb_length);
1392  }
1393  if (it > itEnd)
1394  {
1395  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1396  return false;
1397  }
1398  return true;
1399 };
1400 
1405 template <typename It>
1406 bool ReceiverTimeParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverTimeMsg& msg)
1407 {
1408  if (!BlockHeaderParser(node, it, msg.block_header))
1409  return false;
1410  if (msg.block_header.id != 5914)
1411  {
1412  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1413  std::to_string(msg.block_header.id));
1414  return false;
1415  }
1416  qiLittleEndianParser(it, msg.utc_year);
1417  qiLittleEndianParser(it, msg.utc_month);
1418  qiLittleEndianParser(it, msg.utc_day);
1419  qiLittleEndianParser(it, msg.utc_hour);
1420  qiLittleEndianParser(it, msg.utc_min);
1421  qiLittleEndianParser(it, msg.utc_second);
1422  qiLittleEndianParser(it, msg.delta_ls);
1423  qiLittleEndianParser(it, msg.sync_level);
1424  if (it > itEnd)
1425  {
1426  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1427  return false;
1428  }
1429  return true;
1430 };
1431 
1436 template <typename It>
1437 bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg,
1438  bool use_ros_axis_orientation)
1439 {
1440  if (!BlockHeaderParser(node, it, msg.block_header))
1441  return false;
1442  if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230))
1443  {
1444  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1445  std::to_string(msg.block_header.id));
1446  return false;
1447  }
1448  qiLittleEndianParser(it, msg.gnss_mode);
1449  qiLittleEndianParser(it, msg.error);
1450  qiLittleEndianParser(it, msg.info);
1451  qiLittleEndianParser(it, msg.gnss_age);
1452  qiLittleEndianParser(it, msg.latitude);
1453  qiLittleEndianParser(it, msg.longitude);
1454  qiLittleEndianParser(it, msg.height);
1455  qiLittleEndianParser(it, msg.undulation);
1456  qiLittleEndianParser(it, msg.accuracy);
1457  qiLittleEndianParser(it, msg.latency);
1458  qiLittleEndianParser(it, msg.datum);
1459  ++it; // reserved
1460  qiLittleEndianParser(it, msg.sb_list);
1461  if ((msg.sb_list & 1) != 0)
1462  {
1463  qiLittleEndianParser(it, msg.latitude_std_dev);
1464  qiLittleEndianParser(it, msg.longitude_std_dev);
1465  qiLittleEndianParser(it, msg.height_std_dev);
1466  } else
1467  {
1468  setDoNotUse(msg.latitude_std_dev);
1469  setDoNotUse(msg.longitude_std_dev);
1470  setDoNotUse(msg.height_std_dev);
1471  }
1472  if ((msg.sb_list & 2) != 0)
1473  {
1474  qiLittleEndianParser(it, msg.heading);
1475  qiLittleEndianParser(it, msg.pitch);
1476  qiLittleEndianParser(it, msg.roll);
1477  if (use_ros_axis_orientation)
1478  {
1479  if (validValue(msg.heading))
1480  msg.heading = -msg.heading + 90;
1481  if (validValue(msg.pitch))
1482  msg.pitch = -msg.pitch;
1483  }
1484  } else
1485  {
1486  setDoNotUse(msg.heading);
1487  setDoNotUse(msg.pitch);
1488  setDoNotUse(msg.roll);
1489  }
1490  if ((msg.sb_list & 4) != 0)
1491  {
1492  qiLittleEndianParser(it, msg.heading_std_dev);
1493  qiLittleEndianParser(it, msg.pitch_std_dev);
1494  qiLittleEndianParser(it, msg.roll_std_dev);
1495  } else
1496  {
1497  setDoNotUse(msg.heading_std_dev);
1498  setDoNotUse(msg.pitch_std_dev);
1499  setDoNotUse(msg.roll_std_dev);
1500  }
1501  if ((msg.sb_list & 8) != 0)
1502  {
1503  qiLittleEndianParser(it, msg.ve);
1504  qiLittleEndianParser(it, msg.vn);
1505  qiLittleEndianParser(it, msg.vu);
1506  } else
1507  {
1508  setDoNotUse(msg.ve);
1509  setDoNotUse(msg.vn);
1510  setDoNotUse(msg.vu);
1511  }
1512  if ((msg.sb_list & 16) != 0)
1513  {
1514  qiLittleEndianParser(it, msg.ve_std_dev);
1515  qiLittleEndianParser(it, msg.vn_std_dev);
1516  qiLittleEndianParser(it, msg.vu_std_dev);
1517  } else
1518  {
1519  setDoNotUse(msg.ve_std_dev);
1520  setDoNotUse(msg.vn_std_dev);
1521  setDoNotUse(msg.vu_std_dev);
1522  }
1523  if ((msg.sb_list & 32) != 0)
1524  {
1525  qiLittleEndianParser(it, msg.latitude_longitude_cov);
1526  qiLittleEndianParser(it, msg.latitude_height_cov);
1527  qiLittleEndianParser(it, msg.longitude_height_cov);
1528  } else
1529  {
1530  setDoNotUse(msg.latitude_longitude_cov);
1531  setDoNotUse(msg.latitude_height_cov);
1532  setDoNotUse(msg.longitude_height_cov);
1533  }
1534  if ((msg.sb_list & 64) != 0)
1535  {
1536  qiLittleEndianParser(it, msg.heading_pitch_cov);
1537  qiLittleEndianParser(it, msg.heading_roll_cov);
1538  qiLittleEndianParser(it, msg.pitch_roll_cov);
1539  if (use_ros_axis_orientation)
1540  {
1541  if (validValue(msg.heading_roll_cov))
1542  msg.heading_roll_cov = -msg.heading_roll_cov;
1543  if (validValue(msg.pitch_roll_cov))
1544  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1545  }
1546  } else
1547  {
1548  setDoNotUse(msg.heading_pitch_cov);
1549  setDoNotUse(msg.heading_roll_cov);
1550  setDoNotUse(msg.pitch_roll_cov);
1551  }
1552  if ((msg.sb_list & 128) != 0)
1553  {
1554  qiLittleEndianParser(it, msg.ve_vn_cov);
1555  qiLittleEndianParser(it, msg.ve_vu_cov);
1556  qiLittleEndianParser(it, msg.vn_vu_cov);
1557  } else
1558  {
1559  setDoNotUse(msg.ve_vn_cov);
1560  setDoNotUse(msg.ve_vu_cov);
1561  setDoNotUse(msg.vn_vu_cov);
1562  }
1563  if (it > itEnd)
1564  {
1565  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1566  return false;
1567  }
1568  return true;
1569 };
1570 
1575 template <typename It>
1576 bool IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg,
1577  bool use_ros_axis_orientation)
1578 {
1579  if (!BlockHeaderParser(node, it, msg.block_header))
1580  return false;
1581  if (msg.block_header.id != 4224)
1582  {
1583  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1584  std::to_string(msg.block_header.id));
1585  return false;
1586  }
1587  ++it; // reserved
1588  qiLittleEndianParser(it, msg.serial_port);
1589  qiLittleEndianParser(it, msg.ant_lever_arm_x);
1590  qiLittleEndianParser(it, msg.ant_lever_arm_y);
1591  qiLittleEndianParser(it, msg.ant_lever_arm_z);
1592  qiLittleEndianParser(it, msg.theta_x);
1593  qiLittleEndianParser(it, msg.theta_y);
1594  qiLittleEndianParser(it, msg.theta_z);
1595  if (use_ros_axis_orientation)
1596  {
1597  msg.ant_lever_arm_y = -msg.ant_lever_arm_y;
1598  msg.ant_lever_arm_z = -msg.ant_lever_arm_z;
1599  msg.theta_x = parsing_utilities::wrapAngle180to180(msg.theta_x - 180.0f);
1600  }
1601  if (it > itEnd)
1602  {
1603  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1604  return false;
1605  }
1606  return true;
1607 };
1608 
1613 template <typename It>
1614 bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd,
1615  VelSensorSetupMsg& msg, bool use_ros_axis_orientation)
1616 {
1617  if (!BlockHeaderParser(node, it, msg.block_header))
1618  return false;
1619  if (msg.block_header.id != 4244)
1620  {
1621  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1622  std::to_string(msg.block_header.id));
1623  return false;
1624  }
1625  ++it; // reserved
1626  qiLittleEndianParser(it, msg.port);
1627  qiLittleEndianParser(it, msg.lever_arm_x);
1628  qiLittleEndianParser(it, msg.lever_arm_y);
1629  qiLittleEndianParser(it, msg.lever_arm_z);
1630  if (use_ros_axis_orientation)
1631  {
1632  msg.lever_arm_y = -msg.lever_arm_y;
1633  msg.lever_arm_z = -msg.lever_arm_z;
1634  }
1635  if (it > itEnd)
1636  {
1637  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1638  return false;
1639  }
1640  return true;
1641 };
1642 
1647 template <typename It>
1648 bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd,
1649  ExtSensorMeasMsg& msg, bool use_ros_axis_orientation,
1650  bool& hasImuMeas)
1651 {
1652  if (!BlockHeaderParser(node, it, msg.block_header))
1653  return false;
1654  if (msg.block_header.id != 4050)
1655  {
1656  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " +
1657  std::to_string(msg.block_header.id));
1658  return false;
1659  }
1660  qiLittleEndianParser(it, msg.n);
1661  qiLittleEndianParser(it, msg.sb_length);
1662  if (msg.sb_length != 28)
1663  {
1664  node->log(LogLevel::ERROR,
1665  "Parse error: Wrong sb_length " + std::to_string(msg.sb_length));
1666  return false;
1667  }
1668 
1669  msg.acceleration_x = std::numeric_limits<double>::quiet_NaN();
1670  msg.acceleration_y = std::numeric_limits<double>::quiet_NaN();
1671  msg.acceleration_z = std::numeric_limits<double>::quiet_NaN();
1672 
1673  msg.angular_rate_x = std::numeric_limits<double>::quiet_NaN();
1674  msg.angular_rate_y = std::numeric_limits<double>::quiet_NaN();
1675  msg.angular_rate_z = std::numeric_limits<double>::quiet_NaN();
1676 
1677  msg.velocity_x = std::numeric_limits<double>::quiet_NaN();
1678  msg.velocity_y = std::numeric_limits<double>::quiet_NaN();
1679  msg.velocity_z = std::numeric_limits<double>::quiet_NaN();
1680 
1681  msg.std_dev_x = std::numeric_limits<double>::quiet_NaN();
1682  msg.std_dev_y = std::numeric_limits<double>::quiet_NaN();
1683  msg.std_dev_z = std::numeric_limits<double>::quiet_NaN();
1684 
1685  msg.sensor_temperature = -32768.0f; // do not use value
1686  msg.zero_velocity_flag = std::numeric_limits<double>::quiet_NaN();
1687 
1688  msg.source.resize(msg.n);
1689  msg.sensor_model.resize(msg.n);
1690  msg.type.resize(msg.n);
1691  msg.obs_info.resize(msg.n);
1692  bool hasAcc = false;
1693  bool hasOmega = false;
1694  hasImuMeas = false;
1695  for (size_t i = 0; i < msg.n; i++)
1696  {
1697  qiLittleEndianParser(it, msg.source[i]);
1698  qiLittleEndianParser(it, msg.sensor_model[i]);
1699  qiLittleEndianParser(it, msg.type[i]);
1700  qiLittleEndianParser(it, msg.obs_info[i]);
1701 
1702  switch (msg.type[i])
1703  {
1704  case 0:
1705  {
1706  qiLittleEndianParser(it, msg.acceleration_x);
1707  qiLittleEndianParser(it, msg.acceleration_y);
1708  qiLittleEndianParser(it, msg.acceleration_z);
1709  if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi
1710  {
1711  if (validValue(msg.acceleration_y))
1712  msg.acceleration_y = -msg.acceleration_y;
1713  if (validValue(msg.acceleration_z))
1714  msg.acceleration_z = -msg.acceleration_z;
1715  }
1716  hasAcc = true;
1717  break;
1718  }
1719  case 1:
1720  {
1721  qiLittleEndianParser(it, msg.angular_rate_x);
1722  qiLittleEndianParser(it, msg.angular_rate_y);
1723  qiLittleEndianParser(it, msg.angular_rate_z);
1724  if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi
1725  {
1726  if (validValue(msg.angular_rate_y))
1727  msg.angular_rate_y = -msg.angular_rate_y;
1728  if (validValue(msg.angular_rate_z))
1729  msg.angular_rate_z = -msg.angular_rate_z;
1730  }
1731  hasOmega = true;
1732  break;
1733  }
1734  case 3:
1735  {
1736  qi::parse(it, it + 2, qi::little_word, msg.sensor_temperature);
1737  msg.sensor_temperature /= 100.0f;
1738  std::advance(it, 22); // reserved
1739  break;
1740  }
1741  case 4:
1742  {
1743  qiLittleEndianParser(it, msg.velocity_x);
1744  qiLittleEndianParser(it, msg.velocity_y);
1745  qiLittleEndianParser(it, msg.velocity_z);
1746  qiLittleEndianParser(it, msg.std_dev_x);
1747  qiLittleEndianParser(it, msg.std_dev_y);
1748  qiLittleEndianParser(it, msg.std_dev_z);
1749  if (use_ros_axis_orientation)
1750  {
1751  if (validValue(msg.velocity_y))
1752  msg.velocity_y = -msg.velocity_y;
1753  if (validValue(msg.velocity_z))
1754  msg.velocity_z = -msg.velocity_z;
1755  }
1756  break;
1757  }
1758  case 20:
1759  {
1760  qiLittleEndianParser(it, msg.zero_velocity_flag);
1761  std::advance(it, 16); // reserved
1762  break;
1763  }
1764  default:
1765  {
1766  node->log(
1768  "Unknown external sensor measurement type in SBF ExtSensorMeas.");
1769  std::advance(it, 24);
1770  break;
1771  }
1772  }
1773  }
1774  if (it > itEnd)
1775  {
1776  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1777  return false;
1778  }
1779  hasImuMeas = hasAcc && hasOmega;
1780  return true;
1781 };
BlockHeader::length
uint16_t length
Definition: sbf_structs.hpp:90
VectorInfoGeodMsg
septentrio_gnss_driver::VectorInfoGeod VectorInfoGeodMsg
Definition: typedefs.hpp:122
ReceiverSetup::delta_h
float delta_h
Definition: sbf_structs.hpp:174
BlockHeader::wnc
uint16_t wnc
This is the GPS week counter.
Definition: sbf_structs.hpp:93
DOP::nr_sv
uint8_t nr_sv
Definition: sbf_structs.hpp:148
QualityInd
Struct for the SBF block "QualityInd".
Definition: sbf_structs.hpp:193
ChannelStatus::sb1_length
uint8_t sb1_length
Definition: sbf_structs.hpp:134
BaseVectorGeodParser
bool BaseVectorGeodParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorGeodMsg &msg)
Definition: sbf_structs.hpp:993
ReceiverSetup::agency
std::string agency
Definition: sbf_structs.hpp:168
ChannelStatus::satInfo
std::vector< ChannelSatInfo > satInfo
Definition: sbf_structs.hpp:137
ChannelStateInfo::pvt_info
uint16_t pvt_info
Definition: sbf_structs.hpp:105
setDoNotUse
void setDoNotUse(T &s)
Sets scalar to Do-Not-Use value -2e10.
Definition: sbf_structs.hpp:299
ReceiverSetup::delta_e
float delta_e
Definition: sbf_structs.hpp:175
VectorInfoGeodParser
void VectorInfoGeodParser(It &it, VectorInfoGeodMsg &msg, uint8_t sb_length)
Qi based parser for the SBF sub-block "VectorInfoGeod".
Definition: sbf_structs.hpp:968
qiCharsToStringParser
bool qiCharsToStringParser(It &it, std::string &val, std::size_t num)
Qi parser for char array to string.
Definition: sbf_structs.hpp:357
DOPParser
bool DOPParser(ROSaicNodeBase *node, It it, It itEnd, DOP &msg)
Qi based parser for the SBF block "DOP".
Definition: sbf_structs.hpp:488
ChannelStateInfo
Struct for the SBF sub-block "ChannelStateInfo".
Definition: sbf_structs.hpp:100
ChannelSatInfo::rx_channel
uint8_t rx_channel
Definition: sbf_structs.hpp:120
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_structs.hpp:1030
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_structs.hpp:1648
ReceiverSetup::rx_serial_number
std::string rx_serial_number
Definition: sbf_structs.hpp:169
AgcState::frontend_id
uint8_t frontend_id
Definition: sbf_structs.hpp:207
s
XmlRpcServer s
BlockHeaderParser
bool BlockHeaderParser(ROSaicNodeBase *node, It &it, Hdr &block_header)
Qi based parser for the SBF block "BlockHeader" plus receiver time stamp.
Definition: sbf_structs.hpp:372
DOP::tdop
double tdop
Definition: sbf_structs.hpp:150
BlockHeader::sync_2
uint8_t sync_2
2nd sync byte is @ or 0x40
Definition: sbf_structs.hpp:86
parsing_utilities::wrapAngle180to180
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Definition: parsing_utilities.cpp:49
MAX_NB_INMARSATCHANNELS
static const uint8_t MAX_NB_INMARSATCHANNELS
Inmarsat is a British satellite telecommunications company.
Definition: sbf_structs.hpp:38
ReceiverSetup::ant_type
std::string ant_type
Definition: sbf_structs.hpp:173
ReceiverSetup::monument_idx
uint8_t monument_idx
Definition: sbf_structs.hpp:184
MAXSB_MEASEPOCH_T2
static const uint16_t MAXSB_MEASEPOCH_T2
Max number of bytes that MeasEpochChannelType2 sub-block can consist of.
Definition: sbf_structs.hpp:55
BlockHeader::tow
uint32_t tow
This is the time of week in ms.
Definition: sbf_structs.hpp:92
ERROR
@ ERROR
Definition: typedefs.hpp:166
ChannelSatInfo::n2
uint8_t n2
Definition: sbf_structs.hpp:119
DOP::vpl
float vpl
Definition: sbf_structs.hpp:154
VelCovGeodeticMsg
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:124
ReceiverTimeMsg
septentrio_gnss_driver::ReceiverTime ReceiverTimeMsg
Definition: typedefs.hpp:120
ReceiverSetup::block_header
BlockHeader block_header
Definition: sbf_structs.hpp:163
MAXSB_CHANNELSATINFO
static const uint8_t MAXSB_CHANNELSATINFO
Max number of bytes that ChannelSatInfo sub-block can consist of.
Definition: sbf_structs.hpp:46
QualityInd::indicators
std::vector< uint16_t > indicators
Definition: sbf_structs.hpp:199
ChannelStatus::sb2_length
uint8_t sb2_length
Definition: sbf_structs.hpp:135
ChannelSatInfo::az_rise_set
uint16_t az_rise_set
Definition: sbf_structs.hpp:116
AgcState
Struct for the SBF sub-block "AGCState".
Definition: sbf_structs.hpp:205
INSNavCartMsg
septentrio_gnss_driver::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:133
ReceiverStatus::block_header
BlockHeader block_header
Definition: sbf_structs.hpp:219
ROSaicNodeBase
This class is the base class for abstraction.
Definition: typedefs.hpp:174
MAXSB_NBRANTENNA
static const uint8_t MAXSB_NBRANTENNA
Maximum number of antennas that mosaic etc. can handle.
Definition: sbf_structs.hpp:44
MAXSB_NBVECTORINFO
static const uint8_t MAXSB_NBVECTORINFO
Max number of vector info sub-blocks.
Definition: sbf_structs.hpp:59
ReceiverSetup::ant_serial_nbr
std::string ant_serial_nbr
Definition: sbf_structs.hpp:172
MAXSB_CHANNELSTATEINFO
static const uint16_t MAXSB_CHANNELSTATEINFO
Max number of bytes that ChannelStateInfo sub-block can consist of.
Definition: sbf_structs.hpp:49
ReceiverStatus::up_time
uint32_t up_time
Definition: sbf_structs.hpp:223
PVTGeodeticMsg
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:117
SBF_SYNC_BYTE_1
static const uint8_t SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
Definition: sbf_structs.hpp:62
DOP::hpl
float hpl
Definition: sbf_structs.hpp:153
PosCovGeodeticMsg
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:119
PVTGeodeticParser
bool PVTGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PVTGeodeticMsg &msg)
Qi based parser for the SBF block "PVTGeodetic".
Definition: sbf_structs.hpp:767
ReceiverSetup::longitude
double longitude
Definition: sbf_structs.hpp:181
MeasEpochChannelType1Msg
septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg
Definition: typedefs.hpp:112
ReceiverSetup::rx_version
std::string rx_version
Definition: sbf_structs.hpp:171
NR_OF_ANTENNAS
static const uint8_t NR_OF_ANTENNAS
Using maximum value of NR_OF_ANTENNAS for SBF definitions.
Definition: sbf_structs.hpp:42
PVTCartesianMsg
septentrio_gnss_driver::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:116
ReceiverTimeParser
bool ReceiverTimeParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverTimeMsg &msg)
Struct for the SBF block "ReceiverTime".
Definition: sbf_structs.hpp:1406
VelCovCartesianMsg
septentrio_gnss_driver::VelCovCartesian VelCovCartesianMsg
Definition: typedefs.hpp:123
ReceiverStatus::agc_state
std::vector< AgcState > agc_state
Definition: sbf_structs.hpp:231
PosCovCartesianMsg
septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:118
AgcState::sample_var
uint8_t sample_var
Definition: sbf_structs.hpp:209
parsing_utilities.hpp
Declares utility functions used when parsing messages.
ChannelStateInfo::pvt_status
uint16_t pvt_status
Definition: sbf_structs.hpp:104
ReceiverStatus::cpu_load
uint8_t cpu_load
Definition: sbf_structs.hpp:221
ChannelSatInfo::health_status
uint16_t health_status
Definition: sbf_structs.hpp:117
AgcState::blanking_stat
uint8_t blanking_stat
Definition: sbf_structs.hpp:210
BlockHeader::sync_1
uint8_t sync_1
first sync byte is $ or 0x24
Definition: sbf_structs.hpp:85
AttCovEulerMsg
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:114
PVTCartesianParser
bool PVTCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PVTCartesianMsg &msg)
Qi based parser for the SBF block "PVTCartesian".
Definition: sbf_structs.hpp:712
PosCovCartesianParser
bool PosCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PosCovCartesianMsg &msg)
Qi based parser for the SBF block "PosCovCartesian".
Definition: sbf_structs.hpp:1168
QualityInd::n
uint8_t n
Definition: sbf_structs.hpp:197
ReceiverSetup::product_name
std::string product_name
Definition: sbf_structs.hpp:179
ReceiverSetup::country_code
std::string country_code
Definition: sbf_structs.hpp:186
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_structs.hpp:417
qiLittleEndianParser
bool qiLittleEndianParser(It &it, Val &val)
Qi little endian parsers for numeric values.
Definition: sbf_structs.hpp:316
INSNavGeodMsg
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:134
AgcStateParser
void AgcStateParser(It it, AgcState &msg, uint8_t sb_length)
Struct for the SBF sub-block "AGCState".
Definition: sbf_structs.hpp:1349
ReceiverSetup::gnss_fw_version
std::string gnss_fw_version
Definition: sbf_structs.hpp:178
MAXSB_MEASEPOCH_T1
static const uint8_t MAXSB_MEASEPOCH_T1
Max number of bytes that MeasEpochChannelType1 sub-block can consist of.
Definition: sbf_structs.hpp:52
ReceiverStatus::rx_error
uint32_t rx_error
Definition: sbf_structs.hpp:225
DOP
Struct for the SBF block "DOP".
Definition: sbf_structs.hpp:144
typedefs.hpp
ReceiverStatus::n
uint8_t n
Definition: sbf_structs.hpp:226
ReceiverSetup::marker_name
std::string marker_name
Definition: sbf_structs.hpp:165
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_structs.hpp:40
ReceiverStatus::rx_status
uint32_t rx_status
Definition: sbf_structs.hpp:224
QualityInd::block_header
BlockHeader block_header
Definition: sbf_structs.hpp:195
ReceiverStatus::ext_error
uint8_t ext_error
Definition: sbf_structs.hpp:222
DOP::hdop
double hdop
Definition: sbf_structs.hpp:151
VelCovGeodeticParser
bool VelCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, VelCovGeodeticMsg &msg)
Qi based parser for the SBF block "VelCovGeodetic".
Definition: sbf_structs.hpp:1276
ReceiverStatus::sb_length
uint8_t sb_length
Definition: sbf_structs.hpp:227
ChannelSatInfo::freq_nr
uint8_t freq_nr
Definition: sbf_structs.hpp:115
PosCovGeodeticParser
bool PosCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PosCovGeodeticMsg &msg)
Qi based parser for the SBF block "PosCovGeodetic".
Definition: sbf_structs.hpp:1204
BlockHeader::id
uint16_t id
This is the block ID.
Definition: sbf_structs.hpp:88
ExtSensorMeasMsg
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:137
QualityIndParser
bool QualityIndParser(ROSaicNodeBase *node, It it, It itEnd, QualityInd &msg)
Qi based parser for the SBF block "QualityInd".
Definition: sbf_structs.hpp:1312
ChannelStatus
Struct for the SBF block "ChannelStatus".
Definition: sbf_structs.hpp:129
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_structs.hpp:822
MeasEpochChannelType1Parser
bool MeasEpochChannelType1Parser(ROSaicNodeBase *node, It &it, MeasEpochChannelType1Msg &msg, uint8_t sb1_length, uint8_t sb2_length)
Definition: sbf_structs.hpp:545
ReceiverTimesParser
bool ReceiverTimesParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverTimeMsg &msg)
Struct for the SBF block "ReceiverTime".
Definition: sbf_structs.hpp:681
ReceiverStatus::cmd_count
uint8_t cmd_count
Definition: sbf_structs.hpp:228
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_structs.hpp:239
BlockHeader
Struct for the SBF block's header message.
Definition: sbf_structs.hpp:83
ReceiverSetup
Struct for the SBF block "ReceiverSetup".
Definition: sbf_structs.hpp:161
VectorInfoCartParser
void VectorInfoCartParser(It &it, VectorInfoCartMsg &msg, uint8_t sb_length)
Qi based parser for the SBF sub-block "VectorInfoCart".
Definition: sbf_structs.hpp:906
ChannelStatus::block_header
BlockHeader block_header
Definition: sbf_structs.hpp:131
ReceiverSetup::delta_n
float delta_n
Definition: sbf_structs.hpp:176
ChannelSatInfo
Struct for the SBF sub-block "ChannelSatInfo".
Definition: sbf_structs.hpp:112
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_structs.hpp:1614
AgcState::gain
int8_t gain
Definition: sbf_structs.hpp:208
BaseVectorGeodMsg
septentrio_gnss_driver::BaseVectorGeod BaseVectorGeodMsg
Definition: typedefs.hpp:109
ReceiverStatus::temperature
uint8_t temperature
Definition: sbf_structs.hpp:229
ROSaicNodeBase::log
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:231
ChannelStateInfo::antenna
uint8_t antenna
Definition: sbf_structs.hpp:102
VectorInfoCartMsg
septentrio_gnss_driver::VectorInfoCart VectorInfoCartMsg
Definition: typedefs.hpp:121
VelCovCartesianParser
bool VelCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, VelCovCartesianMsg &msg)
Qi based parser for the SBF block "VelCovCartesian".
Definition: sbf_structs.hpp:1240
ChannelStatusParser
bool ChannelStatusParser(ROSaicNodeBase *node, It it, It itEnd, ChannelStatus &msg)
Qi based parser for the SBF block "ChannelStatus".
Definition: sbf_structs.hpp:449
ReceiverSetup::rx_name
std::string rx_name
Definition: sbf_structs.hpp:170
ReceiverSetup::marker_type
std::string marker_type
Definition: sbf_structs.hpp:177
VelSensorSetupMsg
septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:136
BaseVectorCartMsg
septentrio_gnss_driver::BaseVectorCart BaseVectorCartMsg
Definition: typedefs.hpp:108
DOP::pdop
double pdop
Definition: sbf_structs.hpp:149
ReceiverStatusParser
bool ReceiverStatusParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverStatus &msg)
Struct for the SBF block "ReceiverStatus".
Definition: sbf_structs.hpp:1363
ChannelSatInfo::elev
int8_t elev
Definition: sbf_structs.hpp:118
MeasEpochMsg
septentrio_gnss_driver::MeasEpoch MeasEpochMsg
Definition: typedefs.hpp:111
ChannelStatus::n
uint8_t n
Definition: sbf_structs.hpp:133
ReceiverSetup::receiver_idx
uint8_t receiver_idx
Definition: sbf_structs.hpp:185
ReceiverSetup::height
float height
Definition: sbf_structs.hpp:182
DOP::vdop
double vdop
Definition: sbf_structs.hpp:152
ReceiverSetup::observer
std::string observer
Definition: sbf_structs.hpp:167
MeasEpochChannelType2Msg
septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg
Definition: typedefs.hpp:113
ChannelSatInfo::stateInfo
std::vector< ChannelStateInfo > stateInfo
Definition: sbf_structs.hpp:122
validValue
bool validValue(T s)
Check if value is not set to Do-Not-Use -2e10.
Definition: sbf_structs.hpp:274
BaseVectorCartParser
bool BaseVectorCartParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorCartMsg &msg)
Definition: sbf_structs.hpp:931
BlockHeader::revision
uint8_t revision
This is the block revision.
Definition: sbf_structs.hpp:89
ReceiverSetup::station_code
std::string station_code
Definition: sbf_structs.hpp:183
ReceiverStatus
Struct for the SBF block "ReceiverStatus".
Definition: sbf_structs.hpp:217
IMUSetupMsg
septentrio_gnss_driver::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:135
ChannelStateInfoParser
void ChannelStateInfoParser(It &it, ChannelStateInfo &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "ChannelStateInfo".
Definition: sbf_structs.hpp:402
ReceiverSetup::latitude
double latitude
Definition: sbf_structs.hpp:180
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_structs.hpp:867
SBF_SYNC_BYTE_2
static const uint8_t SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Definition: sbf_structs.hpp:64
ReceiverSetupParser
bool ReceiverSetupParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverSetup &msg)
Qi based parser for the SBF block "ReceiverSetup".
Definition: sbf_structs.hpp:624
MeasEpochChannelType2Parser
void MeasEpochChannelType2Parser(It &it, MeasEpochChannelType2Msg &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "MeasEpochChannelType2".
Definition: sbf_structs.hpp:525
MeasEpochParser
bool MeasEpochParser(ROSaicNodeBase *node, It it, It itEnd, MeasEpochMsg &msg)
Definition: sbf_structs.hpp:581
NR_OF_LOGICALCHANNELS
static const uint8_t NR_OF_LOGICALCHANNELS
Using maximum value of NR_OF_LOGICALCHANNELS for SBF definitions.
Definition: sbf_structs.hpp:36
BlockHeader::crc
uint16_t crc
The check sum.
Definition: sbf_structs.hpp:87
ReceiverSetup::marker_number
std::string marker_number
Definition: sbf_structs.hpp:166
ChannelSatInfo::sv_id
uint8_t sv_id
Definition: sbf_structs.hpp:114
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_structs.hpp:1576
DOP::block_header
BlockHeader block_header
Definition: sbf_structs.hpp:146
ChannelStateInfo::tracking_status
uint16_t tracking_status
Definition: sbf_structs.hpp:103
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_structs.hpp:1437
AttEulerMsg
septentrio_gnss_driver::AttEuler AttEulerMsg
Definition: typedefs.hpp:115


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Nov 10 2022 04:02:01