ROSaic
rx_message.cpp
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 
32 
51 
53 std::pair<uint16_t, TypeOfPVT_Enum> type_of_pvt_pairs[] =
54 {
55  std::make_pair(static_cast<uint16_t>(0), evNoPVT),
56  std::make_pair(static_cast<uint16_t>(1), evStandAlone),
57  std::make_pair(static_cast<uint16_t>(2), evDGPS),
58  std::make_pair(static_cast<uint16_t>(3), evFixed),
59  std::make_pair(static_cast<uint16_t>(4), evRTKFixed),
60  std::make_pair(static_cast<uint16_t>(5), evRTKFloat),
61  std::make_pair(static_cast<uint16_t>(6), evSBAS),
62  std::make_pair(static_cast<uint16_t>(7), evMovingBaseRTKFixed),
63  std::make_pair(static_cast<uint16_t>(8), evMovingBaseRTKFloat),
64  std::make_pair(static_cast<uint16_t>(10), evPPP)
65 };
66 
68 
70 std::pair<std::string, RxID_Enum> rx_id_pairs[] =
71 {
72  std::make_pair("NavSatFix", evNavSatFix),
73  std::make_pair("GPSFix", evGPSFix),
74  std::make_pair("PoseWithCovarianceStamped", evPoseWithCovarianceStamped),
75  std::make_pair("$GPGGA", evGPGGA),
76  std::make_pair("$GPRMC", evGPRMC),
77  std::make_pair("$GPGSA", evGPGSA),
78  std::make_pair("$GPGSV", evGPGSV),
79  std::make_pair("$GLGSV", evGLGSV),
80  std::make_pair("$GAGSV", evGAGSV),
81  std::make_pair("4006", evPVTCartesian),
82  std::make_pair("4007", evPVTGeodetic),
83  std::make_pair("5905", evPosCovCartesian),
84  std::make_pair("5906", evPosCovGeodetic),
85  std::make_pair("5938", evAttEuler),
86  std::make_pair("5939", evAttCovEuler),
87  std::make_pair("GPST", evGPST),
88  std::make_pair("4013", evChannelStatus),
89  std::make_pair("4027", evMeasEpoch),
90  std::make_pair("4001", evDOP),
91  std::make_pair("5908", evVelCovGeodetic),
92  std::make_pair("DiagnosticArray", evDiagnosticArray),
93  std::make_pair("4014", evReceiverStatus),
94  std::make_pair("4082", evQualityInd),
95  std::make_pair("5902", evReceiverSetup)
96 };
97 
99 
101 {
102  rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
103  msg->block_header.sync_1 = data.block_header.sync_1;
104  msg->block_header.sync_2 = data.block_header.sync_2;
105  msg->block_header.crc = data.block_header.crc;
106  msg->block_header.id = data.block_header.id;
107  msg->block_header.length = data.block_header.length;
108  msg->block_header.tow = data.tow;
109  msg->block_header.wnc = data.wnc;
110  msg->mode = data.mode;
111  msg->error = data.error;
112  msg->latitude = data.latitude;
113  msg->longitude = data.longitude;
114  msg->height = data.height;
115  msg->undulation = data.undulation;
116  msg->vn = data.vn;
117  msg->ve = data.ve;
118  msg->vu = data.vu;
119  msg->cog = data.cog;
120  msg->rx_clk_bias = data.rx_clk_bias;
121  msg->rx_clk_drift = data.rx_clk_drift;
122  msg->time_system = data.time_system;
123  msg->datum = data.datum;
124  msg->nr_sv = data.nr_sv;
125  msg->wa_corr_info = data.wa_corr_info;
126  msg->reference_id = data.reference_id;
127  msg->mean_corr_age = data.mean_corr_age;
128  msg->signal_info = data.signal_info;
129  msg->alert_flag = data.alert_flag;
130  msg->nr_bases = data.nr_bases;
131  msg->ppp_info = data.ppp_info;
132  msg->latency = data.latency;
133  msg->h_accuracy = data.h_accuracy;
134  msg->v_accuracy = data.v_accuracy;
135  msg->misc = data.misc;
136  return msg;
137 }
138 
139 
141 {
142  rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
143  msg->block_header.sync_1 = data.block_header.sync_1;
144  msg->block_header.sync_2 = data.block_header.sync_2;
145  msg->block_header.crc = data.block_header.crc;
146  msg->block_header.id = data.block_header.id;
147  msg->block_header.length = data.block_header.length;
148  msg->block_header.tow = data.tow;
149  msg->block_header.wnc = data.wnc;
150  msg->mode = data.mode;
151  msg->error = data.error;
152  msg->x = data.x;
153  msg->y = data.y;
154  msg->z = data.z;
155  msg->undulation = data.undulation;
156  msg->vx = data.vx;
157  msg->vy = data.vy;
158  msg->vz = data.vz;
159  msg->cog = data.cog;
160  msg->rx_clk_bias = data.rx_clk_bias;
161  msg->rx_clk_drift = data.rx_clk_drift;
162  msg->time_system = data.time_system;
163  msg->datum = data.datum;
164  msg->nr_sv = data.nr_sv;
165  msg->wa_corr_info = data.wa_corr_info;
166  msg->reference_id = data.reference_id;
167  msg->mean_corr_age = data.mean_corr_age;
168  msg->signal_info = data.signal_info;
169  msg->alert_flag = data.alert_flag;
170  msg->nr_bases = data.nr_bases;
171  msg->ppp_info = data.ppp_info;
172  msg->latency = data.latency;
173  msg->h_accuracy = data.h_accuracy;
174  msg->v_accuracy = data.v_accuracy;
175  msg->misc = data.misc;
176  return msg;
177 }
178 
180 {
181  rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
182  msg->block_header.sync_1 = data.block_header.sync_1;
183  msg->block_header.sync_2 = data.block_header.sync_2;
184  msg->block_header.crc = data.block_header.crc;
185  msg->block_header.id = data.block_header.id;
186  msg->block_header.length = data.block_header.length;
187  msg->block_header.tow = data.tow;
188  msg->block_header.wnc = data.wnc;
189  msg->mode = data.mode;
190  msg->error = data.error;
191  msg->cov_xx = data.cov_xx;
192  msg->cov_yy = data.cov_yy;
193  msg->cov_zz = data.cov_zz;
194  msg->cov_bb = data.cov_bb;
195  msg->cov_xy = data.cov_xy;
196  msg->cov_xz = data.cov_xz;
197  msg->cov_xb = data.cov_xb;
198  msg->cov_yz = data.cov_yz;
199  msg->cov_yb = data.cov_yb;
200  msg->cov_zb = data.cov_zb;
201  return msg;
202 }
203 
204 
206 {
207  rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
208  msg->block_header.sync_1 = data.block_header.sync_1;
209  msg->block_header.sync_2 = data.block_header.sync_2;
210  msg->block_header.crc = data.block_header.crc;
211  msg->block_header.id = data.block_header.id;
212  msg->block_header.length = data.block_header.length;
213  msg->block_header.tow = data.tow;
214  msg->block_header.wnc = data.wnc;
215  msg->mode = data.mode;
216  msg->error = data.error;
217  msg->cov_latlat = data.cov_latlat;
218  msg->cov_lonlon = data.cov_lonlon;
219  msg->cov_hgthgt = data.cov_hgthgt;
220  msg->cov_bb = data.cov_bb;
221  msg->cov_latlon = data.cov_latlon;
222  msg->cov_lathgt = data.cov_lathgt;
223  msg->cov_latb = data.cov_latb;
224  msg->cov_lonhgt = data.cov_lonhgt;
225  msg->cov_lonb = data.cov_lonb;
226  msg->cov_hb = data.cov_hb;
227  return msg;
228 }
229 
231 {
232  rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
233  msg->block_header.sync_1 = data.block_header.sync_1;
234  msg->block_header.sync_2 = data.block_header.sync_2;
235  msg->block_header.crc = data.block_header.crc;
236  msg->block_header.id = data.block_header.id;
237  msg->block_header.length = data.block_header.length;
238  msg->block_header.tow = data.tow;
239  msg->block_header.wnc = data.wnc;
240  msg->nr_sv = data.nr_sv;
241  msg->error = data.error;
242  msg->mode = data.mode;
243  msg->heading = data.heading;
244  msg->pitch = data.pitch;
245  msg->roll = data.roll;
246  msg->pitch_dot = data.pitch_dot;
247  msg->roll_dot = data.roll_dot;
248  msg->heading_dot = data.heading_dot;
249  return msg;
250 };
251 
253 {
254  rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
255  msg->block_header.sync_1 = data.block_header.sync_1;
256  msg->block_header.sync_2 = data.block_header.sync_2;
257  msg->block_header.crc = data.block_header.crc;
258  msg->block_header.id = data.block_header.id;
259  msg->block_header.length = data.block_header.length;
260  msg->block_header.tow = data.tow;
261  msg->block_header.wnc = data.wnc;
262  msg->error = data.error;
263  msg->cov_headhead = data.cov_headhead;
264  msg->cov_pitchpitch = data.cov_pitchpitch;
265  msg->cov_rollroll = data.cov_rollroll;
266  msg->cov_headpitch = data.cov_headpitch;
267  msg->cov_headroll = data.cov_headroll;
268  msg->cov_pitchroll = data.cov_pitchroll;
269  return msg;
270 };
271 
282 geometry_msgs::PoseWithCovarianceStampedPtr io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback()
283 {
284  geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
285  // Filling in the pose data
286  msg->pose.pose.orientation = parsing_utilities::convertEulerToQuaternion(static_cast<double>(last_atteuler_.heading),
287  static_cast<double>(last_atteuler_.pitch), static_cast<double>(last_atteuler_.roll));
288  msg->pose.pose.position.x = static_cast<double>(last_pvtgeodetic_.longitude)*360/(2*boost::math::constants::pi<double>());
289  msg->pose.pose.position.y = static_cast<double>(last_pvtgeodetic_.latitude)*360/(2*boost::math::constants::pi<double>());
290  msg->pose.pose.position.z = static_cast<double>(last_pvtgeodetic_.height);
291  // Filling in the covariance data in row-major order
292  msg->pose.covariance[0] = static_cast<double>(last_poscovgeodetic_.cov_lonlon);
293  msg->pose.covariance[1] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
294  msg->pose.covariance[2] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
295  msg->pose.covariance[3] = 0;
296  msg->pose.covariance[4] = 0;
297  msg->pose.covariance[5] = 0;
298  msg->pose.covariance[6] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
299  msg->pose.covariance[7] = static_cast<double>(last_poscovgeodetic_.cov_latlat);
300  msg->pose.covariance[8] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
301  msg->pose.covariance[9] = 0;
302  msg->pose.covariance[10] = 0;
303  msg->pose.covariance[11] = 0;
304  msg->pose.covariance[12] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
305  msg->pose.covariance[13] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
306  msg->pose.covariance[14] = static_cast<double>(last_poscovgeodetic_.cov_hgthgt);
307  msg->pose.covariance[15] = 0;
308  msg->pose.covariance[16] = 0;
309  msg->pose.covariance[17] = 0;
310  msg->pose.covariance[18] = 0;
311  msg->pose.covariance[19] = 0;
312  msg->pose.covariance[20] = 0;
313  msg->pose.covariance[21] = static_cast<double>(last_attcoveuler_.cov_rollroll);
314  msg->pose.covariance[22] = static_cast<double>(last_attcoveuler_.cov_pitchroll);
315  msg->pose.covariance[23] = static_cast<double>(last_attcoveuler_.cov_headroll);
316  msg->pose.covariance[24] = 0;
317  msg->pose.covariance[25] = 0;
318  msg->pose.covariance[26] = 0;
319  msg->pose.covariance[27] = static_cast<double>(last_attcoveuler_.cov_pitchroll);
320  msg->pose.covariance[28] = static_cast<double>(last_attcoveuler_.cov_pitchpitch);
321  msg->pose.covariance[29] = static_cast<double>(last_attcoveuler_.cov_headpitch);
322  msg->pose.covariance[30] = 0;
323  msg->pose.covariance[31] = 0;
324  msg->pose.covariance[32] = 0;
325  msg->pose.covariance[33] = static_cast<double>(last_attcoveuler_.cov_headroll);
326  msg->pose.covariance[34] = static_cast<double>(last_attcoveuler_.cov_pitchroll);
327  msg->pose.covariance[35] = static_cast<double>(last_attcoveuler_.cov_headhead);
328 
329  return msg;
330 }
331 
332 diagnostic_msgs::DiagnosticArrayPtr io_comm_rx::RxMessage::DiagnosticArrayCallback()
333 {
334  diagnostic_msgs::DiagnosticArrayPtr msg = boost::make_shared<diagnostic_msgs::DiagnosticArray>();
335  std::string serialnumber(last_receiversetup_.rx_serial_number);
336  diagnostic_msgs::DiagnosticStatusPtr gnss_status = boost::make_shared<diagnostic_msgs::DiagnosticStatus>();
337  // Constructing the "level of operation" field
338  uint16_t indicators_type_mask = static_cast<uint16_t>(255);
339  uint16_t indicators_value_mask = static_cast<uint16_t>(3840);
340  uint16_t qualityind_pos;
341  for(uint16_t i = static_cast<uint16_t>(0); i != last_qualityind_.n; ++i)
342  {
343  if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(0))
344  {
345  qualityind_pos = i;
346  if(((last_qualityind_.indicators[i] & indicators_value_mask) >> 8) == static_cast<uint16_t>(0))
347  {
348  gnss_status->level = diagnostic_msgs::DiagnosticStatus::STALE;
349  }
350  else if(((last_qualityind_.indicators[i] & indicators_value_mask) >> 8) == static_cast<uint16_t>(1)
351  || ((last_qualityind_.indicators[i] & indicators_value_mask) >> 8)== static_cast<uint16_t>(2))
352  {
353  gnss_status->level = diagnostic_msgs::DiagnosticStatus::WARN;
354  }
355  else
356  {
357  gnss_status->level = diagnostic_msgs::DiagnosticStatus::OK;
358  }
359  break;
360  }
361  }
362  // If the ReceiverStatus's RxError field is not 0, then at least one error has been detected.
363  if(last_receiverstatus_.rx_error != static_cast<uint32_t>(0))
364  {
365  gnss_status->level = diagnostic_msgs::DiagnosticStatus::ERROR;
366  }
367  // Creating an array of values associated with the GNSS status
368  gnss_status->values.resize(static_cast<uint16_t>(last_qualityind_.n-1));
369  for(uint16_t i = static_cast<uint16_t>(0); i != static_cast<uint16_t>(last_qualityind_.n); ++i)
370  {
371  if(i == qualityind_pos)
372  {
373  continue;
374  }
375  if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(1))
376  {
377  gnss_status->values[i].key = "GNSS Signals, Main Antenna";
378  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
379  }
380  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(2))
381  {
382  gnss_status->values[i].key = "GNSS Signals, Aux1 Antenna";
383  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
384  }
385  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(11))
386  {
387  gnss_status->values[i].key = "RF Power, Main Antenna";
388  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
389  }
390  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(12))
391  {
392  gnss_status->values[i].key = "RF Power, Aux1 Antenna";
393  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
394  }
395  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(21))
396  {
397  gnss_status->values[i].key = "CPU Headroom";
398  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
399  }
400  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(25))
401  {
402  gnss_status->values[i].key = "OCXO Stability";
403  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
404  }
405  else if((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(30))
406  {
407  gnss_status->values[i].key = "Base Station Measurements";
408  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
409  }
410  else
411  {
412  assert ((last_qualityind_.indicators[i] & indicators_type_mask) == static_cast<uint16_t>(31));
413  gnss_status->values[i].key = "RTK Post-Processing";
414  gnss_status->values[i].value = std::to_string((last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
415  }
416  }
417  gnss_status->hardware_id = serialnumber;
418  gnss_status->name = "GNSS";
419  gnss_status->message = "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)";
420  msg->status.push_back(*gnss_status);
421  return msg;
422 }
423 
430 sensor_msgs::NavSatFixPtr io_comm_rx::RxMessage::NavSatFixCallback()
431 {
432  sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
433  uint16_t mask = 15; // We extract the first four bits using this mask.
434  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.mode)) & mask;
435  switch(type_of_pvt_map[type_of_pvt])
436  {
437  case evNoPVT:
438  {
439  msg->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
440  break;
441  }
442  case evStandAlone: case evFixed:
443  {
444  msg->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
445  break;
446  }
447  case evDGPS: case evRTKFixed: case evRTKFloat: case evMovingBaseRTKFixed: case evMovingBaseRTKFloat: case evPPP:
448  {
449  msg->status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
450  break;
451  }
452  case evSBAS:
453  {
454  msg->status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
455  break;
456  }
457  default:
458  {
459  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
460  }
461  }
462  bool gps_in_pvt = false;
463  bool glo_in_pvt = false;
464  bool com_in_pvt = false;
465  bool gal_in_pvt = false;
466  uint32_t mask_2 = 1;
467  for(int bit = 0; bit != 31; ++bit)
468  {
469  bool in_use = last_pvtgeodetic_.signal_info & mask_2;
470  if (bit <= 5 && in_use)
471  {
472  gps_in_pvt = true;
473  }
474  if (8 <= bit && bit <= 12 && in_use) glo_in_pvt = true;
475  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) com_in_pvt = true;
476  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) gal_in_pvt = true;
477  mask_2 *= 2;
478  }
479  // Below, booleans will be promoted to integers automatically.
480  uint16_t service = gps_in_pvt*1+glo_in_pvt*2+com_in_pvt*4+gal_in_pvt*8;
481  msg->status.service = service;
482  msg->latitude = last_pvtgeodetic_.latitude*360/(2*boost::math::constants::pi<double>());
483  msg->longitude = last_pvtgeodetic_.longitude*360/(2*boost::math::constants::pi<double>());
484  msg->altitude = last_pvtgeodetic_.height;
485  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.cov_lonlon);
486  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
487  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
488  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
489  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.cov_latlat);
490  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
491  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
492  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
493  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.cov_hgthgt);
494  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
495  return msg;
496 }
497 
520 {
521  gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
522 
523  msg->status.satellites_used = static_cast<uint16_t>(last_pvtgeodetic_.nr_sv);
524 
525  // MeasEpoch Processing
526  std::vector<int32_t> cno_tracked;
527  std::vector<int32_t> svid_in_sync;
528  {
529  uint8_t sb1_size = last_measepoch_.sb1_size;
530  uint8_t sb2_size = last_measepoch_.sb2_size;
531  uint8_t *sb_start = &last_measepoch_.data[0];
532  int32_t index = sb_start - &last_measepoch_.block_header.sync_1;
533  for (int32_t i = 0; i < static_cast<int32_t>(last_measepoch_.n); ++i)
534  {
535  // Define MeasEpochChannelType1 struct for the corresponding sub-block
536  MeasEpochChannelType1 *measepoch_channel_type1 =
537  reinterpret_cast<MeasEpochChannelType1*>(&last_measepoch_.block_header.sync_1 + index);
538  svid_in_sync.push_back(static_cast<int32_t>(measepoch_channel_type1->sv_id));
539  uint8_t type_mask = 15; // We extract the first four bits using this mask.
540  if (((measepoch_channel_type1->type & type_mask) == static_cast<uint8_t>(1)) ||
541  ((measepoch_channel_type1->type & type_mask) == static_cast<uint8_t>(2)))
542  {
543  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->cn0)/4);
544  }
545  else
546  {
547  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->cn0)/4+static_cast<int32_t>(10));
548  }
549  index += sb1_size;
550  for (int32_t j = 0; j < static_cast<int32_t>(measepoch_channel_type1->n_type2); j++)
551  {
552  index += sb2_size;
553  }
554  }
555  }
556 
557  // ChannelStatus Processing
558  std::vector<int32_t> svid_in_sync_2;
559  std::vector<int32_t> elevation_tracked;
560  std::vector<int32_t> azimuth_tracked;
561  std::vector<int32_t> svid_pvt;
562  svid_pvt.clear();
563  std::vector<int32_t> ordering;
564  {
565  uint8_t sb1_size = last_channelstatus_.sb1_size;
566  uint8_t sb2_size = last_channelstatus_.sb2_size;
567  uint8_t *sb_start = &last_channelstatus_.data[0];
568  int32_t index = sb_start - &last_channelstatus_.block_header.sync_1;
569  //ROS_DEBUG("index is %i", index); // yields 20, as expected
570 
571  uint16_t azimuth_mask = 511;
572  for (int32_t i = 0; i < static_cast<int32_t>(last_channelstatus_.n); i++)
573  {
574  // Define ChannelSatInfo struct for the corresponding sub-block
575  ChannelSatInfo *channel_sat_info =
576  reinterpret_cast<ChannelSatInfo*>(&last_channelstatus_.block_header.sync_1 + index);
577  bool to_be_added = false;
578  for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
579  {
580  if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info->sv_id))
581  {
582  ordering.push_back(j);
583  to_be_added = true;
584  break;
585  }
586  }
587  if (to_be_added)
588  {
589  svid_in_sync_2.push_back(static_cast<int32_t>(channel_sat_info->sv_id));
590  elevation_tracked.push_back(static_cast<int32_t>(channel_sat_info->elev));
591  azimuth_tracked.push_back(static_cast<int32_t>((channel_sat_info->az_rise_set & azimuth_mask)));
592  }
593  index += sb1_size;
594  for (int32_t j = 0; j < static_cast<int32_t>(channel_sat_info->n2); j++)
595  {
596  // Define ChannelStateInfo struct for the corresponding sub-block
597  ChannelStateInfo *channel_state_info =
598  reinterpret_cast<ChannelStateInfo*>(&last_channelstatus_.block_header.sync_1 + index);
599  bool pvt_status = false;
600  uint16_t pvt_status_mask = std::pow(2,15)+std::pow(2,14);
601  for(int k = 15; k != -1; k -= 2)
602  {
603  uint16_t pvt_status_value = (channel_state_info->pvt_status & pvt_status_mask) >> k-1;
604  if (pvt_status_value == 2)
605  {
606  pvt_status = true;
607  }
608  if (k > 1)
609  {
610  pvt_status_mask = pvt_status_mask - std::pow(2,k) - std::pow(2,k-1)
611  + std::pow(2,k-2) + std::pow(2,k-3);
612  }
613  }
614  if (pvt_status)
615  {
616  svid_pvt.push_back(static_cast<int32_t>(channel_sat_info->sv_id));
617  }
618  index += sb2_size;
619  }
620  }
621  }
622  msg->status.satellite_used_prn = svid_pvt; // Entries such as int32[] in ROS messages are to be treated as std::vectors.
623  msg->status.satellites_visible = static_cast<uint16_t>(svid_in_sync.size());
624  msg->status.satellite_visible_prn = svid_in_sync_2;
625  msg->status.satellite_visible_z = elevation_tracked;
626  msg->status.satellite_visible_azimuth = azimuth_tracked;
627 
628  // Reordering CNO vector to that of all previous arrays
629  std::vector<int32_t> cno_tracked_reordered;
630  if (static_cast<int32_t>(last_channelstatus_.n) != 0)
631  {
632  for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
633  {
634  cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
635  }
636  }
637  msg->status.satellite_visible_snr = cno_tracked_reordered;
638 
639  // PVT Status Analysis
640  uint16_t status_mask = 15; // We extract the first four bits using this mask.
641  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.mode)) & status_mask;
642  switch(type_of_pvt_map[type_of_pvt])
643  {
644  case evNoPVT:
645  {
646  msg->status.status = gps_common::GPSStatus::STATUS_NO_FIX;
647  break;
648  }
649  case evStandAlone: case evFixed:
650  {
651  msg->status.status = gps_common::GPSStatus::STATUS_FIX;
652  break;
653  }
654  case evDGPS: case evRTKFixed: case evRTKFloat: case evMovingBaseRTKFixed: case evMovingBaseRTKFloat: case evPPP:
655  {
656  msg->status.status = gps_common::GPSStatus::STATUS_GBAS_FIX;
657  break;
658  }
659  case evSBAS:
660  {
661  uint16_t reference_id = last_pvtgeodetic_.reference_id;
662  // Here come the PRNs of the 4 WAAS satellites..
663  if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135)
664  {
665  msg->status.status = gps_common::GPSStatus::STATUS_WAAS_FIX;
666  }
667  else
668  {
669  msg->status.status = gps_common::GPSStatus::STATUS_SBAS_FIX;
670  }
671  break;
672  }
673  default:
674  {
675  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
676  }
677  }
678  // Doppler is not used when calculating the velocities of, say, mosaic-x5, hence:
679  msg->status.motion_source = gps_common::GPSStatus::SOURCE_POINTS;
680  // Doppler is not used when calculating the orientation of, say, mosaic-x5, hence:
681  msg->status.orientation_source = gps_common::GPSStatus::SOURCE_POINTS;
682  msg->status.position_source = gps_common::GPSStatus::SOURCE_GPS;
683  msg->latitude = static_cast<double>(last_pvtgeodetic_.latitude)*360/(2*boost::math::constants::pi<double>());
684  msg->longitude = static_cast<double>(last_pvtgeodetic_.longitude)*360/(2*boost::math::constants::pi<double>());
685  msg->altitude = static_cast<double>(last_pvtgeodetic_.height);
686  // Note that cog is of type float32 while track is of type float64.
687  msg->track = static_cast<double>(last_pvtgeodetic_.cog);
688  msg->speed = std::sqrt(std::pow(static_cast<double>(last_pvtgeodetic_.vn), 2) +
689  std::pow(static_cast<double>(last_pvtgeodetic_.ve), 2));
690  msg->climb = static_cast<double>(last_pvtgeodetic_.vu);
691  msg->pitch = static_cast<double>(last_atteuler_.pitch);
692  msg->roll = static_cast<double>(last_atteuler_.roll);
693  if (last_dop_.pdop == static_cast<uint16_t>(0) || last_dop_.tdop == static_cast<uint16_t>(0))
694  {
695  msg->gdop = static_cast<double>(-1);
696  }
697  else
698  {
699  msg->gdop = std::sqrt(std::pow(static_cast<double>(last_dop_.pdop)/100, 2) +
700  std::pow(static_cast<double>(last_dop_.tdop)/100, 2));
701  }
702  if (last_dop_.pdop == static_cast<uint16_t>(0))
703  {
704  msg->pdop = static_cast<double>(-1);
705  }
706  else
707  {
708  msg->pdop = static_cast<double>(last_dop_.pdop)/100;
709  }
710  if (last_dop_.hdop == static_cast<uint16_t>(0))
711  {
712  msg->hdop = static_cast<double>(-1);
713  }
714  else
715  {
716  msg->hdop = static_cast<double>(last_dop_.hdop)/100;
717  }
718  if (last_dop_.vdop == static_cast<uint16_t>(0))
719  {
720  msg->vdop = static_cast<double>(-1);
721  }
722  else
723  {
724  msg->vdop = static_cast<double>(last_dop_.vdop)/100;
725  }
726  if (last_dop_.tdop == static_cast<uint16_t>(0))
727  {
728  msg->tdop = static_cast<double>(-1);
729  }
730  else
731  {
732  msg->tdop = static_cast<double>(last_dop_.tdop)/100;
733  }
734  msg->time = static_cast<double>(last_pvtgeodetic_.tow)/1000 + static_cast<double>(last_pvtgeodetic_.wnc*7*24*60*60);
735  msg->err = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
736  static_cast<double>(last_poscovgeodetic_.cov_lonlon) + static_cast<double>(last_poscovgeodetic_.cov_hgthgt)));
737  msg->err_horz = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
738  static_cast<double>(last_poscovgeodetic_.cov_lonlon)));
739  msg->err_vert = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_hgthgt));
740  msg->err_track = 2*(std::sqrt(std::pow(static_cast<double>(1)/(static_cast<double>(last_pvtgeodetic_.vn) +
741  std::pow(static_cast<double>(last_pvtgeodetic_.ve),2)/static_cast<double>(last_pvtgeodetic_.vn)),2)*
742  static_cast<double>(last_poscovgeodetic_.cov_lonlon)+std::pow((static_cast<double>(last_pvtgeodetic_.ve))/
743  (std::pow(static_cast<double>(last_pvtgeodetic_.vn),2)+std::pow(static_cast<double>(last_pvtgeodetic_.ve),2)),2)*
744  static_cast<double>(last_poscovgeodetic_.cov_latlat)));
745  msg->err_speed = 2*(std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vnvn) +
746  static_cast<double>(last_velcovgeodetic_.cov_veve)));
747  msg->err_climb = 2*std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vuvu));
748  msg->err_time = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_bb));
749  msg->err_pitch = 2*std::sqrt(static_cast<double>(last_attcoveuler_.cov_pitchpitch));
750  msg->err_roll = 2*std::sqrt(static_cast<double>(last_attcoveuler_.cov_rollroll));
751  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.cov_lonlon);
752  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
753  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
754  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.cov_latlon);
755  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.cov_latlat);
756  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
757  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.cov_lonhgt);
758  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.cov_lathgt);
759  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.cov_hgthgt);
760  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
761 
762  return msg;
763 }
764 
768 ros::Time io_comm_rx::timestampSBF(uint32_t tow, bool use_gnss)
769 {
770  if (use_gnss)
771  {
772  uint16_t hh = (tow%(1000*60*60*24))/(60*60*1000);
773  uint16_t mm = ((tow%(1000*60*60*24))-hh*(60*60*1000))/(60*1000);
774  uint16_t ss = ((tow%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000))/(1000);
775  uint16_t hs = ((tow%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000)-ss*1000)/10; // hundredths of a second
776  if (ss >= g_leap_seconds)
777  {
778  ss = ss - g_leap_seconds;
779  }
780  else
781  {
782  if (mm >= 1)
783  {
784  --mm;
785  ss = 60 - (g_leap_seconds - ss);
786  }
787  else
788  {
789  if (hh >= 1)
790  {
791  --hh;
792  mm = 59;
793  ss = 60 - (g_leap_seconds - ss);
794  }
795  else
796  {
797  hh = 23;
798  mm = 59;
799  ss = 60 - (g_leap_seconds - ss);
800  }
801  }
802  }
803  boost::format fmt = boost::format("%02u%02u%02u.%02u") % hh % mm % ss % hs;
804  std::string utc_string = fmt.str();
805  //ROS_DEBUG("UTC string is %s", utc_string.c_str());
806  double utc_double;
807  string_utilities::toDouble(utc_string, utc_double);
808  time_t unix_time_seconds = parsing_utilities::convertUTCtoUnix(utc_double); // This only deals with full seconds.
809  // The following works since there are two digits after the decimal point in the utc_double:
810  uint32_t unix_time_nanoseconds = (static_cast<uint32_t>(utc_double*100)%100)*10000000;
811  ros::Time time_obj(unix_time_seconds, unix_time_nanoseconds);
812  return time_obj;
813  }
814  else
815  {
816  return ros::Time::now();
817  }
818 }
819 
821 {
822  if (found_) return true;
823 
824  // Verify header bytes
825  if (!this->isSBF() && !this->isNMEA() && !this->isResponse() && !(g_read_cd && this->isConnectionDescriptor()))
826  {
827  return false;
828  }
829 
830  found_ = true;
831  return true;
832 }
833 
835 {
836  if (found_)
837  {
838  next();
839  }
840  // Search for message or a response header
841  for( ; count_ > 0; --count_, ++data_)
842  {
843  if (this->isSBF() || this->isNMEA() || this->isResponse() || (g_read_cd && this->isConnectionDescriptor()))
844  {
845  break;
846  }
847  }
848  found_ = true;
849  return data_;
850 }
851 
853 {
854  uint16_t pos = 0;
855  message_size_ = 0;
856  std::size_t count_copy = count_;
857  if (this->isResponse())
858  {
859  do
860  {
861  ++message_size_;
862  ++pos;
863  --count_copy;
864  if (count_copy == 0) break;
865  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED)) ||
866  (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20
867  && data_[pos+3] == 0x20 && data_[pos+4] == 0x4E) || (data_[pos] == CARRIAGE_RETURN &&
868  data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x53) ||
869  (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20
870  && data_[pos+4] == 0x52));
871  }
872  else
873  {
874  do
875  {
876  ++message_size_;
877  ++pos;
878  --count_copy;
879  if (count_copy == 0) break;
880  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED) || data_[pos] == CARRIAGE_RETURN
881  || data_[pos] == LINE_FEED));
882  }
883  return message_size_;
884 }
885 
886 bool io_comm_rx::RxMessage::isMessage(const uint16_t id)
887 {
888  if (this->isSBF())
889  {
890  uint16_t mask = 8191;
891  if (*(reinterpret_cast<const uint16_t *>(data_ + 4)) & mask == static_cast<const uint16_t>(id))
892  // Caution: reinterpret_cast is the most dangerous cast. It's used primarily for particularly
893  // weird conversions and bit manipulations, like turning a raw data stream into actual data.
894  {
895  return true;
896  }
897  else
898  {
899  return false;
900  }
901  }
902  else
903  {
904  return false;
905  }
906 }
907 
908 
910 {
911  if (this->isNMEA())
912  {
913  boost::char_separator<char> sep(",");
914  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
915  std::size_t nmea_size = this->messageSize();
916  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
917  tokenizer tokens(block_in_string,sep);
918  if (*tokens.begin() == id)
919  {
920  return true;
921  }
922  else
923  {
924  return false;
925  }
926  }
927  else
928  {
929  return false;
930  }
931 }
932 
934 {
935  if (count_ >= 2)
936  {
937  if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2)
938  {
939  return true;
940  }
941  else
942  {
943  return false;
944  }
945  }
946  else
947  {
948  return false;
949  }
950 }
951 
953 {
954  if (count_ >= 2)
955  {
957  && data_[1] == NMEA_SYNC_BYTE_2_2))
958  {
959  return true;
960  }
961  else
962  {
963  return false;
964  }
965  }
966  else
967  {
968  return false;
969  }
970 }
971 
973 {
974  if (count_ >= 2)
975  {
977  {
978  return true;
979  }
980  else
981  {
982  return false;
983  }
984  }
985  else
986  {
987  return false;
988  }
989 }
990 
992 {
993  if (count_ >= 2)
994  {
996  {
997  return true;
998  }
999  else
1000  {
1001  return false;
1002  }
1003  }
1004  else
1005  {
1006  return false;
1007  }
1008 }
1009 
1011 {
1012  if (count_ >= 3)
1013  {
1015  {
1016  return true;
1017  }
1018  else
1019  {
1020  return false;
1021  }
1022  }
1023  else
1024  {
1025  return false;
1026  }
1027 }
1028 
1030 {
1031  if (this->isSBF())
1032  {
1033  // Defines bit mask..
1034  // It is not as stated in the firmware: !first! three bits are for revision (not last 3), and rest for block number
1035  uint16_t mask = 8191;
1036  // Bitwise AND gives us all but first 3 bits set to zero, rest unchanged
1037  uint16_t value = (*(reinterpret_cast<const uint16_t*>(data_+4))) & mask;
1038  std::stringstream ss;
1039  ss << value;
1040  return ss.str();
1041  }
1042  if (this->isNMEA())
1043  {
1044  boost::char_separator<char> sep(",");
1045  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
1046  std::size_t nmea_size = this->messageSize();
1047  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
1048  tokenizer tokens(block_in_string,sep);
1049  return *tokens.begin();
1050  }
1051  return std::string(); // less CPU work than return "";
1052 }
1053 
1054 
1055 
1057 {
1058  return data_;
1059 }
1060 
1062 {
1063  return data_ + count_;
1064 }
1065 
1067 {
1068  if (this->isSBF())
1069  {
1070  uint16_t block_length;
1071  // Note that static_cast<uint16_t>(data_[6]) would just take the one byte "data_[6]"
1072  // and cast it as requested, !neglecting! the byte "data_[7]".
1073  block_length = *(reinterpret_cast<const uint16_t*>(data_ + 6));
1074  return block_length;
1075  }
1076  else
1077  {
1078  return 0;
1079  }
1080 }
1081 
1087 {
1088  std::size_t jump_size;
1089  if (found())
1090  {
1091  if (this->isNMEA() || this->isResponse() || (g_read_cd && this->isConnectionDescriptor()))
1092  {
1093  if (g_read_cd && this->isConnectionDescriptor() && g_cd_count == 2)
1094  {
1095  g_read_cd = false;
1096  }
1097  jump_size = static_cast<uint32_t>(1);
1098  }
1099  if (this->isSBF())
1100  {
1101  if (crc_check_)
1102  {
1103  jump_size = static_cast<std::size_t>(this->getBlockLength());
1104  // Some corrupted messages that survive the CRC check (this happened) could tell ROSaic their size is 0,
1105  // which would lead to an endless while loop in the ReadCallback() method of the CallbackHandlers class.
1106  if (jump_size == 0) jump_size = static_cast<std::size_t>(1);
1107  }
1108  else
1109  {
1110  jump_size = static_cast<std::size_t>(1);
1111  }
1112  }
1113  }
1114  found_ = false;
1115  data_ += jump_size; count_ -= jump_size;
1116  //ROS_DEBUG("Jump about to happen with jump size %li and count after jump being %li.", jump_size, count_);
1117  return; // For readability
1118 }
uint16_t pdop
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
Definition: rx_message.cpp:820
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Definition: rx_message.hpp:323
uint8_t sb2_size
Struct for the SBF block "AttCovEuler".
std::map< uint16_t, TypeOfPVT_Enum > TypeOfPVTMap
Shorthand for the map responsible for matching PVTGeodetic&#39;s Mode field to an enum value...
Definition: rx_message.hpp:376
float cov_headroll
uint16_t mode
uint16_t tdop
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
Definition: rx_message.cpp:430
uint8_t time_system
float cov_rollroll
uint16_t reference_id
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
Definition: rx_message.hpp:89
uint8_t wa_corr_info
uint8_t time_system
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
Definition: rx_message.hpp:69
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
double rx_clk_bias
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Definition: rx_message.hpp:328
BlockHeader_t block_header
uint16_t length
Length of the entire message including the header. A multiple of 4 between 8 and 4096.
uint16_t vdop
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
Definition: rx_message.cpp:100
bool toDouble(const std::string &string, double &value)
Interprets the contents of "string" as a floating point number of type double It stores the "string"&#39;...
static DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
Definition: rx_message.hpp:353
BlockHeader_t block_header
float cov_headpitch
static MeasEpoch last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Definition: rx_message.hpp:348
uint16_t v_accuracy
bool isResponse()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:972
uint16_t wnc
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
Definition: rx_message.cpp:179
char rx_serial_number[20]
uint16_t v_accuracy
uint8_t sb1_size
Struct for the SBF block "VelCovGeodetic".
Struct for the SBF sub-block "MeasEpochChannelType1".
Struct for the SBF block "ReceiverSetup".
#define SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Definition: rx_message.hpp:65
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
Definition: rx_message.cpp:205
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
Definition: rx_message.cpp:519
uint16_t crc
The check sum.
Struct for the SBF block "PosCovGeodetic".
float rx_clk_drift
std::string messageID()
float heading_dot
uint16_t latency
uint8_t n
static ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
Definition: rx_message.hpp:343
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
Definition: rx_message.cpp:140
uint16_t ppp_info
uint32_t signal_info
uint8_t misc
void next()
Goes to the start of the next message based on the calculated length of current message.
uint8_t sb2_size
uint8_t nr_bases
float roll_dot
#define NMEA_SYNC_BYTE_2_1
0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message
Definition: rx_message.hpp:73
uint16_t ppp_info
double latitude
uint16_t wnc
uint8_t nr_sv
uint32_t signal_info
Struct for the SBF block "PVTGeodetic".
#define RESPONSE_SYNC_BYTE_3
0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the command was invalid ...
Definition: rx_message.hpp:97
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Definition: rx_message.hpp:333
uint8_t sync_1
first sync byte is $ or 0x24
std::map< std::string, RxID_Enum > RxIDMap
Shorthand for the map responsible for matching ROS message identifiers to an enum value...
Definition: rx_message.hpp:384
Struct for the SBF block "AttEuler".
uint16_t h_accuracy
BlockHeader_t block_header
Defines a class that reads messages handed over from the circular buffer.
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
Definition: rx_message.cpp:230
float rx_clk_drift
uint8_t sb1_size
uint16_t reference_id
bool isErrorMessage()
Determines whether data_ currently points to an error message reply from the Rx.
float pitch_dot
std::size_t count_
Number of unread bytes in the buffer.
Definition: rx_message.hpp:303
uint32_t tow
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:952
Struct for the SBF block "QualityInd".
BlockHeader_t block_header
double longitude
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
Definition: rx_message.hpp:358
bool isMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.
Definition: rx_message.cpp:886
uint32_t tow
uint16_t mean_corr_age
uint8_t alert_flag
Struct for the SBF sub-block "ChannelStateInfo".
static TypeOfPVTMap type_of_pvt_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:381
static ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored...
Definition: rx_message.hpp:363
Struct for the SBF block "PVTCartesian".
float cov_pitchroll
uint16_t getBlockLength()
Gets the length of the SBF block.
uint16_t indicators[40]
ros::Time timestampSBF(uint32_t tow, bool use_gnss)
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
Definition: rx_message.cpp:768
Struct for the SBF block "PosCovCartesian".
bool g_read_cd
uint8_t error
uint8_t alert_flag
uint8_t error
std::size_t messageSize()
Determines size of the message (also command reply) that data_ is currently pointing at...
Definition: rx_message.cpp:852
#define CONNECTION_DESCRIPTOR_BYTE_1
0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after initiating TCP conn...
Definition: rx_message.hpp:101
float heading
float pitch
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
Definition: rx_message.cpp:252
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
uint16_t az_rise_set
BlockHeader_t block_header
uint32_t tow
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
Definition: rx_message.hpp:318
Struct for the SBF sub-block "ChannelSatInfo".
Struct for the SBF block "ChannelStatus".
Struct for the SBF block "MeasEpoch".
std::pair< uint16_t, TypeOfPVT_Enum > type_of_pvt_pairs[]
Pair of iterators to facilitate initialization of the map.
Definition: rx_message.cpp:53
double rx_clk_bias
float undulation
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
Definition: rx_message.hpp:77
diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback()
"Callback" function when constructing diagnostic_msgs::DiagnosticArray messages
Definition: rx_message.cpp:332
BlockHeader_t block_header
Struct for the SBF block "DOP".
#define SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
Definition: rx_message.hpp:61
uint32_t tow
float cov_headhead
uint16_t id
This is the block ID.
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from the Rx
Definition: rx_message.hpp:81
std::pair< std::string, RxID_Enum > rx_id_pairs[]
Pair of iterators to facilitate initialization of the map.
Definition: rx_message.cpp:70
uint16_t hdop
uint16_t h_accuracy
time_t convertUTCtoUnix(double utc_double)
Converts UTC time from the without-colon-delimiter format to Unix Epoch time (a number-of-seconds-sin...
uint8_t data[((80+1) *sizeof(MeasEpochChannelType1)+(((80+1)) *(((7) *(3)) -1)) *sizeof(MeasEpochChannelType2))]
uint32_t rx_error
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
Definition: rx_message.cpp:282
uint8_t datum
static ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored...
Definition: rx_message.hpp:373
bool isSBF()
Determines whether data_ currently points to an SBF block.
Definition: rx_message.cpp:933
bool found_
Whether or not a message has been found.
Definition: rx_message.hpp:291
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
std::size_t message_size_
Helps to determine size of response message / NMEA message / SBF block.
Definition: rx_message.hpp:313
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Definition: rx_message.cpp:834
BlockHeader_t block_header
uint8_t data[(80+1) *sizeof(ChannelSatInfo)+((80+1) *4) *sizeof(ChannelStateInfo)]
uint8_t sync_2
2nd sync byte is @ or 0x40
static QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
Definition: rx_message.hpp:368
uint8_t nr_sv
const uint8_t * data_
Pointer to the buffer of messages.
Definition: rx_message.hpp:298
static RxIDMap rx_id_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:392
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
Definition: rx_message.hpp:338
uint16_t latency
#define LINE_FEED
0x0A is ASCII for "Line Feed", i.e. "New Line"
Definition: rx_message.hpp:93
float roll
float cov_pitchpitch
uint16_t mean_corr_age
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx
Definition: rx_message.hpp:85
uint8_t nr_bases
BlockHeader_t block_header
uint8_t mode
uint8_t wa_corr_info
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
geometry_msgs::Quaternion convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
#define CONNECTION_DESCRIPTOR_BYTE_2
0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after initiating TCP conn...
Definition: rx_message.hpp:105
uint16_t wnc
bool crc_check_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Definition: rx_message.hpp:308
uint8_t error
uint16_t wnc
Struct for the SBF block "ReceiverStatus".