MRPT  2.0.1
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-2020, 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.WriteAs<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  const uint32_t nBytesInStream = in.ReadAs<uint32_t>();
39  msg_body.resize(nBytesInStream);
40  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
41  fixEndianness();
42 }
43 // ------------
44 void Message_NV_OEM6_GENERIC_SHORT_FRAME::dumpToStream(std::ostream& out) const
45 {
46  out << "[Novatel OEM6 GENERIC SHORT FRAME]\n";
47  out << mrpt::format(" Message ID: %u\n", (unsigned)this->header.msg_id);
48 }
49 void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_writeToStream(
51 {
52  out.WriteBuffer(&header, sizeof(header));
53  out.WriteAs<uint32_t>(msg_body.size());
54  if (!msg_body.empty()) out.WriteBuffer(&msg_body[0], msg_body.size());
55 }
56 void Message_NV_OEM6_GENERIC_SHORT_FRAME::internal_readFromStream(
58 {
59  in.ReadBuffer(&header, sizeof(header));
60  const uint32_t nBytesInStream = in.ReadAs<uint32_t>();
61  msg_body.resize(nBytesInStream);
62  if (nBytesInStream) in.ReadBuffer(&msg_body[0], sizeof(nBytesInStream));
63  fixEndianness();
64 }
65 
66 // ------------
67 const std::string& nv_oem6_solution_status::enum2str(int val)
68 {
69  static bool init_map = false;
70  static std::map<int, std::string> val2str;
71  if (!init_map)
72  {
73  init_map = true;
74 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
94 #undef DEF_TYPESTR
95  }
96  auto it = val2str.find(val);
97  static const std::string nullstr("???");
98  return (it == val2str.end()) ? nullstr : it->second;
99 }
100 
101 const std::string& nv_oem6_position_type::enum2str(int val)
102 {
103  static bool init_map = false;
104  static std::map<int, std::string> val2str;
105  if (!init_map)
106  {
107  init_map = true;
108 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
137 #undef DEF_TYPESTR
138  }
139  auto it = val2str.find(val);
140  static const std::string nullstr("???");
141  return (it == val2str.end()) ? nullstr : it->second;
142 }
143 
144 const std::string& nv_oem6_ins_status_type::enum2str(int val)
145 {
146  static bool init_map = false;
147  static std::map<int, std::string> val2str;
148  if (!init_map)
149  {
150  init_map = true;
151 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
160 #undef DEF_TYPESTR
161  }
162  auto it = val2str.find(val);
163  static const std::string nullstr("???");
164  return (it == val2str.end()) ? nullstr : it->second;
165 }
166 
168  const Message_NV_OEM6_BESTPOS::content_t& fields, std::ostream& out)
169 {
170  out << mrpt::format(
171  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
172  (unsigned)(fields.header.ms_in_week));
173  out << mrpt::format(
174  " Solution status: `%s`\n",
175  nv_oem6_solution_status::enum2str(fields.solution_stat).c_str());
176  out << mrpt::format(
177  " Position type : `%s`\n",
178  nv_oem6_position_type::enum2str(fields.position_type).c_str());
179  out << mrpt::format(
180  " Longitude: %.09f deg (std dev: %.06f m) Latitude: %.09f deg (std "
181  "dev: %.06f m)\n",
182  fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
183  out << mrpt::format(
184  " Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m "
185  "(Sum: %.04f m)\n",
186  fields.hgt, fields.hgt_sigma, fields.undulation,
187  fields.hgt + fields.undulation);
188  out << mrpt::format(
189  " Diff age: %.03f Solution age: %.03f\n", fields.diff_age,
190  fields.sol_age);
191  out << mrpt::format(
192  " Base station ID: `%.*s`\n", 4, fields.base_station_id);
193  out << mrpt::format(
194  " Num sat tracked: %u Num sat in solution: %u\n",
195  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
196 }
197 
198 void Message_NV_OEM6_BESTPOS::dumpToStream(std::ostream& out) const
199 {
200  out << "[Novatel OEM6 BESTPOS]\n";
201  generic_dump_BESTPOS(fields, out);
202 }
203 
204 bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions(std::ostream& o) const
205 {
206  o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m "
207  "undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age "
208  "num_sats_tracked num_sats_sol";
209  return true;
210 }
211 bool Message_NV_OEM6_BESTPOS::getAllFieldValues(std::ostream& o) const
212 {
213  o << mrpt::format(
214  "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f "
215  "%u %u",
216  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
217  (unsigned)fields.solution_stat, (unsigned)fields.position_type,
218  fields.lon, fields.lat, fields.hgt, fields.undulation, fields.lon_sigma,
219  fields.lat_sigma, fields.hgt_sigma, fields.diff_age, fields.sol_age,
220  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
221  return true;
222 }
223 
224 // ------------
225 void Message_NV_OEM6_INSPVAS::dumpToStream(std::ostream& out) const
226 {
227  out << "[Novatel OEM6 INSPVAS]\n";
228  out << mrpt::format(
229  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
230  (unsigned)(fields.header.ms_in_week));
231  out << mrpt::format(
232  " INS status: `%s`\n",
233  nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
234  out << mrpt::format(
235  " Longitude: %.09f deg Latitude: %.09f deg Height: %.03f m\n",
236  fields.lon, fields.lat, fields.hgt);
237  out << mrpt::format(
238  " Velocities: North: %.05f East: %.05f Up: %.05f\n", fields.vel_north,
239  fields.vel_east, fields.vel_up);
240  out << mrpt::format(
241  " Attitude: Roll: %.05f Pitch: %.05f Azimuth: %.05f\n", fields.roll,
242  fields.pitch, fields.azimuth);
243 }
244 
245 bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions(std::ostream& o) const
246 {
247  o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 "
248  "vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
249  return true;
250 }
251 bool Message_NV_OEM6_INSPVAS::getAllFieldValues(std::ostream& o) const
252 {
253  o << mrpt::format(
254  "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
255  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
256  (unsigned)fields.ins_status, fields.lon, fields.lat, fields.hgt,
257  fields.vel_north, fields.vel_east, fields.vel_up, fields.roll,
258  fields.pitch, fields.azimuth);
259  return true;
260 }
261 
262 // ------------
263 void Message_NV_OEM6_INSCOVS::dumpToStream(std::ostream& out) const
264 {
265  out << "[Novatel OEM6 INSCOVS]\n";
266  out << mrpt::format(
267  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
268  (unsigned)(fields.header.ms_in_week));
269  out << mrpt::format(
270  " Position cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
271  "%9.03f %9.03f\n",
272  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
273  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
274  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8]);
275  out << mrpt::format(
276  " Attitude cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
277  "%9.03f %9.03f\n",
278  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
279  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
280  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8]);
281  out << mrpt::format(
282  " Velocity cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
283  "%9.03f %9.03f\n",
284  fields.vel_cov[0], fields.vel_cov[1], fields.vel_cov[2],
285  fields.vel_cov[3], fields.vel_cov[4], fields.vel_cov[5],
286  fields.vel_cov[6], fields.vel_cov[7], fields.vel_cov[8]);
287 }
288 
289 bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions(std::ostream& o) const
290 {
291  o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
292  return true;
293 }
294 bool Message_NV_OEM6_INSCOVS::getAllFieldValues(std::ostream& o) const
295 {
296  o << mrpt::format(
297  "%u.%08u "
298  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
299  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
300  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f ",
301  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
302  fields.pos_cov[0], fields.pos_cov[1], fields.pos_cov[2],
303  fields.pos_cov[3], fields.pos_cov[4], fields.pos_cov[5],
304  fields.pos_cov[6], fields.pos_cov[7], fields.pos_cov[8],
305  fields.att_cov[0], fields.att_cov[1], fields.att_cov[2],
306  fields.att_cov[3], fields.att_cov[4], fields.att_cov[5],
307  fields.att_cov[6], fields.att_cov[7], fields.att_cov[8],
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  return true;
312 }
313 
314 // ------------
315 void Message_NV_OEM6_RANGECMP::dumpToStream(std::ostream& out) const
316 {
317  out << "[Novatel OEM6 RANGECMP]\n";
318  out << mrpt::format(
319  " Number of SAT observations: %u\n",
320  static_cast<unsigned int>(this->num_obs));
321 }
322 
323 void Message_NV_OEM6_RANGECMP::internal_writeToStream(
325 {
326  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
327  out << msg_len;
328  out.WriteBuffer(&header, sizeof(header));
329  out << num_obs;
330  ASSERT_EQUAL_(num_obs, obs_data.size());
331  if (num_obs)
332  out.WriteBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
333 }
334 
335 void Message_NV_OEM6_RANGECMP::internal_readFromStream(
337 {
338  uint32_t expected_msg_len;
339  in >> expected_msg_len;
340  in.ReadBuffer(&header, sizeof(header));
341  in >> num_obs;
342  ASSERT_BELOW_(num_obs, 2000);
343  obs_data.resize(num_obs);
344  if (num_obs)
345  in.ReadBuffer(&obs_data[0], sizeof(obs_data[0]) * obs_data.size());
346  fixEndianness();
347 }
348 
349 // ------------
350 void Message_NV_OEM6_RXSTATUS::dumpToStream(std::ostream& out) const
351 {
352  out << "[Novatel OEM6 RXSTATUS]\n";
353  out << mrpt::format(
354  " Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
355 }
356 
357 // ------------
358 void Message_NV_OEM6_RAWEPHEM::dumpToStream(std::ostream& out) const
359 {
360  out << "[Novatel OEM6 RAWEPHEM]\n";
361  out << mrpt::format(
362  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
363  (unsigned)(fields.header.ms_in_week));
364 }
365 
366 // ------------
367 void Message_NV_OEM6_VERSION::dumpToStream(std::ostream& out) const
368 {
369  out << "[Novatel OEM6 VERSION]\n";
370  out << mrpt::format(
371  " Number of components: %u\n",
372  static_cast<unsigned int>(this->num_comps));
373  for (size_t i = 0; i < components.size(); i++)
374  {
375  out << mrpt::format(
376  " Component #%u:\n Model: `%.*s`\n Serial: `%.*s`\n SW "
377  "version:`%.*s`\n",
378  (unsigned int)i, (int)sizeof(components[i].model),
379  components[i].model, (int)sizeof(components[i].serial),
380  components[i].serial, (int)sizeof(components[i].swversion),
381  components[i].swversion);
382  }
383 }
384 
385 void Message_NV_OEM6_VERSION::internal_writeToStream(
387 {
388  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
389  out << msg_len;
390  out.WriteBuffer(&header, sizeof(header));
391  out << num_comps;
392  ASSERT_EQUAL_(num_comps, components.size());
393  if (num_comps)
394  out.WriteBuffer(
395  &components[0], sizeof(components[0]) * components.size());
396 }
397 
398 void Message_NV_OEM6_VERSION::internal_readFromStream(
400 {
401  uint32_t expected_msg_len;
402  in >> expected_msg_len;
403  in.ReadBuffer(&header, sizeof(header));
404  in >> num_comps;
405  ASSERT_BELOW_(num_comps, 2000);
406  components.resize(num_comps);
407  if (num_comps)
408  in.ReadBuffer(
409  &components[0], sizeof(components[0]) * components.size());
410  fixEndianness();
411 }
412 
413 // ------------
414 void Message_NV_OEM6_RAWIMUS::dumpToStream(std::ostream& out) const
415 {
416  out << "[Novatel OEM6 RAWIMUS]\n";
417  out << mrpt::format(
418  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
419  (unsigned)(fields.header.ms_in_week));
420  out << mrpt::format(" Status: 0x%08lu\n", (long)fields.imu_status);
421  out << mrpt::format(
422  " Acel: X=%li Y=%li Z=%li\n", (long)fields.accel_x,
423  -(long)fields.accel_y_neg, (long)fields.accel_z);
424  out << mrpt::format(
425  " Gyro: X=%li Y=%li Z=%li\n", (long)fields.gyro_x,
426  -(long)fields.gyro_y_neg, (long)fields.gyro_z);
427 }
428 
429 bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions(std::ostream& o) const
430 {
431  o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
432  return true;
433 }
434 bool Message_NV_OEM6_RAWIMUS::getAllFieldValues(std::ostream& o) const
435 {
436  o << mrpt::format(
437  "%u.%08u %u %li %li %li %li %li %li", (unsigned)fields.header.week,
438  (unsigned)(fields.header.ms_in_week), (unsigned)fields.imu_status,
439  (long)fields.accel_x, -(long)fields.accel_y_neg, (long)fields.accel_z,
440  (long)fields.gyro_x, -(long)fields.gyro_y_neg, (long)fields.gyro_z);
441  return true;
442 }
443 
444 // ------------
446  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& out)
447 {
448  out << mrpt::format(" Clock status: 0x%08lu\n", (long)fields.clock_status);
449  out << mrpt::format(
450  " GPS week: %lu Seconds: %f\n", (long)fields.week, fields.week_seconds);
451  out << mrpt::format(
452  " Clock offset: %f (std dev = %e)\n", fields.clock_offset,
453  fields.clock_offset_std);
454  out << mrpt::format(" UTC offset: %f\n", fields.utc_offset);
455 }
456 
457 void Message_NV_OEM6_MARKTIME::dumpToStream(std::ostream& out) const
458 {
459  out << "[Novatel OEM6 MARKTIME]\n";
460  generic_dump_MARKTIME(fields, out);
461 }
462 
463 bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions(std::ostream& o) const
464 {
465  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
466  return true;
467 }
469  const Message_NV_OEM6_MARKTIME::content_t& fields, std::ostream& o)
470 {
471  o << mrpt::format(
472  "%u.%08u %u %lu %f %f", (unsigned)fields.header.week,
473  (unsigned)(fields.header.ms_in_week), (unsigned)fields.clock_status,
474  (long unsigned)fields.week, fields.week_seconds, fields.utc_offset);
475 }
476 bool Message_NV_OEM6_MARKTIME::getAllFieldValues(std::ostream& o) const
477 {
479  return true;
480 }
481 // ------------
482 void Message_NV_OEM6_MARK2TIME::dumpToStream(std::ostream& out) const
483 {
484  out << "[Novatel OEM6 MARK2TIME]\n";
486  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
487  out);
488 }
489 bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions(std::ostream& o) const
490 {
491  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
492  return true;
493 }
494 bool Message_NV_OEM6_MARK2TIME::getAllFieldValues(std::ostream& o) const
495 {
497  *reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),
498  o);
499  return true;
500 }
501 
502 // ------------
503 void Message_NV_OEM6_MARKPOS::dumpToStream(std::ostream& out) const
504 {
505  out << "[Novatel OEM6 MARKPOSE]\n";
507  *reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields),
508  out);
509 }
510 
511 // ------------
512 void Message_NV_OEM6_IONUTC::dumpToStream(std::ostream& out) const
513 {
514  out << "[Novatel NV_OEM6_IONUTC]\n";
515  out << mrpt::format(
516  " GPS week: %u ms in week: %u\n", (unsigned)fields.header.week,
517  (unsigned)(fields.header.ms_in_week));
518  out << mrpt::format(
519  " UTC ref week: %u Tot: %u\n", (unsigned)fields.utc_wn,
520  (unsigned)fields.tot);
521  out << mrpt::format(
522  " Leap seconds delta_t: %u future: %u\n", (unsigned)fields.deltat_ls,
523  (unsigned)fields.deltat_lsf);
524 }
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.
void fixEndianness() override
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
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)
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
void generic_getFieldValues_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, std::ostream &o)
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020