Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
15 using namespace std;
16 using namespace mrpt::obs::gnss;
17 
18 nv_oem6_header_t::nv_oem6_header_t() :
19  hdr_len(0),
20  msg_id(0),
21  msg_type(0),
22  port_addr(0),
23  msg_len(0),
24  seq_number(0),
25  idle_percent(0),
26  time_status(0),
27  week(0),
28  ms_in_week(0),
29  receiver_status(0),
30  reserved(0),
31  receiver_sw_version(0)
32 {
33  synch[0]=synch[1]=synch[2]=0;
34 }
35 
37  msg_len(),
38  msg_id(),
39  week(),
40  ms_in_week()
41 {
42  synch[0]=synch[1]=synch[2]=0;
43 }
44 
45 
46 // ------------
48 {
49  out.printf("[Novatel OEM6 GENERIC FRAME]\n");
50  out.printf(" Message ID: %u\n", (unsigned)this->header.msg_id);
51 }
53 {
54  out.WriteBuffer(&header,sizeof(header));
55  out << static_cast<uint32_t>(msg_body.size());
56  if (!msg_body.empty())
57  out.WriteBuffer(&msg_body[0],msg_body.size());
58 }
60 {
61  in.ReadBuffer(&header,sizeof(header));
62  uint32_t nBytesInStream;
63  in >> nBytesInStream;
64  msg_body.resize(nBytesInStream);
65  if (nBytesInStream)
66  in.ReadBuffer(&msg_body[0],sizeof(nBytesInStream));
67 }
68 // ------------
70 {
71  out.printf("[Novatel OEM6 GENERIC SHORT FRAME]\n");
72  out.printf(" Message ID: %u\n", (unsigned)this->header.msg_id);
73 }
75 {
76  out.WriteBuffer(&header,sizeof(header));
77  out << static_cast<uint32_t>(msg_body.size());
78  if (!msg_body.empty())
79  out.WriteBuffer(&msg_body[0],msg_body.size());
80 }
82 {
83  in.ReadBuffer(&header,sizeof(header));
84  uint32_t nBytesInStream;
85  in >> nBytesInStream;
86  msg_body.resize(nBytesInStream);
87  if (nBytesInStream)
88  in.ReadBuffer(&msg_body[0],sizeof(nBytesInStream));
89 }
90 
91 // ------------
93 {
94  static bool init_map = false;
95  static std::map<int,std::string> val2str;
96  if (!init_map) {
97  init_map = true;
98 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_solution_status::_NAME] = #_NAME;
105 #undef DEF_TYPESTR
106  }
108  static const std::string nullstr("???");
109  return (it == val2str.end()) ? nullstr : it->second;
110 }
111 
113 {
114  static bool init_map = false;
115  static std::map<int,std::string> val2str;
116  if (!init_map) {
117  init_map = true;
118 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_position_type::_NAME] = #_NAME;
126 #undef DEF_TYPESTR
127  }
129  static const std::string nullstr("???");
130  return (it == val2str.end()) ? nullstr : it->second;
131 }
132 
134 {
135  static bool init_map = false;
136  static std::map<int,std::string> val2str;
137  if (!init_map) {
138  init_map = true;
139 #define DEF_TYPESTR(_NAME) val2str[nv_oem6_ins_status_type::_NAME] = #_NAME;
143 #undef DEF_TYPESTR
144  }
146  static const std::string nullstr("???");
147  return (it == val2str.end()) ? nullstr : it->second;
148 }
149 
150 void generic_dump_BESTPOS(const Message_NV_OEM6_BESTPOS::content_t &fields, mrpt::utils::CStream &out )
151 {
152  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
153  out.printf(" Solution status: `%s`\n", nv_oem6_solution_status::enum2str(fields.solution_stat).c_str() );
154  out.printf(" Position type : `%s`\n", nv_oem6_position_type::enum2str(fields.position_type).c_str() );
155  out.printf(" Longitude: %.09f deg (std dev: %.06f m) Latitude: %.09f deg (std dev: %.06f m)\n", fields.lon, fields.lon_sigma, fields.lat, fields.lat_sigma);
156  out.printf(" Height (sea level): %.06f m (std dev: %.06f m) Undulation: %.04f m (Sum: %.04f m)\n", fields.hgt, fields.hgt_sigma, fields.undulation,fields.hgt+fields.undulation);
157  out.printf(" Diff age: %.03f Solution age: %.03f\n", fields.diff_age, fields.sol_age);
158  out.printf(" Base station ID: `%.*s`\n", 4,fields.base_station_id);
159  out.printf(" Num sat tracked: %u Num sat in solution: %u\n", (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol);
160 }
161 
162 void Message_NV_OEM6_BESTPOS::dumpToStream( mrpt::utils::CStream &out ) const
163 {
164  out.printf("[Novatel OEM6 BESTPOS]\n");
165  generic_dump_BESTPOS(fields,out);
166 }
167 
168 bool Message_NV_OEM6_BESTPOS::getAllFieldDescriptions( std::ostream &o ) const
169 {
170  o << "gps_week.gps_ms solution_stat position_type lon_deg lat_deg hgt_m undulation_m lon_sigma_m lat_sigma_m hgt_sigma_m diff_age sol_age num_sats_tracked num_sats_sol";
171  return true;
172 }
173 bool Message_NV_OEM6_BESTPOS::getAllFieldValues( std::ostream &o ) const
174 {
175  o << mrpt::format(
176  "%u.%08u %u %u %.09f %.09f %.06f %.04f %.06f %.06f %.06f %.03f %.03f %u %u",
177  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
178  (unsigned)fields.solution_stat,
179  (unsigned)fields.position_type,
180  fields.lon,fields.lat,fields.hgt, fields.undulation,
181  fields.lon_sigma, fields.lat_sigma, fields.hgt_sigma,
182  fields.diff_age, fields.sol_age,
183  (unsigned)fields.num_sats_tracked, (unsigned)fields.num_sats_sol
184  );
185  return true;
186 }
187 
188 
189 
190 // ------------
191 void Message_NV_OEM6_INSPVAS::dumpToStream( mrpt::utils::CStream &out ) const
192 {
193  out.printf("[Novatel OEM6 INSPVAS]\n");
194  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
195  out.printf(" INS status: `%s`\n", nv_oem6_ins_status_type::enum2str(fields.ins_status).c_str());
196  out.printf(" Longitude: %.09f deg Latitude: %.09f deg Height: %.03f m\n", fields.lon, fields.lat, fields.hgt );
197  out.printf(" Velocities: North: %.05f East: %.05f Up: %.05f\n", fields.vel_north, fields.vel_east, fields.vel_up);
198  out.printf(" Attitude: Roll: %.05f Pitch: %.05f Azimuth: %.05f\n", fields.roll, fields.pitch, fields.azimuth);
199 }
200 
201 bool Message_NV_OEM6_INSPVAS::getAllFieldDescriptions( std::ostream &o ) const
202 {
203  o << "gps_week.gps_ms ins_status lon_deg lat_deg ellip_height_WGS84 vel_north vel_east vel_up roll_deg pitch_deg azimuth_deg";
204  return true;
205 }
206 bool Message_NV_OEM6_INSPVAS::getAllFieldValues( std::ostream &o ) const
207 {
208  o << mrpt::format(
209  "%u.%08u %u %.09f %.09f %.06f %.05f %.05f %.05f %.05f %.05f %.05f",
210  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
211  (unsigned)fields.ins_status,
212  fields.lon,fields.lat,fields.hgt,
213  fields.vel_north, fields.vel_east, fields.vel_up,
214  fields.roll, fields.pitch, fields.azimuth
215  );
216  return true;
217 }
218 
219 
220 
221 // ------------
222 void Message_NV_OEM6_INSCOVS::dumpToStream( mrpt::utils::CStream &out ) const
223 {
224  out.printf("[Novatel OEM6 INSCOVS]\n");
225  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
226  out.printf(" Position cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f\n",
227  fields.pos_cov[0],fields.pos_cov[1],fields.pos_cov[2],
228  fields.pos_cov[3],fields.pos_cov[4],fields.pos_cov[5],
229  fields.pos_cov[6],fields.pos_cov[7],fields.pos_cov[8]);
230  out.printf(" Attitude cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f\n",
231  fields.att_cov[0],fields.att_cov[1],fields.att_cov[2],
232  fields.att_cov[3],fields.att_cov[4],fields.att_cov[5],
233  fields.att_cov[6],fields.att_cov[7],fields.att_cov[8]);
234  out.printf(" Velocity cov: %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f\n",
235  fields.vel_cov[0],fields.vel_cov[1],fields.vel_cov[2],
236  fields.vel_cov[3],fields.vel_cov[4],fields.vel_cov[5],
237  fields.vel_cov[6],fields.vel_cov[7],fields.vel_cov[8]);
238 }
239 
240 bool Message_NV_OEM6_INSCOVS::getAllFieldDescriptions( std::ostream &o ) const
241 {
242  o << "gps_week.gps_ms pos_cov(*9) att-cov(*9) vel_cov(*9)";
243  return true;
244 }
245 bool Message_NV_OEM6_INSCOVS::getAllFieldValues( std::ostream &o ) const
246 {
247  o << mrpt::format(
248  "%u.%08u "
249  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
250  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f "
251  "%9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f %9.03f ",
252  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
253  fields.pos_cov[0],fields.pos_cov[1],fields.pos_cov[2],
254  fields.pos_cov[3],fields.pos_cov[4],fields.pos_cov[5],
255  fields.pos_cov[6],fields.pos_cov[7],fields.pos_cov[8],
256  fields.att_cov[0],fields.att_cov[1],fields.att_cov[2],
257  fields.att_cov[3],fields.att_cov[4],fields.att_cov[5],
258  fields.att_cov[6],fields.att_cov[7],fields.att_cov[8],
259  fields.vel_cov[0],fields.vel_cov[1],fields.vel_cov[2],
260  fields.vel_cov[3],fields.vel_cov[4],fields.vel_cov[5],
261  fields.vel_cov[6],fields.vel_cov[7],fields.vel_cov[8]);
262  return true;
263 }
264 
265 // ------------
267 {
268  out.printf("[Novatel OEM6 RANGECMP]\n");
269  out.printf(" Number of SAT observations: %u\n", static_cast<unsigned int>(this->num_obs));
270 }
271 
273 {
274  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
275  out << msg_len;
276  out.WriteBuffer(&header,sizeof(header));
277  out << num_obs;
279  if (num_obs)
280  out.WriteBuffer(&obs_data[0],sizeof(obs_data[0])*obs_data.size());
281 }
282 
284 {
285  uint32_t expected_msg_len;
286  in >> expected_msg_len;
287  in.ReadBuffer(&header,sizeof(header));
288  in >> num_obs;
289  ASSERT_BELOW_(num_obs,2000);
290  obs_data.resize(num_obs);
291  if (num_obs)
292  in.ReadBuffer(&obs_data[0],sizeof(obs_data[0])*obs_data.size());
293 }
294 
295 // ------------
296 void Message_NV_OEM6_RXSTATUS::dumpToStream( mrpt::utils::CStream &out ) const
297 {
298  out.printf("[Novatel OEM6 RXSTATUS]\n");
299  out.printf(" Error code: 0x%04X\n", static_cast<unsigned int>(this->fields.error));
300 }
301 
302 // ------------
303 void Message_NV_OEM6_RAWEPHEM::dumpToStream( mrpt::utils::CStream &out ) const
304 {
305  out.printf("[Novatel OEM6 RAWEPHEM]\n");
306  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
307 }
308 
309 // ------------
311 {
312  out.printf("[Novatel OEM6 VERSION]\n");
313  out.printf(" Number of components: %u\n", static_cast<unsigned int>(this->num_comps));
314  for (unsigned i=0;i<components.size();i++){
315  out.printf(" Component #%u:\n Model: `%.*s`\n Serial: `%.*s`\n SW version:`%.*s`\n",i,
316  (int)sizeof(components[i].model), components[i].model,
317  (int)sizeof(components[i].serial), components[i].serial,
318  (int)sizeof(components[i].swversion), components[i].swversion
319  );
320  }
321 }
322 
324 {
325  const uint32_t msg_len = sizeof(header) + header.msg_len + 4;
326  out << msg_len;
327  out.WriteBuffer(&header,sizeof(header));
328  out << num_comps;
330  if (num_comps)
331  out.WriteBuffer(&components[0],sizeof(components[0])*components.size());
332 }
333 
335 {
336  uint32_t expected_msg_len;
337  in >> expected_msg_len;
338  in.ReadBuffer(&header,sizeof(header));
339  in >> num_comps;
340  ASSERT_BELOW_(num_comps,2000);
341  components.resize(num_comps);
342  if (num_comps)
343  in.ReadBuffer(&components[0],sizeof(components[0])*components.size());
344 }
345 
346 // ------------
347 void Message_NV_OEM6_RAWIMUS::dumpToStream( mrpt::utils::CStream &out ) const
348 {
349  out.printf("[Novatel OEM6 RAWIMUS]\n");
350  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
351  out.printf(" Status: 0x%08lu\n",(long)fields.imu_status);
352  out.printf(" Acel: X=%li Y=%li Z=%li\n",(long)fields.accel_x,-(long)fields.accel_y_neg,(long)fields.accel_z);
353  out.printf(" Gyro: X=%li Y=%li Z=%li\n",(long)fields.gyro_x,-(long)fields.gyro_y_neg,(long)fields.gyro_z);
354 }
355 
356 bool Message_NV_OEM6_RAWIMUS::getAllFieldDescriptions( std::ostream &o ) const
357 {
358  o << "gps_week.gps_ms imu_status acc_x acc_y acc_z gyro_x gyro_y gyro_z";
359  return true;
360 }
361 bool Message_NV_OEM6_RAWIMUS::getAllFieldValues( std::ostream &o ) const
362 {
363  o << mrpt::format(
364  "%u.%08u %u %li %li %li %li %li %li",
365  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
366  (unsigned)fields.imu_status,
367  (long)fields.accel_x,-(long)fields.accel_y_neg,(long)fields.accel_z,
368  (long)fields.gyro_x,-(long)fields.gyro_y_neg,(long)fields.gyro_z
369  );
370  return true;
371 }
372 
373 
374 // ------------
375 void generic_dump_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields,mrpt::utils::CStream &out)
376 {
377  out.printf(" Clock status: 0x%08lu\n",(long)fields.clock_status);
378  out.printf(" GPS week: %lu Seconds: %f\n",(long)fields.week, fields.week_seconds);
379  out.printf(" Clock offset: %f (std dev = %e)\n", fields.clock_offset,fields.clock_offset_std);
380  out.printf(" UTC offset: %f\n", fields.utc_offset);
381 }
382 
383 void Message_NV_OEM6_MARKTIME::dumpToStream( mrpt::utils::CStream &out ) const
384 {
385  out.printf("[Novatel OEM6 MARKTIME]\n");
386  generic_dump_MARKTIME(fields,out);
387 }
388 
389 bool Message_NV_OEM6_MARKTIME::getAllFieldDescriptions( std::ostream &o ) const
390 {
391  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
392  return true;
393 }
394 void generic_getFieldValues_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, std::ostream &o)
395 {
396  o << mrpt::format(
397  "%u.%08u %u %lu %f %f",
398  (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week),
399  (unsigned)fields.clock_status,
400  (long unsigned)fields.week, fields.week_seconds,
401  fields.utc_offset
402  );
403 }
404 bool Message_NV_OEM6_MARKTIME::getAllFieldValues( std::ostream &o ) const
405 {
407  return true;
408 }
409 // ------------
410 void Message_NV_OEM6_MARK2TIME::dumpToStream( mrpt::utils::CStream &out ) const
411 {
412  out.printf("[Novatel OEM6 MARK2TIME]\n");
413  generic_dump_MARKTIME(*reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),out);
414 }
415 bool Message_NV_OEM6_MARK2TIME::getAllFieldDescriptions( std::ostream &o ) const
416 {
417  o << "gps_week.gps_ms clock_status week week_seconds utc_offset";
418  return true;
419 }
420 bool Message_NV_OEM6_MARK2TIME::getAllFieldValues( std::ostream &o ) const
421 {
422  generic_getFieldValues_MARKTIME(*reinterpret_cast<const Message_NV_OEM6_MARKTIME::content_t*>(&fields),o);
423  return true;
424 }
425 
426 
427 // ------------
428 void Message_NV_OEM6_MARKPOS::dumpToStream( mrpt::utils::CStream &out ) const
429 {
430  out.printf("[Novatel OEM6 MARKPOSE]\n");
431  generic_dump_BESTPOS(*reinterpret_cast<const Message_NV_OEM6_BESTPOS::content_t*>(&fields),out);
432 }
433 
434 // ------------
435 void Message_NV_OEM6_IONUTC::dumpToStream( mrpt::utils::CStream &out ) const
436 {
437  out.printf("[Novatel NV_OEM6_IONUTC]\n");
438  out.printf(" GPS week: %u ms in week: %u\n", (unsigned)fields.header.week, (unsigned)(fields.header.ms_in_week));
439  out.printf(" UTC ref week: %u Tot: %u\n",(unsigned)fields.utc_wn, (unsigned)fields.tot);
440  out.printf(" Leap seconds delta_t: %u future: %u\n",(unsigned)fields.deltat_ls, (unsigned)fields.deltat_lsf);
441 }
#define ASSERT_EQUAL_(__A, __B)
bool getAllFieldDescriptions(std::ostream &o) const MRPT_OVERRIDE
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
void dumpToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Dumps the contents of the observation in a human-readable form to a given output stream.
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
_u32 reserved
Definition: rplidar_cmd.h:22
#define ASSERT_BELOW_(__A, __B)
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
void dumpToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Dumps the contents of the observation in a human-readable form to a given output stream.
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLenum GLenum GLuint components
Definition: glext.h:6305
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
when a fix position command is entered, the receiver computes its own position and determines if the ...
_u8 model
Definition: rplidar_cmd.h:21
const std::string OBS_IMPEXP & enum2str(int val)
for nv_position_type_t
void generic_dump_MARKTIME(const Message_NV_OEM6_MARKTIME::content_t &fields, mrpt::utils::CStream &out)
int val
Definition: mrpt_jpeglib.h:953
const std::string OBS_IMPEXP & enum2str(int val)
for nv_ins_status_type_t
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void dumpToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Dumps the contents of the observation in a human-readable form to a given output stream.
GLsizei const GLchar ** string
Definition: glext.h:3919
GNSS (GPS) data structures, mainly for use within mrpt::obs::CObservationGPS.
void generic_dump_BESTPOS(const Message_NV_OEM6_BESTPOS::content_t &fields, mrpt::utils::CStream &out)
std::vector< TCompressedRangeLog > obs_data
test distance exceeded (max of 3 rejections if distance > 10km)
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
GLuint in
Definition: glext.h:6301
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
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 MRPT_OVERRIDE
unsigned __int32 uint32_t
Definition: rptypes.h:49
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
Save to binary stream. Launches an exception upon error.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
const std::string OBS_IMPEXP & enum2str(int val)
for nv_solution_status_t
void dumpToStream(mrpt::utils::CStream &out) const MRPT_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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019