00001
00002
00003
00004
00005
00006
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
00030 if( i==0 ){axis = "X";};
00031 if( i==1 ){axis = "Y";};
00032 if( i==2 ){axis = "Z";};
00033
00034
00035 delete m_rateGyros[string( "RATE_GYRO_" + axis )];
00036 cerr << "Delete RATE_GYRO_" + axis << " Object" << endl;
00037
00038
00039 delete m_accelerometers[string( "ACC_" + axis )];
00040 cerr << "Delete ACC_" + axis << " Object" << endl;
00041
00042
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
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
00077
00078
00079 void Whorl::Initialize( TiXmlHandle tempHandle )
00080 {
00081 cerr << "\n--------------------------------------------------------------" << endl;
00082 cerr << "Creating Whorl Object" << endl;
00083 cerr << "--------------------------------------------------------------" << endl;
00084
00085
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
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
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
00139 Vector tempCGV(3);
00140
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
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
00165 if( i==0 ){axis = "X";};
00166 if( i==1 ){axis = "Y";};
00167 if( i==2 ){axis = "Z";};
00168
00169
00170
00171
00172
00173 m_rateGyros[string( "RATE_GYRO_" + axis )] = new DMURateGyro( docHandle, "RATE_GYRO_" + axis );
00174
00175
00176 m_accelerometers[string( "ACC_" + axis )] = new DMUAccelerometer( docHandle, "ACC_" + axis );
00177
00178
00179
00180
00181
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
00204
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
00217 m_ECI.initialize(6);
00218
00219 cerr << "\nInitialization of Whorl Object Complete" << endl;
00220 cerr << "--------------------------------------------------------------" << endl;
00221 cerr << "--------------------------------------------------------------" << endl;
00222 }
00223
00224
00225 string Whorl::GetHardwareFlag( )
00226 {
00227 return( m_HardwareONOFF );
00228 }
00229
00230
00231
00232
00233
00234
00235
00236
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
00250
00251
00252
00253
00254
00255
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
00269
00270
00271
00272
00273
00274
00275 Vector Whorl::GetQuaternion( )
00276 {
00277 Vector temp(4);
00278 temp = m_recentState( _(1,4) );
00279
00280 return temp;
00281 }
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291 Vector Whorl::GetMRP( )
00292 {
00293
00294 Quaternion attitude( m_recentState( _(1,4) ) );
00295 ModifiedRodriguezParameters mrp( attitude );
00296 mrp.Switch( 1 );
00297
00298 return( mrp );
00299 }
00300
00301
00302
00303
00304
00305
00306
00307
00308 Vector Whorl::GetOmegaBL( )
00309 {
00310 Vector temp(3);
00311 temp = m_recentState( _(5,7) );
00312 return( temp );
00313 }
00314
00315
00316 char *Whorl::GetSimulatorName( )
00317 {
00318 return( m_SimulatorName );
00319 }
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329 Vector Whorl::GetReferenceQuaternion( )
00330 {
00331 Vector temp(4);
00332 temp = m_referenceState( _(1,4) );
00333
00334 return temp;
00335 }
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345 Vector Whorl::GetReferenceMRP( )
00346 {
00347
00348 Quaternion refattitude( GetReferenceQuaternion( ) );
00349 ModifiedRodriguezParameters refmrp( refattitude );
00350 refmrp.Switch( 1 );
00351
00352 return( refmrp );
00353 }
00354
00355
00356
00357
00358
00359
00360
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
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518