~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/novatelINS/src/changedOrcaFiles/gbxnovatelacfr/driver.h

  • Committer: Nick Hillier
  • Date: 2011-08-03 03:16:26 UTC
  • Revision ID: nick.hillier@csiro.au-20110803031626-3ekp14jxlbda9bzf
Initial add of ROS wrappers for novatel INS around the Orca driver.

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
/*
 
2
 * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
 
3
 *               http://gearbox.sf.net/
 
4
 * Copyright (c) 2004-2008 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
 * Modified by Polly Alexander and Nick Hillier
 
12
 * 2009-2011
 
13
 */
 
14
#ifndef GBXNOVATELACFR_DRIVER_H
 
15
#define GBXNOVATELACFR_DRIVER_H
 
16
 
 
17
#include <cstdlib>
 
18
#include <string>
 
19
#include <memory>
 
20
#include <vector>
 
21
 
 
22
// forward declarations
 
23
// users don't need to know about serial devices or Tracers or imu decoders
 
24
namespace gbxserialacfr{
 
25
    class Serial;
 
26
}
 
27
namespace gbxutilacfr{
 
28
    class Tracer;
 
29
}
 
30
namespace gbxnovatelutilacfr{
 
31
    class ImuDecoder;
 
32
}
 
33
 
 
34
/** @ingroup gbx_library_novatel_acfr
 
35
@{ */
 
36
namespace gbxnovatelacfr{
 
37
 
 
38
//! Minimum information to configure the receiver in INS mode
 
39
class SimpleConfig{
 
40
public:
 
41
    //! @param imuType see @ref Config::imuType_ for details
 
42
    //! @param imuToGpsOffset see @ref Config::imuToGpsOffset_ for details
 
43
    SimpleConfig(std::string serialDevice, int baudRate, std::string imuType, std::vector<double > &imuToGpsOffset):
 
44
        serialDevice_(serialDevice),
 
45
        baudRate_(baudRate),
 
46
        imuType_(imuType),
 
47
        imuToGpsOffset_(imuToGpsOffset) {};
 
48
 
 
49
    //!@brief Returns true if the configuration is sane.
 
50
    //
 
51
    //! Checks include:
 
52
    //! - a non-empty device name
 
53
    //! - baud rate is supported by device (9600, 19200, 38400, 115200, 230400)
 
54
    //! - imuType refers to a known type
 
55
    //! - offset has size 3
 
56
    bool isValid() const;
 
57
    //! Dumps the config in human readable form
 
58
    std::string toString();
 
59
 
 
60
    std::string serialDevice_;
 
61
    int baudRate_;
 
62
    std::string imuType_;
 
63
    std::vector<double > imuToGpsOffset_;
 
64
};
 
65
 
 
66
//! Minimum information needed to configure the receiver in GPS only mode
 
67
class GpsOnlyConfig{
 
68
public:
 
69
    GpsOnlyConfig(std::string serialDevice, int baudRate):
 
70
        serialDevice_(serialDevice),
 
71
        baudRate_(baudRate) {};
 
72
 
 
73
    //!@brief Returns true if the configuration is sane.
 
74
    //
 
75
    //! Checks include:
 
76
    //! - a non-empty device name
 
77
    //! - baud rate is supported by device (9600, 19200, 38400, 115200, 230400)
 
78
    bool isValid() const;
 
79
    std::string toString();
 
80
 
 
81
    std::string serialDevice_;
 
82
    int baudRate_;
 
83
};
 
84
 
 
85
//! @brief All the information needed to configure the driver.
 
86
//
 
87
//! The device itself has even more options, consult your manual.
 
88
//! If all these possibilities don't seem to be sufficient, consult your friendly developer for extension (better yet, send a patch)
 
89
//! The easiest way to get a valid (and useful) Config, is to initialise it with a SimpleConfig (for INS operation) or GpsOnlyConfig (for GPS operation)
 
90
class Config{
 
91
public:
 
92
    //! @brief Shortcut to set up a Configuration for INS operation
 
93
    //
 
94
    //! yields a valid config, with reasonable defaults (RawImu 100Hz, InsPva 50Hz, BestGpsPos/Vel 5Hz)
 
95
    Config(const SimpleConfig &simpleCfg);
 
96
    //! @brief Shortcut to set up a Configuration for GPS operation
 
97
    //
 
98
    //! yields a valid config, for gps only operation (BestGpsPos/Vel 20Hz)
 
99
    Config(const GpsOnlyConfig &gpsOnlyCfg); 
 
100
    //! disables everything, so you can (and must) set just the options you need.
 
101
    Config();
 
102
 
 
103
    //! @brief Returns true if the configuration is sane.
 
104
    //
 
105
    //! Checks include:
 
106
    //! - a non-empty device name
 
107
    //! - baud rate is supported by device (9600, 19200, 38400, 57600, 115200, 230400)
 
108
    //! - imuType refers to a known type
 
109
    //! - offset has size 3
 
110
    //! - message rates are consistent and don't exceed serial-data-rate
 
111
    //! - not const, since it has limited self-fixing capabilities (for incorrect message-rates)
 
112
    bool isValid();
 
113
    //! Dumps the config in human readable form
 
114
    std::string toString();
 
115
 
 
116
    //!@name Serial settings
 
117
    //
 
118
    //!@{
 
119
    std::string serialDevice_;
 
120
    int baudRate_;
 
121
    //!@}
 
122
 
 
123
    //!@name IMU settings
 
124
    //
 
125
    //!@{
 
126
    bool enableImu_;
 
127
    std::string imuType_; //!< as expected by the "SETIMUTYPE" command (SPAN Technology for OEMV User Manual Rev 3, Table 15, page 64)
 
128
    //!@}
 
129
 
 
130
    //!@name Data settings
 
131
    //! First we disable all output in the ctor. Then all the messages set to true here are  enabled.
 
132
    //
 
133
    //!@{
 
134
    bool enableInsPva_;
 
135
        bool enableInsCov_; //!cov
 
136
    bool enableGpsPos_;
 
137
    bool enableGpsVel_;
 
138
    bool enableRawImu_;
 
139
    bool ignoreUnknownMessages_; //!< normally we throw an exception, set this to "true" if you want to enable some other message permanently.
 
140
 
 
141
    //!@}
 
142
 
 
143
    //!@name Data rate settings
 
144
    //!Time between messages in seconds (i.e. 0.01 == 100Hz). RawImu can only be reported at
 
145
    //!the "natural" rate of the IMU (100Hz or 200Hz, depending on model).
 
146
    //!We check in isValid() if any of these don't make sense
 
147
    //
 
148
    //!@{
 
149
    double dtInsPva_; //!< 100Hz max, if RawImu is enabled 50Hz max
 
150
        double dtInsCov_; //!cov
 
151
    double dtGpsPos_; //!< 20Hz max, 5Hz max if RawImu or InsPva is enabled
 
152
    double dtGpsVel_; //!< 20Hz max, 5Hz max if RawImu or InsPva is enabled
 
153
    bool fixInvalidRateSettings_; //!< Don't bitch about wrong rates, change them to something sensible
 
154
 
 
155
    //!@}
 
156
 
 
157
    //!@name INS settings
 
158
    //
 
159
    //!@{
 
160
    std::vector<double > imuToGpsOffset_;            //!< vector (xyz [m]) from IMU center to Antenna Phase Center, in IMU coordinates, vital for INS performance, make sure you get this right!
 
161
    std::vector<double > imuToGpsOffsetUncertainty_; //!< optional (size 3 or 0) xyz; !it is unclear to me whether these are factors, or absolute values (in [m])!
 
162
    bool enableInsOffset_;
 
163
    std::vector<double > insOffset_;                 //!< report INS position/velocity offset (xyz [m] in IMU coordinates) from the IMU center; useful e.g. to get data w.r. to robot's center of rotation
 
164
    bool enableInsPhaseUpdate_;                      //!< tightly coupled (phase based vs position based) filter; Chance of better performance in adverse conditions
 
165
    bool enableSetInitAzimuth_;                      //!< set initial azimuth and standard deviation to initialize ins 
 
166
    std::vector<double > initAzimuth_;                 //!< initial azimuth and standard deviation (initial_azimuth std_deviation) both in degrees
 
167
 
 
168
    //!@}
 
169
 
 
170
    //!@name GPS settings
 
171
    //
 
172
    //!@{
 
173
    bool enableCDGPS_; //!< code-differential corrections over satellite (North America/Canada)
 
174
    bool enableSBAS_;  //!< code-differential corrections over satellite on GPS frequencies (WAAS/EGNOS)
 
175
    bool enableRTK_;   //!< carrier-differential corrections (you need to set up your own base-station and wireless link), assumes RTCA corrections on COM2, 9600bps, 8N1 (hardcoded)
 
176
    bool enableUseOfOmniStarCarrier_; //!< carrier-differential corrections OMNIStarXP/HP (you need to get a subscription with them)
 
177
 
 
178
    //!@}
 
179
 
 
180
    //!@name INS settings for fast (dynamic) alignment
 
181
    //! !I'd strongly recommend that you read the manual _very_ closely!
 
182
    //! These guys enable the Span system to do an alignment while moving, they also allow you to mount the IMU in weird ways (e.g. upside down).
 
183
    //! It's worth to accept a fair amount of pain to mount the IMU in the recommended way. Otherwise you'll probably need a good amount of
 
184
    //! experimentation/calibration to get a parameter-set that works.
 
185
    //
 
186
    //!@{
 
187
    bool enableSetImuOrientation_;
 
188
    int setImuOrientation_;
 
189
    bool enableVehicleBodyRotation_;
 
190
    std::vector<double > vehicleBodyRotation_;
 
191
    std::vector<double > vehicleBodyRotationUncertainty_; //!< optional (size 3 or 0)
 
192
 
 
193
    //!@}
 
194
private:
 
195
};
 
196
 
 
197
//! possible Status Messages GenericData can contain
 
198
enum StatusMessageType {
 
199
    NoMsg,       //!< Nothing new, no message
 
200
    Initialising,//!< Nothing wrong, just not quite ready
 
201
    Ok,          //!< All good, but something to say
 
202
    Warning,     //!< Problem, likely to go away
 
203
    Fault        //!< Problem, probably fatal
 
204
};
 
205
 
 
206
//! Novatel's different solution status types.
 
207
//
 
208
//! Explanations from the manual.
 
209
enum GpsSolutionStatusType{
 
210
    SolComputed,       //!< Solution computed
 
211
    InsufficientObs,   //!< Insufficient observations
 
212
    NoConvergence,     //!< No convergence
 
213
    Singularity,       //!< Singularity at parameters matrix
 
214
    CovTrace,          //!< Covariance trace exceeds maximum (trace > 1000 m)
 
215
    TestDist,          //!< Test distance exceeded (maximum of 3 rejections if distance > 10 km)
 
216
    ColdStart,         //!< Not yet converged from cold start
 
217
    VHLimit,           //!< Height or velocity limits exceeded (in accordance with COCOM export licensing restrictions)
 
218
    Variance,          //!< Variance exceeds limits
 
219
    Residuals,         //!< Residuals are too large
 
220
    DeltaPos,          //!< Delta position is too large
 
221
    NegativeVar,       //!< Negative variance
 
222
    IntegrityWarning,  //!< Large residuals make position unreliable
 
223
    InsInactive,       //!< INS has not started yet
 
224
    InsAligning,       //!< INS doing its coarse alignment
 
225
    InsBad,            //!< INS position is bad
 
226
    ImuUnplugged,      //!< No IMU detected
 
227
    Pending,           //!< When a FIX POSITION command is entered, the receiver computes its own position and determines if the fixed position is valid
 
228
    InvalidFix,        //!< The fixed position, entered using the FIX POSITION command, is not valid
 
229
    ReservedGpsSolutionStatusType,
 
230
    UnknownGpsSolutionStatusType
 
231
};
 
232
 
 
233
//! Novatel's different fix types.
 
234
//
 
235
//! Sadly mixed for position/velocity with some INS gear thrown in; explanations from the manual.
 
236
enum GpsPosVelType{
 
237
    None,             //!< No solution
 
238
    FixedPos,         //!< Position has been fixed by the FIX POSITION command or by position averaging
 
239
    FixedHeight,      //!< Position has been fixed by the FIX HEIGHT, or FIX AUTO, command or by position averaging
 
240
    FloatConv,        //!< Solution from floating point carrier phase ambiguities
 
241
    WideLane,         //!< Solution from wide-lane ambiguities
 
242
    NarrowLane,       //!< Solution from narrow-lane ambiguities
 
243
    DopplerVelocity,  //!< Velocity computed using instantaneous Doppler
 
244
    Single,           //!< Single point position
 
245
    PsrDiff,          //!< Pseudorange differential solution
 
246
    Waas,             //!< Solution calculated using corrections from an SBAS
 
247
    Propagated,       //!< Propagated by a Kalman filter without new observations
 
248
    Omnistar,         //!< OmniSTAR VBS position (L1 sub-meter) a
 
249
    L1Float,          //!< Floating L1 ambiguity solution
 
250
    IonoFreeFloat,    //!< Floating ionospheric-free ambiguity solution
 
251
    NarrowFloat,      //!< Floating narrow-lane ambiguity solution
 
252
    L1Int,            //!< Integer L1 ambiguity solution
 
253
    WideInt,          //!< Integer wide-lane ambiguity solution
 
254
    NarrowInt,        //!< Integer narrow-lane ambiguity solution
 
255
    RtkDirectIns,     //!< RTK status where the RTK filter is directly initialized from the INS filter. b
 
256
    Ins,              //!< INS calculated position corrected for the antenna b
 
257
    InsPsrSp,         //!< INS pseudorange single point solution - no DGPS corrections b
 
258
    InsPsrDiff,       //!< INS pseudorange differential solution b
 
259
    InsRtkFloat,      //!< INS RTK floating point ambiguities solution b
 
260
    InsRtkFixed,      //!< INS RTK fixed ambiguities solution b
 
261
    OmniStarHp,       //!< OmniSTAR high precision a
 
262
    OmniStarXp,       //!< OmniSTAR extra precision a
 
263
    CdGps,            //!< Position solution using CDGPS corrections
 
264
    ReservedGpsPosVelType,
 
265
    UnknownGpsPosVelType
 
266
};
 
267
 
 
268
 
 
269
//! possible types GenericData can contain
 
270
enum DataType {
 
271
    //! GenericData is really InsPvaData
 
272
    InsPva,
 
273
    //! GenericData is really InsCovData
 
274
    InsCov,
 
275
    //! GenericData is really BestGpsPosData
 
276
    BestGpsPos,
 
277
    //! GenericData is really BestGpsVelData
 
278
    BestGpsVel,
 
279
    //! GenericData is really RawImuData
 
280
    RawImu
 
281
};
 
282
 
 
283
//! Generic (base) type returned by a read
 
284
class GenericData {
 
285
    public:
 
286
        virtual ~GenericData(){};
 
287
        virtual DataType type() const=0;
 
288
        virtual std::string toString()=0;
 
289
    private:
 
290
};
 
291
 
 
292
//! INS position/velocity/attitude information
 
293
class InsPvaData : public GenericData {
 
294
    public:
 
295
        DataType type() const {
 
296
            return InsPva;
 
297
        }
 
298
        std::string toString();
 
299
        int      gpsWeekNr;         //!< number of full weeks since midnight 05/Jan/1980 (UTC)
 
300
        double   secIntoWeek;       //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
 
301
        double   latitude;          //!< [deg] north positive WGS84
 
302
        double   longitude;         //!< [deg] east positive WGS84
 
303
        double   height;            //!< [m] above ellipsoid WGS84 (heigth_ellipsoid - undulation == height_geoid (aka AMSL)
 
304
 
 
305
        //!@name Velocity vector
 
306
        //!Relativ to true North/East and geoid vertical (I think)
 
307
        //
 
308
        //@{
 
309
        double   northVelocity;     //!< [m/s] south is negative
 
310
        double   eastVelocity;      //!< [m/s] west is negative
 
311
        double   upVelocity;        //!< [m/s] down is negative
 
312
 
 
313
        //@}
 
314
 
 
315
        //!@name Orientation
 
316
        //!The default IMU axis definitions are: Y - forward Z - up X - right hand side
 
317
        //
 
318
        //@{
 
319
        double   roll;              //!< [degree] right handed rotation from local level around y-axes
 
320
        double   pitch;             //!< [degree] right handed rotation from local level around x-axes
 
321
        double   azimuth;           //!< [degree] left handed around z-axes rotation from (true?) north clockwise
 
322
 
 
323
        //@}
 
324
 
 
325
        StatusMessageType statusMessageType;
 
326
        std::string statusMessage;
 
327
 
 
328
        int timeStampSec;  //!< in Computer time, beginning of message at serial port
 
329
        int timeStampUSec; //!< in Computer time, beginning of message at serial port
 
330
};
 
331
 
 
332
//!cov:start INSCOV information
 
333
class InsCovData : public GenericData {
 
334
    public:
 
335
        DataType type() const {
 
336
            return InsCov;
 
337
        }
 
338
        std::string toString();
 
339
        int      gpsWeekNr;         //!< number of full weeks since midnight 05/Jan/1980 (UTC)
 
340
        double   secIntoWeek;       //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
 
341
                
 
342
                double   posxx;
 
343
                double   posxy;
 
344
                double   posxz;
 
345
                double   posyx;
 
346
                double   posyy;
 
347
                double   posyz;
 
348
                double   poszx;
 
349
                double   poszy;
 
350
                double   poszz;
 
351
                
 
352
                double   attxx;
 
353
                double   attxy;
 
354
                double   attxz;
 
355
                double   attyx;
 
356
                double   attyy;
 
357
                double   attyz;
 
358
                double   attzx;
 
359
                double   attzy;
 
360
                double   attzz;
 
361
 
 
362
                double   velxx;
 
363
                double   velxy;
 
364
                double   velxz;
 
365
                double   velyx;
 
366
                double   velyy;
 
367
                double   velyz;
 
368
                double   velzx;
 
369
                double   velzy;
 
370
                double   velzz;
 
371
 
 
372
        int timeStampSec;  //!< in Computer time, beginning of message at serial port
 
373
        int timeStampUSec; //!< in Computer time, beginning of message at serial port
 
374
};
 
375
//!cov:end 
 
376
 
 
377
//! Gps position information
 
378
class BestGpsPosData : public GenericData {
 
379
    public:
 
380
        DataType type() const {
 
381
            return BestGpsPos;
 
382
        }
 
383
        std::string toString();
 
384
        int gpsWeekNr;                          //!< number of full weeks since midnight 05/Jan/1980 (UTC)
 
385
        unsigned int msIntoWeek;                //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
 
386
        GpsSolutionStatusType  solutionStatus;  //
 
387
        GpsPosVelType          positionType;    //
 
388
        double       latitude;                  //!< [deg] north positive
 
389
        double       longitude;                 //!< [deg] east positive
 
390
        double       heightAMSL;                //!< [m] AMSL == above mean sea level (geoid)
 
391
        float        undulation;                //!< [m] aka geoidal seperation: undulation == heigth_ellipsoid - height_geoid/AMSL
 
392
        unsigned int datumId;                   //
 
393
        float        sigmaLatitude;             //!< [m] 1 standard deviation error estimate
 
394
        float        sigmaLongitude;            //!< [m] 1 standard deviation error estimate
 
395
        float        sigmaHeight;               //!< [m] 1 standard deviation error estimate
 
396
        char         baseStationId[4];          //
 
397
        float        diffAge;                   //!< [s] how old the correction info from the basestation is
 
398
        float        solutionAge;               //!< [s] 
 
399
        int          numObservations;           //!< number of observations tracked (?) L1 code/carrier/doppler + L2 code/carrier/doppler?
 
400
        int          numL1Ranges;               //!< number of L1 ranges used in computation (?)
 
401
        int          numL1RangesRTK;            //!< number of L1 ranges above the RTK mask angle (??) number of L1 carrier ranges used?
 
402
        int          numL2RangesRTK;            //!< number of L2 ranges above the RTK mask angle (??) number of L2 carrier ranges used?
 
403
 
 
404
        StatusMessageType statusMessageType;
 
405
        std::string statusMessage;
 
406
 
 
407
        int timeStampSec;  //!< in Computer time, beginning of message at serial port
 
408
        int timeStampUSec; //!< in Computer time, beginning of message at serial port
 
409
};
 
410
 
 
411
//! Gps velocity information
 
412
class BestGpsVelData : public GenericData {
 
413
    public:
 
414
        DataType type() const {
 
415
            return BestGpsVel;
 
416
        }
 
417
        std::string toString();
 
418
        int          gpsWeekNr;                 //!< number of full weeks since midnight 05/Jan/1980 (UTC)
 
419
        unsigned int msIntoWeek;                //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
 
420
        GpsSolutionStatusType  solutionStatus;  //
 
421
        GpsPosVelType          velocityType;    //
 
422
        float        latency;                   //!< [s] gps speed can be calculated from instantanious or integrated doppler. The latter refers to the average speed over the last interval -> is delayed by half an interval
 
423
        float        diffAge;                   //!< [s]
 
424
        double       horizontalSpeed;           //!< [m/s]
 
425
        double       trackOverGround;           //!< [deg] "heading" of the speed vector w. respect to true North
 
426
        double       verticalSpeed;             //!< [m/s]
 
427
 
 
428
        StatusMessageType statusMessageType;
 
429
        std::string statusMessage;
 
430
 
 
431
        int timeStampSec;  //!< in Computer time, beginning of message at serial port
 
432
        int timeStampUSec; //!< in Computer time, beginning of message at serial port
 
433
};
 
434
 
 
435
//! Raw IMU information
 
436
class RawImuData : public GenericData {
 
437
    public:
 
438
        DataType type() const {
 
439
            return RawImu;
 
440
        }
 
441
        std::string toString();
 
442
        int    gpsWeekNr;   //!< number of full weeks since midnight 05/Jan/1980 (UTC)
 
443
        double secIntoWeek; //!< yields GPS-time (together with @ref gpsWeekNr); continous (contrary to UTC which uses leapseconds)
 
444
        //!@name Change in speed
 
445
        //!Divide by dt to get accelerations.
 
446
        //!The default IMU axis definitions are: Y - forward, Z - up, X - right hand side
 
447
        //
 
448
        //@{
 
449
        double zDeltaV;   //!< [m/s] up positive
 
450
        double yDeltaV;   //!< [m/s] forward positive
 
451
        double xDeltaV;   //!< [m/s] right positive
 
452
 
 
453
        //@}
 
454
 
 
455
        //!@name Change in orientation
 
456
        //!Divide by dt to get turn-rates.
 
457
        //!The default IMU axis definitions are: Y - forward, Z - up, X - right hand side
 
458
        //
 
459
        //@{
 
460
        double zDeltaAng; //!< [rad] right handed around z
 
461
        double yDeltaAng; //!< [rad] right handed around y
 
462
        double xDeltaAng; //!< [rad] right handed around x
 
463
 
 
464
        //@}
 
465
 
 
466
        StatusMessageType statusMessageType;
 
467
        std::string statusMessage;
 
468
 
 
469
        int timeStampSec;  //!< in Computer time, beginning of message at serial port
 
470
        int timeStampUSec; //!< in Computer time, beginning of message at serial port
 
471
};
 
472
 
 
473
//! The actual Driver
 
474
//! @brief the idea is to create one of these guys (with a valid Config), and then treat it as a data-source, i.e. call read() on it in some kind of loop
 
475
class Driver {
 
476
public:
 
477
 
 
478
    //! @name Ctor:
 
479
    //! @brief Tries to establish serial communication to the GPS receiver, then configures it
 
480
    //
 
481
    //!@{
 
482
 
 
483
    //! @brief dumps tracing messages to the console
 
484
    //
 
485
    //! implements tracing internally as a gbxutilacfr::TrivialTracer
 
486
    Driver( const Config &cfg);
 
487
    //!@brief full control over tracing (e.g. to syslog)
 
488
    //!@param tracer you need to provide this guy (via new), do NOT delete it, it will be deleted when your Driver object goes out of scope
 
489
    Driver( const Config &cfg, gbxutilacfr::Tracer &tracer);
 
490
    //!@}
 
491
    ~Driver();
 
492
 
 
493
    /*! @brief Blocking read, returns one message
 
494
 
 
495
        Throws gbxutilacfr::Exception when a problem is encountered (derives from std::exception).
 
496
        Throws gbxutilacfr::HardwareException when a (fatal) problem with the hardware is encountered
 
497
        */
 
498
    std::auto_ptr<GenericData> read();
 
499
 
 
500
private:
 
501
 
 
502
    // does the leg-work for the constructor (via the following guys)
 
503
    void configure();
 
504
    // establish a serial connection to the receiver
 
505
    void connectToHardware();
 
506
    // set parameters related to the IMU
 
507
    void configureImu();
 
508
    // set parameters related to the INS
 
509
    void configureIns();
 
510
    // set parameters related to GPS
 
511
    void configureGps();
 
512
    // turn on data messages we are interested in
 
513
    void requestData();
 
514
 
 
515
    std::auto_ptr<gbxnovatelutilacfr::ImuDecoder> imuDecoder_;
 
516
 
 
517
    std::auto_ptr<gbxserialacfr::Serial> serial_;
 
518
    int baud_;
 
519
 
 
520
    Config config_;
 
521
    std::auto_ptr<gbxutilacfr::Tracer> tracerInternal_;
 
522
    gbxutilacfr::Tracer& tracer_;
 
523
};
 
524
 
 
525
 
 
526
} // namespace
 
527
/** @} */
 
528
#endif