gbxnovatelacfr/driver.h

/*
 * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics
 *               http://gearbox.sf.net/
 * Copyright (c) 2004-2010 Matthew Ridley, Ben Upcroft, Michael Moser
 *
 * This distribution is licensed to you under the terms described in
 * the LICENSE file included in this distribution.
 *
 */

#ifndef GBXNOVATELACFR_DRIVER_H
#define GBXNOVATELACFR_DRIVER_H

#include <cstdlib>
#include <string>
#include <memory>
#include <vector>

// forward declarations
// users don't need to know about serial devices or Tracers or imu decoders
namespace gbxserialacfr{
    class Serial;
}
namespace gbxutilacfr{
    class Tracer;
}
namespace gbxnovatelutilacfr{
    class ImuDecoder;
}

namespace gbxnovatelacfr{

class SimpleConfig{
public:
    SimpleConfig(std::string serialDevice, int baudRate, std::string imuType, std::vector<double > &imuToGpsOffset):
        serialDevice_(serialDevice),
        baudRate_(baudRate),
        imuType_(imuType),
        imuToGpsOffset_(imuToGpsOffset) {};

    //
    bool isValid() const;
    std::string toString() const;

    std::string serialDevice_;
    int baudRate_;
    std::string imuType_;
    std::vector<double > imuToGpsOffset_;
};

class GpsOnlyConfig{
public:
    GpsOnlyConfig(std::string serialDevice, int baudRate):
        serialDevice_(serialDevice),
        baudRate_(baudRate) {};

    //
    bool isValid() const;
    std::string toString() const;

    std::string serialDevice_;
    int baudRate_;
};

//
class Config{
public:
    //
    Config(const SimpleConfig &simpleCfg);
    //
    Config(const GpsOnlyConfig &gpsOnlyCfg); 
    Config();

    //
    bool isValid();
    std::string toString() const;

    //
    std::string serialDevice_;
    int baudRate_;

    //
    bool enableImu_;
    std::string imuType_; 


    //
    bool enableInsPva_;
    bool enableGpsPos_;
    bool enableGpsVel_;
    bool enableRawImu_;
    bool ignoreUnknownMessages_; 


    //
    double dtInsPva_; 
    double dtGpsPos_; 
    double dtGpsVel_; 
    bool fixInvalidRateSettings_; 


    //
    std::vector<double > imuToGpsOffset_;            
    std::vector<double > imuToGpsOffsetUncertainty_; 
    bool enableInsOffset_;
    std::vector<double > insOffset_;                 
    bool enableInsPhaseUpdate_;                      


    //
    bool enableCDGPS_; 
    bool enableSBAS_;  
    bool enableRTK_;   
    bool enableUseOfOmniStarCarrier_; 


    //
    bool enableSetImuOrientation_;
    int setImuOrientation_;
    bool enableVehicleBodyRotation_;
    std::vector<double > vehicleBodyRotation_;
    std::vector<double > vehicleBodyRotationUncertainty_; 

private:
};

enum StatusMessageType {
    NoMsg,       
    Initialising,
    Ok,          
    Warning,     
    Fault        
};

std::string toString( StatusMessageType type );


//
enum GpsSolutionStatusType{
    SolComputed=0,                      
    InsufficientObs=1,                  
    NoConvergence=2,                    
    Singularity=3,                      
    CovTrace=4,                         
    TestDist=5,                         
    ColdStart=6,                        
    VHLimit=7,                          
    Variance=8,                         
    Residuals=9,                        
    DeltaPos=10,                        
    NegativeVar=11,                     
    ReservedGpsSolutionStatusType12=12, 
    IntegrityWarning=13,                
    InsInactive=14,                     
    InsAligning=15,                     
    InsBad=16,                          
    ImuUnplugged=17,                    
    Pending=18,                         
    InvalidFix=19,                      
    UnknownGpsSolutionStatusType
};

std::string toString( GpsSolutionStatusType type );

//
enum GpsPosVelType{
    None=0,                     
    FixedPos=1,                 
    FixedHeight=2,              
    ReservedGpsPosVelType3=3,   
    FloatConv=4,                
    WideLane=5,                 
    NarrowLane=6,               
    ReservedGpsPosVelType7=7,   
    DopplerVelocity=8,          
    ReservedGpsPosVelType9=9,   
    ReservedGpsPosVelType10=10, 
    ReservedGpsPosVelType11=11, 
    ReservedGpsPosVelType12=12, 
    ReservedGpsPosVelType13=13, 
    ReservedGpsPosVelType14=14, 
    ReservedGpsPosVelType15=15, 
    Single=16,                  
    PsrDiff=17,                 
    Waas=18,                    
    Propagated=19,              
    Omnistar=20,                
    ReservedGpsPosVelType21=21, 
    ReservedGpsPosVelType22=22, 
    ReservedGpsPosVelType23=23, 
    ReservedGpsPosVelType24=24, 
    ReservedGpsPosVelType25=25, 
    ReservedGpsPosVelType26=26, 
    ReservedGpsPosVelType27=27, 
    ReservedGpsPosVelType28=28, 
    ReservedGpsPosVelType29=29, 
    ReservedGpsPosVelType30=30, 
    ReservedGpsPosVelType31=31, 
    L1Float=32,                 
    IonoFreeFloat=33,           
    NarrowFloat=34,             
    L1Int=48,                   
    WideInt=49,                 
    NarrowInt=50,               
    RtkDirectIns=51,            
    Ins=52,                     
    InsPsrSp=53,                
    InsPsrDiff=54,              
    InsRtkFloat=55,             
    InsRtkFixed=56,             
    OmniStarHp=64,              
    OmniStarXp=65,              
    CdGps=66,                   
    UnknownGpsPosVelType
};

std::string toString( GpsPosVelType type );

enum DataType {
    InsPva,
    BestGpsPos,
    BestGpsVel,
    RawImu
};

class GenericData {
    public:
        virtual ~GenericData(){};
        virtual DataType type() const=0;
        virtual std::string toString() const=0;
    private:
};

class InsPvaData : public GenericData {
    public:
        DataType type() const {
            return InsPva;
        }
        std::string toString() const;
        int      gpsWeekNr;         
        double   secIntoWeek;       
        double   latitude;          
        double   longitude;         
        double   height;            

        //
        double   northVelocity;     
        double   eastVelocity;      
        double   upVelocity;        


        //
        double   roll;              
        double   pitch;             
        double   azimuth;           


        StatusMessageType statusMessageType;
        std::string statusMessage;

        int timeStampSec;  
        int timeStampUSec; 
};

class BestGpsPosData : public GenericData {
    public:
        DataType type() const {
            return BestGpsPos;
        }
        std::string toString() const;
        int gpsWeekNr;                          
        unsigned int msIntoWeek;                
        GpsSolutionStatusType  solutionStatus;  //
        GpsPosVelType          positionType;    //
        double       latitude;                  
        double       longitude;                 
        double       heightAMSL;                
        float        undulation;                
        unsigned int datumId;                   //
        float        sigmaLatitude;             
        float        sigmaLongitude;            
        float        sigmaHeight;               
        char         baseStationId[4];          //
        float        diffAge;                   
        float        solutionAge;               
        int          numObservations;           
        int          numL1Ranges;               
        int          numL1RangesRTK;            
        int          numL2RangesRTK;            

        StatusMessageType statusMessageType;
        std::string statusMessage;

        int timeStampSec;  
        int timeStampUSec; 
};

class BestGpsVelData : public GenericData {
    public:
        DataType type() const {
            return BestGpsVel;
        }
        std::string toString() const;
        int          gpsWeekNr;                 
        unsigned int msIntoWeek;                
        GpsSolutionStatusType  solutionStatus;  //
        GpsPosVelType          velocityType;    //
        float        latency;                   
        float        diffAge;                   
        double       horizontalSpeed;           
        double       trackOverGround;           
        double       verticalSpeed;             

        StatusMessageType statusMessageType;
        std::string statusMessage;

        int timeStampSec;  
        int timeStampUSec; 
};

class RawImuData : public GenericData {
    public:
        DataType type() const {
            return RawImu;
        }
        std::string toString() const;
        int    gpsWeekNr;   
        double secIntoWeek; 



        //
        double zDeltaV;   
        double yDeltaV;   
        double xDeltaV;   


        //
        double zDeltaAng; 
        double yDeltaAng; 
        double xDeltaAng; 


        StatusMessageType statusMessageType;
        std::string statusMessage;

        int timeStampSec;  
        int timeStampUSec; 
};

class Driver {
public:

    //

    //
    Driver( const Config &cfg);
    Driver( const Config &cfg, gbxutilacfr::Tracer &tracer);
    ~Driver();

    std::auto_ptr<GenericData> read();

private:

    // does the leg-work for the constructor (via the following guys)
    void configure();
    // establish a serial connection to the receiver
    void connectToHardware();
    // set parameters related to the IMU
    void configureImu();
    // set parameters related to the INS
    void configureIns();
    // set parameters related to GPS
    void configureGps();
    // turn on data messages we are interested in
    void requestData();

    std::auto_ptr<gbxnovatelutilacfr::ImuDecoder> imuDecoder_;

    std::auto_ptr<gbxserialacfr::Serial> serial_;
    int baud_;

    Config config_;
    std::auto_ptr<gbxutilacfr::Tracer> tracerInternal_;
    gbxutilacfr::Tracer& tracer_;
};


} // namespace
#endif