MRPT  1.9.9
gnss_messages_novatel.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <iostream>
14 #include <map>
15 
16 using namespace std;
17 using namespace mrpt::obs::gnss;
18 
19 // ------------
20 void Message_NV_OEM6_GENERIC_FRAME::dumpToStream(std::ostream& out) const
21 {
22  out << mrpt::format(
23  "[Novatel OEM6 GENERIC FRAME]\n"
24  " Message ID: %u\n",
25  (unsigned)this->header.msg_id);
26 }
27 void Message_NV_OEM6_GENERIC_FRAME::internal_writeToStream(
29 {
30  out.WriteBuffer(&header, sizeof(header));
31  out << static_cast<uint32_t>(msg_body.size());
32  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
33 }
34 void Message_NV_OEM6_GENERIC_FRAME::internal_readFromStream(
36 {
37  in.ReadBuffer(&header, sizeof(header));
38  uint32_t nBytesInStream;
39  in >> nBytesInStream;
40  msg_body.resize(nBytesInStream);
41  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
42  fixEndianness();
43 }
44 // ------------
45 void Message_NV_OEM6_GENERIC_SHORT_FRAME::dumpToStream(std::ostream& out) const
46 {
47  out << "[Novatel OEM6 GENERIC SHORT FRAME]\n";
48  out << mrpt::format(" Message ID: %u\n", (unsigned)this->header.msg_id);
49 }
50 void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_writeToStream(
52 {
53  out.WriteBuffer(&header, sizeof(header));
54  out << static_cast<uint32_t>(msg_body.size());
55  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
56 }
57 void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_readFromStream(
59 {
60  in.ReadBuffer(&header, sizeof(header));
61  uint32_t nBytesInStream;
62  in >> nBytesInStream;
63  msg_body.resize(nBytesInStream);
64  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
65  fixEndianness();
66 }
67 
68 // ------------
70 {
71  static bool init_map = false;
72  static std::map<int, std::string> val2str;
73  if (!init_map)
74  {
75  init_map = true;
76 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
96 #undef DEF_TYPESTR
97  }
98  auto it = val2str.find(val);
99  static const std::string nullstr("???");
100  return (it == val2str.end()) ? nullstr : it->second;
101 }
102 
104 {
105  static bool init_map = false;
106  static std::map<int, std::string> val2str;
107  if (!init_map)
108  {
109  init_map = true;
110 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
139 #undef DEF_TYPESTR
140  }
141  auto it = val2str.find(val);
142  static const std::string nullstr("???");
143  return (it == val2str.end()) ? nullstr : it->second;
144 }
145 
147 {
148  static bool init_map = false;
149  static std::map<int, std::string> val2str;
150  if (!init_map)
151  {
152  init_map = true;
153 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
162 #undef DEF_TYPESTR
163  }
164  auto it = val2str.find(val);
165  static const std::string nullstr("???");
166  return (it == val2str.end()) ? nullstr : it->second;
167 }
168 
170  const Message_NV_OEM6_BESTPOS::content_t& fields, std::ostream& out)
171 {
172  out << mrpt::format(
173  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
174  (unsigned)(fields.header.ms_in_week));
175  out << mrpt::format(
176  " Solution status: `%s`\n",
177  nv_oem6_solution_status::enum2str(fields.solution_stat).c_str());
178  out << mrpt::format(
179  " Position type : `%s`\n",
180  nv_oem6_position_type::enum2str(fields.position_type).c_str());
181  out << mrpt::format(
182  " Longitude: %.09f deg (std dev: %.06f m) Latitude: %.09f deg (std "
183  "dev: %.06f m)\n",
184  fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
185  out << mrpt::format(
186  " Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m "
187  "(Sum: %.04f m)\n",
188  fields.hgt, fields.hgt_sigma, fields.undulation,
189  fields.hgt + fields.undulation);
190  out << mrpt::format(
191  " Diff age: %.03f Solution age: %.03f\n", fields.diff_age,
192  fields.sol_age);
193  out << mrpt::format(
194  " Base station ID: `%.*s`\n", 4, fields.base_station_id);
195  out << mrpt::format(
196  " Num sat tracked: %u Num sat in solution: %u\n",
197  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
198 }
199 
200 void Message_NV_OEM6_BESTPOS::dumpToStream(std::ostream& out) const
201 {
202  out << "[Novatel OEM6 BESTPOS]\n";
203  generic_dump_BESTPOS(fields, out);
204 }
205 
206 bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions(std::ostream& o) const
207 {
208  o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m "
209  "undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age "
210  "num_sats_tracked num_sats_sol";
211  return true;
212 }
213 bool Message_NV_OEM6_BESTPOS::getAllFieldValues(std::ostream& o) const
214 {
215  o << mrpt::format(
216  "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f "
217  "%u %u",
218  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
219  (unsigned)fields.solution_stat, (unsigned)fields.position_type,
220  fields.lon, fields.lat, fields.hgt, fields.undulation, fields.lon_sigma,
221  fields.lat_sigma, fields.hgt_sigma, fields.diff_age, fields.sol_age,
222  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
223  return true;
224 }
225 
226 // ------------
227 void Message_NV_OEM6_INSPVAS::dumpToStream(std::ostream& out) const
228 {
229  out << "[Novatel OEM6 INSPVAS]\n";
230  out << mrpt::format(
231  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
232  (unsigned)(fields.header.ms_in_week));
233  out << mrpt::format(
234  " INS status: `%s`\n",
235  nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
236  out << mrpt::format(
237  " Longitude: %.09f deg Latitude: %.09f deg Height: %.03f m\n",
238  fields.lon, fields.lat, fields.hgt);
239  out << mrpt::format(
240  " Velocities: North: %.05f East: %.05f Up: %.05f\n", fields.vel_north,
241  fields.vel_east, fields.vel_up);
242  out << mrpt::format(
243  " Attitude: Roll: %.05f Pitch: %.05f Azimuth: %.05f\n", fields.roll,
244  fields.pitch, fields.azimuth);
245 }
246 
247 bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions(std::ostream& o) const
248 {
249  o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 "
250  "vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
251  return true;
252 }
253 bool Message_NV_OEM6_INSPVAS::getAllFieldValues(std::ostream& o) const
254 {
255  o << mrpt::format(
256  "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
257  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
258  (unsigned)fields.ins_status, fields.lon, fields.lat, fields.hgt,
259  fields.vel_north, fields.vel_east, fields.vel_up, fields.roll,
260  fields.pitch, fields.azimuth);
261  return true;
262 }
263 
264 // ------------
265 void Message_NV_OEM6_INSCOVS::dumpToStream(std::ostream& out) const
266 {
267  out << "[Novatel OEM6 INSCOVS]\n";
268  out << mrpt::format(
269  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
270  (unsigned)(fields.header.ms_in_week));
271  out << mrpt::format(
272  " Position cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
273  "%9.03f %9.03f\n",
274  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
275  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
276  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8]);
277  out << mrpt::format(
278  " Attitude cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
279  "%9.03f %9.03f\n",
280  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
281  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
282  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8]);
283  out << mrpt::format(
284  " Velocity cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
285  "%9.03f %9.03f\n",
286  fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
287  fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
288  fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
289 }
290 
291 bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions(std::ostream& o) const
292 {
293  o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
294  return true;
295 }
296 bool Message_NV_OEM6_INSCOVS::getAllFieldValues(std::ostream& o) const
297 {
298  o << mrpt::format(
299  "%u.%08u "
300  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
301  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
302  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f ",
303  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
304  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
305  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
306  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8],
307  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
308  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
309  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8],
310  fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
311  fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
312  fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
313  return true;
314 }
315 
316 // ------------
317 void Message_NV_OEM6_RANGECMP::dumpToStream(std::ostream& out) const
318 {
319  out << "[Novatel OEM6 RANGECMP]\n";
320  out << mrpt::format(
321  " Number of SAT observations: %u\n",
322  static_cast<unsigned int>(this->num_obs));
323 }
324 
325 void Message_NV_OEM6_RANGECMP::internal_writeToStream(
327 {
328  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
329  out << msg_len;
330  out.WriteBuffer(&header, sizeof(header));
331  out << num_obs;
332  ASSERT_EQUAL_(num_obs, obs_data.size());
333  if (num_obs)
334  out.WriteBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
335 }
336 
337 void Message_NV_OEM6_RANGECMP::internal_readFromStream(
339 {
340  uint32_t expected_msg_len;
341  in >> expected_msg_len;
342  in.ReadBuffer(&header, sizeof(header));
343  in >> num_obs;
344  ASSERT_BELOW_(num_obs, 2000);
345  obs_data.resize(num_obs);
346  if (num_obs)
347  in.ReadBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
348  fixEndianness();
349 }
350 
351 // ------------
352 void Message_NV_OEM6_RXSTATUS::dumpToStream(std::ostream& out) const
353 {
354  out << "[Novatel OEM6 RXSTATUS]\n";
355  out << mrpt::format(
356  " Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
357 }
358 
359 // ------------
360 void Message_NV_OEM6_RAWEPHEM::dumpToStream(std::ostream& out) const
361 {
362  out << "[Novatel OEM6 RAWEPHEM]\n";
363  out << mrpt::format(
364  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
365  (unsigned)(fields.header.ms_in_week));
366 }
367 
368 // ------------
369 void Message_NV_OEM6_VERSION::dumpToStream(std::ostream& out) const
370 {
371  out << "[Novatel OEM6 VERSION]\n";
372  out << mrpt::format(
373  " Number of components: %u\n",
374  static_cast<unsigned int>(this->num_comps));
375  for (size_t i = 0; i < components.size(); i++)
376  {
377  out << mrpt::format(
378  " Component #%u:\n Model: `%.*s`\n Serial: `%.*s`\n SW "
379  "version:`%.*s`\n",
380  (unsigned int)i, (int)sizeof(components[i].model),
381  components[i].model, (int)sizeof(components[i].serial),
382  components[i].serial, (int)sizeof(components[i].swversion),
383  components[i].swversion);
384  }
385 }
386 
387 void Message_NV_OEM6_VERSION::internal_writeToStream(
389 {
390  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
391  out << msg_len;
392  out.WriteBuffer(&header, sizeof(header));
393  out << num_comps;
394  ASSERT_EQUAL_(num_comps, components.size());
395  if (num_comps)
396  out.WriteBuffer(
397  &components[0], sizeof(components[0]) * components.size());
398 }
399 
400 void Message_NV_OEM6_VERSION::internal_readFromStream(
402 {
403  uint32_t expected_msg_len;
404  in >> expected_msg_len;
405  in.ReadBuffer(&header, sizeof(header));
406  in >> num_comps;
407  ASSERT_BELOW_(num_comps, 2000);
408  components.resize(num_comps);
409  if (num_comps)
410  in.ReadBuffer(
411  &components[0], sizeof(components[0]) * components.size());
412  fixEndianness();
413 }
414 
415 // ------------
416 void Message_NV_OEM6_RAWIMUS::dumpToStream(std::ostream& out) const
417 {
418  out << "[Novatel OEM6 RAWIMUS]\n";
419  out << mrpt::format(
420  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
421  (unsigned)(fields.header.ms_in_week));
422  out << mrpt::format(" Status: 0x%08lu\n", (long)fields.imu_status);
423  out << mrpt::format(
424  " Acel: X=%li Y=%li Z=%li\n", (long)fields.accel_x,
425  -(long)fields.accel_y_neg, (long)fields.accel_z);
426  out << mrpt::format(
427  " Gyro: X=%li Y=%li Z=%li\n", (long)fields.gyro_x,
428  -(long)fields.gyro_y_neg, (long)fields.gyro_z);
429 }
430 
431 bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions(std::ostream& o) const
432 {
433  o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
434  return true;
435 }
436 bool Message_NV_OEM6_RAWIMUS::getAllFieldValues(std::ostream& o) const
437 {
438  o << mrpt::format(
439  "%u.%08u %u %li %li %li %li %li %li", (unsigned)fields.header.week,
440  (unsigned)(fields.header.ms_in_week), (unsigned)fields.imu_status,
441  (long)fields.accel_x, -(long)fields.accel_y_neg, (long)fields.accel_z,
442  (long)fields.gyro_x, -(long)fields.gyro_y_neg, (long)fields.gyro_z);
443  return true;
444 }
445 
446 // ------------
448  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& out)
449 {
450  out << mrpt::format(" Clock status: 0x%08lu\n", (long)fields.clock_status);
451  out << mrpt::format(
452  " GPS week: %lu Seconds: %f\n", (long)fields.week, fields.week_seconds);
453  out << mrpt::format(
454  " Clock offset: %f (std dev = %e)\n", fields.clock_offset,
455  fields.clock_offset_std);
456  out << mrpt::format(" UTC offset: %f\n", fields.utc_offset);
457 }
458 
459 void Message_NV_OEM6_MARKTIME::dumpToStream(std::ostream& out) const
460 {
461  out << "[Novatel OEM6 MARKTIME]\n";
462  generic_dump_MARKTIME(fields, out);
463 }
464 
465 bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions(std::ostream& o) const
466 {
467  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
468  return true;
469 }
471  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& o)
472 {
473  o << mrpt::format(
474  "%u.%08u %u %lu %f %f", (unsigned)fields.header.week,
475  (unsigned)(fields.header.ms_in_week), (unsigned)fields.clock_status,
476  (long unsigned)fields.week, fields.week_seconds, fields.utc_offset);
477 }
478 bool Message_NV_OEM6_MARKTIME::getAllFieldValues(std::ostream& o) const
479 {
481  return true;
482 }
483 // ------------
484 void Message_NV_OEM6_MARK2TIME::dumpToStream(std::ostream& out) const
485 {
486  out << "[Novatel OEM6 MARK2TIME]\n";
488  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
489  out);
490 }
491 bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions(std::ostream& o) const
492 {
493  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
494  return true;
495 }
496 bool Message_NV_OEM6_MARK2TIME::getAllFieldValues(std::ostream& o) const
497 {
499  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
500  o);
501  return true;
502 }
503 
504 // ------------
505 void Message_NV_OEM6_MARKPOS::dumpToStream(std::ostream& out) const
506 {
507  out << "[Novatel OEM6 MARKPOSE]\n";
509  *reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields),
510  out);
511 }
512 
513 // ------------
514 void Message_NV_OEM6_IONUTC::dumpToStream(std::ostream& out) const
515 {
516  out << "[Novatel NV_OEM6_IONUTC]\n";
517  out << mrpt::format(
518  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
519  (unsigned)(fields.header.ms_in_week));
520  out << mrpt::format(
521  " UTC ref week: %u Tot: %u\n", (unsigned)fields.utc_wn,
522  (unsigned)fields.tot);
523  out << mrpt::format(
524  " Leap seconds delta_t: %u future: %u\n", (unsigned)fields.deltat_ls,
525  (unsigned)fields.deltat_lsf);
526 }
test distance exceeded (max of 3 rejections if distance > 10km)
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
when a fix position command is entered, the receiver computes its own position and determines if the ...
const std::string & enum2str(int val)
for nv_position_type_t
STL namespace.
GLenum GLenum GLuint components
Definition: glext.h:7401
void fixEndianness() override
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
void generic_dump_BESTPOS(const Message_NV_OEM6_BESTPOS::content_t &fields, std::ostream &out)
int val
Definition: mrpt_jpeglib.h:957
void generic_dump_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, std::ostream &out)
GLsizei const GLchar ** string
Definition: glext.h:4116
GNSS (GPS) data structures, mainly for use within mrpt::obs::CObservationGPS.
bool getAllFieldDescriptions(std::ostream &o) const override
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
GLuint in
Definition: glext.h:7391
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
#define DEF_TYPESTR(_NAME)
bool getAllFieldValues(std::ostream &o) const override



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019