Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages | Examples

Whorl.cpp

Go to the documentation of this file.
00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file Whorl.cpp
00003 *  \brief Implementation of the Whorl class.
00004 *  \author $Author: jayhawk_hokie $
00005 *  \version $Revision: 1.21 $
00006 *  \date    $Date: 2007/08/31 16:00:19 $
00007 *//////////////////////////////////////////////////////////////////////////////////////////////////
00008 /*! 
00009 */
00010 //////////////////////////////////////////////////////////////////////////////////////////////////
00011 
00012 #include "Whorl.h"
00013 
00014 Whorl::Whorl( )
00015 { }
00016 
00017 Whorl::~Whorl( )
00018 {
00019         cerr << "\n--------------------------------------------------------------" << endl; 
00020         cerr << "Deinitializing Whorl Object" << endl;
00021         cerr << "--------------------------------------------------------------" << endl; 
00022         string axis;
00023 
00024         switch( s_mapStringValues[ m_HardwareONOFF ] )
00025         {
00026                 case ON:
00027                         for( int i =0; i < 3; i++ )
00028                         {
00029                                 // set up a string to designate axis
00030                                 if( i==0 ){axis = "X";};
00031                                 if( i==1 ){axis = "Y";};
00032                                 if( i==2 ){axis = "Z";};
00033                 
00034                                 /* Delete Rate Gyros */
00035                                 delete m_rateGyros[string( "RATE_GYRO_" + axis )];
00036                                 cerr << "Delete RATE_GYRO_" + axis << " Object" << endl; 
00037                 
00038                                 /* Delete Accelerometers */
00039                                 delete m_accelerometers[string( "ACC_" + axis )];
00040                                 cerr << "Delete ACC_" + axis << " Object" << endl; 
00041                 
00042                                 /* Delete Momentum Wheel */
00043                                 string activation;
00044                                 m_WheelOptionString >> activation;      
00045                                 switch( s_mapStringValues[ activation ] )
00046                                 {
00047                                         case ON:
00048                                                 delete m_momentumWheels[ string( "REACTION_" + axis ) ] ;
00049                                                 cerr << "Delete REACTION_" + axis << " Object" << endl; 
00050                                                 break;
00051                                         case OFF:
00052                                                 break;
00053                                         default:
00054                                                 break;
00055                                 }       
00056                         }
00057                         /* Delete Magnetometer */               
00058                         delete m_magnetometer;
00059                         cerr << "Delete Magnetometer Object" << endl; 
00060                         break;
00061                 case OFF:
00062                         break;
00063                 default:
00064                         break;
00065         }
00066 
00067         sleep(1);
00068         
00069         cerr << "\nDeinitialization of Whorl Object Complete" << endl;
00070         cerr << "--------------------------------------------------------------" << endl; 
00071         cerr << "--------------------------------------------------------------" << endl; 
00072 
00073         sleep(2);
00074 }
00075 
00076 /*! \brief Whorl object initialization
00077  *  @param tempHandle XML config handle         
00078  */
00079 void Whorl::Initialize( TiXmlHandle tempHandle ) 
00080 {
00081         cerr << "\n--------------------------------------------------------------" << endl; 
00082         cerr << "Creating Whorl Object" << endl;
00083         cerr << "--------------------------------------------------------------" << endl; 
00084 
00085         // Define Map
00086         s_mapStringValues["ON"] = ON;
00087         s_mapStringValues["OFF"] = OFF;
00088         s_mapStringValues["molpe"] = molpe;
00089         s_mapStringValues["hierax"] = hierax;
00090         s_mapStringValues["sphigx"] = sphigx;
00091 
00092         // Determine simulator from network name. This could change in the future
00093         char networkName[30];
00094         if( gethostname( networkName, 30) )
00095         {
00096                 cerr << "\nWARNING: Could not determine Network Name from Simulator. \n " << endl;
00097         }
00098 
00099         switch( s_mapStringValues[ networkName ] )
00100         {
00101                 case molpe:
00102                         m_SimulatorName = "WHORL-I";
00103                         cerr << "Object Being Created for Simulator: " << m_SimulatorName << endl;
00104                         break;
00105                 case hierax:
00106                         m_SimulatorName = "WHORL-II";
00107                         cerr << "Object Being Created for Simulator: " << m_SimulatorName << endl;
00108                         break;
00109                 default:
00110                         m_SimulatorName = "WHORL-III";
00111                         cerr << "\nWARNING: Not Using Whorl-I or Whorl-II. Initializing Whorl Object for Whorl-III (Software Simulator) on " << networkName << endl << endl;
00112         }
00113 
00114 
00115         TiXmlHandle docHandle = tempHandle.FirstChild( m_SimulatorName );
00116         int response;
00117 
00118         m_HardwareONOFF = docHandle.Child( "HARDWARE_PROPERTIES", 0 ).Element() -> Attribute( "selection" );
00119         
00120         m_recentParameter.initialize(9);
00121         m_recentState.initialize(7);
00122         m_referenceState.initialize(6);
00123         m_controlError.initialize(6);
00124         m_recentControl.initialize(3);
00125         
00126         /* Get inertia matrix from xml config file */
00127         Vector tempMOI(6);
00128         Matrix inertia = simulatorInertia( tempHandle, m_SimulatorName );
00129         tempMOI(1) = inertia(1,1);
00130         tempMOI(2) = inertia(1,2);
00131         tempMOI(3) = inertia(1,3);
00132         tempMOI(4) = inertia(2,2);
00133         tempMOI(5) = inertia(2,3);
00134         tempMOI(6) = inertia(3,3);
00135         
00136         m_recentParameter(_(1,6)) = tempMOI;
00137 
00138         /* Get center of mass location from xml config file */
00139         Vector tempCGV(3);
00140         // loop for each vector element 
00141         for(int iterator=0; iterator<3; iterator++)
00142         {
00143                 response = docHandle.FirstChild( "PHYSICAL_PROPERTIES" ).
00144                 FirstChild( "CENTER_MASS"  ).Child( "VECTOR", iterator ).
00145                 Element() -> QueryDoubleAttribute( "value", &tempCGV(VectorIndexBase + iterator) );
00146                 checkResponse(response);
00147          }
00148 
00149         /* Initial Quaternion */
00150         Vector tempQuat(4);
00151         tempQuat(1) = 0;
00152         tempQuat(2) = 0;
00153         tempQuat(3) = 0;
00154         tempQuat(4) = 1;
00155         
00156         string axis;
00157         switch( s_mapStringValues[ m_HardwareONOFF ] )
00158         {
00159                 case ON:
00160                         cerr << "Activating Hardware Components. " << endl;
00161 
00162                         for( int i =0; i < 3; i++ )
00163                         {
00164                                 // set up a string to designate axis
00165                                 if( i==0 ){axis = "X";};
00166                                 if( i==1 ){axis = "Y";};
00167                                 if( i==2 ){axis = "Z";};
00168                 
00169                                 /* --- Sensors --- */
00170 
00171                                 // create new sensor objects and set up maps
00172                                 /* Rate Gyros */
00173                                 m_rateGyros[string( "RATE_GYRO_" + axis )] = new DMURateGyro( docHandle, "RATE_GYRO_" + axis );
00174         
00175                                 /* Accelerometers */
00176                                 m_accelerometers[string( "ACC_" + axis )] = new DMUAccelerometer( docHandle, "ACC_" + axis );
00177                 
00178                                 /* --- Control Devices --- */
00179 
00180                                 /* Momentum Wheels */
00181                                 // Check for Momentum Wheel Activation
00182                                 string activation;
00183                                 activation = docHandle.FirstChild( "HARDWARE_PROPERTIES" ).
00184                                         FirstChild( "WHEELS"  ).Child( "REACTION_" + axis, 0 ).
00185                                         Element() -> Attribute( "selection" );
00186 
00187                                 m_WheelOptionString << activation << " " ;      
00188         
00189                                 switch( s_mapStringValues[ activation ] )
00190                                 {
00191                                         case ON:
00192                                                 m_momentumWheels[ string( "REACTION_" + axis ) ] 
00193                                                         = new MomentumWheel( docHandle, "REACTION_" + axis );
00194                                                 break;
00195                                         case OFF:
00196                                                 cerr << "REACTION_" + axis << " set to OFF. " << endl;
00197                                                 break;
00198                                         default:
00199                                                 break;
00200                                 }
00201                         }
00202 
00203                         /* Sensors Continued */
00204                                 /* Magnetometer */              
00205                                 m_magnetometer = new Magnetometer( docHandle );
00206 
00207                         break;
00208                 case OFF:
00209                         cerr << "Hardware Not Activated. " << endl;
00210                         break;
00211                 default:
00212                         cerr << "\nWARNING: Hardware ON/OFF specification error."  << endl;
00213                         exit( 0 );
00214         }
00215 
00216         /* Orbit */
00217         m_ECI.initialize(6);
00218 
00219         cerr << "\nInitialization of Whorl Object Complete" << endl;
00220         cerr << "--------------------------------------------------------------" << endl; 
00221         cerr << "--------------------------------------------------------------" << endl; 
00222 }
00223 
00224 /*! \brief Get the hardware flag that informs the user whether the hardware is activated. */
00225 string Whorl::GetHardwareFlag( )
00226 {
00227         return( m_HardwareONOFF );
00228 }
00229 
00230 /** @brief Get the recent Moment of Inertia matrix in body design axes 
00231  *
00232  * This function extracts the 6 moment of inertia elements from the
00233  * parameter Vector, then builds and returns that matrix.
00234  * This information is returned in whatever reference frame the data is 
00235  * initially provided in (in the config file).  Generally, this will
00236  * be the body-design reference frame. 
00237  */
00238 Matrix Whorl::GetMOI( ) 
00239 {
00240         Matrix temp(3,3);
00241         
00242         temp(1,1) = m_recentParameter(1);       temp(1,2) = m_recentParameter(2);       temp(1,3) = m_recentParameter(3);
00243         temp(2,1) = m_recentParameter(2);       temp(2,2) = m_recentParameter(4);       temp(2,3) = m_recentParameter(5);
00244         temp(3,1) = m_recentParameter(3);       temp(3,2) = m_recentParameter(5);       temp(3,3) = m_recentParameter(6);
00245 
00246         return temp;
00247 }               
00248         
00249 /** @brief Get the recent center of gravity vector, mg*r 
00250  *
00251  * This function extracts the 3-term center of gravity Vector from the 
00252  * parameter Vector and returns it.  
00253  * This information is returned in whatever reference frame the data is 
00254  * initially provided in (in the config file).  Generally, this will
00255  * be the body-design reference frame. 
00256  */
00257 Vector Whorl::GetCGVector( ) 
00258 {
00259         Vector temp(3);
00260         
00261         temp(1) = m_recentParameter(7);
00262         temp(2) = m_recentParameter(8);
00263         temp(3) = m_recentParameter(9);
00264         
00265         return temp;
00266 }       
00267         
00268 /** @brief Get the recent attitude estimate, qbl 
00269  *
00270  * This function pulls the quaternion out of the most recent state Vector.
00271  * This information is returned in whatever reference frame the data is 
00272  * initially provided in (in the config file).  Generally, this will
00273  * be the body-design reference frame. 
00274  */
00275 Vector Whorl::GetQuaternion( ) 
00276 {
00277         Vector temp(4);
00278         temp = m_recentState( _(1,4) );
00279         
00280         return temp;
00281 }
00282         
00283 /*! \brief Get the recent attitude estimate, MRP body 
00284  *
00285  * This function pulls the MRP vector out of the most recent state Vector by
00286  * calling the GetQuaternion function and converting to MRP vector.
00287  * This information is returned in whatever reference frame the data is 
00288  * initially provided in (in the config file).  Generally, this will
00289  * be the body-design reference frame. 
00290  */
00291 Vector Whorl::GetMRP( ) 
00292 {
00293        /* Convert from Quaternion to MRP attitude representation */
00294         Quaternion attitude( m_recentState( _(1,4) ) );
00295         ModifiedRodriguezParameters mrp( attitude );
00296         mrp.Switch( 1 ); // force switching if greater than 180 deg.
00297         
00298         return( mrp );
00299 }
00300         
00301 /** @brief Get the recent angular velocity estimate, \f$\omega\f$ 
00302  *
00303  * This function pulls the angular velocity out of the most recent state Vector.
00304  * This information is returned in whatever reference frame the data is 
00305  * initially provided in (in the config file).  Generally, this will
00306  * be the body-design reference frame. 
00307  */
00308 Vector Whorl::GetOmegaBL( ) 
00309 {
00310         Vector temp(3);
00311         temp = m_recentState( _(5,7) );
00312         return( temp );
00313 }
00314 
00315 /*! \brief Get Simulator Name */
00316 char *Whorl::GetSimulatorName( )
00317 {
00318         return( m_SimulatorName );
00319 }
00320 
00321 
00322 /** @brief Get the recent Reference attitude estimate, qbl 
00323  *
00324  * This function pulls the quaternion out of the most recent reference state Vector.
00325  * This information is returned in whatever reference frame the data is 
00326  * initially provided in (in the config file).  Generally, this will
00327  * be the body-design reference frame. 
00328  */
00329 Vector Whorl::GetReferenceQuaternion( ) 
00330 {
00331         Vector temp(4);
00332         temp = m_referenceState( _(1,4) );
00333         
00334         return temp;
00335 }
00336         
00337 /*! \brief Get the recent Reference attitude estimate, MRP body 
00338  *
00339  * This function pulls the MRP vector out of the most recent reference state Vector by
00340  * calling the GetQuaternion function and converting to MRP vector.
00341  * This information is returned in whatever reference frame the data is 
00342  * initially provided in (in the config file).  Generally, this will
00343  * be the body-design reference frame. 
00344  */
00345 Vector Whorl::GetReferenceMRP( ) 
00346 {
00347        /* Convert from Quaternion to MRP attitude representation */
00348         Quaternion refattitude( GetReferenceQuaternion( ) );
00349         ModifiedRodriguezParameters refmrp( refattitude );
00350         refmrp.Switch( 1 ); // force switching if greater than 180 deg.
00351         
00352         return( refmrp );
00353 }
00354         
00355 /** @brief Get the recent Reference angular velocity estimate, \f$\omega_r\f$ 
00356  *
00357  * This function pulls the angular velocity out of the most recent Reference state Vector.
00358  * This information is returned in whatever reference frame the data is 
00359  * initially provided in (in the config file).  Generally, this will
00360  * be the body-design reference frame. 
00361  */
00362 Vector Whorl::GetReferenceOmegaBL( ) 
00363 {
00364         Vector temp(3);
00365         temp = m_referenceState( _(5,7) );
00366                 
00367         return( temp );
00368 }
00369 
00370 Vector Whorl::GetControlError( )
00371 {
00372         return( m_controlError );
00373 }
00374 
00375 void Whorl::SetMRPError( Vector _mrpError )
00376 {
00377         m_controlError( _(1,3) ) = _mrpError;
00378 }
00379 
00380 void Whorl::SetAngularRateError( Vector _rateError )
00381 {
00382         m_controlError( _(4,6) ) = _rateError;
00383 }
00384 
00385 void Whorl::SetReferenceQuaternion( Vector _quaternion )
00386 {
00387         m_referenceState( _(1,4) ) = _quaternion;
00388 }
00389 
00390 void Whorl::SetReferenceOmegaBL( Vector _rate )
00391 {
00392         m_referenceState( _(5,7) ) = _rate;
00393 }
00394 
00395 void Whorl::SetECI( Vector _ECI )
00396 {
00397         m_ECI = _ECI;
00398 }
00399 
00400 void Whorl::SetCOE( Keplerian _COE )
00401 {
00402         m_COE = _COE;
00403 }
00404 
00405 void Whorl::SetOscCOE( Keplerian _OscCOE )
00406 {
00407         m_OscCOE = _OscCOE;
00408 }
00409 
00410 Vector Whorl::GetECI( )
00411 {
00412         return( m_ECI );
00413 }
00414 
00415 Keplerian Whorl::GetCOE( )
00416 {
00417         return( m_COE );
00418 }
00419 
00420 Keplerian Whorl::GetOscCOE( )
00421 {
00422         return( m_OscCOE );
00423 }
00424 
00425 void Whorl::SetOrbitControl( Vector _control )
00426 {
00427         m_OrbitControl = _control;
00428 }
00429 
00430 Vector Whorl::GetOrbitControl( )
00431 {
00432         return( m_OrbitControl );
00433 }
00434 
00435 // Do not change the comments below - they will be added automatically by CVS
00436 /*****************************************************************************
00437 *       $Log: Whorl.cpp,v $
00438 *       Revision 1.21  2007/08/31 16:00:19  jayhawk_hokie
00439 *       Added Orbit Control Capability.
00440 *       
00441 *       Revision 1.20  2007/07/24 09:40:35  jayhawk_hokie
00442 *       Removed matrix and vector pointers.
00443 *       
00444 *       Revision 1.19  2007/05/29 19:14:24  jayhawk_hokie
00445 *       Added Constructor Comments.
00446 *       
00447 *       Revision 1.18  2007/04/09 14:00:10  jayhawk_hokie
00448 *       Modifications to parsing XML config file.
00449 *       
00450 *       Revision 1.17  2007/03/27 19:10:02  jayhawk_hokie
00451 *       Added Switch condition for turning on and off devices.
00452 *       
00453 *       Revision 1.16  2007/01/19 21:31:32  jayhawk_hokie
00454 *       *** empty log message ***
00455 *       
00456 *       Revision 1.15  2007/01/12 20:30:08  bwilliam
00457 *       Modified to use XML parser and updated hardware maps.
00458 *       
00459 *       Revision 1.14  2005/04/26 17:20:37  cakinli
00460 *       Updated Makefiles to build a single libdsacss.a in the src directory to alleviate linking issues with the multiple, per-directory libraries.
00461 *       
00462 *       Revision 1.13  2004/07/29 18:52:43  sawrigh3
00463 *       Removed Tilt Sensor instantiation
00464 *       
00465 *       Revision 1.12  2004/04/28 19:42:56  bstreetman
00466 *       Added Tilt Sensor
00467 *       
00468 *       Revision 1.11  2003/08/19 17:41:45  simpliciter
00469 *       Fixed bug in state parsing (omega).
00470 *       
00471 *       Revision 1.10  2003/08/18 19:03:15  mavandyk
00472 *       Added initial condition and parameter parsing and pulled out controller and observer stuff.
00473 *       
00474 *       Revision 1.9  2003/08/14 18:42:59  nicmcp
00475 *       Added config parsing for parameters and states, needs to be debugged
00476 *       
00477 *       Revision 1.8  2003/08/13 23:15:32  mavandyk
00478 *       Altered structure as outline in meeting.
00479 *       
00480 *       Revision 1.7  2003/08/06 21:44:21  mavandyk
00481 *       Altered to get config parsing to work.
00482 *       
00483 *       Revision 1.6  2003/07/25 14:25:20  mavandyk
00484 *       Fixed bugs, It compiles.
00485 *       
00486 *       Revision 1.5  2003/07/22 21:26:27  mavandyk
00487 *       Fixed bugs.
00488 *       
00489 *       Revision 1.4  2003/07/08 20:45:10  cakinli
00490 *       Updated Whorl::Initialize() to use new cfgBody struct and CfgParse class,
00491 *       and incorporated device type names, indices, and index member in cfgBody struct.
00492 *       
00493 *       Revision 1.3  2003/07/07 14:37:58  simpliciter
00494 *       Minor modifications.  Measurement stuff needs work.
00495 *       
00496 *       Revision 1.2  2003/07/04 16:12:18  simpliciter
00497 *       Fixed variable and function names.
00498 *       
00499 *       Revision 1.1  2003/07/04 14:08:05  simpliciter
00500 *       Rearranged directory tree.
00501 *       
00502 *       Revision 1.4  2003/07/03 21:18:12  simpliciter
00503 *       Added functions based on requirements of Observer and Controller classes.
00504 *       
00505 *       Revision 1.3  2003/07/02 16:36:32  nicmcp
00506 *       added some comments
00507 *       
00508 *       Revision 1.2  2003/06/17 23:32:28  simpliciter
00509 *       Updated based on comments from Andy.
00510 *       
00511 *       Revision 1.1.1.1  2003/06/06 18:44:15  simpliciter
00512 *       Initial submission.
00513 *       
00514 *       Revision 1.1.1.1  2003/06/06 14:44:15  simpliciter
00515 *       Initial import.
00516 *       
00517 *
00518 ******************************************************************************/

Generated on Wed Sep 5 12:54:27 2007 for DSACSS Operational Code by  doxygen 1.3.9.1