Navigator.h

Go to the documentation of this file.

#ifndef NAVIGATOR_H_
#define NAVIGATOR_H_

// include parent class
#include "component/SyncComponent.h" // should include component/Component.h in turn
// include classes that define any object that is a member of this class
#include "data/Matrix3x3.h"
#include "data/Point3D.h"
// forward declare classes for which this class includes a pointer
class ConfigReader;
class UniversalDataReader;
class UniversalDataWriter;

class Navigator : public SyncNavigationComponent
{
public:

    friend class DeadReckoner;

    Navigator( const Str& name, const Module* module ); // TODO: Can we remove the module arg somehow? all navigators are going to be in the navigationModule.

    virtual ~Navigator();

    virtual void initialize( void )
    {}

    virtual void run( void )
    {}

protected:

    ConfigReader* verbosityLevelConfigReader_;
    ConfigReader* allowableFailuresConfigReader_;
    ConfigReader* accuracyPremultiplierConfigReader_;
    ConfigReader* orientationStaleAfterConfigReader_;
    ConfigReader* velocityStaleAfterConfigReader_;

    UniversalDataReader* timeFixReader_;
    UniversalDataReader* latitudeFixReader_;
    UniversalDataReader* longitudeFixReader_;
    UniversalBlobReader* vehicleOrientationReader_;

    UniversalDataWriter* latitudeWriter_;
    UniversalDataWriter* longitudeWriter_;
    UniversalDataWriter* depthWriter_;
    UniversalDataWriter* horizontalPathLengthSinceLastFixWriter_;

    UniversalDataWriter* fixDistanceMadeGoodWriter_;
    UniversalDataWriter* fixHorizontalPathLengthSinceLastFixWriter_;
    UniversalDataWriter* fixResidualDistanceWriter_;
    UniversalDataWriter* fixResidualBearingWriter_;
    UniversalDataWriter* fixResidualPercentDistanceTraveledWriter_;

    DataWriter* elapsedSinceOrientationReadWriter_;
    DataWriter* elapsedSinceVelocityReadWriter_;

    int verbosity_;
    int allowableFailures_;
    double accuracyPremultiplier_; // configuration variable to allow the user to make one navigator preferred over others
    int inputMeasurementStartupAllowance_; // seconds to wait after startup before considering lack of data a fault
    Timestamp startTime_, navTimeStamp_;
    // bool orientationIsValid_, velocityIsValid_; // flags to keep track of whether supporting data is valid and was read successfully
    Timestamp orientationReadTime_, velocityReadTime_; // timestamps to track when valid orientation and velocity data was last read
    Timespan orientationStaleAfter_, velocityStaleAfter_; // timspans to define when data is considered stale
    double timeFix_, latitudeFix_, longitudeFix_; // geographic position of most recent fix (GPS, maybe acoustic in future...)
    double lastTimeFix_, lastLatitudeFix_, lastLongitudeFix_; // geographic position of last fix (GPS, maybe acoustic in future...)
    double horizontalPathLengthSinceLastFix_; // length in meters of the path in the horizontal plane since the last external fix (used to trigger DR error calculations)
    Point3D velocityRelativeToGroundInVehicleFrame_; // self-explanatory
    Matrix3x3 rotationFromVehicleToNavigationFrame_; // vehicle orientation as a rotation matrix
    Point3D velocityRelativeToGroundInNavigationFrame_; // self-explanatory
    Point3D displacementRelativeToGroundInNavigationFrame_; // self-explanatory
    double latitude_, longitude_, depth_; // geographic position of new estimate
    double lastLatitude_, lastLongitude_, lastDepth_; // estimated geographic position at end of last cycle
    double timeAccuracy_, latitudeAccuracy_, longitudeAccuracy_, depthAccuracy_, horizontalPathLengthSinceLastFixAccuracy_; // accuracy of new estimate

    // read config parameters
    void readConfigs( void );

    // check for a position fix from an external source
    void readExternalFix( void );

    // get the orientation of the vehicle as a rotation matrix
    void readVehicleOrientation( void );

    virtual void readVehicleVelocity( void )
    {}

    // write the estimated position of the vehicle
    void writeVehiclePosition( void );

    // evaluate and report the residual between the position estimate and an external fix
    void evaluateAndRecordFixResidual( void );
};

#endif /* NAVIGATOR_H_ */