INTRODUCTION
Overview
Download and Install
Documentation
Publications

REPOSITORY
Libraries

DEVELOPER
Dev Guide
Dashboard

PEOPLE
Contributors
Users

SourceForge.net Logo
Project
Download
Mailing lists

 

         
gbxnovatelacfr/driver.h
1 /*
2  * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
3  * http://gearbox.sf.net/
4  * Copyright (c) 2004-2010 Matthew Ridley, Ben Upcroft, Michael Moser
5  *
6  * This distribution is licensed to you under the terms described in
7  * the LICENSE file included in this distribution.
8  *
9  */
10 
11 #ifndef GBXNOVATELACFR_DRIVER_H
12 #define GBXNOVATELACFR_DRIVER_H
13 
14 #include <cstdlib>
15 #include <string>
16 #include <memory>
17 #include <vector>
18 
19 // forward declarations
20 // users don't need to know about serial devices or Tracers or imu decoders
21 namespace gbxserialacfr{
22  class Serial;
23 }
24 namespace gbxutilacfr{
25  class Tracer;
26 }
27 namespace gbxnovatelutilacfr{
28  class ImuDecoder;
29 }
30 
33 namespace gbxnovatelacfr{
34 
37 public:
40  SimpleConfig(std::string serialDevice, int baudRate, std::string imuType, std::vector<double > &imuToGpsOffset):
41  serialDevice_(serialDevice),
42  baudRate_(baudRate),
43  imuType_(imuType),
44  imuToGpsOffset_(imuToGpsOffset) {};
45 
47  //
53  bool isValid() const;
55  std::string toString() const;
56 
57  std::string serialDevice_;
58  int baudRate_;
59  std::string imuType_;
60  std::vector<double > imuToGpsOffset_;
61 };
62 
65 public:
66  GpsOnlyConfig(std::string serialDevice, int baudRate):
67  serialDevice_(serialDevice),
68  baudRate_(baudRate) {};
69 
71  //
75  bool isValid() const;
76  std::string toString() const;
77 
78  std::string serialDevice_;
79  int baudRate_;
80 };
81 
83 //
87 class Config{
88 public:
90  //
92  Config(const SimpleConfig &simpleCfg);
94  //
96  Config(const GpsOnlyConfig &gpsOnlyCfg);
98  Config();
99 
101  //
109  bool isValid();
111  std::string toString() const;
112 
114  //
116  std::string serialDevice_;
117  int baudRate_;
119 
121  //
123  bool enableImu_;
124  std::string imuType_;
125 
129  //
131  bool enableInsPva_;
132  bool enableGpsPos_;
133  bool enableGpsVel_;
134  bool enableRawImu_;
136 
138 
143  //
145  double dtInsPva_;
146  double dtGpsPos_;
147  double dtGpsVel_;
149 
151 
153  //
155  std::vector<double > imuToGpsOffset_;
156  std::vector<double > imuToGpsOffsetUncertainty_;
157  bool enableInsOffset_;
158  std::vector<double > insOffset_;
160 
162 
164  //
167  bool enableSBAS_;
168  bool enableRTK_;
170 
172 
178  //
180  bool enableSetImuOrientation_;
181  int setImuOrientation_;
182  bool enableVehicleBodyRotation_;
183  std::vector<double > vehicleBodyRotation_;
184  std::vector<double > vehicleBodyRotationUncertainty_;
185 
187 private:
188 };
189 
194  Ok,
197 };
198 
200 std::string toString( StatusMessageType type );
201 
202 
204 //
217  DeltaPos=10,
223  InsBad=16,
225  Pending=18,
227  UnknownGpsSolutionStatusType
228 };
229 
231 std::string toString( GpsSolutionStatusType type );
232 
234 //
237  None=0,
253  Single=16,
254  PsrDiff=17,
255  Waas=18,
257  Omnistar=20,
269  L1Float=32,
272  L1Int=48,
273  WideInt=49,
276  Ins=52,
277  InsPsrSp=53,
283  CdGps=66,
284  UnknownGpsPosVelType
285 };
286 
288 std::string toString( GpsPosVelType type );
289 
291 enum DataType {
300 };
301 
303 class GenericData {
304  public:
305  virtual ~GenericData(){};
306  virtual DataType type() const=0;
307  virtual std::string toString() const=0;
308  private:
309 };
310 
312 class InsPvaData : public GenericData {
313  public:
314  DataType type() const {
315  return InsPva;
316  }
317  std::string toString() const;
318  int gpsWeekNr;
319  double secIntoWeek;
320  double latitude;
321  double longitude;
322  double height;
323 
326  //
328  double northVelocity;
329  double eastVelocity;
330  double upVelocity;
331 
333 
336  //
338  double roll;
339  double pitch;
340  double azimuth;
341 
343 
344  StatusMessageType statusMessageType;
345  std::string statusMessage;
346 
349 };
350 
352 class BestGpsPosData : public GenericData {
353  public:
354  DataType type() const {
355  return BestGpsPos;
356  }
357  std::string toString() const;
358  int gpsWeekNr;
359  unsigned int msIntoWeek;
360  GpsSolutionStatusType solutionStatus; //
361  GpsPosVelType positionType; //
362  double latitude;
363  double longitude;
364  double heightAMSL;
365  float undulation;
366  unsigned int datumId; //
369  float sigmaHeight;
370  char baseStationId[4]; //
371  float diffAge;
372  float solutionAge;
377 
378  StatusMessageType statusMessageType;
379  std::string statusMessage;
380 
383 };
384 
386 class BestGpsVelData : public GenericData {
387  public:
388  DataType type() const {
389  return BestGpsVel;
390  }
391  std::string toString() const;
392  int gpsWeekNr;
393  unsigned int msIntoWeek;
394  GpsSolutionStatusType solutionStatus; //
395  GpsPosVelType velocityType; //
396  float latency;
397  float diffAge;
400  double verticalSpeed;
401 
402  StatusMessageType statusMessageType;
403  std::string statusMessage;
404 
407 };
408 
410 class RawImuData : public GenericData {
411  public:
412  DataType type() const {
413  return RawImu;
414  }
415  std::string toString() const;
416  int gpsWeekNr;
417  double secIntoWeek;
418  //
423  double zDeltaV;
424  double yDeltaV;
425  double xDeltaV;
426 
428 
432  //
434  double zDeltaAng;
435  double yDeltaAng;
436  double xDeltaAng;
437 
439 
440  StatusMessageType statusMessageType;
441  std::string statusMessage;
442 
445 };
446 
449 class Driver {
450 public:
451 
454  //
456 
458  //
460  Driver( const Config &cfg);
463  Driver( const Config &cfg, gbxutilacfr::Tracer &tracer);
465  ~Driver();
466 
472  std::auto_ptr<GenericData> read();
473 
474 private:
475 
476  // does the leg-work for the constructor (via the following guys)
477  void configure();
478  // establish a serial connection to the receiver
479  void connectToHardware();
480  // set parameters related to the IMU
481  void configureImu();
482  // set parameters related to the INS
483  void configureIns();
484  // set parameters related to GPS
485  void configureGps();
486  // turn on data messages we are interested in
487  void requestData();
488 
489  std::auto_ptr<gbxnovatelutilacfr::ImuDecoder> imuDecoder_;
490 
491  std::auto_ptr<gbxserialacfr::Serial> serial_;
492  int baud_;
493 
494  Config config_;
495  std::auto_ptr<gbxutilacfr::Tracer> tracerInternal_;
496  gbxutilacfr::Tracer& tracer_;
497 };
498 
499 
500 } // namespace
502 #endif
double azimuth
[degree] left handed around z-axes rotation from (true?) north clockwise
Definition: gbxnovatelacfr/driver.h:340
Floating narrow-lane ambiguity solution.
Definition: gbxnovatelacfr/driver.h:271
double dtInsPva_
100Hz max, if RawImu is enabled 50Hz max
Definition: gbxnovatelacfr/driver.h:145
Solution from floating point carrier phase ambiguities.
Definition: gbxnovatelacfr/driver.h:241
int timeStampSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:443
int gpsWeekNr
number of full weeks since midnight 05/Jan/1980 (UTC)
Definition: gbxnovatelacfr/driver.h:358
double heightAMSL
[m] AMSL == above mean sea level (geoid)
Definition: gbxnovatelacfr/driver.h:364
Nothing wrong, just not quite ready.
Definition: gbxnovatelacfr/driver.h:193
Pseudorange differential solution.
Definition: gbxnovatelacfr/driver.h:254
unsigned int msIntoWeek
yields GPS-time (together with gpsWeekNr); continous (contrary to UTC which uses leapseconds) ...
Definition: gbxnovatelacfr/driver.h:393
int timeStampSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:381
Solution from narrow-lane ambiguities.
Definition: gbxnovatelacfr/driver.h:243
All the information needed to configure the driver.
Definition: gbxnovatelacfr/driver.h:87
The fixed position, entered using the FIX POSITION command, is not valid.
Definition: gbxnovatelacfr/driver.h:226
double dtGpsPos_
20Hz max, 5Hz max if RawImu or InsPva is enabled
Definition: gbxnovatelacfr/driver.h:146
double latitude
[deg] north positive WGS84
Definition: gbxnovatelacfr/driver.h:320
Not yet converged from cold start.
Definition: gbxnovatelacfr/driver.h:213
bool enableInsPhaseUpdate_
tightly coupled (phase based vs position based) filter; Chance of better performance in adverse condi...
Definition: gbxnovatelacfr/driver.h:159
float undulation
[m] aka geoidal seperation: undulation == heigth_ellipsoid - height_geoid/AMSL
Definition: gbxnovatelacfr/driver.h:365
std::vector< double > insOffset_
report INS position/velocity offset (xyz [m] in IMU coordinates) from the IMU center; useful e...
Definition: gbxnovatelacfr/driver.h:158
OmniSTAR high precision a.
Definition: gbxnovatelacfr/driver.h:281
bool enableUseOfOmniStarCarrier_
carrier-differential corrections OMNIStarXP/HP (you need to get a subscription with them) ...
Definition: gbxnovatelacfr/driver.h:169
INS position is bad.
Definition: gbxnovatelacfr/driver.h:223
Solution computed.
Definition: gbxnovatelacfr/driver.h:207
Generic (base) type returned by a read.
Definition: gbxnovatelacfr/driver.h:303
StatusMessageType
possible Status Messages GenericData can contain
Definition: gbxnovatelacfr/driver.h:191
All good, but something to say.
Definition: gbxnovatelacfr/driver.h:194
Negative variance.
Definition: gbxnovatelacfr/driver.h:218
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:246
int numL1RangesRTK
number of L1 ranges above the RTK mask angle (??) number of L1 carrier ranges used?
Definition: gbxnovatelacfr/driver.h:375
int numL2RangesRTK
number of L2 ranges above the RTK mask angle (??) number of L2 carrier ranges used?
Definition: gbxnovatelacfr/driver.h:376
double yDeltaV
[m/s] forward positive
Definition: gbxnovatelacfr/driver.h:424
INS doing its coarse alignment.
Definition: gbxnovatelacfr/driver.h:222
double secIntoWeek
yields GPS-time (together with gpsWeekNr); continous (contrary to UTC which uses leapseconds) ...
Definition: gbxnovatelacfr/driver.h:319
double eastVelocity
[m/s] west is negative
Definition: gbxnovatelacfr/driver.h:329
GenericData is really BestGpsVelData.
Definition: gbxnovatelacfr/driver.h:297
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:249
double upVelocity
[m/s] down is negative
Definition: gbxnovatelacfr/driver.h:330
Single point position.
Definition: gbxnovatelacfr/driver.h:253
double xDeltaAng
[rad] right handed around x
Definition: gbxnovatelacfr/driver.h:436
INS has not started yet.
Definition: gbxnovatelacfr/driver.h:221
Encapsulates a serial port.
Definition: serial.h:43
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:244
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:247
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:265
GpsSolutionStatusType
Novatel&#39;s different solution status types.
Definition: gbxnovatelacfr/driver.h:206
SimpleConfig(std::string serialDevice, int baudRate, std::string imuType, std::vector< double > &imuToGpsOffset)
Definition: gbxnovatelacfr/driver.h:40
Test distance exceeded (maximum of 3 rejections if distance > 10 km)
Definition: gbxnovatelacfr/driver.h:212
Novatel GPS/INS driver.
Definition: gbxnovatelacfr/driver.cpp:95
bool fixInvalidRateSettings_
Don&#39;t bitch about wrong rates, change them to something sensible.
Definition: gbxnovatelacfr/driver.h:148
int gpsWeekNr
number of full weeks since midnight 05/Jan/1980 (UTC)
Definition: gbxnovatelacfr/driver.h:392
float latency
[s] gps speed can be calculated from instantanious or integrated doppler. The latter refers to the av...
Definition: gbxnovatelacfr/driver.h:396
double trackOverGround
[deg] "heading" of the speed vector w. respect to true North
Definition: gbxnovatelacfr/driver.h:399
OmniSTAR extra precision a.
Definition: gbxnovatelacfr/driver.h:282
std::string imuType_
Definition: gbxnovatelacfr/driver.h:124
GenericData is really RawImuData.
Definition: gbxnovatelacfr/driver.h:299
No IMU detected.
Definition: gbxnovatelacfr/driver.h:224
Propagated by a Kalman filter without new observations.
Definition: gbxnovatelacfr/driver.h:256
Delta position is too large.
Definition: gbxnovatelacfr/driver.h:217
bool enableRTK_
carrier-differential corrections (you need to set up your own base-station and wireless link)...
Definition: gbxnovatelacfr/driver.h:168
double longitude
[deg] east positive
Definition: gbxnovatelacfr/driver.h:363
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:267
double longitude
[deg] east positive WGS84
Definition: gbxnovatelacfr/driver.h:321
int numL1Ranges
number of L1 ranges used in computation (?)
Definition: gbxnovatelacfr/driver.h:374
RTK status where the RTK filter is directly initialized from the INS filter. b.
Definition: gbxnovatelacfr/driver.h:275
Covariance trace exceeds maximum (trace > 1000 m)
Definition: gbxnovatelacfr/driver.h:211
double pitch
[degree] right handed rotation from local level around x-axes
Definition: gbxnovatelacfr/driver.h:339
Simple serial port interface.
Definition: gbxnovatelacfr/driver.h:21
int gpsWeekNr
number of full weeks since midnight 05/Jan/1980 (UTC)
Definition: gbxnovatelacfr/driver.h:318
double xDeltaV
[m/s] right positive
Definition: gbxnovatelacfr/driver.h:425
float sigmaLongitude
[m] 1 standard deviation error estimate
Definition: gbxnovatelacfr/driver.h:368
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:264
Solution calculated using corrections from an SBAS.
Definition: gbxnovatelacfr/driver.h:255
INS pseudorange differential solution b.
Definition: gbxnovatelacfr/driver.h:278
No solution.
Definition: gbxnovatelacfr/driver.h:237
std::vector< double > vehicleBodyRotationUncertainty_
optional (size 3 or 0)
Definition: gbxnovatelacfr/driver.h:184
DataType
possible types GenericData can contain
Definition: gbxnovatelacfr/driver.h:291
Raw IMU information.
Definition: gbxnovatelacfr/driver.h:410
Height or velocity limits exceeded (in accordance with COCOM export licensing restrictions) ...
Definition: gbxnovatelacfr/driver.h:214
bool enableCDGPS_
code-differential corrections over satellite (North America/Canada)
Definition: gbxnovatelacfr/driver.h:166
INS calculated position corrected for the antenna b.
Definition: gbxnovatelacfr/driver.h:276
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:252
float sigmaHeight
[m] 1 standard deviation error estimate
Definition: gbxnovatelacfr/driver.h:369
Local and remote tracing.
Definition: tracer.h:111
INS RTK fixed ambiguities solution b.
Definition: gbxnovatelacfr/driver.h:280
GenericData is really BestGpsPosData.
Definition: gbxnovatelacfr/driver.h:295
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:250
Definition: gbxnovatelacfr/driver.h:27
double zDeltaV
[m/s] up positive
Definition: gbxnovatelacfr/driver.h:423
int numObservations
number of observations tracked (?) L1 code/carrier/doppler + L2 code/carrier/doppler?
Definition: gbxnovatelacfr/driver.h:373
Insufficient observations.
Definition: gbxnovatelacfr/driver.h:208
When a FIX POSITION command is entered, the receiver computes its own position and determines if the ...
Definition: gbxnovatelacfr/driver.h:225
double zDeltaAng
[rad] right handed around z
Definition: gbxnovatelacfr/driver.h:434
double secIntoWeek
Definition: gbxnovatelacfr/driver.h:417
INS position/velocity/attitude information.
Definition: gbxnovatelacfr/driver.h:312
Gps velocity information.
Definition: gbxnovatelacfr/driver.h:386
double latitude
[deg] north positive
Definition: gbxnovatelacfr/driver.h:362
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:268
std::vector< double > imuToGpsOffset_
vector (xyz [m]) from IMU center to Antenna Phase Center, in IMU coordinates, vital for INS performan...
Definition: gbxnovatelacfr/driver.h:155
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:262
std::vector< double > imuToGpsOffsetUncertainty_
optional (size 3 or 0) xyz; !it is unclear to me whether these are factors, or absolute values (in [m...
Definition: gbxnovatelacfr/driver.h:156
bool ignoreUnknownMessages_
normally we throw an exception, set this to "true" if you want to enable some other message permanent...
Definition: gbxnovatelacfr/driver.h:135
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:261
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:240
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:251
double northVelocity
[m/s] south is negative
Definition: gbxnovatelacfr/driver.h:328
int timeStampUSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:444
unsigned int msIntoWeek
yields GPS-time (together with gpsWeekNr); continous (contrary to UTC which uses leapseconds) ...
Definition: gbxnovatelacfr/driver.h:359
Position has been fixed by the FIX POSITION command or by position averaging.
Definition: gbxnovatelacfr/driver.h:238
GpsPosVelType
Novatel&#39;s different fix types.
Definition: gbxnovatelacfr/driver.h:236
float diffAge
[s] how old the correction info from the basestation is
Definition: gbxnovatelacfr/driver.h:371
Floating ionospheric-free ambiguity solution.
Definition: gbxnovatelacfr/driver.h:270
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:260
No convergence.
Definition: gbxnovatelacfr/driver.h:209
float diffAge
[s]
Definition: gbxnovatelacfr/driver.h:397
Residuals are too large.
Definition: gbxnovatelacfr/driver.h:216
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:258
int timeStampUSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:348
double dtGpsVel_
20Hz max, 5Hz max if RawImu or InsPva is enabled
Definition: gbxnovatelacfr/driver.h:147
Problem, likely to go away.
Definition: gbxnovatelacfr/driver.h:195
Integer wide-lane ambiguity solution.
Definition: gbxnovatelacfr/driver.h:273
INS pseudorange single point solution - no DGPS corrections b.
Definition: gbxnovatelacfr/driver.h:277
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:259
ACFR utilities.
Definition: gbxnovatelacfr/driver.h:24
Minimum information to configure the receiver in INS mode.
Definition: gbxnovatelacfr/driver.h:36
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:266
Singularity at parameters matrix.
Definition: gbxnovatelacfr/driver.h:210
float solutionAge
[s]
Definition: gbxnovatelacfr/driver.h:372
double verticalSpeed
[m/s]
Definition: gbxnovatelacfr/driver.h:400
double height
[m] above ellipsoid WGS84 (heigth_ellipsoid - undulation == height_geoid (aka AMSL) ...
Definition: gbxnovatelacfr/driver.h:322
the idea is to create one of these guys (with a valid Config), and then treat it as a data-source...
Definition: gbxnovatelacfr/driver.h:449
double yDeltaAng
[rad] right handed around y
Definition: gbxnovatelacfr/driver.h:435
Problem, probably fatal.
Definition: gbxnovatelacfr/driver.h:196
Variance exceeds limits.
Definition: gbxnovatelacfr/driver.h:215
float sigmaLatitude
[m] 1 standard deviation error estimate
Definition: gbxnovatelacfr/driver.h:367
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:263
GenericData is really InsPvaData.
Definition: gbxnovatelacfr/driver.h:293
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:248
Position has been fixed by the FIX HEIGHT, or FIX AUTO, command or by position averaging.
Definition: gbxnovatelacfr/driver.h:239
int timeStampUSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:382
Solution from wide-lane ambiguities.
Definition: gbxnovatelacfr/driver.h:242
Minimum information needed to configure the receiver in GPS only mode.
Definition: gbxnovatelacfr/driver.h:64
int timeStampSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:347
Gps position information.
Definition: gbxnovatelacfr/driver.h:352
int gpsWeekNr
number of full weeks since midnight 05/Jan/1980 (UTC)
Definition: gbxnovatelacfr/driver.h:416
Position solution using CDGPS corrections.
Definition: gbxnovatelacfr/driver.h:283
Nothing new, no message.
Definition: gbxnovatelacfr/driver.h:192
double roll
[degree] right handed rotation from local level around y-axes
Definition: gbxnovatelacfr/driver.h:338
Value Reserved for future use.
Definition: gbxnovatelacfr/driver.h:219
Floating L1 ambiguity solution.
Definition: gbxnovatelacfr/driver.h:269
Velocity computed using instantaneous Doppler.
Definition: gbxnovatelacfr/driver.h:245
int timeStampUSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:406
OmniSTAR VBS position (L1 sub-meter) a.
Definition: gbxnovatelacfr/driver.h:257
double horizontalSpeed
[m/s]
Definition: gbxnovatelacfr/driver.h:398
bool enableSBAS_
code-differential corrections over satellite on GPS frequencies (WAAS/EGNOS)
Definition: gbxnovatelacfr/driver.h:167
Integer L1 ambiguity solution.
Definition: gbxnovatelacfr/driver.h:272
INS RTK floating point ambiguities solution b.
Definition: gbxnovatelacfr/driver.h:279
Large residuals make position unreliable.
Definition: gbxnovatelacfr/driver.h:220
Integer narrow-lane ambiguity solution.
Definition: gbxnovatelacfr/driver.h:274
int timeStampSec
in Computer time, beginning of message at serial port
Definition: gbxnovatelacfr/driver.h:405
 

Generated for GearBox by  doxygen 1.4.5