MRPT  1.9.9
gnss_messages_novatel.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <map>
14 #include <iostream>
15 
16 using namespace std;
17 using namespace mrpt::obs::gnss;
18 
19 nv_oem6_header_t::nv_oem6_header_t()
20  : hdr_len(0),
21  msg_id(0),
22  msg_type(0),
23  port_addr(0),
24  msg_len(0),
25  seq_number(0),
26  idle_percent(0),
27  time_status(0),
28  week(0),
29  ms_in_week(0),
30  receiver_status(0),
31  reserved(0),
32  receiver_sw_version(0)
33 {
34  synch[0] = synch[1] = synch[2] = 0;
35 }
36 
38  : msg_len(), msg_id(), week(), ms_in_week()
39 {
40  synch[0] = synch[1] = synch[2] = 0;
41 }
42 
43 // ------------
44 void Message_NV_OEM6_GENERIC_FRAME::dumpToStream(std::ostream& out) const
45 {
46  out << mrpt::format(
47  "[Novatel OEM6 GENERIC FRAME]\n"
48  " Message ID: %u\n",
49  (unsigned)this->header.msg_id);
50 }
53 {
54  out.WriteBuffer(&header, sizeof(header));
55  out << static_cast<uint32_t>(msg_body.size());
56  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
57 }
60 {
61  in.ReadBuffer(&header, sizeof(header));
62  uint32_t nBytesInStream;
63  in >> nBytesInStream;
64  msg_body.resize(nBytesInStream);
65  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
66 }
67 // ------------
69 {
70  out << mrpt::format("[Novatel OEM6 GENERIC SHORT FRAME]\n");
71  out << mrpt::format(" Message ID: %u\n", (unsigned)this->header.msg_id);
72 }
75 {
76  out.WriteBuffer(&header, sizeof(header));
77  out << static_cast<uint32_t>(msg_body.size());
78  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
79 }
82 {
83  in.ReadBuffer(&header, sizeof(header));
84  uint32_t nBytesInStream;
85  in >> nBytesInStream;
86  msg_body.resize(nBytesInStream);
87  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
88 }
89 
90 // ------------
92 {
93  static bool init_map = false;
94  static std::map<int, std::string> val2str;
95  if (!init_map)
96  {
97  init_map = true;
98 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
118 #undef DEF_TYPESTR
119  }
121  static const std::string nullstr("???");
122  return (it == val2str.end()) ? nullstr : it->second;
123 }
124 
126 {
127  static bool init_map = false;
128  static std::map<int, std::string> val2str;
129  if (!init_map)
130  {
131  init_map = true;
132 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
161 #undef DEF_TYPESTR
162  }
164  static const std::string nullstr("???");
165  return (it == val2str.end()) ? nullstr : it->second;
166 }
167 
169 {
170  static bool init_map = false;
171  static std::map<int, std::string> val2str;
172  if (!init_map)
173  {
174  init_map = true;
175 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
184 #undef DEF_TYPESTR
185  }
187  static const std::string nullstr("???");
188  return (it == val2str.end()) ? nullstr : it->second;
189 }
190 
192  const Message_NV_OEM6_BESTPOS::content_t& fields, std::ostream& out)
193 {
194  out << mrpt::format(
195  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
196  (unsigned)(fields.header.ms_in_week));
197  out << mrpt::format(
198  " Solution status: `%s`\n",
199  nv_oem6_solution_status::enum2str(fields.solution_stat).c_str());
200  out << mrpt::format(
201  " Position type : `%s`\n",
202  nv_oem6_position_type::enum2str(fields.position_type).c_str());
203  out << mrpt::format(
204  " Longitude: %.09f deg (std dev: %.06f m) Latitude: %.09f deg (std "
205  "dev: %.06f m)\n",
206  fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
207  out << mrpt::format(
208  " Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m "
209  "(Sum: %.04f m)\n",
210  fields.hgt, fields.hgt_sigma, fields.undulation,
211  fields.hgt + fields.undulation);
212  out << mrpt::format(
213  " Diff age: %.03f Solution age: %.03f\n", fields.diff_age,
214  fields.sol_age);
215  out << mrpt::format(
216  " Base station ID: `%.*s`\n", 4, fields.base_station_id);
217  out << mrpt::format(
218  " Num sat tracked: %u Num sat in solution: %u\n",
219  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
220 }
221 
222 void Message_NV_OEM6_BESTPOS::dumpToStream(std::ostream& out) const
223 {
224  out << "[Novatel OEM6 BESTPOS]\n";
225  generic_dump_BESTPOS(fields, out);
226 }
227 
228 bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions(std::ostream& o) const
229 {
230  o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m "
231  "undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age "
232  "num_sats_tracked num_sats_sol";
233  return true;
234 }
235 bool Message_NV_OEM6_BESTPOS::getAllFieldValues(std::ostream& o) const
236 {
237  o << mrpt::format(
238  "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f "
239  "%u %u",
240  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
241  (unsigned)fields.solution_stat, (unsigned)fields.position_type,
242  fields.lon, fields.lat, fields.hgt, fields.undulation, fields.lon_sigma,
243  fields.lat_sigma, fields.hgt_sigma, fields.diff_age, fields.sol_age,
244  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
245  return true;
246 }
247 
248 // ------------
249 void Message_NV_OEM6_INSPVAS::dumpToStream(std::ostream& out) const
250 {
251  out << mrpt::format("[Novatel OEM6 INSPVAS]\n");
252  out << mrpt::format(
253  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
254  (unsigned)(fields.header.ms_in_week));
255  out << mrpt::format(
256  " INS status: `%s`\n",
257  nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
258  out << mrpt::format(
259  " Longitude: %.09f deg Latitude: %.09f deg Height: %.03f m\n",
260  fields.lon, fields.lat, fields.hgt);
261  out << mrpt::format(
262  " Velocities: North: %.05f East: %.05f Up: %.05f\n", fields.vel_north,
263  fields.vel_east, fields.vel_up);
264  out << mrpt::format(
265  " Attitude: Roll: %.05f Pitch: %.05f Azimuth: %.05f\n", fields.roll,
266  fields.pitch, fields.azimuth);
267 }
268 
269 bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions(std::ostream& o) const
270 {
271  o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 "
272  "vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
273  return true;
274 }
275 bool Message_NV_OEM6_INSPVAS::getAllFieldValues(std::ostream& o) const
276 {
277  o << mrpt::format(
278  "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
279  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
280  (unsigned)fields.ins_status, fields.lon, fields.lat, fields.hgt,
281  fields.vel_north, fields.vel_east, fields.vel_up, fields.roll,
282  fields.pitch, fields.azimuth);
283  return true;
284 }
285 
286 // ------------
287 void Message_NV_OEM6_INSCOVS::dumpToStream(std::ostream& out) const
288 {
289  out << mrpt::format("[Novatel OEM6 INSCOVS]\n");
290  out << mrpt::format(
291  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
292  (unsigned)(fields.header.ms_in_week));
293  out << mrpt::format(
294  " Position cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
295  "%9.03f %9.03f\n",
296  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
297  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
298  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8]);
299  out << mrpt::format(
300  " Attitude cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
301  "%9.03f %9.03f\n",
302  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
303  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
304  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8]);
305  out << mrpt::format(
306  " Velocity cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
307  "%9.03f %9.03f\n",
308  fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
309  fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
310  fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
311 }
312 
313 bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions(std::ostream& o) const
314 {
315  o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
316  return true;
317 }
318 bool Message_NV_OEM6_INSCOVS::getAllFieldValues(std::ostream& o) const
319 {
320  o << mrpt::format(
321  "%u.%08u "
322  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
323  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
324  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f ",
325  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
326  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
327  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
328  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8],
329  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
330  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
331  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8],
332  fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
333  fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
334  fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
335  return true;
336 }
337 
338 // ------------
339 void Message_NV_OEM6_RANGECMP::dumpToStream(std::ostream& out) const
340 {
341  out << mrpt::format("[Novatel OEM6 RANGECMP]\n");
342  out << mrpt::format(
343  " Number of SAT observations: %u\n",
344  static_cast<unsigned int>(this->num_obs));
345 }
346 
349 {
350  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
351  out << msg_len;
352  out.WriteBuffer(&header, sizeof(header));
353  out << num_obs;
354  ASSERT_EQUAL_(num_obs, obs_data.size());
355  if (num_obs)
356  out.WriteBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
357 }
358 
361 {
362  uint32_t expected_msg_len;
363  in >> expected_msg_len;
364  in.ReadBuffer(&header, sizeof(header));
365  in >> num_obs;
366  ASSERT_BELOW_(num_obs, 2000);
367  obs_data.resize(num_obs);
368  if (num_obs)
369  in.ReadBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
370 }
371 
372 // ------------
373 void Message_NV_OEM6_RXSTATUS::dumpToStream(std::ostream& out) const
374 {
375  out << mrpt::format("[Novatel OEM6 RXSTATUS]\n");
376  out << mrpt::format(
377  " Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
378 }
379 
380 // ------------
381 void Message_NV_OEM6_RAWEPHEM::dumpToStream(std::ostream& out) const
382 {
383  out << mrpt::format("[Novatel OEM6 RAWEPHEM]\n");
384  out << mrpt::format(
385  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
386  (unsigned)(fields.header.ms_in_week));
387 }
388 
389 // ------------
390 void Message_NV_OEM6_VERSION::dumpToStream(std::ostream& out) const
391 {
392  out << mrpt::format("[Novatel OEM6 VERSION]\n");
393  out << mrpt::format(
394  " Number of components: %u\n",
395  static_cast<unsigned int>(this->num_comps));
396  for (unsigned i = 0; i < components.size(); i++)
397  {
398  out << mrpt::format(
399  " Component #%u:\n Model: `%.*s`\n Serial: `%.*s`\n SW "
400  "version:`%.*s`\n",
401  i, (int)sizeof(components[i].model), components[i].model,
402  (int)sizeof(components[i].serial), components[i].serial,
403  (int)sizeof(components[i].swversion), components[i].swversion);
404  }
405 }
406 
409 {
410  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
411  out << msg_len;
412  out.WriteBuffer(&header, sizeof(header));
413  out << num_comps;
415  if (num_comps)
416  out.WriteBuffer(
417  &components[0], sizeof(components[0]) * components.size());
418 }
419 
422 {
423  uint32_t expected_msg_len;
424  in >> expected_msg_len;
425  in.ReadBuffer(&header, sizeof(header));
426  in >> num_comps;
427  ASSERT_BELOW_(num_comps, 2000);
428  components.resize(num_comps);
429  if (num_comps)
430  in.ReadBuffer(
431  &components[0], sizeof(components[0]) * components.size());
432 }
433 
434 // ------------
435 void Message_NV_OEM6_RAWIMUS::dumpToStream(std::ostream& out) const
436 {
437  out << mrpt::format("[Novatel OEM6 RAWIMUS]\n");
438  out << mrpt::format(
439  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
440  (unsigned)(fields.header.ms_in_week));
441  out << mrpt::format(" Status: 0x%08lu\n", (long)fields.imu_status);
442  out << mrpt::format(
443  " Acel: X=%li Y=%li Z=%li\n", (long)fields.accel_x,
444  -(long)fields.accel_y_neg, (long)fields.accel_z);
445  out << mrpt::format(
446  " Gyro: X=%li Y=%li Z=%li\n", (long)fields.gyro_x,
447  -(long)fields.gyro_y_neg, (long)fields.gyro_z);
448 }
449 
450 bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions(std::ostream& o) const
451 {
452  o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
453  return true;
454 }
455 bool Message_NV_OEM6_RAWIMUS::getAllFieldValues(std::ostream& o) const
456 {
457  o << mrpt::format(
458  "%u.%08u %u %li %li %li %li %li %li", (unsigned)fields.header.week,
459  (unsigned)(fields.header.ms_in_week), (unsigned)fields.imu_status,
460  (long)fields.accel_x, -(long)fields.accel_y_neg, (long)fields.accel_z,
461  (long)fields.gyro_x, -(long)fields.gyro_y_neg, (long)fields.gyro_z);
462  return true;
463 }
464 
465 // ------------
467  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& out)
468 {
469  out << mrpt::format(" Clock status: 0x%08lu\n", (long)fields.clock_status);
470  out << mrpt::format(
471  " GPS week: %lu Seconds: %f\n", (long)fields.week, fields.week_seconds);
472  out << mrpt::format(
473  " Clock offset: %f (std dev = %e)\n", fields.clock_offset,
474  fields.clock_offset_std);
475  out << mrpt::format(" UTC offset: %f\n", fields.utc_offset);
476 }
477 
478 void Message_NV_OEM6_MARKTIME::dumpToStream(std::ostream& out) const
479 {
480  out << mrpt::format("[Novatel OEM6 MARKTIME]\n");
481  generic_dump_MARKTIME(fields, out);
482 }
483 
484 bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions(std::ostream& o) const
485 {
486  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
487  return true;
488 }
490  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& o)
491 {
492  o << mrpt::format(
493  "%u.%08u %u %lu %f %f", (unsigned)fields.header.week,
494  (unsigned)(fields.header.ms_in_week), (unsigned)fields.clock_status,
495  (long unsigned)fields.week, fields.week_seconds, fields.utc_offset);
496 }
497 bool Message_NV_OEM6_MARKTIME::getAllFieldValues(std::ostream& o) const
498 {
500  return true;
501 }
502 // ------------
503 void Message_NV_OEM6_MARK2TIME::dumpToStream(std::ostream& out) const
504 {
505  out << mrpt::format("[Novatel OEM6 MARK2TIME]\n");
507  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
508  out);
509 }
510 bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions(std::ostream& o) const
511 {
512  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
513  return true;
514 }
515 bool Message_NV_OEM6_MARK2TIME::getAllFieldValues(std::ostream& o) const
516 {
518  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
519  o);
520  return true;
521 }
522 
523 // ------------
524 void Message_NV_OEM6_MARKPOS::dumpToStream(std::ostream& out) const
525 {
526  out << mrpt::format("[Novatel OEM6 MARKPOSE]\n");
528  *reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields),
529  out);
530 }
531 
532 // ------------
533 void Message_NV_OEM6_IONUTC::dumpToStream(std::ostream& out) const
534 {
535  out << mrpt::format("[Novatel NV_OEM6_IONUTC]\n");
536  out << mrpt::format(
537  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
538  (unsigned)(fields.header.ms_in_week));
539  out << mrpt::format(
540  " UTC ref week: %u Tot: %u\n", (unsigned)fields.utc_wn,
541  (unsigned)fields.tot);
542  out << mrpt::format(
543  " Leap seconds delta_t: %u future: %u\n", (unsigned)fields.deltat_ls,
544  (unsigned)fields.deltat_lsf);
545 }
void internal_readFromStream(mrpt::serialization::CArchive &in) override
Save to binary stream.
const std::string & enum2str(int val)
for nv_ins_status_type_t
void dumpToStream(std::ostream &out) const override
Dumps the contents of the observation in a human-readable form to a given output stream.
void dumpToStream(std::ostream &out) const override
Dumps the contents of the observation in a human-readable form to a given output stream.
_u32 reserved
Definition: rplidar_cmd.h:20
test distance exceeded (max of 3 rejections if distance > 10km)
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
when a fix position command is entered, the receiver computes its own position and determines if the ...
void dumpToStream(std::ostream &out) const override
Dumps the contents of the observation in a human-readable form to a given output stream.
const std::string & enum2str(int val)
for nv_position_type_t
STL namespace.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:48
GLenum GLenum GLuint components
Definition: glext.h:7282
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
Save to binary stream.
const std::string & enum2str(int val)
for nv_solution_status_t
void internal_readFromStream(mrpt::serialization::CArchive &in) override
Save to binary stream.
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
Save to binary stream.
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
Save to binary stream.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
_u8 model
Definition: rplidar_cmd.h:19
void generic_dump_BESTPOS(const Message_NV_OEM6_BESTPOS::content_t &fields, std::ostream &out)
int val
Definition: mrpt_jpeglib.h:955
void generic_dump_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, std::ostream &out)
void internal_readFromStream(mrpt::serialization::CArchive &in) override
Save to binary stream.
GLsizei const GLchar ** string
Definition: glext.h:4101
GNSS (GPS) data structures, mainly for use within mrpt::obs::CObservationGPS.
std::vector< TCompressedRangeLog > obs_data
bool getAllFieldDescriptions(std::ostream &o) const override
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLuint in
Definition: glext.h:7274
void generic_getFieldValues_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, std::ostream &o)
covariance trace exceeds maximum (trace>1000m)
the fixed position entered using the fix position command is not valid
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
Save to binary stream.
#define DEF_TYPESTR(_NAME)
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool getAllFieldValues(std::ostream &o) const override
const Scalar * const_iterator
Definition: eigen_plugins.h:27
void internal_readFromStream(mrpt::serialization::CArchive &in) override
Save to binary stream.
void dumpToStream(std::ostream &out) const override
Dumps the contents of the observation in a human-readable form to a given output stream.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020