//------------------------------------------------------------------------------ std::string SpiceAttitude::GetStringParameter(const Integer id, const Integer index) const { if (id == ATTITUDE_KERNEL_NAME) { if ((index < 0) || (index >= (Integer) ck.size())) { std::string errmsg = "Error attempting to retrieve CK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } return ck.at(index); } if (id == SC_CLOCK_KERNEL_NAME) { if ((index < 0) || (index >= (Integer) sclk.size())) { std::string errmsg = "Error attempting to retrieve SCLK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } return sclk.at(index); } if (id == FRAME_KERNEL_NAME) { if ((index < 0) || (index >= (Integer) fk.size())) { std::string errmsg = "Error attempting to retrieve FK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } return fk.at(index); } return Attitude::GetStringParameter(id, index); }
//------------------------------------------------------------------------------ bool SpiceAttitude::SetStringParameter(const Integer id, const std::string &value, const Integer index) { #ifdef DEBUG_SPICE_ATTITUDE_GET_SET MessageInterface::ShowMessage( "Entering SetStringParameter with id = %d, value = \"%s\", index = %d\n", id, value.c_str(), index); #endif if (id == ATTITUDE_KERNEL_NAME) { if ((index < 0) || (index > (Integer) ck.size())) { std::string errmsg = "Error attempting to set CK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } if (index == (Integer) ck.size()) ck.push_back(value); else ck.at(index) = value; return true; } if (id == SC_CLOCK_KERNEL_NAME) { if ((index < 0) || (index > (Integer) sclk.size())) { std::string errmsg = "Error attempting to set SCLK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } if (index == (Integer) sclk.size()) sclk.push_back(value); else sclk.at(index) = value; return true; } if (id == FRAME_KERNEL_NAME) { if ((index < 0) || (index > (Integer) fk.size())) { std::string errmsg = "Error attempting to set FK kernel name for object "; errmsg += scName + " - index out-of-bounds.\n"; throw AttitudeException(errmsg); } if (index == (Integer) fk.size()) fk.push_back(value); else fk.at(index) = value; return true; } return Attitude::SetStringParameter(id, value, index); }
//------------------------------------------------------------------------------ void SpiceAttitude::ComputeCosineMatrixAndAngularVelocity(Real atTime) { if (!isInitialized || needsReinit) Initialize(); #ifdef __USE_SPICE__ reader->GetTargetOrientation(scName, naifId, refFrameNaifId, atTime, dcm, angVel); #else std::string errmsg = "Error - attempting to use SpiceAttitude when "; errmsg += "SPICE is not included in the GMAT build.\n"; throw AttitudeException(errmsg); #endif }
//--------------------------------------------------------------------------- bool CCSDSAttitude::Initialize() { #ifdef DEBUG_CCSDS_ATTITUDE MessageInterface::ShowMessage("Entering CCSDSAttitude::Initialize\n"); #endif bool isOK = Attitude::Initialize(); if (!isOK) return false; // Changed to use aemFileFullPath (LOJ: 2014.06.26) //if (aemFile == "") if (aemFileFullPath == "") { std::string errmsg = "Error - AEM file name not set on CCSDS-AEM object.\n"; throw AttitudeException(errmsg); } //reader->SetFile(aemFile); reader->SetFile(aemFileFullPath); reader->Initialize(); return true; }
void NadirPointing::ComputeCosineMatrixAndAngularVelocity(Real atTime) { //#ifdef DEBUG_NADIRPOINTING //#endif #ifdef DEBUG_NADIR_POINTING_COMPUTE MessageInterface::ShowMessage("In NadirPointing, computing at time %le\n", atTime); MessageInterface::ShowMessage("ModeOfConstraint = %s\n", modeOfConstraint.c_str()); if (!owningSC) MessageInterface::ShowMessage("--- owningSC is NULL!!!!\n"); if (!refBody) MessageInterface::ShowMessage("--- refBody is NULL!!!!\n"); #endif if (!isInitialized || needsReinit) { Initialize(); // Check for unset reference body pointer if (!refBody) { std::string attEx = "Reference body "; attEx += refBodyName + " not defined for attitude of type \""; attEx += "NadirPointing\""; throw AttitudeException(attEx); } } /// using a spacecraft object, *owningSC A1Mjd theTime(atTime); Rvector6 scState = ((SpaceObject*) owningSC)->GetMJ2000State(theTime); Rvector6 refState = refBody->GetMJ2000State(theTime); Rvector3 pos(scState[0] - refState[0], scState[1] - refState[1], scState[2] - refState[2]); Rvector3 vel(scState[3] - refState[3], scState[4] - refState[4], scState[5] - refState[5]); #ifdef DEBUG_NADIR_POINTING_COMPUTE MessageInterface::ShowMessage(" scState = %s\n", scState.ToString().c_str()); MessageInterface::ShowMessage(" refState = %s\n", refState.ToString().c_str()); MessageInterface::ShowMessage(" pos = %s\n", pos.ToString().c_str()); MessageInterface::ShowMessage(" vel = %s\n", vel.ToString().c_str()); #endif // Error message if ( bodyAlignmentVector.GetMagnitude() < 1.0e-5 ) { throw AttitudeException("The size of BodyAlignmentVector is too small."); } else if ( bodyConstraintVector.GetMagnitude() < 1.0e-5 ) { throw AttitudeException("The size of BodyConstraintVector is too small."); } else if ( bodyAlignmentVector.GetUnitVector()*bodyConstraintVector.GetUnitVector() > ( 1.0 - 1.0e-5) ) { throw AttitudeException("BodyAlignmentVector and BodyConstraintVector are the same."); } else if ( pos.GetMagnitude() < 1.0e-5 ) { throw AttitudeException("A position vector is zero."); } else if ( vel.GetMagnitude() < 1.0e-5 ) { throw AttitudeException("A velocity vector is zero."); } else if ( Cross(pos,vel).GetMagnitude() < 1.0e-5 ) { throw AttitudeException("Cross product of position and velocity is zero: LVLH frame cannot be defined."); } Rvector3 normal = Cross(pos,vel); Rvector3 xhat = pos/pos.GetMagnitude(); Rvector3 yhat = normal/normal.GetMagnitude(); Rvector3 zhat = Cross(xhat,yhat); #ifdef DEBUG_NADIR_POINTING_COMPUTE MessageInterface::ShowMessage(" normal = %s\n", normal.ToString().c_str()); MessageInterface::ShowMessage(" xhat = %s\n", xhat.ToString().c_str()); MessageInterface::ShowMessage(" yhat = %s\n", yhat.ToString().c_str()); MessageInterface::ShowMessage(" zhat = %s\n", zhat.ToString().c_str()); #endif // RiI calculation (from inertial to LVLH) Rmatrix33 RIi; RIi(0,0) = xhat(0); RIi(1,0) = xhat(1); RIi(2,0) = xhat(2); RIi(0,1) = yhat(0); RIi(1,1) = yhat(1); RIi(2,1) = yhat(2); RIi(0,2) = zhat(0); RIi(1,2) = zhat(1); RIi(2,2) = zhat(2); // Set alignment/constraint vector in body frame if ( modeOfConstraint == "OrbitNormal" ) { referenceVector[0] = -1; referenceVector[1] = 0; referenceVector[2] = 0; constraintVector[0] = 0; constraintVector[1] = 1; constraintVector[2] = 0; } else if ( modeOfConstraint == "VelocityConstraint" ) { referenceVector[0] = -1; referenceVector[1] = 0; referenceVector[2] = 0; constraintVector[0] = 0; constraintVector[1] = 0; constraintVector[2] = 1; } // RBi calculation using TRIAD (from LVLH to body frame) Rmatrix33 RiB = TRIAD( bodyAlignmentVector, bodyConstraintVector, referenceVector, constraintVector ); // the rotation matrix (from body to inertial) dcm = RIi*RiB; // the final rotation matrix (from inertial to body) dcm = dcm.Transpose(); // We do NOT calculate angular velocity for Nadir. // TODO : Throw AttitudeException?? /* Rmatrix33 wxIBB = - RBi*RiIDot*dcm.Transpose(); Rvector3 wIBB; wIBB.Set(wxIBB(2,1),wxIBB(0,2),wxIBB(1,0)); */ }
//--------------------------------------------------------------------------- bool SpiceAttitude::Initialize() { #ifdef DEBUG_SPICE_ATTITUDE MessageInterface::ShowMessage("Entering SpiceAttitude::Initialize\n"); #endif bool isOK = Attitude::Initialize(); if (!isOK) return false; if (scName == "") { std::string errmsg = "Error - object name not set on SpiceAttitude object.\n"; throw AttitudeException(errmsg); } if (ck.empty()) { std::string errmsg = "Error - no CK pointing kernel(s) set on SpiceAttitude for object "; errmsg += scName + "\n"; throw AttitudeException(errmsg); } if (sclk.empty()) { std::string errmsg = "Error - no SCLK clock kernel(s) set on SpiceAttitude for object "; errmsg += scName + "\n"; throw AttitudeException(errmsg); } if (fk.empty()) { std::string warnmsg = "Warning - no FK frame kernel(s) set on SpiceAttitude for object "; warnmsg += scName + ". A Frame Kernel may be necessary.\n"; MessageInterface::ShowMessage(warnmsg.c_str()); } #ifdef __USE_SPICE__ // Load the CK kernel(s) for (StringArray::iterator j = ck.begin(); j != ck.end(); ++j) { reader->LoadKernel(*j); } // Load the SCLK kernel(s) for (StringArray::iterator j = sclk.begin(); j != sclk.end(); ++j) { reader->LoadKernel(*j); } // Load the FK kernel(s), if any for (StringArray::iterator j = fk.begin(); j != fk.end(); ++j) { reader->LoadKernel(*j); } #endif if (naifId == UNDEFINED_NAIF_ID) { #ifdef __USE_SPICE__ naifId = reader->GetNaifID(scName); #ifdef DEBUG_SPICE_ATTITUDE MessageInterface::ShowMessage( "Retrieved NAIF ID for %s -> %d\n", scName.c_str(), naifId); #endif if (naifId == 0) { std::string errmsg = "Error - NAIF ID not available for object \n"; errmsg += scName + "\n"; throw AttitudeException(errmsg); } #else std::string errmsg = "Error - NAIF ID not set on SpiceAttitude object.\n"; throw AttitudeException(errmsg); #endif } if (refFrameNaifId == UNDEFINED_NAIF_ID_REF_FRAME) { std::string errmsg = "Error - NAIF ID for object reference frame not set on SpiceAttitude object.\n"; throw AttitudeException(errmsg); } return true; }