~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.cpp

  • 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
/*
 
12
 * Modified by Polly Alexander and Nick Hillier
 
13
 * 2009-2011
 
14
 */
 
15
 
 
16
#include <gbxnovatelacfr/driver.h>
 
17
 
 
18
#include <gbxnovatelacfr/gbxnovatelutilacfr/serialconnectivity.h>
 
19
#include <gbxnovatelacfr/gbxnovatelutilacfr/novatelmessages.h>
 
20
#include <gbxnovatelacfr/gbxnovatelutilacfr/imudecoder.h>
 
21
#include <gbxnovatelacfr/gbxnovatelutilacfr/receiverstatusdecoder.h>
 
22
#include <gbxnovatelacfr/gbxnovatelutilacfr/crc32.h>
 
23
 
 
24
#include <gbxserialacfr/gbxserialacfr.h>
 
25
#include <gbxutilacfr/gbxutilacfr.h>
 
26
#include <gbxutilacfr/trivialtracer.h>
 
27
 
 
28
#include <iostream>
 
29
#include <iomanip>
 
30
#include <sstream>
 
31
#include <vector>
 
32
 
 
33
#include <stdlib.h>
 
34
#include <assert.h>
 
35
#include <string.h>
 
36
#include <math.h>
 
37
#include <errno.h>
 
38
#include <limits.h>
 
39
#include <stdio.h>
 
40
 
 
41
#include <sys/time.h>
 
42
#include <time.h>
 
43
 
 
44
using namespace std;
 
45
using namespace gbxserialacfr;
 
46
 
 
47
namespace gnua = gbxnovatelutilacfr;
 
48
namespace gna = gbxnovatelacfr;
 
49
namespace gua = gbxutilacfr;
 
50
 
 
51
namespace {
 
52
#pragma pack(push,1)
 
53
    // Our quick and dirty message-decoder
 
54
    // Maximum size is (legacy) hardcoded to 516bytes. The real upper limit (for long headers) would be 65535+28+4 bytes.
 
55
    // Problems should hopefully be picked up by checkParserAssumptions().
 
56
    //
 
57
    // Todo: write a proper parser
 
58
    static const int rawMsgSize = 516; // maximum size of message we can decode (including header, crc, and data)
 
59
    static const int longMsgDataSize = rawMsgSize - sizeof(gnua::Oem4BinaryHeader);
 
60
    static const int shortMsgDataSize = rawMsgSize - sizeof(gnua::Oem4ShortBinaryHeader);
 
61
    union NovatelMessage{
 
62
        struct{
 
63
            gnua::Oem4BinaryHeader header;
 
64
            uint8_t               data[longMsgDataSize];
 
65
        };
 
66
        struct{
 
67
            gnua::Oem4ShortBinaryHeader shortHeader;
 
68
            uint8_t               shortData[shortMsgDataSize];
 
69
        };
 
70
        uint8_t rawMessage[rawMsgSize];
 
71
 
 
72
        // these guys are used to directly decode messages;
 
73
        // obviously fails on endian mismatch, any sort of size mismatch and is rather nasty in general;
 
74
        // feel free to implement something better
 
75
        gnua::BestGpsPosLogB bestGpsPos;
 
76
        gnua::BestGpsVelLogB bestGpsVel;
 
77
        gnua::InsPvaLogSB   insPva;
 
78
                gnua::InsCovLogSB   insCov; //!cov
 
79
        gnua::RawImuLogSB   rawImu;
 
80
    };
 
81
#pragma pack(pop)
 
82
    // this guy checks if the assumptions for the gear above are correct (abort()s through assert() otherwise)
 
83
    void checkParserAssumptions();
 
84
 
 
85
    // returns the id of the message it read (msg), or throws an gbxutilacfr::Exception
 
86
    uint16_t readNovatelMessage(union NovatelMessage &msg, struct timeval &timeStamp, gbxserialacfr::Serial *serial);
 
87
 
 
88
    // take novatel data and create stuff according to our external api
 
89
    std::auto_ptr<gna::GenericData> createExternalMsg(gnua::InsPvaLogSB &insPva, struct timeval &timeStamp);
 
90
        std::auto_ptr<gna::GenericData> createExternalMsg(gnua::InsCovLogSB &insCov, struct timeval &timeStamp); //!cov
 
91
    std::auto_ptr<gna::GenericData> createExternalMsg(gnua::BestGpsPosLogB &bestGpsPos, struct timeval &timeStamp);
 
92
    std::auto_ptr<gna::GenericData> createExternalMsg(gnua::BestGpsVelLogB &bestGpsVel, struct timeval &timeStamp);
 
93
    std::auto_ptr<gna::GenericData> createExternalMsg(gnua::RawImuLogSB &rawImu, struct timeval &timeStamp, gnua::ImuDecoder *imuDecoder);
 
94
    enum gna::GpsSolutionStatusType externalGpsSolutionStatus(uint32_t novatelGpsSolutionStatus);
 
95
    enum gna::GpsPosVelType externalGpsPosVelType(uint32_t novatelGpsPosVelType);
 
96
 
 
97
    // helper functions for the toString() gear
 
98
    std::string statusToString(gna::StatusMessageType statusMessageType, std::string statusMessage);
 
99
    std::string doubleVectorToString(vector<double > &vec, std::string seperator = std::string(" "));
 
100
}
 
101
 
 
102
namespace gbxnovatelacfr
 
103
{
 
104
Driver::Driver( const Config& cfg) :
 
105
    baud_(115200),
 
106
    config_(cfg),
 
107
    tracerInternal_(new gbxutilacfr::TrivialTracer()),
 
108
    tracer_(*(tracerInternal_.get()))
 
109
{
 
110
    //assert(0 != tracer_.get());
 
111
    configure();
 
112
    return;
 
113
}
 
114
 
 
115
Driver::Driver( const Config& cfg,
 
116
        gbxutilacfr::Tracer &tracer) :
 
117
    baud_(115200),
 
118
    config_(cfg),
 
119
    tracerInternal_(0),
 
120
    tracer_(tracer)
 
121
{
 
122
    //assert(0 != tracer_.get());
 
123
    configure();
 
124
    return;
 
125
}
 
126
 
 
127
void
 
128
Driver::configure( ) {
 
129
    if(false == config_.isValid()){
 
130
        throw (gua::Exception(ERROR_INFO, "Invalid Configuration!"));
 
131
    }
 
132
 
 
133
    checkParserAssumptions();
 
134
 
 
135
    // configure serial port
 
136
    baud_ = config_.baudRate_;
 
137
    std::string serialDevice = config_.serialDevice_;
 
138
    serial_.reset(new Serial( serialDevice, baud_, Serial::Timeout(1,0) ));
 
139
    assert(0 != serial_.get());
 
140
    serial_->setDebugLevel(0);
 
141
 
 
142
    connectToHardware();
 
143
    // just in case something is running... stops the novatel logging any messages
 
144
    serial_->writeString( "unlogall\r\n" );
 
145
    serial_->drain();
 
146
    serial_->flush();
 
147
    configureImu();
 
148
    configureIns();
 
149
    configureGps();
 
150
    requestData();
 
151
    serial_->flush();
 
152
    tracer_.info("Setup done, starting normal operation!");
 
153
    return;
 
154
}
 
155
 
 
156
Driver::~Driver() {
 
157
    // just in case something is running... stops the novatel logging any messages
 
158
    try{
 
159
        tracer_.info("Stopping NovatelSpan driver!");
 
160
        serial_->flush();
 
161
        serial_->writeString( "unlogall\r\n" );
 
162
        serial_->drain();
 
163
        tracer_.info("NovatelSpan driver stopped!");
 
164
    }
 
165
    catch(...){
 
166
        //no throwing from destructors
 
167
    }
 
168
}
 
169
 
 
170
void
 
171
Driver::connectToHardware() {
 
172
    // baudrates we test for; this is
 
173
    // _not_ all the baudrates the receiver
 
174
    // can possible be set to
 
175
    int baudrates[]={
 
176
        9600,
 
177
        19200,
 
178
        38400,
 
179
        57600,
 
180
        115200,
 
181
        230400
 
182
    };
 
183
    int currentBaudrate = 0;
 
184
    bool correctBaudrate = false;
 
185
 
 
186
    tracer_.info( "Trying to hook up to receiver at different Baudrates" );
 
187
    int maxTry = 4;
 
188
    int successThresh = 4;
 
189
    int timeOutMsec = 250;
 
190
    std::string challenge("unlogall\r\n");
 
191
    std::string ack("<OK");
 
192
    size_t i=0;
 
193
    while(false == correctBaudrate && i<sizeof baudrates/sizeof baudrates[0]){
 
194
        currentBaudrate = baudrates[i];
 
195
        correctBaudrate = gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, currentBaudrate);
 
196
        i++;
 
197
    }
 
198
    if(false == correctBaudrate){
 
199
        throw ( gua::Exception(ERROR_INFO, "!Failed to establish a connection to the receiver! Check physical connections; Check manually (minicom) for Baudrates < 9600kb/s."));
 
200
    }
 
201
    // ok, we've got a working link
 
202
    std::stringstream ss;
 
203
    ss << "Established connection at "
 
204
        << currentBaudrate << "bps; "
 
205
        << "Resetting to configured speed: "
 
206
        << baud_ << "bps";
 
207
    tracer_.info(ss.str());
 
208
    char str[256];
 
209
    sprintf( str,"com com1 %d n 8 1 n off on\r\n", baud_ );
 
210
    serial_->writeString( str );
 
211
    if(false == gnua::testConnectivity( challenge, ack, *(serial_.get()), timeOutMsec, maxTry, successThresh, baud_)){
 
212
        throw ( gua::Exception(ERROR_INFO, "!Failed to reset connection to configured baudrate!"));
 
213
    }
 
214
    return;
 
215
}
 
216
 
 
217
void
 
218
Driver::configureImu() {
 
219
    std::string challenge = "";
 
220
    std::string ack = "<OK";
 
221
    std::string errorResponse = "";
 
222
    int timeOutMsec = 200;
 
223
 
 
224
    if(config_.enableImu_){
 
225
        tracer_.info("Configuring IMU, switching INS ON!");
 
226
        imuDecoder_.reset(gnua::createImuDecoder(config_.imuType_));
 
227
        std::stringstream ss;
 
228
        // tell the novatel what serial port the imu is attached to (com3 == aux)
 
229
//        challenge = ( "interfacemode com3 imu imu on\r\n" );
 
230
//        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
231
//            ss.str("");
 
232
//            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
233
//            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
234
//        }
 
235
//        // the type of imu being used
 
236
//        ss << "setimutype "
 
237
//            << config_.imuType_
 
238
//            << "\r\n";
 
239
//        challenge = ss.str();
 
240
//        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
241
//            ss.str("");
 
242
//            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
243
//            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
244
//        }
 
245
        challenge = "inscommand enable\r\n";
 
246
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
247
            ss.str("");
 
248
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
249
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
250
        }
 
251
        //force the IMU to re-align at every startup
 
252
        //put = serial_->writeString( "inscommand reset\r\n" );
 
253
        //tracer_.info("Reset IMU; Waiting 5 seconds before continuing!");
 
254
        //sleep(5);
 
255
    }else{
 
256
        tracer_.info("No IMU, switching INS OFF!");
 
257
        challenge = "inscommand disable\r\n";
 
258
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
259
            std::stringstream ss;
 
260
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
261
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
262
        }
 
263
    }
 
264
    return;
 
265
}
 
266
 
 
267
void
 
268
Driver::configureIns() {
 
269
    std::string challenge = "";
 
270
    std::string ack = "<OK";
 
271
    std::string errorResponse = "";
 
272
    int timeOutMsec = 200;
 
273
 
 
274
    if(config_.enableSetImuOrientation_ && config_.enableImu_){
 
275
        std::stringstream ss;
 
276
        // imu orientation constant
 
277
        // this tells the imu where its z axis (up) is pointing. constants defined in manual.
 
278
        // with imu mounted upside down, constant is 6 and axes are remapped: x = y, y = x, -z = z 
 
279
        ss << "setimuorientation " << config_.setImuOrientation_ << "\r\n";
 
280
        challenge = ss.str();
 
281
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
282
            ss.str("");
 
283
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
284
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
285
        }
 
286
    }
 
287
 
 
288
    if(config_.enableSetInitAzimuth_ && config_.enableImu_){
 
289
        std::stringstream ss;
 
290
        // ins set initial azimuth and standard deviation (both in degress) to bootstrap ins
 
291
        ss << "setinitazimuth "
 
292
             << config_.initAzimuth_[0] << " "
 
293
             << config_.initAzimuth_[1]  << "\r\n";
 
294
        challenge = ss.str();
 
295
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
296
            ss.str("");
 
297
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
298
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
299
        }
 
300
    }
 
301
    if(config_.enableVehicleBodyRotation_ && config_.enableImu_){
 
302
        std::stringstream ss;
 
303
        // vehicle to imu body rotation
 
304
        // angular offset from the vehicle to the imu body. unclear how this relates to imu orientation command 
 
305
        // the novatel docs are not especially clear on this stuff; It's highly recommended to mount the IMU
 
306
        // exactly as advised by novatel and just ignore this
 
307
        ss << "vehiclebodyrotation "
 
308
            << config_.vehicleBodyRotation_[0] << " "
 
309
            << config_.vehicleBodyRotation_[1] << " "
 
310
            << config_.vehicleBodyRotation_[2] << " ";
 
311
        if(3 == config_.vehicleBodyRotationUncertainty_.size()){
 
312
            // optional, vehicle to imu body rotation uncertainty
 
313
            ss << config_.vehicleBodyRotationUncertainty_[0] << " " 
 
314
                << config_.vehicleBodyRotationUncertainty_[1] << " "
 
315
                << config_.vehicleBodyRotationUncertainty_[2];
 
316
        }
 
317
        ss << "\r\n";
 
318
        challenge = ss.str();
 
319
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
320
            ss.str("");
 
321
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
322
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
323
        }
 
324
 
 
325
        std::stringstream sa;
 
326
        // apply vehicle body rotation parameters
 
327
        sa << "applyvehiclebodyrotation enable"
 
328
              << "\r\n";
 
329
        challenge = sa.str();
 
330
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
331
            sa.str("");
 
332
            sa << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
333
            throw ( gua::Exception(ERROR_INFO, sa.str()));
 
334
        }
 
335
    }
 
336
 
 
337
    if(config_.enableImu_){
 
338
        std::stringstream ss;
 
339
        // The span system kalman fiter needs this info; make _sure_ you do this right
 
340
        ss << "setimutoantoffset "
 
341
            << config_.imuToGpsOffset_[0] << " "
 
342
            << config_.imuToGpsOffset_[1] << " "
 
343
            << config_.imuToGpsOffset_[2];
 
344
 
 
345
        if( 3 == config_.imuToGpsOffsetUncertainty_.size() ){
 
346
            ss << " "
 
347
                << config_.imuToGpsOffsetUncertainty_[0] << " "
 
348
                << config_.imuToGpsOffsetUncertainty_[1] << " "
 
349
                << config_.imuToGpsOffsetUncertainty_[2];
 
350
        }
 
351
        ss << "\r\n";
 
352
        challenge = ss.str();
 
353
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
354
            ss.str("");
 
355
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
356
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
357
        }
 
358
    }
 
359
    return;
 
360
}
 
361
 
 
362
void
 
363
Driver::configureGps() {
 
364
    std::string challenge = "";
 
365
    std::string ack = "<OK";
 
366
    std::string errorResponse = "";
 
367
    int timeOutMsec = 200;
 
368
 
 
369
    // hardcoded settings first
 
370
 
 
371
    // turn off posave as this command implements position averaging for base stations.
 
372
    // make sure that fixposition has not been set
 
373
    challenge = ( "fix none\r\n" );
 
374
    if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
375
        std::stringstream ss;
 
376
        ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
377
        throw ( gua::Exception(ERROR_INFO, ss.str()));
 
378
    }
 
379
    // select the geodetic datum for operation of the receiver (wgs84 = default)
 
380
    challenge = ( "datum wgs84\r\n" );
 
381
    if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
382
        std::stringstream ss;
 
383
        ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
384
        throw ( gua::Exception(ERROR_INFO, ss.str()));
 
385
    }
 
386
    //Let the receiver figure out which range corrections are best
 
387
    challenge = ( "PSRDIFFSOURCE AUTO\r\n" );
 
388
    if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
389
        std::stringstream ss;
 
390
        ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
391
        throw ( gua::Exception(ERROR_INFO, ss.str()));
 
392
    }
 
393
 
 
394
    // CDGPS
 
395
    if(config_.enableCDGPS_){
 
396
        tracer_.info("Turning on CDGPS!");
 
397
        challenge = ( "ASSIGNLBAND CDGPS 1547547 4800\r\n" );
 
398
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
399
            std::stringstream ss;
 
400
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
401
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
402
        }
 
403
    }
 
404
 
 
405
    // turn SBAS on/off (essentially global DGPS)
 
406
    if(config_.enableSBAS_){
 
407
        tracer_.info("Turning on SBAS!");
 
408
        challenge = ( "SBASCONTROL ENABLE Auto 0 ZEROTOTWO\r\n");
 
409
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
410
            std::stringstream ss;
 
411
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
412
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
413
        }
 
414
        //we try to use WAAS satellites even below the horizon
 
415
        challenge = ( "WAASECUTOFF -5.0\r\n");
 
416
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
417
            std::stringstream ss;
 
418
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
419
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
420
        }
 
421
    }
 
422
    else{
 
423
        tracer_.info("Turning off SBAS!");
 
424
        challenge = ( "SBASCONTROL DISABLE Auto 0 NONE\r\n");
 
425
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
426
            std::stringstream ss;
 
427
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
428
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
429
        }
 
430
    }
 
431
 
 
432
    // rtk
 
433
    if(config_.enableRTK_){
 
434
        tracer_.info("Turning on RTK!");
 
435
        //challenge = ( "com com2,9600,n,8,1,n,off,on\r\n" );
 
436
        challenge = ( "com com2,9600,n,8,1,n,off,off\r\n" );
 
437
        cout << challenge << "this is really here also" << endl;
 
438
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
439
            std::stringstream ss;
 
440
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
441
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
442
        }
 
443
        //challenge = ( "interfacemode com2 rtca none\r\n" );
 
444
        challenge = ( "interfacemode com2 rtcmv3 none off \r\n" );
 
445
        cout << challenge << "this is really here" << endl;
 
446
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
447
            std::stringstream ss;
 
448
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
449
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
450
        }
 
451
    }
 
452
 
 
453
//    if(config_.enableUseOfOmniStarCarrier_){
 
454
//        //Let the receiver figure out which rtk corrections are best
 
455
//        challenge = ( "RTKSOURCE AUTO\r\n" );
 
456
//        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
457
//            std::stringstream ss;
 
458
//            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
459
//            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
460
//        }
 
461
//    }else{
 
462
//        //We only use our own rtk corrections; _not_ OmniSTAR HP/XP
 
463
//        challenge = ( "RTKSOURCE RTCA ANY\r\n" );
 
464
//        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
465
//            std::stringstream ss;
 
466
//            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
467
//            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
468
//        }
 
469
//    }
 
470
    return;
 
471
}
 
472
 
 
473
void
 
474
Driver::requestData() {
 
475
    std::string challenge = "";
 
476
    std::string ack = "<OK";
 
477
    std::string errorResponse = "";
 
478
    int timeOutMsec = 200;
 
479
    //we assume that the config_ has been checked at this point (isValid())
 
480
    //so we don't need to check that the rates make sense
 
481
 
 
482
    // GPS messages
 
483
 
 
484
    // gps position without ins
 
485
    if(config_.enableGpsPos_){
 
486
        std::stringstream ss;
 
487
        ss << "Turning on GPS position at " << 1.0/config_.dtGpsPos_ << "Hz!";
 
488
        tracer_.info(ss.str().c_str());
 
489
        ss.str("");
 
490
        ss << "log bestgpsposb ontime " << config_.dtGpsPos_ << "\r\n";
 
491
        challenge = ss.str();
 
492
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
493
            ss.str("");
 
494
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
495
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
496
        }
 
497
    }
 
498
 
 
499
    // gps velocity without ins
 
500
    if(config_.enableGpsVel_){
 
501
        std::stringstream ss;
 
502
        ss << "Turning on GPS velocity at " << 1.0/config_.dtGpsVel_ << "Hz!";
 
503
        tracer_.info(ss.str().c_str());
 
504
        ss.str("");
 
505
        ss << "log bestgpsvelb ontime " << config_.dtGpsVel_ << "\r\n";
 
506
        challenge = ss.str();
 
507
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
508
            ss.str("");
 
509
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
510
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
511
        }
 
512
    }
 
513
 
 
514
 
 
515
    // INS messages
 
516
 
 
517
    // pva data in wgs84 coordinates
 
518
    if(config_.enableInsPva_){
 
519
        std::stringstream ss;
 
520
        ss << "Turning on INS position/velocity/orientation at " << 1.0/config_.dtInsPva_ << "Hz!";
 
521
        tracer_.info(ss.str().c_str());
 
522
        ss.str("");
 
523
        ss << "log inspvasb ontime " << config_.dtInsPva_ << "\r\n";
 
524
        challenge = ss.str();
 
525
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
526
            ss.str("");
 
527
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
528
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
529
        }
 
530
    }
 
531
 
 
532
        //!cov:start ins cov data
 
533
    if(config_.enableInsCov_){
 
534
        std::stringstream ss;
 
535
        ss << "Turning on INSCOV at " << 1.0/config_.dtInsCov_ << "Hz!";
 
536
        tracer_.info(ss.str().c_str());
 
537
        ss.str("");
 
538
        ss << "log inscovsb ontime " << config_.dtInsCov_ << "\r\n";
 
539
        challenge = ss.str();
 
540
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
541
            ss.str("");
 
542
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
543
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
544
        }
 
545
    }
 
546
    //!cov:end
 
547
 
 
548
    // IMU messages
 
549
 
 
550
    // raw accelerometer and gyro data
 
551
    if(config_.enableRawImu_){
 
552
        tracer_.info("Turning on raw imu data!");
 
553
        challenge = ( "log rawimusb onnew\r\n" );
 
554
        //challenge = ( "log rawimusb onchanged\r\n" );
 
555
        if(false == gnua::sendCmdWaitForResponse(challenge, ack, errorResponse, *(serial_.get()), timeOutMsec)){
 
556
            std::stringstream ss;
 
557
            ss << " Failure!\n Tried to send: " << challenge << " Receiver responded: " << errorResponse;
 
558
            throw ( gua::Exception(ERROR_INFO, ss.str()));
 
559
        }
 
560
    }
 
561
 
 
562
    return;
 
563
}
 
564
 
 
565
std::auto_ptr<GenericData>
 
566
Driver::read(){
 
567
    union NovatelMessage msg;
 
568
    std::auto_ptr<GenericData> data;
 
569
    data.reset(0);
 
570
    struct timeval timeStamp = {0,0};
 
571
 
 
572
    // read msg from hardware
 
573
    do{
 
574
        // Timeouts are not adjusted once a serial call returns;
 
575
        // So we could be stuck here for longer than the set timeout.
 
576
        int ret = serial_->bytesAvailableWait();
 
577
        if ( ret > 0 ) {
 
578
            switch(readNovatelMessage(msg, timeStamp, serial_.get())){
 
579
                case gnua::InsPvaSBLogType:
 
580
                    data = createExternalMsg(msg.insPva, timeStamp);
 
581
                    break;
 
582
                            //!cov:start
 
583
                case gnua::InsCovSBLogType:
 
584
                    data = createExternalMsg(msg.insCov, timeStamp);
 
585
                    break;
 
586
                                //!cov:end
 
587
                case gnua::BestGpsVelBLogType:
 
588
                    data = createExternalMsg(msg.bestGpsVel, timeStamp);
 
589
                    break;
 
590
                case gnua::BestGpsPosBLogType:
 
591
                    data = createExternalMsg(msg.bestGpsPos, timeStamp);
 
592
                    break;
 
593
                case gnua::RawImuSBLogType:
 
594
                    data = createExternalMsg(msg.rawImu, timeStamp, imuDecoder_.get());
 
595
                    break;
 
596
                case gnua::InvalidLogType:
 
597
                    {
 
598
                        std::stringstream ss;
 
599
                        ss <<"Id invalid, looks like we didn't get anything from the receiver!" << std::endl;
 
600
                        throw ( gua::Exception(ERROR_INFO, ss.str()) );
 
601
                    }
 
602
                    break;
 
603
                default:
 
604
                    {
 
605
                        std::stringstream ss;
 
606
                        ss <<"Got unexpected message type from receiver; id: " << msg.header.msgId << std::endl;
 
607
                        if(config_.ignoreUnknownMessages_){
 
608
                            tracer_.warning(ss.str());
 
609
                        }else{
 
610
                            throw ( gua::Exception(ERROR_INFO, ss.str()) );
 
611
                        }
 
612
                    }
 
613
                    break;
 
614
            }
 
615
        }
 
616
        else {
 
617
            throw ( gua::Exception(ERROR_INFO, "Timed out while waiting for data"));
 
618
        }
 
619
    }while(NULL == data.get()); // repeat till we get a known message
 
620
 
 
621
    return data;
 
622
}
 
623
 
 
624
Config::Config(const SimpleConfig &simpleCfg) :
 
625
    serialDevice_(simpleCfg.serialDevice_),
 
626
    baudRate_(simpleCfg.baudRate_),
 
627
    enableImu_(true),
 
628
    imuType_(simpleCfg.imuType_),
 
629
    enableInsPva_(true),
 
630
    enableInsCov_(true), //!cov
 
631
    enableGpsPos_(true),
 
632
    enableGpsVel_(true),
 
633
    enableRawImu_(true),
 
634
    ignoreUnknownMessages_(false),
 
635
    dtInsPva_(0.02),
 
636
        dtInsCov_(0.02), //!cov
 
637
    dtGpsPos_(0.2),
 
638
    dtGpsVel_(0.2),
 
639
    fixInvalidRateSettings_(false),
 
640
    imuToGpsOffset_(simpleCfg.imuToGpsOffset_),
 
641
    imuToGpsOffsetUncertainty_(0,0.0),
 
642
    initAzimuth_(0,0.0),
 
643
    enableInsOffset_(false),
 
644
    enableSetInitAzimuth_(false),
 
645
    enableInsPhaseUpdate_(false),
 
646
    enableCDGPS_(false),
 
647
    enableSBAS_(false),
 
648
    enableRTK_(false),
 
649
    enableUseOfOmniStarCarrier_(false),
 
650
    enableSetImuOrientation_(false),
 
651
    enableVehicleBodyRotation_(false) {
 
652
}
 
653
Config::Config(const GpsOnlyConfig &gpsOnlyCfg) :
 
654
    serialDevice_(gpsOnlyCfg.serialDevice_),
 
655
    baudRate_(gpsOnlyCfg.baudRate_),
 
656
    enableImu_(false),
 
657
    enableInsPva_(false),
 
658
    enableInsCov_(false), //!cov
 
659
    enableGpsPos_(true),
 
660
    enableGpsVel_(true),
 
661
    enableRawImu_(false),
 
662
    ignoreUnknownMessages_(false),
 
663
    dtGpsPos_(0.05),
 
664
    dtGpsVel_(0.05),
 
665
    fixInvalidRateSettings_(false),
 
666
    enableCDGPS_(false),
 
667
    enableSBAS_(false),
 
668
    enableRTK_(false),
 
669
    enableUseOfOmniStarCarrier_(false) {
 
670
}
 
671
Config::Config() :
 
672
    serialDevice_(""),
 
673
    baudRate_(0),
 
674
    enableImu_(false),
 
675
    imuType_(""),
 
676
    enableInsPva_(false),
 
677
        enableInsCov_(false), //!cov
 
678
    enableGpsPos_(false),
 
679
    enableGpsVel_(false),
 
680
    enableRawImu_(false),
 
681
    ignoreUnknownMessages_(false),
 
682
    dtInsPva_(1.0),
 
683
        dtInsCov_(1.0), //!cov
 
684
    dtGpsPos_(1.0),
 
685
    dtGpsVel_(1.0),
 
686
    fixInvalidRateSettings_(false),
 
687
    imuToGpsOffset_(0,0.0),
 
688
    imuToGpsOffsetUncertainty_(0,0.0),
 
689
    initAzimuth_(0,0.0),
 
690
    enableInsOffset_(false),
 
691
    insOffset_(0,0.0),
 
692
    enableInsPhaseUpdate_(false),
 
693
    enableCDGPS_(false),
 
694
    enableSBAS_(false),
 
695
    enableRTK_(false),
 
696
    enableUseOfOmniStarCarrier_(false),
 
697
    enableSetImuOrientation_(false),
 
698
    enableSetInitAzimuth_(false),
 
699
    setImuOrientation_(0),
 
700
    enableVehicleBodyRotation_(false),
 
701
    vehicleBodyRotation_(0,0.0),
 
702
    vehicleBodyRotationUncertainty_(0,0.0) {
 
703
}
 
704
 
 
705
bool
 
706
Config::isValid() {
 
707
    bool valid = true;
 
708
    //check serial setup
 
709
    if( 0 == serialDevice_.compare("")
 
710
        || (9600 != baudRate_
 
711
                && 19200 != baudRate_
 
712
                && 38400 != baudRate_
 
713
                && 57600 != baudRate_
 
714
                && 115200 != baudRate_
 
715
                && 230400 != baudRate_)){
 
716
        std::cerr << "serial settings invalid\n";
 
717
        valid = false;
 
718
    }
 
719
 
 
720
    //imu gear
 
721
    if(enableImu_
 
722
            && ( 0 == imuType_.compare("")
 
723
                || 3 != imuToGpsOffset_.size()
 
724
                || ( 3 != imuToGpsOffsetUncertainty_.size()
 
725
                    && 0  != imuToGpsOffsetUncertainty_.size()))){
 
726
        std::cerr << "imuType/imuToGpsOffset invalid\n";
 
727
        valid = false;
 
728
    }
 
729
    if(enableImu_ && enableSetImuOrientation_
 
730
            && ( 0 > setImuOrientation_
 
731
                || 6 < setImuOrientation_)){
 
732
        std::cerr << "setImuOrientation invalid\n";
 
733
        valid = false;
 
734
    }
 
735
    if(enableImu_ && enableVehicleBodyRotation_
 
736
            && ( 3 != vehicleBodyRotation_.size()
 
737
                || ( 3 != vehicleBodyRotationUncertainty_.size()
 
738
                    && 0 != vehicleBodyRotationUncertainty_.size()))){
 
739
        std::cerr << "vehicleBodyRotation invalid\n";
 
740
        valid = false;
 
741
    }
 
742
 
 
743
    //ins gear
 
744
    if(enableImu_ && enableInsOffset_
 
745
            && 3 != insOffset_.size()){
 
746
        std::cerr << "insOffset invalid\n";
 
747
        valid = false;
 
748
    }
 
749
    if(enableImu_ && enableSetInitAzimuth_
 
750
            && ( 2 != initAzimuth_.size() )){
 
751
        std::cerr << "initAzimuth invalid\n";
 
752
        valid = false;
 
753
    }
 
754
 
 
755
    //data
 
756
    if(false == (enableInsPva_ || enableInsCov_ || enableGpsPos_ || enableGpsVel_ || enableRawImu_)){
 
757
        std::cerr << "data settings invalid, you need to enable at least one message\n";
 
758
        valid = false;
 
759
    }
 
760
    if(enableRawImu_ && 0.02 > dtInsPva_){
 
761
        if(fixInvalidRateSettings_){
 
762
            std::cerr << "data rate for InsPva too high; must be 50Hz or less if RawImu is enabled. FIXED!\n";
 
763
            dtInsPva_ = 0.02;
 
764
        }else{
 
765
            std::cerr << "data rate for InsPva too high; must be 50Hz or less if RawImu is enabled\n";
 
766
            valid = false;
 
767
        }
 
768
    }
 
769
        //!cov:start
 
770
    if(enableRawImu_ && 0.02 > dtInsCov_){
 
771
        if(fixInvalidRateSettings_){
 
772
            std::cerr << "data rate for InsCov too high; must be 50Hz or less if RawImu is enabled. FIXED!\n";
 
773
            dtInsCov_ = 0.02;
 
774
        }else{
 
775
            std::cerr << "data rate for InsCov too high; must be 50Hz or less if RawImu is enabled\n";
 
776
            valid = false;
 
777
        }
 
778
    }
 
779
        //!cov:end
 
780
    if( (enableRawImu_ || enableInsPva_) && 0.2 > dtGpsPos_ ){
 
781
        if(fixInvalidRateSettings_){
 
782
            std::cerr << "data rate for BestGpsPos too high; must be 5Hz or less if RawImu or InsPva is enabled. FIXED!\n";
 
783
            dtGpsPos_ = 0.2;
 
784
        }else{
 
785
            std::cerr << "data rate for BestGpsPos too high; must be 5Hz or less if RawImu or InsPva is enabled\n";
 
786
            valid = false;
 
787
        }
 
788
    }
 
789
    if( (enableRawImu_ || enableInsPva_) && 0.2 > dtGpsVel_ ){
 
790
        if(fixInvalidRateSettings_){
 
791
            std::cerr << "data rate for BestGpsVel too high; must be 5Hz or less if RawImu or InsPva is enabled. FIXED!\n";
 
792
            dtGpsVel_ = 0.2;
 
793
        }else{
 
794
            std::cerr << "data rate for BestGpsVel too high; must be 5Hz or less if RawImu or InsPva is enabled\n";
 
795
            valid = false;
 
796
        }
 
797
    }
 
798
 
 
799
    //datarate
 
800
    double dataRate = 0.0;
 
801
    if(enableRawImu_){
 
802
        dataRate += 1.0/0.01 * 8 * sizeof (gnua::RawImuLogSB); // TODO: there is one IMU running at 200Hz, no idea how to check for it
 
803
    }
 
804
    if(enableInsPva_){
 
805
        dataRate += 1.0/dtInsPva_ * 8 * sizeof (gnua::InsPvaLogSB);
 
806
    }
 
807
        //!cov:start
 
808
    if(enableInsCov_){
 
809
        dataRate += 1.0/dtInsCov_ * 8 * sizeof (gnua::InsCovLogSB);
 
810
    }
 
811
        //!cov:end
 
812
    if(enableGpsPos_){
 
813
        dataRate += 1.0/dtGpsPos_ * 8 * sizeof (gnua::BestGpsPosLogB);
 
814
    }
 
815
    if(enableGpsVel_){
 
816
        dataRate += 1.0/dtGpsVel_ * 8 * sizeof (gnua::BestGpsVelLogB);
 
817
    }
 
818
    if(dataRate > (double)baudRate_){
 
819
        std::cerr << "The combined data-rate of the messages/message rates you configured excceeds the configured baud rate " << dataRate << " / " << baudRate_ << "\n";
 
820
        valid = false;
 
821
    }else if(dataRate > 0.95 * baudRate_){
 
822
        std::cerr << "The combined data-rate of the messages/message rates you configured is more than 95\% of the configured baud rate! " << dataRate << " / " << baudRate_ << "\n";
 
823
        std::cerr << "Consider using a higher baud rate (maximum 230400) or less messages or lower message-rates.";
 
824
    }
 
825
 
 
826
    return valid;
 
827
}
 
828
 
 
829
std::string
 
830
Config::toString(){
 
831
    std::stringstream ss;
 
832
    ss << "serialDevice_: " << serialDevice_ << " ";
 
833
    ss << "baudRate_: " << baudRate_ << " ";
 
834
    ss << "enableImu_: " << enableImu_ << " ";
 
835
    ss << "imuType_: " << imuType_ << " ";
 
836
    ss << "enableInsPva_: " << enableInsPva_ << " ";
 
837
    ss << "enableInsCov_: " << enableInsCov_ << " "; //!cov
 
838
    ss << "enableGpsPos_: " << enableGpsPos_ << " ";
 
839
    ss << "enableGpsVel_: " << enableGpsVel_ << " ";
 
840
    ss << "enableRawImu_: " << enableRawImu_ << " ";
 
841
    ss << "ignoreUnknownMessages_: " << ignoreUnknownMessages_ << " ";
 
842
    ss << "dtInsPva_: " << dtInsPva_ << " ";
 
843
    ss << "dtInsCov_: " << dtInsCov_ << " "; //!cov
 
844
    ss << "dtGpsPos_: " << dtGpsPos_ << " ";
 
845
    ss << "dtGpsVel_: " << dtGpsVel_ << " ";
 
846
    ss << "fixInvalidRateSettings_: " << fixInvalidRateSettings_ << " ";
 
847
    ss << "imuToGpsOffset_: " << doubleVectorToString(imuToGpsOffset_) << " ";
 
848
    ss << "imuToGpsOffsetUncertainty_: " << doubleVectorToString(imuToGpsOffsetUncertainty_) << " ";
 
849
    ss << "enableInsOffset_: " << enableInsOffset_ << " ";
 
850
    ss << "insOffset_: " << doubleVectorToString(insOffset_) << " ";
 
851
    ss << "enableSetInitAzimuth_: " << enableSetInitAzimuth_ << " ";
 
852
    ss << "initAzimuth_: " << doubleVectorToString(initAzimuth_) << " ";
 
853
    ss << "enableInsPhaseUpdate_: " << enableInsPhaseUpdate_ << " ";
 
854
    ss << "enableCDGPS_: " << enableCDGPS_ << " ";
 
855
    ss << "enableSBAS_: " << enableSBAS_ << " ";
 
856
    ss << "enableRTK_: " << enableRTK_ << " ";
 
857
    ss << "enableUseOfOmniStarCarrier_: " << enableUseOfOmniStarCarrier_ << " ";
 
858
    ss << "enableSetImuOrientation_: " << enableSetImuOrientation_ << " ";
 
859
    ss << "setImuOrientation_: " << setImuOrientation_ << " ";
 
860
    ss << "enableVehicleBodyRotation_: " << enableVehicleBodyRotation_ << " ";
 
861
    ss << "vehicleBodyRotation_: " << doubleVectorToString(vehicleBodyRotation_) << " ";
 
862
    ss << "vehicleBodyRotationUncertainty_: " << doubleVectorToString(vehicleBodyRotationUncertainty_);
 
863
    return ss.str();
 
864
}
 
865
 
 
866
bool
 
867
SimpleConfig::isValid() const {
 
868
    return 0 != serialDevice_.compare("")
 
869
        && imuType_.compare("")
 
870
        && 3 == imuToGpsOffset_.size()
 
871
        && (9600 == baudRate_
 
872
                || 19200 == baudRate_
 
873
                || 38400 == baudRate_
 
874
                || 57600 == baudRate_
 
875
                || 115200 == baudRate_
 
876
                || 230400 == baudRate_) ;
 
877
}
 
878
 
 
879
std::string
 
880
SimpleConfig::toString(){
 
881
    std::stringstream ss;
 
882
    ss << "serialDevice_: " << serialDevice_ << " ";
 
883
    ss << "baudRate_: " << baudRate_ << " ";
 
884
    ss << "imuType_: " << imuType_ << " ";
 
885
    ss << "imuToGpsOffset_: " << doubleVectorToString(imuToGpsOffset_);
 
886
    return ss.str();
 
887
}
 
888
 
 
889
bool
 
890
GpsOnlyConfig::isValid() const {
 
891
    return 0 != serialDevice_.compare("")
 
892
        && (9600 == baudRate_
 
893
                || 19200 == baudRate_
 
894
                || 38400 == baudRate_
 
895
                || 57600 == baudRate_
 
896
                || 115200 == baudRate_
 
897
                || 230400 == baudRate_) ;
 
898
}
 
899
 
 
900
std::string
 
901
GpsOnlyConfig::toString(){
 
902
    std::stringstream ss;
 
903
    ss << "serialDevice_: " << serialDevice_ << " ";
 
904
    ss << "baudRate_: " << baudRate_;
 
905
    return ss.str();
 
906
}
 
907
 
 
908
std::string
 
909
InsPvaData::toString(){
 
910
    std::stringstream ss;
 
911
    ss << "InsPvaData ";
 
912
    ss << "timeStampSec " << timeStampSec << " ";
 
913
    ss << "timeStampUSec " << timeStampUSec << " ";
 
914
    ss << "gpsWeekNr " << std::fixed << std::setprecision(4) << gpsWeekNr << " ";
 
915
    ss << "secIntoWeek " << secIntoWeek << " ";
 
916
    ss << "latitude " << std::setprecision(9) << latitude << " "; // ~1/10mm resolution (equator)
 
917
    ss << "longitude " << longitude << " ";
 
918
    ss << "height " << std::setprecision(4) << height << " ";
 
919
    ss << "northVelocity " << northVelocity << " ";
 
920
    ss << "eastVelocity " << eastVelocity << " ";
 
921
    ss << "upVelocity " << upVelocity << " ";
 
922
    ss << "roll " << roll << " ";
 
923
    ss << "pitch " << pitch << " ";
 
924
    ss << "azimuth " << azimuth << " ";
 
925
    ss << statusToString(statusMessageType, statusMessage);
 
926
    return ss.str();
 
927
};
 
928
 
 
929
//!cov:start
 
930
std::string
 
931
InsCovData::toString(){
 
932
    std::stringstream ss;
 
933
    ss << "InsCovData ";
 
934
    ss << "timeStampSec " << timeStampSec << " ";
 
935
    ss << "timeStampUSec " << timeStampUSec << " ";
 
936
    ss << "gpsWeekNr " << std::fixed << std::setprecision(4) << gpsWeekNr << " ";
 
937
    ss << "secIntoWeek " << secIntoWeek << " ";
 
938
 
 
939
        ss << "posxx " << std::setprecision(9) << posxx << " "; 
 
940
        ss << "posxy " << std::setprecision(9) << posxy << " "; 
 
941
        ss << "posxz " << std::setprecision(9) << posxz << " "; 
 
942
        ss << "posyx " << std::setprecision(9) << posyx << " "; 
 
943
        ss << "posyy " << std::setprecision(9) << posyy << " "; 
 
944
        ss << "posyz " << std::setprecision(9) << posyz << " "; 
 
945
        ss << "poszx " << std::setprecision(9) << poszx << " "; 
 
946
        ss << "poszy " << std::setprecision(9) << poszy << " "; 
 
947
        ss << "poszz " << std::setprecision(9) << poszz << " "; 
 
948
        
 
949
        ss << "attxx " << attxx << " "; 
 
950
        ss << "attxy " << attxy << " "; 
 
951
        ss << "attxz " << attxz << " "; 
 
952
        ss << "attyx " << attyx << " "; 
 
953
        ss << "attyy " << attyy << " "; 
 
954
        ss << "attyz " << attyz << " "; 
 
955
        ss << "attzx " << attzx << " "; 
 
956
        ss << "attzy " << attzy << " "; 
 
957
        ss << "attzz " << attzz << " "; 
 
958
 
 
959
        ss << "velxx " << velxx << " "; 
 
960
        ss << "velxy " << velxy << " "; 
 
961
        ss << "velxz " << velxz << " "; 
 
962
        ss << "velyx " << velyx << " "; 
 
963
        ss << "velyy " << velyy << " "; 
 
964
        ss << "velyz " << velyz << " "; 
 
965
        ss << "velzx " << velzx << " "; 
 
966
        ss << "velzy " << velzy << " "; 
 
967
        ss << "velzz " << velzz << " "; 
 
968
    return ss.str();
 
969
};
 
970
//!cov:end
 
971
 
 
972
std::string
 
973
BestGpsPosData::toString(){
 
974
    std::stringstream ss;
 
975
    ss << "BestGpsPosData ";
 
976
    ss << "timeStampSec " << timeStampSec << " ";
 
977
    ss << "timeStampUSec " << timeStampUSec << " ";
 
978
    ss << "gpsWeekNr " << gpsWeekNr << " ";
 
979
    ss << "msIntoWeek " << msIntoWeek << " ";
 
980
    ss << "solutionStatus " << solutionStatus << " ";
 
981
    ss << "positionType " << positionType << " ";
 
982
    ss << "latitude " << std::fixed << std::setprecision(9) << latitude << " "; // ~1/10mm resolution (equator)
 
983
    ss << "longitude " << longitude << " ";
 
984
    ss << "heightAMSL " << std::setprecision(4) << heightAMSL << " ";
 
985
    ss << "undulation " << undulation << " ";
 
986
    ss << "datumId " << datumId << " ";
 
987
    ss << "sigmaLatitude " << sigmaLatitude << " ";
 
988
    ss << "sigmaLongitude " << sigmaLongitude << " ";
 
989
    ss << "sigmaHeight " << sigmaHeight << " ";
 
990
    ss << "baseStationId ["
 
991
        << baseStationId[0]
 
992
        << baseStationId[1]
 
993
        << baseStationId[2]
 
994
        << baseStationId[3]
 
995
        << "] ";
 
996
    ss << "diffAge " << diffAge << " ";
 
997
    ss << "solutionAge " << solutionAge << " ";
 
998
    ss << "numObservations " << numObservations << " ";
 
999
    ss << "numL1Ranges " << numL1Ranges << " ";
 
1000
    ss << "numL1RangesRTK " << numL1RangesRTK << " ";
 
1001
    ss << "numL2RangesRTK " << numL2RangesRTK << " ";
 
1002
    ss << statusToString(statusMessageType, statusMessage);
 
1003
    return ss.str();
 
1004
};
 
1005
 
 
1006
std::string
 
1007
BestGpsVelData::toString(){
 
1008
    std::stringstream ss;
 
1009
    ss << "BestGpsVelData ";
 
1010
    ss << "timeStampSec " << timeStampSec << " ";
 
1011
    ss << "timeStampUSec " << timeStampUSec << " ";
 
1012
    ss << "gpsWeekNr " << gpsWeekNr << " ";
 
1013
    ss << "msIntoWeek " << msIntoWeek << " ";
 
1014
    ss << "solutionStatus " << solutionStatus << " ";
 
1015
    ss << "velocityType " << velocityType << " ";
 
1016
    ss << "latency " << std::fixed << std::setprecision(4) << latency << " ";
 
1017
    ss << "diffAge " << diffAge << " ";
 
1018
    ss << "horizontalSpeed " << horizontalSpeed << " ";
 
1019
    ss << "trackOverGround " << trackOverGround << " ";
 
1020
    ss << "verticalSpeed " << verticalSpeed << " ";
 
1021
    ss << statusToString(statusMessageType, statusMessage);
 
1022
    return ss.str();
 
1023
};
 
1024
 
 
1025
std::string
 
1026
RawImuData::toString(){
 
1027
    std::stringstream ss;
 
1028
    ss << "RawImuData ";
 
1029
    ss << "timeStampSec " << timeStampSec << " ";
 
1030
    ss << "timeStampUSec " << timeStampUSec << " ";
 
1031
    ss << "gpsWeekNr " << std::fixed << gpsWeekNr << " ";
 
1032
    ss << "secIntoWeek " << std::setprecision(4) << secIntoWeek << " ";
 
1033
    ss << "zDeltaV "  << std::setprecision(10)<< zDeltaV << " "; // ~1/10 LSB for best IMU
 
1034
    ss << "yDeltaV " << yDeltaV << " ";
 
1035
    ss << "xDeltaV " << xDeltaV << " ";
 
1036
    ss << "zDeltaAng " <<  std::setprecision(11) << zDeltaAng << " ";  // ~1/10 LSB for best IMU
 
1037
    ss << "yDeltaAng " << yDeltaAng << " ";
 
1038
    ss << "xDeltaAng " << xDeltaAng << " ";
 
1039
    ss << statusToString(statusMessageType, statusMessage);
 
1040
    return ss.str();
 
1041
};
 
1042
 
 
1043
} //namespace
 
1044
 
 
1045
namespace{
 
1046
    void
 
1047
    checkParserAssumptions(){
 
1048
        // we have a fairly dodgy parser, which makes unsound assumptions about the binary layout of things in memory.
 
1049
        // This gear makes sure that we fail at runtime instead of delivering garbage.
 
1050
        assert((8 == CHAR_BIT) && "Unsupported size of byte: our parser won't work on this platform");
 
1051
        assert((8 == sizeof(double)) && "Unsupported size of double: our parser won't work on this platform");
 
1052
        assert((4 == sizeof(float)) && "Unsupported size of float: our parser won't work on this platform");
 
1053
 
 
1054
        union EndianCheck{
 
1055
            uint32_t full;
 
1056
            uint8_t bytes[4];
 
1057
        };
 
1058
        EndianCheck endianCheck;
 
1059
        endianCheck.bytes[0] = 0x21;
 
1060
        endianCheck.bytes[1] = 0x43;
 
1061
        endianCheck.bytes[2] = 0x65;
 
1062
        endianCheck.bytes[3] = 0x87;
 
1063
        assert(0x87654321 == endianCheck.full && "Unsupported endianness: our parser won't work on this platform");
 
1064
        assert(rawMsgSize == sizeof(union NovatelMessage)
 
1065
                && "Overflow: Someone included a log definition in [NovatelMessage] which is bigger than the buffer for it"
 
1066
                && "Depending on the pickiness of your hardware this might actually work, but I'll shut down on general principle");
 
1067
        return;
 
1068
    }
 
1069
 
 
1070
    uint16_t
 
1071
    readNovatelMessage(union NovatelMessage &msg, struct timeval &timeStamp, gbxserialacfr::Serial *serial) {
 
1072
        uint16_t id = gnua::InvalidLogType;
 
1073
        msg.header.sb1 = 0;
 
1074
 
 
1075
        // skip everything until we get the first sync byte
 
1076
        do{
 
1077
            if( 1 != serial->readFull( &msg.header.sb1, 1 )){
 
1078
                throw ( gua::Exception(ERROR_INFO, "Timed out while trying to read sync byte 1") );
 
1079
            }
 
1080
        }while( msg.header.sb1 != 0xaa );
 
1081
 
 
1082
        // get timestamp after the first byte for accuracy
 
1083
        gettimeofday(&timeStamp, NULL);
 
1084
 
 
1085
        // read the second and third sync byte
 
1086
        if( 1 != serial->readFull( &msg.header.sb2, 1) ){
 
1087
            throw ( gua::Exception(ERROR_INFO, "Timed out while trying to read sync byte 2" ) );
 
1088
        }
 
1089
        if (msg.header.sb2 != 0x44 ) {
 
1090
            std::stringstream ss;
 
1091
            ss << "Expected sync byte 2 (0x44) got: " << hex << msg.header.sb2;
 
1092
            throw ( gua::Exception(ERROR_INFO, ss.str()) );
 
1093
        }
 
1094
        if( 1 != serial->readFull( &msg.header.sb3, 1 )){
 
1095
            throw ( gua::Exception(ERROR_INFO, "Timed out while trying to read sync byte 3") );
 
1096
        }
 
1097
 
 
1098
        // figure out what binary format we have, and read the full packet
 
1099
        switch( msg.header.sb3 ) {
 
1100
            case 0x12: //long packet
 
1101
                if( // how long is the header ?
 
1102
                    1 != serial->readFull( &msg.header.headerLength, 1 )
 
1103
                    // read all of the header
 
1104
                    || msg.header.headerLength-4 != serial->readFull( &msg.header.msgId, msg.header.headerLength-4 )
 
1105
                    // read the  message data plus 4 bytes for the crc
 
1106
                    || msg.header.msgLength+4 != serial->readFull( &msg.data, msg.header.msgLength+4 )
 
1107
                  ){
 
1108
                    throw ( gua::Exception(ERROR_INFO, "Timed out while trying to read long packet") );
 
1109
                }
 
1110
 
 
1111
                id = msg.header.msgId;
 
1112
 
 
1113
                if(0 != gnua::crc( msg.rawMessage, msg.header.msgLength+msg.header.headerLength+4 )){
 
1114
                    throw ( gua::Exception(ERROR_INFO, "CRC Error" ) );
 
1115
                }
 
1116
                break;
 
1117
 
 
1118
            case 0x13: //short packet
 
1119
                if( // read rest of the header 12 bytes - 3 bytes already read, then the actual data plus 4 bytes for the CRC
 
1120
                    9 != serial->readFull( &msg.shortHeader.msgLength, 9 )
 
1121
                    || msg.shortHeader.msgLength+4 != serial->readFull( &msg.shortData, msg.shortHeader.msgLength+4 )
 
1122
                  ){
 
1123
                    throw ( gua::Exception(ERROR_INFO, "Timed out while trying to read short packet") );
 
1124
                }
 
1125
 
 
1126
                id = msg.shortHeader.msgId;
 
1127
 
 
1128
                if( 0 != gnua::crc( msg.rawMessage,msg.shortHeader.msgLength + 16 )){
 
1129
                    throw ( gua::Exception(ERROR_INFO, "CRC Error" ) );
 
1130
                }
 
1131
                break;
 
1132
 
 
1133
            default: //bollocks
 
1134
                throw ( gua::Exception(ERROR_INFO, "Unknown binary packet format" ) );
 
1135
        }
 
1136
        return  id;
 
1137
    }
 
1138
 
 
1139
 
 
1140
    std::auto_ptr<gna::GenericData>
 
1141
    createExternalMsg(gnua::InsPvaLogSB &insPva, struct timeval &timeStamp){
 
1142
        gna::InsPvaData *data = new gna::InsPvaData;
 
1143
        std::auto_ptr<gna::GenericData> genericData( data );
 
1144
 
 
1145
        //data
 
1146
        data->gpsWeekNr = insPva.data.gpsWeekNr;
 
1147
        data->secIntoWeek = insPva.data.secIntoWeek;
 
1148
        data->latitude = insPva.data.latitude;
 
1149
        data->longitude = insPva.data.longitude;
 
1150
        data->height = insPva.data.height;
 
1151
        data->northVelocity = insPva.data.northVelocity;
 
1152
        data->eastVelocity = insPva.data.eastVelocity;
 
1153
        data->upVelocity = insPva.data.upVelocity;
 
1154
        data->roll = insPva.data.roll;
 
1155
        data->pitch = insPva.data.pitch;
 
1156
        data->azimuth = insPva.data.azimuth;
 
1157
 
 
1158
        //timestamp
 
1159
        data->timeStampSec = timeStamp.tv_sec;
 
1160
        data->timeStampUSec = timeStamp.tv_usec;
 
1161
 
 
1162
        //status
 
1163
        switch( insPva.data.insStatus ) {
 
1164
            case 0:
 
1165
                data->statusMessage = "Ins is inactive";
 
1166
                data->statusMessageType = gna::Initialising; // TODO: how can we distinguish between the startup-behavior and a genuine fault during operation?? Timer from when we first established a connection?
 
1167
                break;
 
1168
            case 1:
 
1169
                data->statusMessage = "Ins is aligning";
 
1170
                data->statusMessageType = gna::Initialising;
 
1171
                break;
 
1172
            case 2:
 
1173
                data->statusMessage = "Ins solution is bad";
 
1174
                data->statusMessageType = gna::Warning;
 
1175
                break;
 
1176
            case 3:
 
1177
                data->statusMessage = "Ins solution is good";
 
1178
                data->statusMessageType = gna::Ok;
 
1179
                break;
 
1180
            case 4://fallthrough
 
1181
            case 5:
 
1182
                {
 
1183
                    stringstream ss;
 
1184
                    ss << "Reserved value?? Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
 
1185
                    data->statusMessage = ss.str();
 
1186
                    data->statusMessageType = gna::Warning;
 
1187
                }
 
1188
                break;
 
1189
            case 6:
 
1190
                data->statusMessage = "Bad Ins Gps agreement";
 
1191
                data->statusMessageType = gna::Warning;
 
1192
                break;
 
1193
            case 7:
 
1194
                data->statusMessage = "Ins alignment is complete but vehicle must perform maneuvers so that the attitude can converge";
 
1195
                data->statusMessageType = gna::Ok; // TODO: this is not quite Ok, check if it is usable (no sudden jumps, etc.)
 
1196
                break;
 
1197
            default:
 
1198
                {
 
1199
                    stringstream ss;
 
1200
                    ss <<  "Unknown Ins Status. Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
 
1201
                    data->statusMessage = ss.str();
 
1202
                    data->statusMessageType = gna::Warning;
 
1203
                }
 
1204
                break;
 
1205
        }
 
1206
 
 
1207
        return genericData;
 
1208
    }
 
1209
 
 
1210
        //!cov:start
 
1211
    std::auto_ptr<gna::GenericData>
 
1212
    createExternalMsg(gnua::InsCovLogSB &insCov, struct timeval &timeStamp){
 
1213
        gna::InsCovData *data = new gna::InsCovData;
 
1214
        std::auto_ptr<gna::GenericData> genericData( data );
 
1215
 
 
1216
        //data
 
1217
        data->gpsWeekNr = insCov.data.gpsWeekNr;
 
1218
        data->secIntoWeek = insCov.data.secIntoWeek;
 
1219
                
 
1220
        data->posxx = insCov.data.posxx;
 
1221
        data->posxy = insCov.data.posxy;
 
1222
        data->posxz = insCov.data.posxz;
 
1223
        data->posyx = insCov.data.posyx;
 
1224
        data->posyy = insCov.data.posyy;
 
1225
        data->posyz = insCov.data.posyz;
 
1226
        data->poszx = insCov.data.poszx;
 
1227
        data->poszy = insCov.data.poszy;
 
1228
        data->poszz = insCov.data.poszz;
 
1229
                
 
1230
        data->attxx = insCov.data.attxx;
 
1231
        data->attxy = insCov.data.attxy;
 
1232
        data->attxz = insCov.data.attxz;
 
1233
        data->attyx = insCov.data.attyx;
 
1234
        data->attyy = insCov.data.attyy;
 
1235
        data->attyz = insCov.data.attyz;
 
1236
        data->attzx = insCov.data.attzx;
 
1237
        data->attzy = insCov.data.attzy;
 
1238
        data->attzz = insCov.data.attzz;
 
1239
                
 
1240
        data->velxx = insCov.data.velxx;
 
1241
        data->velxy = insCov.data.velxy;
 
1242
        data->velxz = insCov.data.velxz;
 
1243
        data->velyx = insCov.data.velyx;
 
1244
        data->velyy = insCov.data.velyy;
 
1245
        data->velyz = insCov.data.velyz;
 
1246
        data->velzx = insCov.data.velzx;
 
1247
        data->velzy = insCov.data.velzy;
 
1248
        data->velzz = insCov.data.velzz;
 
1249
                
 
1250
        //timestamp
 
1251
        data->timeStampSec = timeStamp.tv_sec;
 
1252
        data->timeStampUSec = timeStamp.tv_usec;
 
1253
 
 
1254
        //status
 
1255
//        switch( insPva.data.insStatus ) {
 
1256
//            case 0:
 
1257
//                data->statusMessage = "Ins is inactive";
 
1258
//                data->statusMessageType = gna::Initialising; // TODO: how can we distinguish between the startup-behavior and a genuine fault during operation?? Timer from when we first established a connection?
 
1259
//                break;
 
1260
//            case 1:
 
1261
//                data->statusMessage = "Ins is aligning";
 
1262
//                data->statusMessageType = gna::Initialising;
 
1263
//                break;
 
1264
//            case 2:
 
1265
//                data->statusMessage = "Ins solution is bad";
 
1266
//                data->statusMessageType = gna::Warning;
 
1267
//                break;
 
1268
//            case 3:
 
1269
//                data->statusMessage = "Ins solution is good";
 
1270
//                data->statusMessageType = gna::Ok;
 
1271
//                break;
 
1272
//            case 4://fallthrough
 
1273
//            case 5:
 
1274
//                {
 
1275
//                    stringstream ss;
 
1276
//                    ss << "Reserved value?? Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
 
1277
//                    data->statusMessage = ss.str();
 
1278
//                    data->statusMessageType = gna::Warning;
 
1279
//                }
 
1280
//                break;
 
1281
//            case 6:
 
1282
//                data->statusMessage = "Bad Ins Gps agreement";
 
1283
//                data->statusMessageType = gna::Warning;
 
1284
//                break;
 
1285
//            case 7:
 
1286
//                data->statusMessage = "Ins alignment is complete but vehicle must perform maneuvers so that the attitude can converge";
 
1287
//                data->statusMessageType = gna::Ok; // TODO: this is not quite Ok, check if it is usable (no sudden jumps, etc.)
 
1288
//                break;
 
1289
//            default:
 
1290
//                {
 
1291
//                    stringstream ss;
 
1292
//                    ss <<  "Unknown Ins Status. Check NovatelSpan manual for \"" << insPva.data.insStatus << "\" as INS status";
 
1293
//                    data->statusMessage = ss.str();
 
1294
//                    data->statusMessageType = gna::Warning;
 
1295
//                }
 
1296
//                break;
 
1297
//        }
 
1298
 
 
1299
        return genericData;
 
1300
    }
 
1301
        //!cov:end      
 
1302
        
 
1303
    std::auto_ptr<gna::GenericData>
 
1304
    createExternalMsg(gnua::BestGpsPosLogB &bestGpsPos, struct timeval &timeStamp){
 
1305
        gna::BestGpsPosData *data = new gna::BestGpsPosData;
 
1306
        std::auto_ptr<gna::GenericData> genericData( data );
 
1307
 
 
1308
        //data
 
1309
        data->gpsWeekNr = bestGpsPos.header.gpsWeekNr;
 
1310
        data->msIntoWeek = bestGpsPos.header.msIntoWeek;
 
1311
        data->solutionStatus = externalGpsSolutionStatus( bestGpsPos.data.solutionStatus );
 
1312
        data->positionType = externalGpsPosVelType( bestGpsPos.data.positionType );
 
1313
        data->latitude = bestGpsPos.data.latitude;
 
1314
        data->longitude = bestGpsPos.data.longitude;
 
1315
        data->heightAMSL = bestGpsPos.data.heightAMSL;
 
1316
        data->undulation = bestGpsPos.data.undulation;
 
1317
        data->datumId = bestGpsPos.data.datumId;
 
1318
        data->sigmaLatitude = bestGpsPos.data.sigmaLatitude;
 
1319
        data->sigmaLongitude = bestGpsPos.data.sigmaLongitude;
 
1320
        data->sigmaHeight = bestGpsPos.data.sigmaHeight;
 
1321
        data->baseStationId[0] = bestGpsPos.data.baseStationId[0];
 
1322
        data->baseStationId[1] = bestGpsPos.data.baseStationId[1];
 
1323
        data->baseStationId[2] = bestGpsPos.data.baseStationId[2];
 
1324
        data->baseStationId[3] = bestGpsPos.data.baseStationId[3];
 
1325
        data->diffAge = bestGpsPos.data.diffAge;
 
1326
        data->solutionAge = bestGpsPos.data.solutionAge;
 
1327
        data->numObservations = bestGpsPos.data.numObservations;
 
1328
        data->numL1Ranges = bestGpsPos.data.numL1Ranges;
 
1329
        data->numL1RangesRTK = bestGpsPos.data.numL1RangesRTK;
 
1330
        data->numL2RangesRTK = bestGpsPos.data.numL2RangesRTK;
 
1331
 
 
1332
        //time
 
1333
        data->timeStampSec = timeStamp.tv_sec;
 
1334
        data->timeStampUSec = timeStamp.tv_usec;
 
1335
 
 
1336
        //status
 
1337
        static bool lastStatusWasGood = false;
 
1338
        if(true == gnua::receiverStatusIsGood(bestGpsPos.header.receiverStatus)){
 
1339
            if (true == lastStatusWasGood){
 
1340
                // still all good, no need to be chatty
 
1341
                data->statusMessageType = gna::NoMsg;
 
1342
                data->statusMessage = "";
 
1343
                lastStatusWasGood = true;
 
1344
            }else{
 
1345
                // we are good now, report it
 
1346
                data->statusMessageType = gna::Ok;
 
1347
                data->statusMessage = "all is good";
 
1348
                lastStatusWasGood = true;
 
1349
            }
 
1350
        }else if(true == gnua::receiverStatusIsWarning(bestGpsPos.header.receiverStatus)){
 
1351
            data->statusMessageType = gna::Warning;
 
1352
            data->statusMessage = gnua::receiverStatusToString(bestGpsPos.header.receiverStatus);
 
1353
            lastStatusWasGood = false;
 
1354
        }else if(true == gnua::receiverStatusIsError(bestGpsPos.header.receiverStatus)){
 
1355
            data->statusMessageType = gna::Fault;
 
1356
            data->statusMessage = gnua::receiverStatusToString(bestGpsPos.header.receiverStatus);
 
1357
            lastStatusWasGood = false;
 
1358
        }else if(true == gnua::receiverStatusIsFatal(bestGpsPos.header.receiverStatus)){
 
1359
            //ouch, need to bug out
 
1360
            std::stringstream ss;
 
1361
            ss << "Receiver reports hardware-error! This is not recoverable!\n"
 
1362
                << "You can get additional information by sending \"log rxstatusa once\" to the receiver (minicom/other-terminal-program)!\n"
 
1363
                << "GpsStatus:\n";
 
1364
            ss << gnua::receiverStatusToString(bestGpsPos.header.receiverStatus);
 
1365
            lastStatusWasGood = false;
 
1366
            throw ( gua::HardwareException(ERROR_INFO, ss.str()) );
 
1367
        }else if(true == gnua::receiverStatusIsReservedValue(bestGpsPos.header.receiverStatus)){
 
1368
            //whoops
 
1369
            data->statusMessageType = gna::Warning;
 
1370
            std::stringstream ss;
 
1371
            ss << "Got GPS status which used to be a reserved value: 0x" << hex << bestGpsPos.header.receiverStatus
 
1372
                << " Please check your manual what this means, and tell us (gearbox-devel@lists.sourceforge.net) about it.\n"
 
1373
                << "Thanks.\n";
 
1374
            data->statusMessage = ss.str();
 
1375
            lastStatusWasGood = false;
 
1376
        }else{
 
1377
            // Can't happen
 
1378
            data->statusMessageType = gna::Warning;
 
1379
            std::stringstream ss;
 
1380
            ss << "Failed to decode GPS status: 0x" << hex << bestGpsPos.header.receiverStatus
 
1381
                << " Please send a bug-report to gearbox-devel@lists.sourceforge.net.\n"
 
1382
                << "Include this message and details about your hardware/software configuration.\n"
 
1383
                << "Thanks.\n";
 
1384
            data->statusMessage = ss.str();
 
1385
            lastStatusWasGood = false;
 
1386
        }
 
1387
        return genericData;
 
1388
 
 
1389
    }
 
1390
 
 
1391
    std::auto_ptr<gna::GenericData>
 
1392
    createExternalMsg(gnua::BestGpsVelLogB &bestGpsVel, struct timeval &timeStamp){
 
1393
        gna::BestGpsVelData *data = new gna::BestGpsVelData;
 
1394
        std::auto_ptr<gna::GenericData> genericData( data );
 
1395
 
 
1396
        //data
 
1397
        data->gpsWeekNr = bestGpsVel.header.gpsWeekNr;
 
1398
        data->msIntoWeek = bestGpsVel.header.msIntoWeek;
 
1399
        data->solutionStatus = externalGpsSolutionStatus( bestGpsVel.data.solutionStatus );
 
1400
        data->velocityType = externalGpsPosVelType( bestGpsVel.data.velocityType );
 
1401
        data->latency = bestGpsVel.data.latency;
 
1402
        data->diffAge = bestGpsVel.data.diffAge;
 
1403
        data->horizontalSpeed = bestGpsVel.data.horizontalSpeed;
 
1404
        data->trackOverGround = bestGpsVel.data.trackOverGround;
 
1405
        data->verticalSpeed = bestGpsVel.data.verticalSpeed;
 
1406
 
 
1407
        //time
 
1408
        data->timeStampSec = timeStamp.tv_sec;
 
1409
        data->timeStampUSec = timeStamp.tv_usec;
 
1410
 
 
1411
        //status
 
1412
        static bool lastStatusWasGood = false;
 
1413
        if(true == gnua::receiverStatusIsGood(bestGpsVel.header.receiverStatus)){
 
1414
            if (true == lastStatusWasGood){
 
1415
                // still all good, no need to be chatty
 
1416
                data->statusMessageType = gna::NoMsg;
 
1417
                data->statusMessage = "";
 
1418
                lastStatusWasGood = true;
 
1419
            }else{
 
1420
                // we are good now, report it
 
1421
                data->statusMessageType = gna::Ok;
 
1422
                data->statusMessage = "all is good";
 
1423
                lastStatusWasGood = true;
 
1424
            }
 
1425
        }else if(true == gnua::receiverStatusIsWarning(bestGpsVel.header.receiverStatus)){
 
1426
            data->statusMessageType = gna::Warning;
 
1427
            data->statusMessage = gnua::receiverStatusToString(bestGpsVel.header.receiverStatus);
 
1428
            lastStatusWasGood = false;
 
1429
        }else if(true == gnua::receiverStatusIsError(bestGpsVel.header.receiverStatus)){
 
1430
            data->statusMessageType = gna::Fault;
 
1431
            data->statusMessage = gnua::receiverStatusToString(bestGpsVel.header.receiverStatus);
 
1432
            lastStatusWasGood = false;
 
1433
        }else if(true == gnua::receiverStatusIsFatal(bestGpsVel.header.receiverStatus)){
 
1434
            //ouch, need to bug out
 
1435
            std::stringstream ss;
 
1436
            ss << "Receiver reports hardware-error! This is not recoverable!\n"
 
1437
                << "You can get additional information by sending \"log rxstatusa once\" to the receiver (minicom/other-terminal-program)!\n"
 
1438
                << "GpsStatus:\n";
 
1439
            ss << gnua::receiverStatusToString(bestGpsVel.header.receiverStatus);
 
1440
            lastStatusWasGood = false;
 
1441
            throw ( gua::HardwareException(ERROR_INFO, ss.str()) );
 
1442
        }else if(true == gnua::receiverStatusIsReservedValue(bestGpsVel.header.receiverStatus)){
 
1443
            //whoops
 
1444
            data->statusMessageType = gna::Warning;
 
1445
            std::stringstream ss;
 
1446
            ss << "Got GPS status which used to be a reserved value: 0x" << hex << bestGpsVel.header.receiverStatus
 
1447
                << " Please check your manual what this means, and tell us (gearbox-devel@lists.sourceforge.net) about it.\n"
 
1448
                << "Thanks.\n";
 
1449
            data->statusMessage = ss.str();
 
1450
            lastStatusWasGood = false;
 
1451
        }else{
 
1452
            // Can't happen
 
1453
            data->statusMessageType = gna::Warning;
 
1454
            std::stringstream ss;
 
1455
            ss << "Failed to decode GPS status: 0x" << hex << bestGpsVel.header.receiverStatus
 
1456
                << " Please send a bug-report to gearbox-devel@lists.sourceforge.net.\n"
 
1457
                << "Include this message and details about your hardware/software configuration.\n"
 
1458
                << "Thanks.\n";
 
1459
            data->statusMessage = ss.str();
 
1460
            lastStatusWasGood = false;
 
1461
        }
 
1462
 
 
1463
 
 
1464
 
 
1465
 
 
1466
        return genericData;
 
1467
    }
 
1468
 
 
1469
    std::auto_ptr<gna::GenericData>
 
1470
    createExternalMsg(gnua::RawImuLogSB &rawImu, struct timeval &timeStamp, gnua::ImuDecoder *imuDecoder){
 
1471
        gna::RawImuData *data = new gna::RawImuData;
 
1472
        std::auto_ptr<gna::GenericData> genericData( data );
 
1473
 
 
1474
        //data
 
1475
        data->gpsWeekNr = rawImu.data.gpsWeekNr;
 
1476
        data->secIntoWeek = rawImu.data.secIntoWeek;
 
1477
        //accels
 
1478
        data->zDeltaV = imuDecoder->accelCnt2MperSec(rawImu.data.zAccelCnt);
 
1479
        data->yDeltaV = -1.0*imuDecoder->accelCnt2MperSec(rawImu.data.yNegativAccelCnt);
 
1480
        data->xDeltaV = imuDecoder->accelCnt2MperSec(rawImu.data.xAccelCnt);
 
1481
        //gyros
 
1482
        data->zDeltaAng = imuDecoder->gyroCnt2Rad(rawImu.data.zGyroCnt);
 
1483
        data->yDeltaAng = -1.0*imuDecoder->gyroCnt2Rad(rawImu.data.yNegativGyroCnt);
 
1484
        data->xDeltaAng = imuDecoder->gyroCnt2Rad(rawImu.data.xGyroCnt);
 
1485
 
 
1486
        //time
 
1487
        data->timeStampSec = timeStamp.tv_sec;
 
1488
        data->timeStampUSec = timeStamp.tv_usec;
 
1489
 
 
1490
        //status
 
1491
        static bool lastStatusWasGood = false;
 
1492
        if(true == imuDecoder->statusIsGood(rawImu.data.imuStatus)){
 
1493
            if (true == lastStatusWasGood){
 
1494
                // still all good, no need to be chatty
 
1495
                data->statusMessageType = gna::NoMsg;
 
1496
                data->statusMessage = "";
 
1497
                lastStatusWasGood = true;
 
1498
            }else{
 
1499
                // we are good now, report it
 
1500
                data->statusMessageType = gna::Ok;
 
1501
                data->statusMessage = "all is good";
 
1502
                lastStatusWasGood = true;
 
1503
            }
 
1504
        }else{
 
1505
            //whoops
 
1506
            data->statusMessageType = gna::Fault; // warning?
 
1507
            data->statusMessage = imuDecoder->statusToString(rawImu.data.imuStatus);
 
1508
            lastStatusWasGood = false;
 
1509
        }
 
1510
        return genericData;
 
1511
    }
 
1512
 
 
1513
    enum gna::GpsSolutionStatusType externalGpsSolutionStatus(uint32_t novatelGpsSolutionStatus){
 
1514
        enum gna::GpsSolutionStatusType external;
 
1515
        switch(novatelGpsSolutionStatus){
 
1516
            case 0: external = gna::SolComputed; break;
 
1517
            case 1: external = gna::InsufficientObs; break;
 
1518
            case 2: external = gna::NoConvergence; break;
 
1519
            case 3: external = gna::Singularity; break;
 
1520
            case 4: external = gna::CovTrace; break;
 
1521
            case 5: external = gna::TestDist; break;
 
1522
            case 6: external = gna::ColdStart; break;
 
1523
            case 7: external = gna::VHLimit; break;
 
1524
            case 8: external = gna::Variance; break;
 
1525
            case 9: external = gna::Residuals; break;
 
1526
            case 10: external = gna::DeltaPos; break;
 
1527
            case 11: external = gna::NegativeVar; break;
 
1528
            case 13: external = gna::IntegrityWarning; break;
 
1529
            case 14: external = gna::InsInactive; break;
 
1530
            case 15: external = gna::InsAligning; break;
 
1531
            case 16: external = gna::InsBad; break;
 
1532
            case 17: external = gna::ImuUnplugged; break;
 
1533
            case 18: external = gna::Pending; break;
 
1534
            case 19: external = gna::InvalidFix; break;
 
1535
 
 
1536
            case 12: external = gna::ReservedGpsSolutionStatusType; break;
 
1537
            default: external = gna::UnknownGpsSolutionStatusType; break;
 
1538
        }
 
1539
        return external;
 
1540
    }
 
1541
 
 
1542
    enum gna::GpsPosVelType externalGpsPosVelType(uint32_t novatelGpsPosVelType){
 
1543
        enum gna::GpsPosVelType external;
 
1544
        switch(novatelGpsPosVelType){
 
1545
            case 0: external = gna::None; break;
 
1546
            case 1: external = gna::FixedPos; break;
 
1547
            case 2: external = gna::FixedHeight; break;
 
1548
            case 4: external = gna::FloatConv; break;
 
1549
            case 5: external = gna::WideLane; break;
 
1550
            case 6: external = gna::NarrowLane; break;
 
1551
            case 8: external = gna::DopplerVelocity; break;
 
1552
            case 16: external = gna::Single; break;
 
1553
            case 17: external = gna::PsrDiff; break;
 
1554
            case 18: external = gna::Waas; break;
 
1555
            case 19: external = gna::Propagated; break;
 
1556
            case 20: external = gna::Omnistar; break;
 
1557
            case 32: external = gna::L1Float; break;
 
1558
            case 33: external = gna::IonoFreeFloat; break;
 
1559
            case 34: external = gna::NarrowFloat; break;
 
1560
            case 48: external = gna::L1Int; break;
 
1561
            case 49: external = gna::WideInt; break;
 
1562
            case 50: external = gna::NarrowInt; break;
 
1563
            case 51: external = gna::RtkDirectIns; break;
 
1564
            case 52: external = gna::Ins; break;
 
1565
            case 53: external = gna::InsPsrSp; break;
 
1566
            case 54: external = gna::InsPsrDiff; break;
 
1567
            case 55: external = gna::InsRtkFloat; break;
 
1568
            case 56: external = gna::InsRtkFixed; break;
 
1569
            case 64: external = gna::OmniStarHp; break;
 
1570
            case 65: external = gna::OmniStarXp; break;
 
1571
            case 66: external = gna::CdGps; break;
 
1572
 
 
1573
            case 3: //fallthrough
 
1574
            case 7: //fallthrough
 
1575
            case 9: case 10: case 11: case 12: case 13: case 14: case 15: //fallthrough
 
1576
            case 21: case 22: case 23: case 24: case 25: case 26: case 27: case 28: case 29:  case 30: case 31: //fallthrough
 
1577
            // these guys are _not_ named as reserved in the manual case 34: case 35: case 36: case 37: case 38: case 38:  case 39: case 40: case 41: case 42: case 43: case 44: case 45: case 46: case 47:
 
1578
                     external = gna::ReservedGpsPosVelType; break;
 
1579
            default: external = gna::UnknownGpsPosVelType; break;
 
1580
        }
 
1581
        return external;
 
1582
    }
 
1583
 
 
1584
    std::string doubleVectorToString(vector<double > &vec, std::string seperator){
 
1585
        std::stringstream ss;
 
1586
        int max = vec.size();
 
1587
        ss << "[";
 
1588
        for (int i=0; i<max; i++){
 
1589
            ss << vec[i] << seperator;
 
1590
        }
 
1591
        ss << "]";
 
1592
        return ss.str();
 
1593
    }
 
1594
 
 
1595
    std::string statusToString(gna::StatusMessageType statusMessageType, std::string statusMessage){
 
1596
        std::stringstream ss;
 
1597
        switch(statusMessageType){
 
1598
            case gna::NoMsg:
 
1599
                   ss << "NoMsg" << " ";
 
1600
                   break;
 
1601
            case  gna::Initialising:
 
1602
                   ss << "Initialising" << " ";
 
1603
                   break;
 
1604
            case  gna::Ok:
 
1605
                   ss << "Ok" << " ";
 
1606
                   break;
 
1607
            case  gna::Warning:
 
1608
                   ss << "Warning" << " ";
 
1609
                   break;
 
1610
            case gna::Fault:
 
1611
                   ss << "Fault" << " ";
 
1612
                   break;
 
1613
            default:
 
1614
                   ss << "UnknownStatus" << " ";
 
1615
        }
 
1616
        ss << "[" << statusMessage << "]";
 
1617
        return ss.str();
 
1618
    }
 
1619
}//namespace