DSACSS Operational Code: COENaaszController.cpp Source File

00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file COENaaszController.cpp
00003 *  \brief Implementation of the COENaaszController class.
00004 *  \author $Author: jayhawk_hokie $
00005 *  \version $Revision: 1.1 $
00006 *  \date    $Date: 2007/08/31 15:58:35 $
00007 *//////////////////////////////////////////////////////////////////////////////////////////////////
00008 /*! 
00009 */
00010 //////////////////////////////////////////////////////////////////////////////////////////////////
00011 
00012 #include "COENaaszController.h"
00013 
00014 /* Constructor */
00015 COENaaszController::COENaaszController( )
00016 { 
00017 }
00018 
00019 COENaaszController::COENaaszController( Whorl*ptr_whorl )
00020 { 
00021         m_whorl = ptr_whorl;
00022         Initialize( );
00023         
00024 }
00025 
00026 /* Deconstructor */
00027 COENaaszController::~COENaaszController() 
00028 { }
00029 
00030 int COENaaszController::Initialize( ) 
00031 {
00032 
00033         /// Host name and port for communicating with GPS_Sim_Propagator
00034         static const char               SimPropagator_HOST[]      = "severian.aoe.vt.edu"; //
00035         static const unsigned short     SimPropagator_PORT        = 30000; // ports range from 5000-5005
00036 
00037         m_accelerationVector = new AccelerationMessage;
00038         m_accelerationVector -> Connect( SimPropagator_HOST, SimPropagator_PORT );
00039 
00040         /* Control Acceleratin Gains */
00041         m_Gains.initialize(6);
00042         double dt = 2; // [s]
00043         Keplerian SpaceVehicleMeanCOE( m_whorl->GetCOE( ) );
00044         double a, e, i, Lon, Arg, tru, E, M, M0;
00045         a = SpaceVehicleMeanCOE.GetSemimajorAxis( );
00046         e = SpaceVehicleMeanCOE.GetEccentricity( );
00047         i = SpaceVehicleMeanCOE.GetInclination( );
00048         Lon = SpaceVehicleMeanCOE.GetLongAscNode( );
00049         Arg = SpaceVehicleMeanCOE.GetArgPerigee( );
00050         tru = SpaceVehicleMeanCOE.GetTrueAnomaly( );
00051         E = SpaceVehicleMeanCOE.GetEccentricAnomaly( );
00052         M = SpaceVehicleMeanCOE.GetMeanAnomaly( );
00053         M0 = M;
00054 
00055         if( M0 > PI ) // set short distance
00056         {
00057                 M0 = M0 - 2*PI;
00058         }
00059         if( M0 < - PI ) // set short distance
00060         {
00061                  M0 = M0 + 2*PI;
00062         }
00063         Vector eHAT(6);
00064         eHAT(1) = a;
00065         eHAT(2) = e;
00066         eHAT(3) = i;
00067         eHAT(4) = Lon;
00068         eHAT(5) = Arg;
00069         eHAT(6) = M0; // initial Mean Anomaly
00070         
00071         Vector ECI = m_whorl->GetECI( );        
00072 
00073         m_Gains = Gains( ECI(_(1,3)), ECI(_(4,6)), eHAT, dt);
00074 
00075         /* Space Vehicle Id */
00076         m_vehicleID = 1;
00077 
00078         /* Acceleration Duration [s] */
00079         m_duration = 2;
00080 
00081         /* Space Vehicle Mass [kg] */
00082         m_mass = 100;
00083 
00084         /* Current Time */
00085         m_vehicleTime = Now( );
00086 
00087         /* Initial Time */
00088         m_initialTime = Now( );
00089 
00090         /* Reset delta V time history from orbit propagator */
00091         m_accelerationVector->ResetValues( );
00092 
00093         return( 0 );
00094 }
00095 
00096 
00097 
00098 int COENaaszController::Run( )
00099 {
00100 
00101         m_vehicleTime = Now( );
00102 
00103         Keplerian SpaceVehicleMeanCOE( m_whorl->GetCOE( ) );
00104         double a, e, i, Lon, Arg, tru, E, M, M0;
00105         a = SpaceVehicleMeanCOE.GetSemimajorAxis( );
00106         e = SpaceVehicleMeanCOE.GetEccentricity( );
00107         i = SpaceVehicleMeanCOE.GetInclination( );
00108         Lon = SpaceVehicleMeanCOE.GetLongAscNode( );
00109         Arg = SpaceVehicleMeanCOE.GetArgPerigee( );
00110         tru = SpaceVehicleMeanCOE.GetTrueAnomaly( );
00111         E = SpaceVehicleMeanCOE.GetEccentricAnomaly( );
00112         M = SpaceVehicleMeanCOE.GetMeanAnomaly( );
00113         M0 = M - SpaceVehicleMeanCOE.GetMeanMotion( ) * ( m_vehicleTime.GetSeconds( ) - m_initialTime.GetSeconds( ) );;
00114 
00115         if( M0 > PI ) // set short distance
00116         {
00117                 M0 = M0 - 2*PI;
00118         }
00119         if( M0 < - PI ) // set short distance
00120         {
00121                  M0 = M0 + 2*PI;
00122         }
00123         Vector eHAT(6);
00124         eHAT(1) = a;
00125         eHAT(2) = e;
00126         eHAT(3) = i;
00127         eHAT(4) = Lon;
00128         eHAT(5) = Arg;
00129         eHAT(6) = M0; // initial Mean Anomaly
00130         
00131         Vector ECI = m_whorl->GetECI( );        
00132 
00133         Vector de(6);
00134         
00135 
00136         /* Radial, In-Track, Cross-Track control acceleration [m/s^2] */
00137         Vector u = COENaasz( ECI(_(1,3)), ECI(_(4,6)), de, eHAT, m_Gains, tru);
00138 
00139         /* Current Time */
00140         m_vehicleTime = Now( );
00141 
00142         /* Set Orbit Control Acceleration */
00143         m_whorl->SetOrbitControl( u );
00144 
00145         /* Send control accleration to GPS simulator propagator (running on Severian) */
00146         m_accelerationVector->SendAccelerationMessage( m_vehicleID, m_vehicleTime.GetSeconds( ), m_duration, u, m_mass );
00147 
00148         return( 0 );
00149 }
00150 
00151 /*! \brief Determination of Feedback Control Gains
00152  *  based on Naasz and Hall.
00153  *  @param ECI is position  x, y, z
00154  *  @param VECI is the velocity Vx, Vy, Vz
00155  *  @param eHAT is the mean orbit eletments - a, e, i, Lon, Arg, Mo 
00156  */
00157 Vector COENaaszController::Gains(Vector ECI, Vector VECI, Vector eHAT, double dt)
00158 {
00159         // 
00160         // Determine key parameters
00161         double H, p, b, r;
00162         cals(ECI, VECI, eHAT(1), H, p, b, r);
00163         //
00164         // Calculate gains      
00165         Vector gains(6);
00166         double Ka,Ke,Ki,KOMEGA,Komega,KMo; // gains
00167 
00168         Ka      = pow(H,2)/(4*pow(eHAT(1),4)*pow((1+eHAT(2)),2)*dt);
00169         Ke      = pow(H,2)/(4*pow(p,2)*dt);
00170         Ki      = pow(   ( H+eHAT(2)*H*cos( eHAT(5) + asin( eHAT(2)*sin(eHAT(5)) ) ) )
00171                          /(p*( -1 + pow( eHAT(2),2 )*pow( sin( eHAT(5) ),2 ) ) ),2 )/dt;
00172         KOMEGA  = pow(H*sin( eHAT(3) )*(-1 + eHAT(2)*sin( eHAT(5) + asin( eHAT(2)*cos(eHAT(5) ) ) ) )
00173                         /(p*(1-pow( eHAT(2),2 )*pow(cos( eHAT(5) ),2 ) ) ),2)/dt;
00174         Komega  = pow( eHAT(2)*H/p,2 )/4*(1-pow( eHAT(2),2 )/4)/dt;
00175         KMo     = pow( eHAT(1)*eHAT(2)*H / (2*b*p),2 )*(1-pow( eHAT(2),2 )/4)*dt;
00176 
00177         gains(1) = Ka;
00178         gains(2) = Ke;
00179         gains(3) = Ki;
00180         gains(4) = KOMEGA;
00181         gains(5) = Komega;
00182         gains(6) = KMo; // Issue with M resetting due to 0 deg. need to implement correction.
00183 
00184         return ( gains );
00185 }
00186 
00187 /*! \brief Classical Orbit Element Controller using mean orbit elements 
00188  *  Based on Naasz and Hall
00189  * [Current State - Desired State] = de = [d(a) de di dOmega domega dM0)
00190  *  @param ECI is position  x, y, z
00191  *  @param VECI is the velocity Vx, Vy, Vz
00192  *  @param de
00193  *  @param eHAT = [a e i Omega omega M0)
00194  *  @param gains
00195  */
00196 Vector COENaaszController::COENaasz(Vector ECI, Vector VECI, Vector de, Vector eHAT, Vector gains, double tru)
00197 {
00198 
00199         double a = eHAT(1); // (km)
00200         double e = eHAT(2); //
00201         double i = eHAT(3); // (rad)
00202         //double OMEGA = eHAT(4); // (rad)
00203         double omega = eHAT(5); // (rad)
00204         //double Mo = eHAT(6); // 
00205 
00206         // 
00207         // Determine key parameters
00208         double H, p, b, r;
00209         cals(ECI, VECI, a, H, p, b, r);
00210 
00211         Vector Vx(6);
00212         Vx(1)   = gains(1)*de(1);
00213         Vx(2)   = gains(2)*de(2);
00214         Vx(3)   = gains(3)*de(3);
00215         Vx(4)   = gains(4)*de(4);
00216         Vx(5)   = gains(5)*de(5);
00217         Vx(6)   = gains(6)*de(6);
00218 
00219         //
00220         // Control Matrix
00221                 CAMdoubleMatrix gx(6,3);
00222                 gx(1,1)         = 2*pow(a,2)*e*sin(tru)/H;
00223                 gx(1,2)         = 2*pow(a,2)*p/(H*r);
00224                 gx(1,3)         = 0;
00225                 gx(2,1)         = p*sin(tru)/H;
00226                 gx(2,2)         = ((p+r)*cos(tru)+r*e)/H;
00227                 gx(2,3)         = 0;
00228                 gx(3,1)         = 0;
00229                 gx(3,2)         = 0;
00230                 gx(3,3)         = r*cos(omega+tru)/H;
00231                 gx(4,1)         = 0;
00232                 gx(4,2)         = 0;
00233                 gx(4,3)         = r*sin(omega+tru)/(H*sin(i));
00234                 gx(5,1)         = -p*cos(tru)/(H*e);
00235                 gx(5,2)         = (p+r)*sin(tru)/(H*e);
00236                 gx(5,3)         = -r*sin(omega+tru)*cos(i)/(H*sin(i));
00237                 gx(6,1)         = b*(p*cos(tru)-2*r*e)/(a*H*e);
00238                 gx(6,2)         = -b*(p+r)*sin(tru)/(a*H*e);
00239                 gx(6,3)         = 0;
00240                 //cout << "Matrix (gx): \n" << gx << endl;
00241                 //cout << "Matrix (~gx): \n" << (~gx) << endl;
00242                 //cout << "Vx: \n " << Vx << endl;
00243 
00244         //
00245         // Determine Control Acceleration Vector in Hill's frame (u_r, u_theta, u_h)
00246                 Vector ucl(3);
00247                 Vector udesired(3);
00248                 udesired =  -(~gx)*(Vx);
00249                 //cout << "udesired: \n" << udesired(1) << " " << udesired(2) << " " << udesired(3) << endl;
00250 
00251                 Vector thrust = thruster(udesired);
00252 
00253                 ucl = thrust/MASS;
00254 
00255         return( ucl );
00256 }
00257 
00258 
00259 
00260 // Do not change the comments below - they will be added automatically by CVS
00261 /*****************************************************************************
00262 *       $Log: COENaaszController.cpp,v $
00263 *       Revision 1.1  2007/08/31 15:58:35  jayhawk_hokie
00264 *       Initial Submission.
00265 *       
00266 *       
00267 *       
00268 *
00269 ******************************************************************************/