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 #ifndef SBFStructs_HPP
32 #define SBFStructs_HPP
33 
34 #include <cstdint>
35 
37 #ifndef NR_OF_LOGICALCHANNELS
38 #define NR_OF_LOGICALCHANNELS 80
39 #endif
40 #ifndef MAX_NB_INMARSATCHANNELS
42 #define MAX_NB_INMARSATCHANNELS 1
43 #endif
44 #ifndef MAX_NR_OF_SIGNALS_PER_SATELLITE
46 #define MAX_NR_OF_SIGNALS_PER_SATELLITE 7
47 #endif
48 #ifndef NR_OF_ANTENNAS
50 #define NR_OF_ANTENNAS 3
51 #endif
52 #ifndef MAXSB_NBRANTENNA
54 #define MAXSB_NBRANTENNA 4
55 #endif
56 #ifndef MAXSB_CHANNELSATINFO
58 #define MAXSB_CHANNELSATINFO (NR_OF_LOGICALCHANNELS + MAX_NB_INMARSATCHANNELS)
59 #endif
60 #ifndef MAXSB_CHANNELSTATEINFO
62 #define MAXSB_CHANNELSTATEINFO (MAXSB_CHANNELSATINFO * MAXSB_NBRANTENNA)
63 #endif
64 #ifndef MAXSB_MEASEPOCH_T1
66 #define MAXSB_MEASEPOCH_T1 (NR_OF_LOGICALCHANNELS + MAX_NB_INMARSATCHANNELS)
67 #endif
68 #ifndef MAXSB_MEASEPOCH_T2
70 #define MAXSB_MEASEPOCH_T2 ((MAXSB_MEASEPOCH_T1) * (((MAX_NR_OF_SIGNALS_PER_SATELLITE) * (NR_OF_ANTENNAS)) - 1))
71 #endif
72 #ifndef SBF_INSNAVGEOD_LENGTH_1
74 #define SBF_INSNAVGEOD_LENGTH_1 16
75 #endif
76 #ifndef SBF_INSNAVCART_LENGTH_1
78 #define SBF_INSNAVCART_LENGTH_1 16
79 #endif
80 #ifndef SBF_EXTEVENTINSNAVGEOD_LENGTH_1
82 #define SBF_EXTEVENTINSNAVGEOD_LENGTH_1 16
83 #endif
84 #ifndef SBF_EXTEVENTINSNAVCART_LENGTH_1
86 #define SBF_EXTEVENTINSNAVCART_LENGTH_1 16
87 #endif
88 #ifndef SBF_CHANNELSTATUS_DATA_LENGTH
90 #define SBF_CHANNELSTATUS_DATA_LENGTH \
91  MAXSB_CHANNELSATINFO * sizeof(ChannelSatInfo) + \
92  MAXSB_CHANNELSTATEINFO * sizeof(ChannelStateInfo)
93 #endif
94 #ifndef MEASEPOCH_DATA_LENGTH
96 #define MEASEPOCH_DATA_LENGTH \
97  (MAXSB_MEASEPOCH_T1 * sizeof(MeasEpochChannelType1) + \
98  MAXSB_MEASEPOCH_T2 * sizeof(MeasEpochChannelType2))
99 #endif
100 #ifndef SBF_EXTSENSORMEAS_1_0_EXTSENSORMEAS_LENGTH
102 #define SBF_EXTSENSORMEAS_1_0_EXTSENSORMEAS_LENGTH 4
103 #endif
104 
105 // Couple of redefinitions
106 #define SBF_INSNAVCART_LENGTH SBF_INSNAVCART_LENGTH_1
107 #define SBF_EXTSENSORMEAS_1_EXTSENSORMEAS_LENGTH SBF_EXTSENSORMEAS_1_0_EXTSENSORMEAS_LENGTH
108 #define SBF_EXTEVENTINSNAVCART_LENGTH SBF_EXTEVENTINSNAVCART_LENGTH_1
109 #define SBF_EXTEVENTINSNAVGEOD_LENGTH SBF_EXTEVENTINSNAVGEOD_LENGTH_1
110 #define SBF_INSNAVGEOD_LENGTH SBF_INSNAVGEOD_LENGTH_1
111 
113 static const uint8_t SBF_SYNC_BYTE_1 = 0x24;
115 static const uint8_t SBF_SYNC_BYTE_2 = 0x40;
116 
117 // C++
118 #include <algorithm>
119 // Boost
120 #include <boost/spirit/include/qi.hpp>
121 
124 
136 {
137  uint8_t sync_1;
138  uint8_t sync_2;
139  uint16_t crc;
140  uint16_t id;
141  uint8_t revision;
142  uint16_t length;
143  uint32_t tow;
145  uint16_t wnc;
146 } ;
147 
153 {
154  uint8_t antenna;
155  uint16_t tracking_status;
156  uint16_t pvt_status;
157  uint16_t pvt_info;
158 };
159 
165 {
166  uint8_t sv_id;
167  uint8_t freq_nr;
168  uint16_t az_rise_set;
169  uint16_t health_status;
170  int8_t elev;
171  uint8_t n2;
172  uint8_t rx_channel;
173 
174  std::vector<ChannelStateInfo> stateInfo;
175 };
176 
182 {
184 
185  uint8_t n;
186  uint8_t sb1_length;
187  uint8_t sb2_length;
188 
189  std::vector<ChannelSatInfo> satInfo;
190 };
191 
196 struct DOP
197 {
199 
200  uint8_t nr_sv;
201  double pdop;
202  double tdop;
203  double hdop;
204  double vdop;
205  float hpl;
206  float vpl;
207 };
208 
214 {
216 
217  std::string marker_name;
218  std::string marker_number;
219  std::string observer;
220  std::string agency;
221  std::string rx_serial_number;
222  std::string rx_name;
223  std::string rx_version;
224  std::string ant_serial_nbr;
225  std::string ant_type;
226  float delta_h; /* [m] */
227  float delta_e; /* [m] */
228  float delta_n; /* [m] */
229  std::string marker_type;
230  std::string gnss_fw_version;
231  std::string product_name;
232  double latitude;
233  double longitude;
234  float height;
235  std::string station_code;
236  uint8_t monument_idx;
237  uint8_t receiver_idx;
238  std::string country_code;
239 };
240 
246 {
248 
249  uint8_t n = 0;
250 
251  std::vector<uint16_t> indicators;
252 };
253 
257 struct AgcState
258 {
259  uint8_t frontend_id;
260  int8_t gain;
261  uint8_t sample_var;
262  uint8_t blanking_stat;
263 };
264 
270 {
272 
273  uint8_t cpu_load;
274  uint8_t ext_error;
275  uint32_t up_time;
276  uint32_t rx_status;
277  uint32_t rx_error;
278  uint8_t n;
279  uint8_t sb_length;
280  uint8_t cmd_count;
281  uint8_t temperature;
282 
283  std::vector<AgcState> agc_state;
284 };
285 
291 static const std::array<uint16_t, 256> CRC_LOOK_UP = {
292  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129,
293  0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252,
294  0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c,
295  0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
296  0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672,
297  0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
298  0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861,
299  0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
300  0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
301  0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5,
302  0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b,
303  0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
304  0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9,
305  0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3,
306  0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c,
307  0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
308  0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3,
309  0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
310  0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676,
311  0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
312  0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c,
313  0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16,
314  0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b,
315  0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
316  0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36,
317  0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
318 
319 namespace qi = boost::spirit::qi;
320 
325 template<typename T>
326 bool validValue(T s)
327 {
328  static_assert(std::is_same<uint16_t, T>::value ||
329  std::is_same<uint32_t, T>::value ||
330  std::is_same<float, T>::value ||
331  std::is_same<double, T>::value);
332  if (std::is_same<uint16_t, T>::value)
333  {
334  return (s != static_cast<uint16_t>(65535));
335  }
336  else if (std::is_same<uint32_t, T>::value)
337  {
338  return (s != 4294967295u);
339  }
340  else if (std::is_same<float, T>::value)
341  {
342  return (s != -2e10f);
343  }
344  else if (std::is_same<double, T>::value)
345  {
346  return (s != -2e10);
347  }
348 }
349 
354 template<typename T>
355 void setDoNotUse(T& s)
356 {
357  static_assert(std::is_same<float, T>::value ||
358  std::is_same<double, T>::value);
359  if (std::is_same<float, T>::value)
360  {
361  s = -2e10f;
362  }
363  else if (std::is_same<double, T>::value)
364  {
365  s = -2e10;
366  }
367 }
368 
373 template<typename It, typename Val>
374 bool qiLittleEndianParser(It& it, Val& val)
375 {
376  static_assert(std::is_same<int8_t, Val>::value ||
377  std::is_same<uint8_t, Val>::value ||
378  std::is_same<int16_t, Val>::value ||
379  std::is_same<uint16_t, Val>::value ||
380  std::is_same<int32_t, Val>::value ||
381  std::is_same<uint32_t, Val>::value ||
382  std::is_same<int64_t, Val>::value ||
383  std::is_same<uint64_t, Val>::value ||
384  std::is_same<float, Val>::value ||
385  std::is_same<double, Val>::value);
386 
387  if (std::is_same<int8_t, Val>::value)
388  {
389  return qi::parse(it, it + 1, qi::char_, val);
390  }
391  else if (std::is_same<uint8_t, Val>::value)
392  {
393  return qi::parse(it, it + 1, qi::byte_, val);
394  }
395  else if ((std::is_same<int16_t, Val>::value) || (std::is_same<uint16_t, Val>::value))
396  {
397  return qi::parse(it, it + 2, qi::little_word, val);
398  }
399  else if ((std::is_same<int32_t, Val>::value) || (std::is_same<uint32_t, Val>::value))
400  {
401  return qi::parse(it, it + 4, qi::little_dword, val);
402  }
403  else if ((std::is_same<int64_t, Val>::value) || (std::is_same<uint64_t, Val>::value))
404  {
405  return qi::parse(it, it + 8, qi::little_qword, val);
406  }
407  else if (std::is_same<float, Val>::value)
408  {
409  return qi::parse(it, it + 4, qi::little_bin_float, val);
410  }
411  else if (std::is_same<double, Val>::value)
412  {
413  return qi::parse(it, it + 8, qi::little_bin_double, val);
414  }
415 }
416 
421 template<typename It>
422 bool qiCharsToStringParser(It& it, std::string& val, std::size_t num)
423 {
424  bool success = false;
425  val.clear();
426  success = qi::parse(it, it + num, qi::repeat(num)[qi::char_], val);
427  // remove string termination characters '\0'
428  val.erase(std::remove(val.begin(), val.end(), '\0'), val.end());
429  return success;
430 }
431 
436 template<typename It, typename Hdr>
437 bool BlockHeaderParser(ROSaicNodeBase* node, It& it, Hdr& block_header)
438 {
439  qiLittleEndianParser(it, block_header.sync_1);
440  if (block_header.sync_1 != SBF_SYNC_BYTE_1)
441  {
442  node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 1.");
443  return false;
444  }
445  qiLittleEndianParser(it, block_header.sync_2);
446  if (block_header.sync_2 != SBF_SYNC_BYTE_2)
447  {
448  node->log(LogLevel::ERROR, "Parse error: Wrong sync byte 2.");
449  return false;
450  }
451  qiLittleEndianParser(it, block_header.crc);
452  uint16_t ID;
453  qiLittleEndianParser(it, ID);
454  block_header.id = ID & 8191; // lower 13 bits are id
455  block_header.revision = ID >> 13; // upper 3 bits are revision
456  qiLittleEndianParser(it, block_header.length);
457  qiLittleEndianParser(it, block_header.tow);
458  qiLittleEndianParser(it, block_header.wnc);
459  return true;
460 }
461 
466 template<typename It>
467 void ChannelStateInfoParser(It& it, ChannelStateInfo& msg, uint8_t sb2_length)
468 {
469  qiLittleEndianParser(it, msg.antenna);
470  ++it; // reserved
474  std::advance(it, sb2_length - 8); // skip padding
475 };
476 
481 template<typename It>
482 bool ChannelSatInfoParser(ROSaicNodeBase* node, It& it, ChannelSatInfo& msg, uint8_t sb1_length, uint8_t sb2_length)
483 {
484  qiLittleEndianParser(it, msg.sv_id);
485  qiLittleEndianParser(it, msg.freq_nr);
486  std::advance(it, 2); // reserved
489  qiLittleEndianParser(it, msg.elev);
490  qiLittleEndianParser(it, msg.n2);
491  if (msg.n2 > MAXSB_MEASEPOCH_T2)
492  {
493  node->log(LogLevel::ERROR, "Parse error: Too many ChannelStateInfo " + std::to_string(msg.n2));
494  return false;
495  }
497  ++it; // reserved
498  std::advance(it, sb1_length - 12); // skip padding
499  msg.stateInfo.resize(msg.n2);
500  for (auto& stateInfo : msg.stateInfo)
501  {
502  ChannelStateInfoParser(it, stateInfo, sb2_length);
503  }
504  return true;
505 };
506 
511 template<typename It>
512 bool ChannelStatusParser(ROSaicNodeBase* node, It it, It itEnd, ChannelStatus& msg)
513 {
514  if(!BlockHeaderParser(node, it, msg.block_header))
515  return false;
516  if (msg.block_header.id != 4013)
517  {
518  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
519  return false;
520  }
521  qiLittleEndianParser(it, msg.n);
522  if (msg.n > MAXSB_CHANNELSATINFO)
523  {
524  node->log(LogLevel::ERROR, "Parse error: Too many ChannelSatInfo " + std::to_string(msg.n));
525  return false;
526  }
529  std::advance(it, 3); // reserved
530  msg.satInfo.resize(msg.n);
531  for (auto& satInfo : msg.satInfo)
532  {
533  if (!ChannelSatInfoParser(node, it, satInfo, msg.sb1_length, msg.sb2_length))
534  return false;
535  }
536  if (it > itEnd)
537  {
538  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
539  return false;
540  }
541  return true;
542 };
543 
548 template<typename It>
549 bool DOPParser(ROSaicNodeBase* node, It it, It itEnd, DOP& msg)
550 {
551 
552  if(!BlockHeaderParser(node, it, msg.block_header))
553  return false;
554  if (msg.block_header.id != 4001)
555  {
556  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
557  return false;
558  }
559  qiLittleEndianParser(it, msg.nr_sv);
560  ++it; // reserved
561  uint16_t temp;
562  qiLittleEndianParser(it, temp);
563  msg.pdop = temp / 100.0;
564  qiLittleEndianParser(it, temp);
565  msg.tdop = temp / 100.0;
566  qiLittleEndianParser(it, temp);
567  msg.hdop = temp / 100.0;
568  qiLittleEndianParser(it, temp);
569  msg.vdop = temp / 100.0;
570  qiLittleEndianParser(it, msg.hpl);
571  qiLittleEndianParser(it, msg.vpl);
572  if (it > itEnd)
573  {
574  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
575  return false;
576  }
577  return true;
578 };
579 
584 template<typename It>
585 void MeasEpochChannelType2Parser(It& it, MeasEpochChannelType2Msg& msg, uint8_t sb2_length)
586 {
587  qiLittleEndianParser(it, msg.type);
588  qiLittleEndianParser(it, msg.lock_time);
589  qiLittleEndianParser(it, msg.cn0);
590  qiLittleEndianParser(it, msg.offsets_msb);
591  qiLittleEndianParser(it, msg.carrier_msb);
592  qiLittleEndianParser(it, msg.obs_info);
593  qiLittleEndianParser(it, msg.code_offset_lsb);
594  qiLittleEndianParser(it, msg.carrier_lsb);
595  qiLittleEndianParser(it, msg.doppler_offset_lsb);
596  std::advance(it, sb2_length - 12); // skip padding
597 };
598 
603 template<typename It>
604 bool MeasEpochChannelType1Parser(ROSaicNodeBase* node, It& it, MeasEpochChannelType1Msg& msg, uint8_t sb1_length, uint8_t sb2_length)
605 {
606  qiLittleEndianParser(it, msg.rx_channel);
607  qiLittleEndianParser(it, msg.type);
608  qiLittleEndianParser(it, msg.sv_id);
609  qiLittleEndianParser(it, msg.misc);
610  qiLittleEndianParser(it, msg.code_lsb);
611  qiLittleEndianParser(it, msg.doppler);
612  qiLittleEndianParser(it, msg.carrier_lsb);
613  qiLittleEndianParser(it, msg.carrier_msb);
614  qiLittleEndianParser(it, msg.cn0);
615  qiLittleEndianParser(it, msg.lock_time);
616  qiLittleEndianParser(it, msg.obs_info);
617  qiLittleEndianParser(it, msg.n2);
618  std::advance(it, sb1_length - 20); // skip padding
619  if (msg.n2 > MAXSB_MEASEPOCH_T2)
620  {
621  node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType2 " + std::to_string(msg.n2));
622  return false;
623  }
624  msg.type2.resize(msg.n2);
625  for (auto& type2 : msg.type2)
626  {
627  MeasEpochChannelType2Parser(it, type2, sb2_length);
628  }
629  return true;
630 };
631 
636 template<typename It>
637 bool MeasEpochParser(ROSaicNodeBase* node, It it, It itEnd, MeasEpochMsg& msg)
638 {
639  if(!BlockHeaderParser(node, it, msg.block_header))
640  return false;
641  if (msg.block_header.id != 4027)
642  {
643  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
644  return false;
645  }
646  qiLittleEndianParser(it, msg.n);
647  if (msg.n > MAXSB_MEASEPOCH_T1)
648  {
649  node->log(LogLevel::ERROR, "Parse error: Too many MeasEpochChannelType1 " + std::to_string(msg.n));
650  return false;
651  }
652  qiLittleEndianParser(it, msg.sb1_length);
653  qiLittleEndianParser(it, msg.sb2_length);
654  qiLittleEndianParser(it, msg.common_flags);
655  if (msg.block_header.revision > 0)
656  qiLittleEndianParser(it, msg.cum_clk_jumps);
657  ++it; // reserved
658  msg.type1.resize(msg.n);
659  for (auto& type1 : msg.type1)
660  {
661  if (!MeasEpochChannelType1Parser(node, it, type1, msg.sb1_length, msg.sb2_length))
662  return false;
663  }
664  if (it > itEnd)
665  {
666  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
667  return false;
668  }
669  return true;
670 };
671 
676 template<typename It>
677 bool ReceiverSetupParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverSetup& msg)
678 {
679  if(!BlockHeaderParser(node, it, msg.block_header))
680  return false;
681  if (msg.block_header.id != 5902)
682  {
683  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
684  return false;
685  }
686  std::advance(it, 2); // reserved
687  qiCharsToStringParser(it, msg.marker_name, 60);
689  qiCharsToStringParser(it, msg.observer, 20);
690  qiCharsToStringParser(it, msg.agency, 40);
692  qiCharsToStringParser(it, msg.rx_name, 20);
693  qiCharsToStringParser(it, msg.rx_version, 20);
695  qiCharsToStringParser(it, msg.ant_type, 20);
696  qiLittleEndianParser(it, msg.delta_h);
697  qiLittleEndianParser(it, msg.delta_e);
698  qiLittleEndianParser(it, msg.delta_n);
699  if (msg.block_header.revision > 0)
700  qiCharsToStringParser(it, msg.marker_type, 20);
701  if (msg.block_header.revision > 1)
703  if (msg.block_header.revision > 2)
704  qiCharsToStringParser(it, msg.product_name, 40);
705  if (msg.block_header.revision > 3)
706  {
709  qiLittleEndianParser(it, msg.height);
710  qiCharsToStringParser(it, msg.station_code, 10);
714  }
715  else
716  {
717  setDoNotUse(msg.latitude);
718  setDoNotUse(msg.longitude);
719  setDoNotUse(msg.height);
720  }
721  if (it > itEnd)
722  {
723  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
724  return false;
725  }
726  return true;
727 };
728 
733 template<typename It>
734 bool PVTCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PVTCartesianMsg& msg)
735 {
736  if(!BlockHeaderParser(node, it, msg.block_header))
737  return false;
738  if (msg.block_header.id != 4006)
739  {
740  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
741  return false;
742  }
743  qiLittleEndianParser(it, msg.mode);
744  qiLittleEndianParser(it, msg.error);
745  qiLittleEndianParser(it, msg.x);
746  qiLittleEndianParser(it, msg.y);
747  qiLittleEndianParser(it, msg.z);
748  qiLittleEndianParser(it, msg.undulation);
749  qiLittleEndianParser(it, msg.vx);
750  qiLittleEndianParser(it, msg.vy);
751  qiLittleEndianParser(it, msg.vz);
752  qiLittleEndianParser(it, msg.cog);
753  qiLittleEndianParser(it, msg.rx_clk_bias);
754  qiLittleEndianParser(it, msg.rx_clk_drift);
755  qiLittleEndianParser(it, msg.time_system);
756  qiLittleEndianParser(it, msg.datum);
757  qiLittleEndianParser(it, msg.nr_sv);
758  qiLittleEndianParser(it, msg.wa_corr_info);
759  qiLittleEndianParser(it, msg.reference_id);
760  qiLittleEndianParser(it, msg.mean_corr_age);
761  qiLittleEndianParser(it, msg.signal_info);
762  qiLittleEndianParser(it, msg.alert_flag);
763  if (msg.block_header.revision > 0)
764  {
765  qiLittleEndianParser(it, msg.nr_bases);
766  qiLittleEndianParser(it, msg.ppp_info);
767  }
768  if (msg.block_header.revision > 1)
769  {
770  qiLittleEndianParser(it, msg.latency);
771  qiLittleEndianParser(it, msg.h_accuracy);
772  qiLittleEndianParser(it, msg.v_accuracy);
773  qiLittleEndianParser(it, msg.misc);
774  }
775  if (it > itEnd)
776  {
777  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
778  return false;
779  }
780  return true;
781 }
782 
787 template<typename It>
788 bool PVTGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PVTGeodeticMsg& msg)
789 {
790  if(!BlockHeaderParser(node, it, msg.block_header))
791  return false;
792  if (msg.block_header.id != 4007)
793  {
794  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
795  return false;
796  }
797  qiLittleEndianParser(it, msg.mode);
798  qiLittleEndianParser(it, msg.error);
799  qiLittleEndianParser(it, msg.latitude);
800  qiLittleEndianParser(it, msg.longitude);
801  qiLittleEndianParser(it, msg.height);
802  qiLittleEndianParser(it, msg.undulation);
803  qiLittleEndianParser(it, msg.vn);
804  qiLittleEndianParser(it, msg.ve);
805  qiLittleEndianParser(it, msg.vu);
806  qiLittleEndianParser(it, msg.cog);
807  qiLittleEndianParser(it, msg.rx_clk_bias);
808  qiLittleEndianParser(it, msg.rx_clk_drift);
809  qiLittleEndianParser(it, msg.time_system);
810  qiLittleEndianParser(it, msg.datum);
811  qiLittleEndianParser(it, msg.nr_sv);
812  qiLittleEndianParser(it, msg.wa_corr_info);
813  qiLittleEndianParser(it, msg.reference_id);
814  qiLittleEndianParser(it, msg.mean_corr_age);
815  qiLittleEndianParser(it, msg.signal_info);
816  qiLittleEndianParser(it, msg.alert_flag);
817  if (msg.block_header.revision > 0)
818  {
819  qiLittleEndianParser(it, msg.nr_bases);
820  qiLittleEndianParser(it, msg.ppp_info);
821  }
822  if (msg.block_header.revision > 1)
823  {
824  qiLittleEndianParser(it, msg.latency);
825  qiLittleEndianParser(it, msg.h_accuracy);
826  qiLittleEndianParser(it, msg.v_accuracy);
827  qiLittleEndianParser(it, msg.misc);
828  }
829  if (it > itEnd)
830  {
831  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
832  return false;
833  }
834  return true;
835 }
836 
841 template<typename It>
842 bool AttEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttEulerMsg& msg, bool use_ros_axis_orientation)
843 {
844  if(!BlockHeaderParser(node, it, msg.block_header))
845  return false;
846  if (msg.block_header.id != 5938)
847  {
848  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
849  return false;
850  }
851  qiLittleEndianParser(it, msg.nr_sv);
852  qiLittleEndianParser(it, msg.error);
853  qiLittleEndianParser(it, msg.mode);
854  std::advance(it, 2); // reserved
855  qiLittleEndianParser(it, msg.heading);
856  qiLittleEndianParser(it, msg.pitch);
857  qiLittleEndianParser(it, msg.roll);
858  qiLittleEndianParser(it, msg.pitch_dot);
859  qiLittleEndianParser(it, msg.roll_dot);
860  qiLittleEndianParser(it, msg.heading_dot);
861  if (use_ros_axis_orientation)
862  {
863  if (validValue(msg.heading))
864  msg.heading = -msg.heading + 90;
865  if (validValue(msg.pitch))
866  msg.pitch = -msg.pitch;
867  if (validValue(msg.pitch_dot))
868  msg.pitch_dot = -msg.pitch_dot;
869  if (validValue(msg.heading_dot))
870  msg.heading_dot = -msg.heading_dot;
871  }
872  if (it > itEnd)
873  {
874  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
875  return false;
876  }
877  return true;
878 };
879 
884 template<typename It>
885 bool AttCovEulerParser(ROSaicNodeBase* node, It it, It itEnd, AttCovEulerMsg& msg, bool use_ros_axis_orientation)
886 {
887  if(!BlockHeaderParser(node, it, msg.block_header))
888  return false;
889  if (msg.block_header.id != 5939)
890  {
891  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
892  return false;
893  }
894  ++it; // reserved
895  qiLittleEndianParser(it, msg.error);
896  qiLittleEndianParser(it, msg.cov_headhead);
897  qiLittleEndianParser(it, msg.cov_pitchpitch);
898  qiLittleEndianParser(it, msg.cov_rollroll);
899  qiLittleEndianParser(it, msg.cov_headpitch);
900  qiLittleEndianParser(it, msg.cov_headroll);
901  qiLittleEndianParser(it, msg.cov_pitchroll);
902  if (use_ros_axis_orientation)
903  {
904  if (validValue(msg.cov_headroll))
905  msg.cov_headroll = -msg.cov_headroll;
906  if (validValue(msg.cov_pitchroll))
907  msg.cov_pitchroll = -msg.cov_pitchroll;
908  }
909  if (it > itEnd)
910  {
911  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
912  return false;
913  }
914  return true;
915 };
916 
921 template<typename It>
922 bool INSNavCartParser(ROSaicNodeBase* node, It it, It itEnd, INSNavCartMsg& msg, bool use_ros_axis_orientation)
923 {
924  if(!BlockHeaderParser(node, it, msg.block_header))
925  return false;
926  if ((msg.block_header.id != 4225) && (msg.block_header.id != 4229))
927  {
928  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
929  return false;
930  }
931  qiLittleEndianParser(it, msg.gnss_mode);
932  qiLittleEndianParser(it, msg.error);
933  qiLittleEndianParser(it, msg.info);
934  qiLittleEndianParser(it, msg.gnss_age);
935  qiLittleEndianParser(it, msg.x);
936  qiLittleEndianParser(it, msg.y);
937  qiLittleEndianParser(it, msg.z);
938  qiLittleEndianParser(it, msg.accuracy);
939  qiLittleEndianParser(it, msg.latency);
940  qiLittleEndianParser(it, msg.datum);
941  ++it; //reserved
942  qiLittleEndianParser(it, msg.sb_list);
943  if((msg.sb_list & 1) !=0)
944  {
945  qiLittleEndianParser(it, msg.x_std_dev);
946  qiLittleEndianParser(it, msg.y_std_dev);
947  qiLittleEndianParser(it, msg.z_std_dev);
948  }
949  else
950  {
951  setDoNotUse(msg.x_std_dev);
952  setDoNotUse(msg.y_std_dev);
953  setDoNotUse(msg.z_std_dev);
954  }
955  if((msg.sb_list & 2) !=0)
956  {
957  qiLittleEndianParser(it, msg.heading);
958  qiLittleEndianParser(it, msg.pitch);
959  qiLittleEndianParser(it, msg.roll);
960  if (use_ros_axis_orientation)
961  {
962  if (validValue(msg.heading))
963  msg.heading = -msg.heading + 90;
964  if (validValue(msg.pitch))
965  msg.pitch = -msg.pitch;
966  }
967  }
968  else
969  {
970  setDoNotUse(msg.heading);
971  setDoNotUse(msg.pitch );
972  setDoNotUse(msg.roll);
973  }
974  if((msg.sb_list & 4) !=0)
975  {
976  qiLittleEndianParser(it, msg.heading_std_dev);
977  qiLittleEndianParser(it, msg.pitch_std_dev);
978  qiLittleEndianParser(it, msg.roll_std_dev);
979  }
980  else
981  {
982  setDoNotUse(msg.heading_std_dev);
983  setDoNotUse(msg.pitch_std_dev );
984  setDoNotUse(msg.roll_std_dev);
985  }
986  if((msg.sb_list & 8) !=0)
987  {
988  qiLittleEndianParser(it, msg.vx);
989  qiLittleEndianParser(it, msg.vy);
990  qiLittleEndianParser(it, msg.vz);
991  }
992  else
993  {
994  setDoNotUse(msg.vx);
995  setDoNotUse(msg.vy);
996  setDoNotUse(msg.vz);
997  }
998  if((msg.sb_list & 16) !=0)
999  {
1000  qiLittleEndianParser(it, msg.vx_std_dev);
1001  qiLittleEndianParser(it, msg.vy_std_dev);
1002  qiLittleEndianParser(it, msg.vz_std_dev);
1003  }
1004  else
1005  {
1006  setDoNotUse(msg.vx_std_dev);
1007  setDoNotUse(msg.vy_std_dev);
1008  setDoNotUse(msg.vz_std_dev);
1009  }
1010  if((msg.sb_list & 32) !=0)
1011  {
1012  qiLittleEndianParser(it, msg.xy_cov);
1013  qiLittleEndianParser(it, msg.xz_cov);
1014  qiLittleEndianParser(it, msg.yz_cov);
1015  }
1016  else
1017  {
1018  setDoNotUse(msg.xy_cov);
1019  setDoNotUse(msg.xz_cov);
1020  setDoNotUse(msg.yz_cov);
1021  }
1022  if((msg.sb_list & 64) !=0)
1023  {
1024  qiLittleEndianParser(it, msg.heading_pitch_cov);
1025  qiLittleEndianParser(it, msg.heading_roll_cov);
1026  qiLittleEndianParser(it, msg.pitch_roll_cov);
1027  if (use_ros_axis_orientation)
1028  {
1029  if (validValue(msg.heading_roll_cov))
1030  msg.heading_roll_cov = -msg.heading_roll_cov;
1031  if (validValue(msg.pitch_roll_cov))
1032  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1033  }
1034  }
1035  else
1036  {
1037  setDoNotUse(msg.heading_pitch_cov);
1038  setDoNotUse(msg.heading_roll_cov);
1039  setDoNotUse(msg.pitch_roll_cov);
1040  }
1041  if((msg.sb_list & 128) !=0)
1042  {
1043  qiLittleEndianParser(it, msg.vx_vy_cov);
1044  qiLittleEndianParser(it, msg.vx_vz_cov);
1045  qiLittleEndianParser(it, msg.vy_vz_cov);
1046  }
1047  else
1048  {
1049  setDoNotUse(msg.vx_vy_cov);
1050  setDoNotUse(msg.vx_vz_cov);
1051  setDoNotUse(msg.vy_vz_cov);
1052  }
1053  if (it > itEnd)
1054  {
1055  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1056  return false;
1057  }
1058  return true;
1059 };
1060 
1065 template<typename It>
1066 bool PosCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, PosCovCartesianMsg& msg)
1067 {
1068  if(!BlockHeaderParser(node, it, msg.block_header))
1069  return false;
1070  if (msg.block_header.id != 5905)
1071  {
1072  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1073  return false;
1074  }
1075  qiLittleEndianParser(it, msg.mode);
1076  qiLittleEndianParser(it, msg.error);
1077  qiLittleEndianParser(it, msg.cov_xx);
1078  qiLittleEndianParser(it,msg.cov_yy);
1079  qiLittleEndianParser(it,msg.cov_zz);
1080  qiLittleEndianParser(it,msg.cov_bb);
1081  qiLittleEndianParser(it,msg.cov_xy);
1082  qiLittleEndianParser(it,msg.cov_xz);
1083  qiLittleEndianParser(it,msg.cov_xb);
1084  qiLittleEndianParser(it,msg.cov_yz);
1085  qiLittleEndianParser(it,msg.cov_yb);
1086  qiLittleEndianParser(it,msg.cov_zb);
1087  if (it > itEnd)
1088  {
1089  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1090  return false;
1091  }
1092  return true;
1093 };
1094 
1099 template<typename It>
1100 bool PosCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, PosCovGeodeticMsg& msg)
1101 {
1102  if(!BlockHeaderParser(node, it, msg.block_header))
1103  return false;
1104  if (msg.block_header.id != 5906)
1105  {
1106  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1107  return false;
1108  }
1109  qiLittleEndianParser(it, msg.mode);
1110  qiLittleEndianParser(it, msg.error);
1111  qiLittleEndianParser(it, msg.cov_latlat);
1112  qiLittleEndianParser(it, msg.cov_lonlon);
1113  qiLittleEndianParser(it, msg.cov_hgthgt);
1114  qiLittleEndianParser(it, msg.cov_bb);
1115  qiLittleEndianParser(it, msg.cov_latlon);
1116  qiLittleEndianParser(it, msg.cov_lathgt);
1117  qiLittleEndianParser(it, msg.cov_latb);
1118  qiLittleEndianParser(it, msg.cov_lonhgt);
1119  qiLittleEndianParser(it, msg.cov_lonb);
1120  qiLittleEndianParser(it, msg.cov_hb);
1121  if (it > itEnd)
1122  {
1123  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1124  return false;
1125  }
1126  return true;
1127 };
1128 
1133 template<typename It>
1134 bool VelCovCartesianParser(ROSaicNodeBase* node, It it, It itEnd, VelCovCartesianMsg& msg)
1135 {
1136  if(!BlockHeaderParser(node, it, msg.block_header))
1137  return false;
1138  if (msg.block_header.id != 5907)
1139  {
1140  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1141  return false;
1142  }
1143  qiLittleEndianParser(it, msg.mode);
1144  qiLittleEndianParser(it, msg.error);
1145  qiLittleEndianParser(it, msg.cov_vxvx);
1146  qiLittleEndianParser(it, msg.cov_vyvy);
1147  qiLittleEndianParser(it, msg.cov_vzvz);
1148  qiLittleEndianParser(it, msg.cov_dtdt);
1149  qiLittleEndianParser(it, msg.cov_vxvy);
1150  qiLittleEndianParser(it, msg.cov_vxvz);
1151  qiLittleEndianParser(it, msg.cov_vxdt);
1152  qiLittleEndianParser(it, msg.cov_vyvz);
1153  qiLittleEndianParser(it, msg.cov_vydt);
1154  qiLittleEndianParser(it, msg.cov_vzdt);
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 VelCovGeodeticParser(ROSaicNodeBase* node, It it, It itEnd, VelCovGeodeticMsg& msg)
1169 {
1170  if(!BlockHeaderParser(node, it, msg.block_header))
1171  return false;
1172  if (msg.block_header.id != 5908)
1173  {
1174  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1175  return false;
1176  }
1177  qiLittleEndianParser(it, msg.mode);
1178  qiLittleEndianParser(it, msg.error);
1179  qiLittleEndianParser(it, msg.cov_vnvn);
1180  qiLittleEndianParser(it, msg.cov_veve);
1181  qiLittleEndianParser(it, msg.cov_vuvu);
1182  qiLittleEndianParser(it, msg.cov_dtdt);
1183  qiLittleEndianParser(it, msg.cov_vnve);
1184  qiLittleEndianParser(it, msg.cov_vnvu);
1185  qiLittleEndianParser(it, msg.cov_vndt);
1186  qiLittleEndianParser(it, msg.cov_vevu);
1187  qiLittleEndianParser(it, msg.cov_vedt);
1188  qiLittleEndianParser(it, msg.cov_vudt);
1189  if (it > itEnd)
1190  {
1191  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1192  return false;
1193  }
1194  return true;
1195 };
1196 
1201 template<typename It>
1202 bool QualityIndParser(ROSaicNodeBase* node, It it, It itEnd, QualityInd& msg)
1203 {
1204  if(!BlockHeaderParser(node, it, msg.block_header))
1205  return false;
1206  if (msg.block_header.id != 4082)
1207  {
1208  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1209  return false;
1210  }
1211  qiLittleEndianParser(it, msg.n);
1212  if (msg.n > 40)
1213  {
1214  node->log(LogLevel::ERROR, "Parse error: Too many indicators " + std::to_string(msg.n));
1215  return false;
1216  }
1217  ++it; // reserved
1218  msg.indicators.resize(msg.n);
1219  std::vector<uint16_t> indicators;
1220  for (auto& indicators : msg.indicators)
1221  {
1222  qiLittleEndianParser(it, indicators);
1223  }
1224  if (it > itEnd)
1225  {
1226  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1227  return false;
1228  }
1229  return true;
1230 };
1231 
1236 template<typename It>
1237 void AgcStateParser(It it, AgcState& msg, uint8_t sb_length)
1238 {
1240  qiLittleEndianParser(it, msg.gain);
1243  std::advance(it, sb_length - 4); // skip padding
1244 };
1245 
1250 template<typename It>
1251 bool ReceiverStatusParser(ROSaicNodeBase* node, It it, It itEnd, ReceiverStatus& msg)
1252 {
1253  if(!BlockHeaderParser(node, it, msg.block_header))
1254  return false;
1255  if (msg.block_header.id != 4014)
1256  {
1257  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1258  return false;
1259  }
1260  qiLittleEndianParser(it, msg.cpu_load);
1262  qiLittleEndianParser(it, msg.up_time);
1264  qiLittleEndianParser(it, msg.rx_error);
1265  qiLittleEndianParser(it, msg.n);
1266  if (msg.n > 18)
1267  {
1268  node->log(LogLevel::ERROR, "Parse error: Too many AGCState " + std::to_string(msg.n));
1269  return false;
1270  }
1274  msg.agc_state.resize(msg.n);
1275  for (auto& agc_state : msg.agc_state)
1276  {
1277  AgcStateParser(it, agc_state, msg.sb_length);
1278  }
1279  if (it > itEnd)
1280  {
1281  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1282  return false;
1283  }
1284  return true;
1285 };
1286 
1291 template<typename It>
1292 bool INSNavGeodParser(ROSaicNodeBase* node, It it, It itEnd, INSNavGeodMsg& msg, bool use_ros_axis_orientation)
1293 {
1294  if(!BlockHeaderParser(node, it, msg.block_header))
1295  return false;
1296  if ((msg.block_header.id != 4226) && (msg.block_header.id != 4230))
1297  {
1298  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1299  return false;
1300  }
1301  qiLittleEndianParser(it, msg.gnss_mode);
1302  qiLittleEndianParser(it, msg.error);
1303  qiLittleEndianParser(it, msg.info);
1304  qiLittleEndianParser(it, msg.gnss_age);
1305  qiLittleEndianParser(it, msg.latitude);
1306  qiLittleEndianParser(it, msg.longitude);
1307  qiLittleEndianParser(it, msg.height);
1308  qiLittleEndianParser(it, msg.undulation);
1309  qiLittleEndianParser(it, msg.accuracy);
1310  qiLittleEndianParser(it, msg.latency);
1311  qiLittleEndianParser(it, msg.datum);
1312  ++it; //reserved
1313  qiLittleEndianParser(it, msg.sb_list);
1314  if((msg.sb_list & 1) !=0)
1315  {
1316  qiLittleEndianParser(it, msg.latitude_std_dev);
1317  qiLittleEndianParser(it, msg.longitude_std_dev);
1318  qiLittleEndianParser(it, msg.height_std_dev);
1319  }
1320  else
1321  {
1322  setDoNotUse(msg.latitude_std_dev);
1323  setDoNotUse(msg.longitude_std_dev);
1324  setDoNotUse(msg.height_std_dev);
1325  }
1326  if((msg.sb_list & 2) !=0)
1327  {
1328  qiLittleEndianParser(it, msg.heading);
1329  qiLittleEndianParser(it, msg.pitch);
1330  qiLittleEndianParser(it, msg.roll);
1331  if (use_ros_axis_orientation)
1332  {
1333  if (validValue(msg.heading))
1334  msg.heading = -msg.heading + 90;
1335  if (validValue(msg.pitch))
1336  msg.pitch = -msg.pitch;
1337  }
1338  }
1339  else
1340  {
1341  setDoNotUse(msg.heading);
1342  setDoNotUse(msg.pitch );
1343  setDoNotUse(msg.roll);
1344  }
1345  if((msg.sb_list & 4) !=0)
1346  {
1347  qiLittleEndianParser(it, msg.heading_std_dev);
1348  qiLittleEndianParser(it, msg.pitch_std_dev);
1349  qiLittleEndianParser(it, msg.roll_std_dev);
1350  }
1351  else
1352  {
1353  setDoNotUse(msg.heading_std_dev);
1354  setDoNotUse(msg.pitch_std_dev );
1355  setDoNotUse(msg.roll_std_dev);
1356  }
1357  if((msg.sb_list & 8) !=0)
1358  {
1359  qiLittleEndianParser(it, msg.ve);
1360  qiLittleEndianParser(it, msg.vn);
1361  qiLittleEndianParser(it, msg.vu);
1362  }
1363  else
1364  {
1365  setDoNotUse(msg.ve);
1366  setDoNotUse(msg.vn);
1367  setDoNotUse(msg.vu);
1368  }
1369  if((msg.sb_list & 16) !=0)
1370  {
1371  qiLittleEndianParser(it, msg.ve_std_dev);
1372  qiLittleEndianParser(it, msg.vn_std_dev);
1373  qiLittleEndianParser(it, msg.vu_std_dev);
1374  }
1375  else
1376  {
1377  setDoNotUse(msg.ve_std_dev);
1378  setDoNotUse(msg.vn_std_dev);
1379  setDoNotUse(msg.vu_std_dev);
1380  }
1381  if((msg.sb_list & 32) !=0)
1382  {
1383  qiLittleEndianParser(it, msg.latitude_longitude_cov);
1384  qiLittleEndianParser(it, msg.latitude_height_cov);
1385  qiLittleEndianParser(it, msg.longitude_height_cov);
1386  }
1387  else
1388  {
1389  setDoNotUse(msg.latitude_longitude_cov);
1390  setDoNotUse(msg.latitude_height_cov);
1391  setDoNotUse(msg.longitude_height_cov );
1392  }
1393  if((msg.sb_list & 64) !=0)
1394  {
1395  qiLittleEndianParser(it, msg.heading_pitch_cov);
1396  qiLittleEndianParser(it, msg.heading_roll_cov);
1397  qiLittleEndianParser(it, msg.pitch_roll_cov);
1398  if (use_ros_axis_orientation)
1399  {
1400  if (validValue(msg.heading_roll_cov))
1401  msg.heading_roll_cov = -msg.heading_roll_cov;
1402  if (validValue(msg.pitch_roll_cov))
1403  msg.pitch_roll_cov = -msg.pitch_roll_cov;
1404  }
1405  }
1406  else
1407  {
1408  setDoNotUse(msg.heading_pitch_cov);
1409  setDoNotUse(msg.heading_roll_cov);
1410  setDoNotUse(msg.pitch_roll_cov);
1411  }
1412  if((msg.sb_list & 128) !=0)
1413  {
1414  qiLittleEndianParser(it, msg.ve_vn_cov);
1415  qiLittleEndianParser(it, msg.ve_vu_cov);
1416  qiLittleEndianParser(it, msg.vn_vu_cov);
1417  }
1418  else
1419  {
1420  setDoNotUse(msg.ve_vn_cov);
1421  setDoNotUse(msg.ve_vu_cov);
1422  setDoNotUse(msg.vn_vu_cov);
1423  }
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 IMUSetupParser(ROSaicNodeBase* node, It it, It itEnd, IMUSetupMsg& msg, bool use_ros_axis_orientation)
1438 {
1439  if(!BlockHeaderParser(node, it, msg.block_header))
1440  return false;
1441  if (msg.block_header.id != 4224)
1442  {
1443  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1444  return false;
1445  }
1446  ++it; //reserved
1447  qiLittleEndianParser(it, msg.serial_port);
1448  qiLittleEndianParser(it, msg.ant_lever_arm_x);
1449  qiLittleEndianParser(it, msg.ant_lever_arm_y);
1450  qiLittleEndianParser(it, msg.ant_lever_arm_z);
1451  qiLittleEndianParser(it, msg.theta_x);
1452  qiLittleEndianParser(it, msg.theta_y);
1453  qiLittleEndianParser(it, msg.theta_z);
1454  if (use_ros_axis_orientation)
1455  {
1456  msg.ant_lever_arm_y = -msg.ant_lever_arm_y;
1457  msg.ant_lever_arm_z = -msg.ant_lever_arm_z;
1458  msg.theta_x = parsing_utilities::wrapAngle180to180(msg.theta_x - 180.0f);
1459  }
1460  if (it > itEnd)
1461  {
1462  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1463  return false;
1464  }
1465  return true;
1466 };
1467 
1472 template<typename It>
1473 bool VelSensorSetupParser(ROSaicNodeBase* node, It it, It itEnd, VelSensorSetupMsg& msg, bool use_ros_axis_orientation)
1474 {
1475  if(!BlockHeaderParser(node, it, msg.block_header))
1476  return false;
1477  if (msg.block_header.id != 4244)
1478  {
1479  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1480  return false;
1481  }
1482  ++it; //reserved
1483  qiLittleEndianParser(it, msg.port);
1484  qiLittleEndianParser(it, msg.lever_arm_x);
1485  qiLittleEndianParser(it, msg.lever_arm_y);
1486  qiLittleEndianParser(it, msg.lever_arm_z);
1487  if (use_ros_axis_orientation)
1488  {
1489  msg.lever_arm_y = -msg.lever_arm_y;
1490  msg.lever_arm_z = -msg.lever_arm_z;
1491  }
1492  if (it > itEnd)
1493  {
1494  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1495  return false;
1496  }
1497  return true;
1498 };
1499 
1504 template<typename It>
1505 bool ExtSensorMeasParser(ROSaicNodeBase* node, It it, It itEnd, ExtSensorMeasMsg& msg, bool use_ros_axis_orientation)
1506 {
1507  if(!BlockHeaderParser(node, it, msg.block_header))
1508  return false;
1509  if (msg.block_header.id != 4050)
1510  {
1511  node->log(LogLevel::ERROR, "Parse error: Wrong header ID " + std::to_string(msg.block_header.id));
1512  return false;
1513  }
1514  qiLittleEndianParser(it, msg.n);
1515  qiLittleEndianParser(it, msg.sb_length);
1516  if (msg.sb_length != 28)
1517  {
1518  node->log(LogLevel::ERROR, "Parse error: Wrong sb_length " + std::to_string(msg.sb_length));
1519  return false;
1520  }
1521 
1522  msg.acceleration_x = std::numeric_limits<double>::quiet_NaN();
1523  msg.acceleration_y = std::numeric_limits<double>::quiet_NaN();
1524  msg.acceleration_z = std::numeric_limits<double>::quiet_NaN();
1525 
1526  msg.angular_rate_x = std::numeric_limits<double>::quiet_NaN();
1527  msg.angular_rate_y = std::numeric_limits<double>::quiet_NaN();
1528  msg.angular_rate_z = std::numeric_limits<double>::quiet_NaN();
1529 
1530  msg.velocity_x = std::numeric_limits<double>::quiet_NaN();
1531  msg.velocity_y = std::numeric_limits<double>::quiet_NaN();
1532  msg.velocity_z = std::numeric_limits<double>::quiet_NaN();
1533 
1534  msg.std_dev_x = std::numeric_limits<double>::quiet_NaN();
1535  msg.std_dev_y = std::numeric_limits<double>::quiet_NaN();
1536  msg.std_dev_z = std::numeric_limits<double>::quiet_NaN();
1537 
1538  msg.sensor_temperature = -32768.0f; //do not use value
1539  msg.zero_velocity_flag = std::numeric_limits<double>::quiet_NaN();
1540 
1541  msg.source.resize(msg.n);
1542  msg.sensor_model.resize(msg.n);
1543  msg.type.resize(msg.n);
1544  msg.obs_info.resize(msg.n);
1545  for (size_t i = 0; i < msg.n; i++)
1546  {
1547  qiLittleEndianParser(it, msg.source[i]);
1548  qiLittleEndianParser(it, msg.sensor_model[i]);
1549  qiLittleEndianParser(it, msg.type[i]);
1550  qiLittleEndianParser(it, msg.obs_info[i]);
1551 
1552  switch (msg.type[i])
1553  {
1554  case 0:
1555  {
1556  qiLittleEndianParser(it, msg.acceleration_x);
1557  qiLittleEndianParser(it, msg.acceleration_y);
1558  qiLittleEndianParser(it, msg.acceleration_z);
1559  if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi
1560  {
1561  if (validValue(msg.acceleration_y))
1562  msg.acceleration_y = -msg.acceleration_y;
1563  if (validValue(msg.acceleration_z))
1564  msg.acceleration_z = -msg.acceleration_z;
1565  }
1566  break;
1567  }
1568  case 1:
1569  {
1570  qiLittleEndianParser(it, msg.angular_rate_x);
1571  qiLittleEndianParser(it, msg.angular_rate_y);
1572  qiLittleEndianParser(it, msg.angular_rate_z);
1573  if (!use_ros_axis_orientation) // IMU is mounted upside down in SBi
1574  {
1575  if (validValue(msg.angular_rate_y))
1576  msg.angular_rate_y = -msg.angular_rate_y;
1577  if (validValue(msg.angular_rate_z))
1578  msg.angular_rate_z = -msg.angular_rate_z;
1579  }
1580  break;
1581  }
1582  case 3:
1583  {
1584  qi::parse(it, it + 2, qi::little_word, msg.sensor_temperature);
1585  msg.sensor_temperature /= 100.0f;
1586  std::advance(it, 22); // reserved
1587  break;
1588  }
1589  case 4:
1590  {
1591  qiLittleEndianParser(it, msg.velocity_x);
1592  qiLittleEndianParser(it, msg.velocity_y);
1593  qiLittleEndianParser(it, msg.velocity_z);
1594  qiLittleEndianParser(it, msg.std_dev_x);
1595  qiLittleEndianParser(it, msg.std_dev_y);
1596  qiLittleEndianParser(it, msg.std_dev_z);
1597  if (use_ros_axis_orientation)
1598  {
1599  if (validValue(msg.velocity_y))
1600  msg.velocity_y = -msg.velocity_y;
1601  if (validValue(msg.velocity_z))
1602  msg.velocity_z = -msg.velocity_z;
1603  }
1604  break;
1605  }
1606  case 20:
1607  {
1608  qiLittleEndianParser(it, msg.zero_velocity_flag);
1609  std::advance(it, 16); // reserved
1610  break;
1611  }
1612  default:
1613  {
1614  node->log(LogLevel::ERROR, "Unknown external sensor measurement type in SBF ExtSensorMeas.");
1615  std::advance(it, 24);
1616  break;
1617  }
1618  }
1619  }
1620  if (it > itEnd)
1621  {
1622  node->log(LogLevel::ERROR, "Parse error: iterator past end.");
1623  return false;
1624  }
1625  return true;
1626 };
1627 
1628 #endif // SBFStructs_HPP
uint32_t up_time
bool qiCharsToStringParser(It &it, std::string &val, std::size_t num)
Qi parser for char array to string.
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:107
uint8_t temperature
double pdop
uint32_t rx_error
uint16_t az_rise_set
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.
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:100
std::string ant_serial_nbr
std::string rx_version
void MeasEpochChannelType2Parser(It &it, MeasEpochChannelType2Msg &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "MeasEpochChannelType2".
uint16_t wnc
This is the GPS week counter.
std::string marker_type
std::vector< ChannelStateInfo > stateInfo
BlockHeader block_header
bool ExtSensorMeasParser(ROSaicNodeBase *node, It it, It itEnd, ExtSensorMeasMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "ExtSensorMeas".
std::string observer
double tdop
bool PVTCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PVTCartesianMsg &msg)
Qi based parser for the SBF block "PVTCartesian".
bool IMUSetupParser(ROSaicNodeBase *node, It it, It itEnd, IMUSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "IMUSetup".
void AgcStateParser(It it, AgcState &msg, uint8_t sb_length)
Struct for the SBF sub-block "AGCState".
uint8_t rx_channel
bool QualityIndParser(ROSaicNodeBase *node, It it, It itEnd, QualityInd &msg)
Qi based parser for the SBF block "QualityInd".
bool PVTGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PVTGeodeticMsg &msg)
Qi based parser for the SBF block "PVTGeodetic".
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:103
std::string country_code
septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:119
septentrio_gnss_driver::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:116
bool INSNavCartParser(ROSaicNodeBase *node, It it, It itEnd, INSNavCartMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavCart".
std::vector< uint16_t > indicators
Struct for the SBF sub-block "AGCState".
septentrio_gnss_driver::MeasEpoch MeasEpochMsg
Definition: typedefs.hpp:97
uint8_t nr_sv
std::string marker_name
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:120
Struct for the SBF block&#39;s header message.
Struct for the SBF block "DOP".
uint8_t sb1_length
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:208
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
bool ReceiverStatusParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverStatus &msg)
Struct for the SBF block "ReceiverStatus".
septentrio_gnss_driver::AttEuler AttEulerMsg
Definition: typedefs.hpp:101
BlockHeader block_header
uint8_t revision
This is the block revision.
bool AttCovEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttCovEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttCovEuler".
septentrio_gnss_driver::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:118
septentrio_gnss_driver::MeasEpochChannelType2 MeasEpochChannelType2Msg
Definition: typedefs.hpp:99
uint8_t sync_1
first sync byte is $ or 0x24
Struct for the SBF block "QualityInd".
float vpl
int8_t gain
void setDoNotUse(T &s)
Sets scalar to Do-Not-Use value -2e10.
bool VelSensorSetupParser(ROSaicNodeBase *node, It it, It itEnd, VelSensorSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "VelSensorSetup".
uint16_t length
bool MeasEpochParser(ROSaicNodeBase *node, It it, It itEnd, MeasEpochMsg &msg)
std::string rx_name
bool ChannelStatusParser(ROSaicNodeBase *node, It it, It itEnd, ChannelStatus &msg)
Qi based parser for the SBF block "ChannelStatus".
uint8_t monument_idx
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".
#define MAXSB_MEASEPOCH_T2
Max number of bytes that MeasEpochChannelType2 sub-block can consist of.
Definition: sbf_structs.hpp:70
bool AttEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttEuler".
double vdop
BlockHeader block_header
BlockHeader block_header
double hdop
bool PosCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PosCovGeodeticMsg &msg)
Qi based parser for the SBF block "PosCovGeodetic".
static const uint8_t SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
bool INSNavGeodParser(ROSaicNodeBase *node, It it, It itEnd, INSNavGeodMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavGeod".
bool BlockHeaderParser(ROSaicNodeBase *node, It &it, Hdr &block_header)
Qi based parser for the SBF block "BlockHeader" plus receiver time stamp.
std::vector< AgcState > agc_state
std::string station_code
septentrio_gnss_driver::MeasEpochChannelType1 MeasEpochChannelType1Msg
Definition: typedefs.hpp:98
Declares utility functions used when parsing messages.
bool MeasEpochChannelType1Parser(ROSaicNodeBase *node, It &it, MeasEpochChannelType1Msg &msg, uint8_t sb1_length, uint8_t sb2_length)
uint8_t sb2_length
uint8_t receiver_idx
bool ReceiverSetupParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverSetup &msg)
Qi based parser for the SBF block "ReceiverSetup".
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:117
Struct for the SBF block "ChannelStatus".
float hpl
septentrio_gnss_driver::VelCovCartesian VelCovCartesianMsg
Definition: typedefs.hpp:106
void ChannelStateInfoParser(It &it, ChannelStateInfo &msg, uint8_t sb2_length)
Qi based parser for the SBF sub-block "ChannelStateInfo".
static const uint8_t SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
bool PosCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PosCovCartesianMsg &msg)
Qi based parser for the SBF block "PosCovCartesian".
bool VelCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, VelCovCartesianMsg &msg)
Qi based parser for the SBF block "VelCovCartesian".
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:105
uint8_t sync_2
2nd sync byte is @ or 0x40
septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:104
std::string agency
BlockHeader block_header
bool validValue(T s)
Check if value is not set to Do-Not-Use -2e10.
uint16_t health_status
std::vector< ChannelSatInfo > satInfo
std::string marker_number
uint8_t blanking_stat
Struct for the SBF block "ReceiverStatus".
std::string product_name
std::string rx_serial_number
uint32_t rx_status
This class is the base class for abstraction.
Definition: typedefs.hpp:160
uint16_t tracking_status
bool qiLittleEndianParser(It &it, Val &val)
Qi little endian parsers for numeric values.
std::string gnss_fw_version
#define MAXSB_CHANNELSATINFO
Max number of bytes that ChannelSatInfo sub-block can consist of.
Definition: sbf_structs.hpp:58
uint8_t sample_var
Struct for the SBF block "ReceiverSetup".
Struct for the SBF sub-block "ChannelSatInfo".
#define MAXSB_MEASEPOCH_T1
Max number of bytes that MeasEpochChannelType1 sub-block can consist of.
Definition: sbf_structs.hpp:66
septentrio_gnss_driver::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:102
bool VelCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, VelCovGeodeticMsg &msg)
Qi based parser for the SBF block "VelCovGeodetic".
uint8_t frontend_id
bool DOPParser(ROSaicNodeBase *node, It it, It itEnd, DOP &msg)
Qi based parser for the SBF block "DOP".
Struct for the SBF sub-block "ChannelStateInfo".
uint16_t crc
The check sum.
std::string ant_type
uint16_t id
This is the block ID.
uint32_t tow
This is the time of week in ms.


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Jun 23 2022 02:11:38