GPSObserver.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file GPSObserver.cpp
*  \brief Observes attitute with the GPS algorithm
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.1 $
*  \date    $Date: 2007/08/31 16:09:30 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*!
*/
//////////////////////////////////////////////////////////////////////////////////////////////////


#include "GPSObserver.h"

using namespace std;

/*! \brief Default Constructor */
GPSObserver::GPSObserver( )
{
        Initialize( );
}

/*! \brief Create a GPS observer from XML handle and whorl object
 *  @param handle XML handle with current whorl as starting node
 *  @param ptr_whorl Pointer to a whorl object
 */
GPSObserver::GPSObserver( TiXmlHandle handle, Whorl* ptr_whorl )
{
        m_whorl = ptr_whorl;

        Initialize( );

        // call parse function to get inertial measurement vectors
        Parse( handle );
}

/*! \brief DeConstructor for GPSObserver object */
GPSObserver::~GPSObserver( )
{ }

/*! \brief Initialize GPS observer
*/
int GPSObserver::Initialize( )
{
        /* Host name and port for communicating with the GPS receiver */
        static const char               GPS_HOST[]      = "128.173.89.201"; // euripides.ece.vt.edu in GPS Lab
        static const unsigned short     GPS_PORT        = 5002; // ports range from 5000-5005

        /* Create Object associated with Ashtech G12 GPS Receiver */
        m_SpaceVehicle.Connect( GPS_HOST, GPS_PORT );

        m_WGS84Vector.initialize(6);
        m_ECEFVector.initialize(6);
        m_ECIVector.initialize(6);

        m_InitialTime = Now( );
        
        return( 0 );
}

/*! \brief Parse inertial measurement vectors from configuration file
        * @param handle XML handle with current whorl as starting node
        */
void GPSObserver::Parse( TiXmlHandle handle )
{
/*
        int response;
        // create handles with magnetometer and accelerometer nodes as starting points
        TiXmlHandle accelElement =  handle.FirstChild( m_whorl->GetSimulatorName( ) ).FirstChild("HARDWARE_PROPERTIES").FirstChild("DMU_ACCELEROMETER");
        TiXmlHandle magElement = handle.FirstChild( m_whorl->GetSimulatorName( ) ).FirstChild("HARDWARE_PROPERTIES").FirstChild("MAGNETOMETER");

        // get magnetometer inertial vector and normalize
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueX", &(m_magInertial)(1) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueY", &(m_magInertial)(2) );
        checkResponse(response);
        response = magElement.Child("INERTIAL_VECTOR", 0).Element() ->  QueryDoubleAttribute( "valueZ", &(m_magInertial)(3) );
        checkResponse(response);

        // Normalize magnetmeter inertial vector
        normalize( m_magInertial );

        // get accelerometer inertial vector and normalize
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueX", &(m_accelInertial)(1) );
        checkResponse(response);
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueY", &(m_accelInertial)(2) );
        checkResponse(response);
        response = accelElement.Child("INERTIAL_VECTOR", 0).Element() -> QueryDoubleAttribute( "valueZ", &(m_accelInertial)(3) );
        checkResponse(response);
*/

}

/*! \brief Take measurements and update state vector in whorl object */
int GPSObserver::Run( )
{
        
        /* Get Current Position */
        if ( m_SpaceVehicle.GetCurrentPosition( m_SpaceVehiclePosition ) )
        {
                cerr << "WARNING: GPS Status returned failure.  "<< endl;
                return( 0 );
        }

        if (  m_SpaceVehiclePosition.m_numSatellites <= 4 )
        {
                cerr << "WARNING: Number of GPS Satellites less than 4.  "<< endl;
                return( 0 );
        }


        /* Print gps data to screen */
        //GPS2Screen( m_SpaceVehiclePosition );

        TangentPlaneState( m_SpaceVehiclePosition, m_WGS84Vector );
        
        m_ECEFVector = WGS842ECEF( m_WGS84Vector(1), m_WGS84Vector(2), m_WGS84Vector(3),  m_WGS84Vector(4),  m_WGS84Vector(5),  m_WGS84Vector(6) );

        /* Determine Time */
        static ssfTime currentTime = Now( );
        static tm currentTimetm;
        currentTimetm = currentTime.GetDateTime( );
        tm G12Time;
        G12Time.tm_mday = currentTimetm.tm_mday;
        G12Time.tm_mon = currentTimetm.tm_mon;
        G12Time.tm_year = currentTimetm.tm_year;
        G12Time.tm_hour = int( m_SpaceVehiclePosition.m_UTC_Timetag ) /10000; // hour
        G12Time.tm_min = int( ( m_SpaceVehiclePosition.m_UTC_Timetag - double( G12Time.tm_hour )*10000 ) /100 ); // minute
        G12Time.tm_sec = int( ( m_SpaceVehiclePosition.m_UTC_Timetag - G12Time.tm_hour*10000 ) - G12Time.tm_min*100 ); // second
        static double u_sec = ( ( m_SpaceVehiclePosition.m_UTC_Timetag - G12Time.tm_hour*10000 ) - G12Time.tm_min*100 ) - G12Time.tm_sec; // second
        static ssfTime GPSTime( G12Time );
        static ssfTime Time( GPSTime.GetSeconds( ) + u_sec );


        m_ECIVector = ECEF2ECI( m_ECEFVector, Time.GetJulianDate( ) );

        /* Determine Osculating Orbital Elements */
        m_SpaceVehicleOsculatingCOE.SetOsculatingOrbitalElements( m_ECIVector/1000 );

        /* Determine Mean Orbital Elements */
        m_SpaceVehicleMeanCOE = m_SpaceVehicleOsculatingCOE.GetMeanOrbitalElements( ) ;
        static double a, e, i, Lon, Arg, tru, E, M, M0;
                                a = m_SpaceVehicleMeanCOE.GetSemimajorAxis( );
                                e = m_SpaceVehicleMeanCOE.GetEccentricity( );
                                i = m_SpaceVehicleMeanCOE.GetInclination( );
                                Lon = m_SpaceVehicleMeanCOE.GetLongAscNode( );
                                Arg = m_SpaceVehicleMeanCOE.GetArgPerigee( );
                                tru = m_SpaceVehicleMeanCOE.GetTrueAnomaly( );
                                E = m_SpaceVehicleMeanCOE.GetEccentricAnomaly( );
                                M = m_SpaceVehicleMeanCOE.GetMeanAnomaly( );
                                M0 = M - m_SpaceVehicleMeanCOE.GetMeanMotion( ) * ( Time.GetSeconds( ) - m_InitialTime.GetSeconds( ) );
        /*
                                cout << endl << endl << "Mean CLassical Orbital Elements: " <<  endl
                                << "Semi-Major Axis (km): " << a << endl
                                << "Eccentricity: " << e << endl
                                << "Inclination (deg): " << Rad2Deg(i) << endl
                                << "Longitude of Ascending Node (deg): " << Rad2Deg(Lon) <<  endl
                                << "Argument of Peripsis (deg): " << Rad2Deg(Arg) << endl
                                //<< "True Anomaly (deg): " << Rad2Deg(tru) << endl
                                 << "Mean Anomaly: " << M <<  endl;
        */

        m_whorl->SetCOE( m_SpaceVehicleMeanCOE );
        m_whorl->SetOscCOE( m_SpaceVehicleOsculatingCOE.GetOsculatingOrbitalElements( ) );
        m_whorl->SetECI( m_ECIVector );

        return( 0 );
}

Vector GPSObserver::GetWGS84( )
{
        return( m_WGS84Vector );
}

Vector GPSObserver::GetECEF( )
{
        return( m_ECEFVector );
}

Vector GPSObserver::GetECI( )
{
        return( m_ECIVector );
}

Keplerian GPSObserver::GetOsculatingOrbitalElements( )
{
        return( m_SpaceVehicleOsculatingCOE.GetOsculatingOrbitalElements( ) );
}

Keplerian GPSObserver::GetMeanOrbitalElements( )
{
        return( m_SpaceVehicleMeanCOE );
}


void GPSObserver::GPS2Screen( AshtechG12_GPS_PhysicalDevice::Position _spaceVehicle )
{
        cout << "......Navigation Information Obtained from GPS Receiver......." << endl;
        cout << "RTCM... " << _spaceVehicle.m_RTCM << endl;
        cout << "Num Sats...   " <<  _spaceVehicle.m_numSatellites << endl;
        printf ("UTC Timetag: %6.2f\n", _spaceVehicle.m_UTC_Timetag);
        cout << "latitude... " << _spaceVehicle.m_latitude << endl;
        cout << "latitude sector... " << _spaceVehicle.m_latitudeSector << endl;
        cout << "long... " << _spaceVehicle.m_longitude << endl;
        cout << "long sector... " << _spaceVehicle.m_longitudeSector << endl;
        cout << "alt.. " << _spaceVehicle.m_altitude << endl;
        cout << "ground.. " << _spaceVehicle.m_groundTrack << endl;
        cout << "speed.. " << _spaceVehicle.m_groundSpeed << endl;
        cout << "vert.. " << _spaceVehicle.m_verticalVelocity << endl;
        cout << "Pdop.. " << _spaceVehicle.m_PDOP << endl;
        cout << "hdop.. " << _spaceVehicle.m_HDOP << endl;
        cout << "vdop.. " << _spaceVehicle.m_VDOP << endl;
        cout << "Tdop.. " << _spaceVehicle.m_TDOP << endl;
}


// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: GPSObserver.cpp,v $
*       Revision 1.1  2007/08/31 16:09:30  jayhawk_hokie
*       Initial Submission.
*
*
*
******************************************************************************/

---