MRPTracking.cpp

Go to the documentation of this file.

//////////////////////////////////////////////////////////////////////////////////////////////////
/*! \file MRPTracking.cpp
*  \brief The MRPTracking class contains the control law for MRP and angular velocity control.
*         It is derived from the Contorller class.
*  \author $Author: jayhawk_hokie $
*  \version $Revision: 1.4 $
*  \date    $Date: 2007/08/31 15:53:26 $
*//////////////////////////////////////////////////////////////////////////////////////////////////
/*! 
*/
//////////////////////////////////////////////////////////////////////////////////////////////////

#include "MRPTracking.h"

/* Constructor */
MRPTracking::MRPTracking( )
{ 
}

MRPTracking::MRPTracking( Whorl* ptr_whorl ) 
{
        /* Defined in Controller Class. */
        m_whorl = ptr_whorl;
        
        Initialize( );
        
}

/* Deconstructor */
MRPTracking::~MRPTracking() 
{ 
}

int MRPTracking::Initialize( ) 
{
        m_controlTorque.initialize(3);  
        
        m_uMax =1.5; // [N-m]

        /* Set scalar gain for MRP vector. */
        m_mrpGain = 2.5;

        /* Set the contorl gain matrix for angular rates. Semi-automatic gain selection */
        Matrix _inertiaTemp = m_whorl->GetMOI( );
        m_gainMatrix.initialize(3,3);
        for ( int i=1; i<4; i++ )
        {
                m_gainMatrix(i,i) = pow( ( m_mrpGain* _inertiaTemp(i,i) ), 0.5 );
        }

        /* Get orientation of Momentum Wheels. Defined in Controller Class. */
        A = FindA( );

        /* Get Momentum Wheel spin axis MOI. Defined in Controller Class */
        MOI_sw = FindMOI_sw( );
        
        /* Reference Model */
        Quaternion qri( 0.0, 0.0, 0.0, 1.0 );
        m_quaternionReference(1) = qri(1);
        m_quaternionReference(2) = qri(2);
        m_quaternionReference(3) = qri(3);
        m_quaternionReference(4) = qri(4);
        m_mrpReference = ModifiedRodriguezParameters( qri );
        m_angularRateReference(1) = 0; 
        m_angularRateReference(2) = 0.0; 
        m_angularRateReference(3) = 0.0; // [rad/s] 

        ssfTime time;
        time = Now( );
        m_previousTime = time.GetSeconds( );

        return( 0 );
}

int MRPTracking::Run( )
{
        static ssfTime currentTime;
        currentTime = Now( );
        static ssfTime deltaTime;
        deltaTime = ( currentTime.GetSeconds( ) - m_previousTime );
        //m_previousTime = currentTime.GetSeconds( ); // update time
        // dont' update time, thus provides duration time in seconds.   

        /* Desired State. Reference Model */
        //ReferenceModelSC( deltaTime );
        ReferenceTrajectory( deltaTime );       

        ModifiedRodriguezParameters mrp( m_whorl->GetMRP( ) );
        ModifiedRodriguezParameters mrpReference( m_mrpReference );

        /* Determine Rotation Matirx */
        Rotation RBN( mrp ); // body relative to inertial frame
        Rotation RRN( mrpReference ); // reference relative to inertial frame
        Rotation RBR; // body relative to reference frame
        RBR = RBN*~RRN;

        /* Error */
        static Vector angularRateError(3);
        angularRateError = m_whorl->GetOmegaBL( ) - RBR*(m_angularRateReference); // angular rate error [rad/s]
        ModifiedRodriguezParameters mrpError;
        mrpError = mrp - mrpReference; // mrp error
        mrpError.Switch( 1 ); // important to activate switch condition

        /* Set Whorl Error Values */
        m_whorl->SetMRPError( mrpError );       
        m_whorl->SetAngularRateError( angularRateError );       

        /* Set Whorl Reference Parameters */
        m_whorl->SetReferenceOmegaBL( m_angularRateReference ); 
        m_whorl->SetReferenceQuaternion( m_quaternionReference );       

        /* Reaction Wheel Speed */
        static Vector wheelSpeed(3);
        double meastime;
        m_whorl->GetMomentumWheel("REACTION_X")->GetWheelSpeed( wheelSpeed(1), meastime );
        m_whorl->GetMomentumWheel("REACTION_Y")->GetWheelSpeed( wheelSpeed(2), meastime );
        m_whorl->GetMomentumWheel("REACTION_Z")->GetWheelSpeed( wheelSpeed(3), meastime );

        /* Reaction Wheel Angular Momentum */
        static Vector hs(3);
        for ( int i=1; i<4; i++ )
        {
                static Vector w(3);
                w(1) = A(1,i);
                w(2) = A(2,i);
                w(3) = A(3,i);
                static Matrix ws(1,1);
                ws = ~w*m_whorl->GetOmegaBL( );
                hs(i) = MOI_sw(i,i) * ( ws(1,1) + wheelSpeed(i) );
        }
        
        /* MRP tracking control law - attitude and angular rate */
        m_controlTorque = (~A)*( -m_mrpGain*( mrpError ) - (m_gainMatrix)*angularRateError 
                + skew( m_whorl->GetOmegaBL( ) )*(m_whorl->GetMOI( )*m_whorl->GetOmegaBL( ) + (A)*hs) 
                + m_whorl->GetMOI( )*( RBR.GetDCM( )*(m_angularAccelReference) 
                - skew( m_whorl->GetOmegaBL( ) )*RBR.GetDCM( )*(m_angularRateReference) ) ); // [N-m]
        
        /* Wheel Saturation Function */
        m_controlTorque = WheelSaturation( m_controlTorque );

        /* Set the desird contorl torque in the whorl object */
        m_whorl->SetControl( m_controlTorque );

        /* Set the torque for the momentum wheels to produce */
        SetWheelTorque( m_controlTorque );

        return( 0 );
}

// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: MRPTracking.cpp,v $
*       Revision 1.4  2007/08/31 15:53:26  jayhawk_hokie
*       Modified.
*       
*       Revision 1.3  2007/07/24 09:26:41  jayhawk_hokie
*       Updated.
*       
*       Revision 1.2  2007/05/31 17:21:16  jayhawk_hokie
*       *** empty log message ***
*       
*       Revision 1.1  2007/05/23 22:26:01  jayhawk_hokie
*       Initial submission.
*       
*       
*
******************************************************************************/

---