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 TDRSSTwoWayRange::Evaluate(bool withEvents)
{
bool retval = false;
if (!initialized)
InitializeMeasurement();
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Entered TDRSSTwoWayRange::Evaluate()\n");
MessageInterface::ShowMessage(" ParticipantCount: %d\n",
participants.size());
#endif
if (withEvents == false)
{
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation without "
"events\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
"Evaluating TDRSS 2-Way Range without events");
#endif
if (CheckLOS(0, 1, NULL) && CheckLOS(1, 2, NULL))
{
// Calculate the range vector between the groundstation and the TDRS
CalculateRangeVectorInertial(0, 1);
Rvector3 outState;
// Set feasibility off of topospheric horizon, set by the Z value in topo
// coords
std::string updateAll = "All";
UpdateRotationMatrix(currentMeasurement.epoch, updateAll);
outState = R_o_j2k * rangeVecInertial;
currentMeasurement.feasibilityValue = outState[2];
#ifdef CHECK_PARTICIPANT_LOCATIONS
MessageInterface::ShowMessage("Evaluating without events\n");
MessageInterface::ShowMessage("Calculating TDRSS 2-Way Range at epoch "
"%.12lf\n", currentMeasurement.epoch);
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
Rvector3 bfLoc = R_o_j2k * p1Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s",
participants[0]->GetName().c_str(),
bfLoc.ToString().c_str());
bfLoc = R_o_j2k * p2Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s\n",
participants[1]->GetName().c_str(),
bfLoc.ToString().c_str());
#endif
if (currentMeasurement.feasibilityValue > 0.0)
{
currentMeasurement.isFeasible = true;
currentMeasurement.value[0] = rangeVecInertial.GetMagnitude();
currentMeasurement.eventCount = 4;
retval = true;
}
}
else
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0.0;
currentMeasurement.eventCount = 0;
}
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Calculating Range at epoch %.12lf\n",
currentMeasurement.epoch);
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
MessageInterface::ShowMessage(" Range Vector (inertial): %s\n",
rangeVecInertial.ToString().c_str());
MessageInterface::ShowMessage(" R(Groundstation) dot RangeVec = %lf\n",
currentMeasurement.feasibilityValue);
MessageInterface::ShowMessage(" Feasibility: %s\n",
(currentMeasurement.isFeasible ? "true" : "false"));
MessageInterface::ShowMessage(" Range is %.12lf\n",
currentMeasurement.value[0]);
MessageInterface::ShowMessage(" EventCount is %d\n",
currentMeasurement.eventCount);
#endif
#ifdef SHOW_RANGE_CALC
MessageInterface::ShowMessage("Range at epoch %.12lf is ",
currentMeasurement.epoch);
if (currentMeasurement.isFeasible)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("********************************************\n"
"Evaluating TDRSS 2-Way Range with located events");
MessageInterface::ShowMessage("Calculating TDRSS 2-Way Range at epoch "
"%.12lf\n", currentMeasurement.epoch);
#endif
// Get the range from the downlink
Rvector3 r1, r2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
Rvector3 rVector = r2 - r1;
Real realRange = rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at downlink: %.12lf %.12lf %.12lf\n",
participants[0]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at downlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Downlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
r1 = backlinkLeg.GetPosition(participants[1]);
r2 = backlinkLeg.GetPosition(participants[2]);
if (CheckSat2SatLOS(r1, r2, NULL) == false)
return false;
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at backlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at backlink: %.12lf %.12lf %.12lf\n",
participants[2]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Backlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
// Add the pseudorange from the transponder delay
targetDelay = GetDelay(2,0);
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" TDRSS target delay is %.12lf\n",
targetDelay);
#endif
realRange += GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M *
targetDelay;
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Delay Range (%le sec delay): "
"%.12lf\n", targetDelay,
(GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/GmatMathConstants::KM_TO_M * targetDelay));
#endif
r1 = forwardlinkLeg.GetPosition(participants[2]);
r2 = forwardlinkLeg.GetPosition(participants[1]);
if (CheckSat2SatLOS(r1, r2, NULL) == false)
return false;
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[2]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" forwardlink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
r1 = uplinkLeg.GetPosition(participants[1]);
r2 = uplinkLeg.GetPosition(participants[0]);
rVector = r2 - r1;
realRange += rVector.GetMagnitude();
#ifdef VIEW_PARTICIPANT_STATES
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
MessageInterface::ShowMessage(" %s at frwdlink: %.12lf %.12lf %.12lf\n",
participants[0]->GetName().c_str(), r2[0], r2[1], r2[2]);
#endif
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Uplink Range: %.12lf\n",
rVector.GetMagnitude());
#endif
currentMeasurement.value[0] = realRange / 2.0;
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage(" Calculated Range: %.12lf\n",
currentMeasurement.value[0]);
#endif
retval = true;
}
return retval;
}
//------------------------------------------------------------------------------
bool USNTwoWayRange::Evaluate(bool withEvents)
{
bool retval = false;
if (!initialized)
InitializeMeasurement();
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Entered USNTwoWayRange::Evaluate()\n");
MessageInterface::ShowMessage(" ParticipantCount: %d\n",
participants.size());
#endif
if (withEvents == false)
{
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("USN 2-Way Range Calculation without "
"events\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
"Evaluating USN 2-Way Range without events");
#endif
CalculateRangeVectorInertial();
Rvector3 outState;
// Set feasibility off of topocentric horizon, set by the Z value in topo
// coords
std::string updateAll = "All";
UpdateRotationMatrix(currentMeasurement.epoch, updateAll);
outState = R_o_j2k * rangeVecInertial;
currentMeasurement.feasibilityValue = outState[2];
#ifdef CHECK_PARTICIPANT_LOCATIONS
MessageInterface::ShowMessage("Evaluating without events\n");
MessageInterface::ShowMessage("Calculating USN 2-Way Range at epoch "
"%.12lf\n", currentMeasurement.epoch);
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
Rvector3 bfLoc = R_o_j2k * p1Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s",
participants[0]->GetName().c_str(),
bfLoc.ToString().c_str());
bfLoc = R_o_j2k * p2Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s\n",
participants[1]->GetName().c_str(),
bfLoc.ToString().c_str());
#endif
if (currentMeasurement.feasibilityValue > 0.0)
{
currentMeasurement.isFeasible = true;
currentMeasurement.value[0] = rangeVecInertial.GetMagnitude();
currentMeasurement.eventCount = 2;
retval = true;
}
else
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0.0;
currentMeasurement.eventCount = 0;
}
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Calculating Range at epoch %.12lf\n",
currentMeasurement.epoch);
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
MessageInterface::ShowMessage(" Range Vector: %s\n",
rangeVecInertial.ToString().c_str());
MessageInterface::ShowMessage(" R(Groundstation) dot RangeVec = %lf\n",
currentMeasurement.feasibilityValue);
MessageInterface::ShowMessage(" Feasibility: %s\n",
(currentMeasurement.isFeasible ? "true" : "false"));
MessageInterface::ShowMessage(" Range is %.12lf\n",
currentMeasurement.value[0]);
MessageInterface::ShowMessage(" EventCount is %d\n",
currentMeasurement.eventCount);
#endif
#ifdef SHOW_RANGE_CALC
MessageInterface::ShowMessage("Range at epoch %.12lf is ",
currentMeasurement.epoch);
if (currentMeasurement.isFeasible)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("USN 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES_WITH_EVENTS
DumpParticipantStates("********************************************\n"
"Evaluating USN 2-Way Range with located events");
#endif
// 1. Get the range from the down link
Rvector3 r1, r2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("r1 = (%f, %f, %f)\n", r1.Get(0), r1.Get(1), r1.Get(2));
MessageInterface::ShowMessage("r2 = (%f, %f, %f)\n", r2.Get(0), r2.Get(1), r2.Get(2));
#endif
Rvector3 downlinkVector = r2 - r1; // rVector = r2 - r1;
downlinkRange = downlinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range = r2-r1: %.12lf km\n",
downlinkRange);
#endif
// 2. Calculate down link range rate:
Rvector3 p1V = downlinkLeg.GetVelocity(participants[0]);
Rvector3 p2V = downlinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("p1V = (%f, %f, %f)\n", p1V.Get(0), p1V.Get(1), p1V.Get(2));
MessageInterface::ShowMessage("p2V = (%f, %f, %f)\n", p2V.Get(0), p2V.Get(1), p2V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector downRRateVec = p2V - p1V /* - r12_j2k_vel*/;
Rvector3 rangeUnit = downlinkVector.GetUnitVector();
downlinkRangeRate = downRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range Rate: %.12lf km/s\n",
downlinkRangeRate);
#endif
// 3. Get the transponder delay
targetDelay = GetDelay(1,0);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(
" USN Transponder delay for %s = %.12lf s\n",
participants[1]->GetName().c_str(), targetDelay);
#endif
// 4. Get the range from the uplink
Rvector3 r3, r4;
r3 = uplinkLeg.GetPosition(participants[0]);
r4 = uplinkLeg.GetPosition(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("r3 = (%f, %f, %f)\n", r3.Get(0), r3.Get(1), r3.Get(2));
MessageInterface::ShowMessage("r4 = (%f, %f, %f)\n", r4.Get(0), r4.Get(1), r4.Get(2));
#endif
Rvector3 uplinkVector = r4 - r3;
uplinkRange = uplinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range = r4-r3: %.12lf km\n",
uplinkRange);
#endif
// 5. Calculate up link range rate
Rvector3 p3V = uplinkLeg.GetVelocity(participants[0]);
Rvector3 p4V = uplinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("p3V = (%f, %f, %f)\n", p3V.Get(0), p3V.Get(1), p3V.Get(2));
MessageInterface::ShowMessage("p4V = (%f, %f, %f)\n", p4V.Get(0), p4V.Get(1), p4V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector upRRateVec = p4V - p3V /* - r12_j2k_vel*/ ;
rangeUnit = uplinkVector.GetUnitVector();
uplinkRangeRate = upRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range Rate: %.12lf km/s\n",
uplinkRangeRate);
#endif
// 5.1. Target range rate: Do we need this as well?
targetRangeRate = (downlinkRangeRate + uplinkRangeRate) / 2.0;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Target Range Rate: %.12lf km/s\n",
targetRangeRate);
#endif
// 6. Get sensors used in USN 2-ways range
ObjectArray objList1;
ObjectArray objList2;
ObjectArray objList3;
// objList1 := all transmitters in participantHardware list
// objList2 := all receivers in participantHardware list
// objList3 := all transponders in participantHardware list
if (participantHardware.empty()||
((!participantHardware.empty())&&
participantHardware[0].empty()&&
participantHardware[1].empty()
)
)
{
// DO NOT LEAVE THIS TYPE OF MESSAGE IN THE CODE WITHOUT #ifdef WRAPPERS!!!
//MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
Real realRange = (uplinkRange + downlinkRange)/2;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Range = %.12lf km\n", realRange);
#endif
// Set value for currentMeasurement
currentMeasurement.value[0] = realRange;
currentMeasurement.isFeasible = true;
return true;
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[0].begin();
hw != this->participantHardware[0].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transmitter")
objList1.push_back(*hw);
if ((*hw)->GetTypeName() == "Receiver")
objList2.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[1].begin();
hw != this->participantHardware[1].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transponder")
objList3.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
if (objList1.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 transmitter to send signal.\n");
throw new MeasurementException("The first participant does not have only 1 transmitter to send signal.\n");
}
if (objList2.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 receiver to receive signal.\n");
throw new MeasurementException("The first participant does not have only 1 receiver to receive signal.\n");
}
if (objList3.size() != 1)
{
MessageInterface::ShowMessage("The second participant does not have only 1 transponder to transpond signal.\n");
throw new MeasurementException("The second participant does not have only 1 transponder to transpond signal.\n");
}
Transmitter* gsTransmitter = (Transmitter*)objList1[0];
Receiver* gsReceiver = (Receiver*)objList2[0];
Transponder* scTransponder = (Transponder*)objList3[0];
if (gsTransmitter == NULL)
{
MessageInterface::ShowMessage("Transmitter is NULL object.\n");
throw new GmatBaseException("Transmitter is NULL object.\n");
}
if (gsReceiver == NULL)
{
MessageInterface::ShowMessage("Receiver is NULL object.\n");
throw new GmatBaseException("Receiver is NULL object.\n");
}
if (scTransponder == NULL)
{
MessageInterface::ShowMessage("Transponder is NULL object.\n");
throw new GmatBaseException("Transponder is NULL object.\n");
}
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" List of sensors: %s, %s, %s\n",
gsTransmitter->GetName().c_str(), gsReceiver->GetName().c_str(),
scTransponder->GetName().c_str());
#endif
// 7. Get frequency from transmitter of ground station (participants[0])
Signal* uplinkSignal = gsTransmitter->GetSignal();
Real uplinkFreq = uplinkSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" UpLink signal frequency = %.12lf MHz\n", uplinkFreq);
#endif
// 8. Calculate media correction for uplink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Media correction for uplink leg\n");
#endif
Real roundTripTime = ((uplinkRange + downlinkRange)*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)/GmatTimeConstants::SECS_PER_DAY;
// DO NOT LEAVE THIS TYPE OF MESSAGE IN THE CODE WITHOUT #ifdef WRAPPERS!!!
//MessageInterface::ShowMessage("Round trip time = %.12lf\n", roundTripTime);
RealArray uplinkCorrection = CalculateMediaCorrection(uplinkFreq, r1, r2, currentMeasurement.epoch - roundTripTime);
Real uplinkRangeCorrection = uplinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real uplinkRealRange = uplinkRange + uplinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink range correction = %.12lf km\n",uplinkRangeCorrection);
MessageInterface::ShowMessage(" Uplink real range = %.12lf km\n",uplinkRealRange);
#endif
// 9. Doppler shift the frequency from the transmitter using uplinkRangeRate:
Real uplinkDSFreq = (1 - uplinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*uplinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Doppler shift frequency = %.12lf MHz\n", uplinkDSFreq);
#endif
// 10.Set frequency for the input signal of transponder
Signal* inputSignal = scTransponder->GetSignal(0);
inputSignal->SetValue(uplinkDSFreq);
scTransponder->SetSignal(inputSignal, 0);
// 11. Check the transponder feasibility to receive the input signal:
if (scTransponder->IsFeasible(0) == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The transponder is unfeasible to receive uplink signal.\n");
throw new GmatBaseException("The transponder is unfeasible to receive uplink signal.\n");
}
// 12. Get frequency of transponder output signal
Signal* outputSignal = scTransponder->GetSignal(1);
Real downlinkFreq = outputSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink frequency = %.12lf Mhz\n", downlinkFreq);
#endif
// 13. Doppler shift the transponder output frequency by the downlinkRangeRate:
Real downlinkDSFreq = (1 - downlinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*downlinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Doppler shift frequency = %.12lf MHz\n", downlinkDSFreq);
#endif
// 14. Set frequency on receiver
Signal* downlinkSignal = gsReceiver->GetSignal();
downlinkSignal->SetValue(downlinkDSFreq);
// 15. Check the receiver feasibility to receive the downlink signal
if (gsReceiver->IsFeasible() == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
throw new MeasurementException("The receiver is unfeasible to receive downlink signal.\n");
}
// 16. Calculate media correction for downlink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Media correction for downlink leg\n");
#endif
RealArray downlinkCorrection = CalculateMediaCorrection(downlinkDSFreq, r3, r4, currentMeasurement.epoch);
Real downlinkRangeCorrection = downlinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real downlinkRealRange = downlinkRange + downlinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink range correction = %.12lf km\n",downlinkRangeCorrection);
MessageInterface::ShowMessage(" Downlink real range = %.12lf km\n",downlinkRealRange);
#endif
// 17. Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// 18. Calculate real range
Real realRange = uplinkRealRange + downlinkRealRange +
targetDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Calculated real range = %.12lf km\n", realRange/2);
#endif
// 19. Set value for currentMeasurement
currentMeasurement.value[0] = realRange / 2.0;
currentMeasurement.isFeasible = true;
#ifdef PRELIMINARY_DERIVATIVE_CHECK
MessageInterface::ShowMessage("Participants:\n ");
for (UnsignedInt i = 0; i < participants.size(); ++i)
MessageInterface::ShowMessage(" %d: %s of type %s\n", i,
participants[i]->GetName().c_str(),
participants[i]->GetTypeName().c_str());
Integer id = participants[1]->GetType() * 250 +
participants[1]->GetParameterID("CartesianX");
CalculateMeasurementDerivatives(participants[1], id);
#endif
retval = true;
}
return retval;
}
//------------------------------------------------------------------------------
bool DSNTwoWayRange::Evaluate(bool withEvents)
{
bool retval = false;
if (!initialized)
InitializeMeasurement();
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Entered DSNTwoWayRange::Evaluate(%s)\n",
(withEvents ? "true" : "false"));
MessageInterface::ShowMessage(" ParticipantCount: %d\n",
participants.size());
#endif
if (withEvents == false)
{
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("DSN 2-Way Range Calculation without "
"events\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES
DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
"Evaluating DSN 2-Way Range without events");
#endif
CalculateRangeVectorInertial();
Rvector3 outState;
// Set feasibility off of topocentric horizon, set by the Z value in topo
// coords
std::string updateAll = "All";
UpdateRotationMatrix(currentMeasurement.epoch, updateAll);
outState = R_o_j2k * rangeVecInertial;
currentMeasurement.feasibilityValue = outState[2];
#ifdef CHECK_PARTICIPANT_LOCATIONS
MessageInterface::ShowMessage("Evaluating without events\n");
MessageInterface::ShowMessage("Calculating DSN 2-Way Range at epoch "
"%.12lf\n", currentMeasurement.epoch);
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" J2K Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
Rvector3 bfLoc = R_o_j2k * p1Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s",
participants[0]->GetName().c_str(),
bfLoc.ToString().c_str());
bfLoc = R_o_j2k * p2Loc;
MessageInterface::ShowMessage(" BodyFixed Location of %s: %s\n",
participants[1]->GetName().c_str(),
bfLoc.ToString().c_str());
#endif
if (currentMeasurement.feasibilityValue > 0.0)
{
currentMeasurement.isFeasible = true;
currentMeasurement.value[0] = rangeVecInertial.GetMagnitude();
currentMeasurement.eventCount = 2;
SetHardwareDelays(false);
retval = true;
}
else
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0.0;
currentMeasurement.eventCount = 0;
}
#ifdef DEBUG_RANGE_CALC
MessageInterface::ShowMessage("Calculating Range at epoch %.12lf\n",
currentMeasurement.epoch);
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[0]->GetName().c_str(),
currentMeasurement.participantIDs[0].c_str(),
p1Loc.ToString().c_str());
MessageInterface::ShowMessage(" Location of %s, id = '%s': %s",
participants[1]->GetName().c_str(),
currentMeasurement.participantIDs[1].c_str(),
p2Loc.ToString().c_str());
MessageInterface::ShowMessage(" Range Vector: %s\n",
rangeVecInertial.ToString().c_str());
MessageInterface::ShowMessage(" R(Groundstation) dot RangeVec = %lf\n",
currentMeasurement.feasibilityValue);
MessageInterface::ShowMessage(" Feasibility: %s\n",
(currentMeasurement.isFeasible ? "true" : "false"));
MessageInterface::ShowMessage(" Range is %.12lf\n",
currentMeasurement.value[0]);
MessageInterface::ShowMessage(" EventCount is %d\n",
currentMeasurement.eventCount);
#endif
#ifdef SHOW_RANGE_CALC
MessageInterface::ShowMessage("Range at epoch %.12lf is ",
currentMeasurement.epoch);
if (currentMeasurement.isFeasible)
MessageInterface::ShowMessage("feasible, value = %.12lf\n",
currentMeasurement.value[0]);
else
MessageInterface::ShowMessage("not feasible\n");
#endif
}
else
{
// Calculate the corrected range measurement
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("\n\n DSN 2-Way Range Calculation:\n");
#endif
#ifdef VIEW_PARTICIPANT_STATES_WITH_EVENTS
DumpParticipantStates("********************************************\n"
"Evaluating DSN 2-Way Range with located events");
#endif
// 1. Get the range from the down link
Rvector3 r1, r2;
Real t1, t2;
r1 = downlinkLeg.GetPosition(participants[0]);
r2 = downlinkLeg.GetPosition(participants[1]);
t1 = downlinkLeg.GetEventData((GmatBase*) participants[0]).epoch;
t2 = downlinkLeg.GetEventData((GmatBase*) participants[1]).epoch;
Rmatrix33 mt = downlinkLeg.GetEventData((GmatBase*) participants[0]).rInertial2obj.Transpose();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("1. Get downlink leg range:\n");
MessageInterface::ShowMessage(" Ground station position in FK5: r1 = (%f, %f, %f)km at epoch = %18.12lf\n", r1.Get(0), r1.Get(1), r1.Get(2), t1);
MessageInterface::ShowMessage(" Spacecraft position in FK5 : r2 = (%f, %f, %f)km at epoch = %18.12lf\n", r2.Get(0), r2.Get(1), r2.Get(2), t2);
MessageInterface::ShowMessage(" Transformation matrix from Earth fixed coordinate system to FK5 coordinate system at epoch = %18.12lf:\n", t1);
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(0,0), mt(0,1), mt(0,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(1,0), mt(1,1), mt(1,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt(2,0), mt(2,1), mt(2,2));
#endif
Rvector3 downlinkVector = r2 - r1; // rVector = r2 - r1;
downlinkRange = downlinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range = r2-r1: %.12lf km\n",
downlinkRange);
#endif
// 2. Calculate down link range rate:
Rvector3 p1V = downlinkLeg.GetVelocity(participants[0]);
Rvector3 p2V = downlinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("2. Get downlink leg range rate:\n");
MessageInterface::ShowMessage(" Ground station velocity in FK5: p1V = (%f, %f, %f)km/s\n", p1V.Get(0), p1V.Get(1), p1V.Get(2));
MessageInterface::ShowMessage(" Spacecraft velocity in FK5 : p2V = (%f, %f, %f)km/s\n", p2V.Get(0), p2V.Get(1), p2V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector downRRateVec = p2V - p1V /* - r12_j2k_vel*/;
Rvector3 rangeUnit = downlinkVector.GetUnitVector();
downlinkRangeRate = downRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Range Rate: %.12lf km/s\n",
downlinkRangeRate);
#endif
// 3. Get the range from the uplink
Rvector3 r3, r4;
Real t3, t4;
r3 = uplinkLeg.GetPosition(participants[0]);
r4 = uplinkLeg.GetPosition(participants[1]);
t3 = uplinkLeg.GetEventData((GmatBase*) participants[0]).epoch;
t4 = uplinkLeg.GetEventData((GmatBase*) participants[1]).epoch;
Rmatrix33 mt1 = uplinkLeg.GetEventData((GmatBase*) participants[0]).rInertial2obj.Transpose();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("3. Get uplink leg range:\n");
MessageInterface::ShowMessage(" Spacecraft position in FK5 : r4 = (%f, %f, %f)km at epoch = %18.12lf\n", r4.Get(0), r4.Get(1), r4.Get(2), t4);
MessageInterface::ShowMessage(" Ground station position in FK5: r3 = (%f, %f, %f)km at epoch = %18.12lf\n", r3.Get(0), r3.Get(1), r3.Get(2), t3);
MessageInterface::ShowMessage(" Transformation matrix from Earth fixed coordinate system to FK5 coordinate system at epoch = %18.12lf:\n", t3);
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(0,0), mt1(0,1), mt1(0,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(1,0), mt1(1,1), mt1(1,2));
MessageInterface::ShowMessage(" %18.12lf %18.12lf %18.12lf\n", mt1(2,0), mt1(2,1), mt1(2,2));
#endif
Rvector3 uplinkVector = r4 - r3;
uplinkRange = uplinkVector.GetMagnitude();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range = r4-r3: %.12lf km\n",
uplinkRange);
#endif
// 4. Calculate up link range rate
Rvector3 p3V = uplinkLeg.GetVelocity(participants[0]);
Rvector3 p4V = uplinkLeg.GetVelocity(participants[1]);
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("4. Get uplink leg range rate:\n");
MessageInterface::ShowMessage(" Ground station velocity in FK5: p3V = (%f, %f, %f)km/s\n", p3V.Get(0), p3V.Get(1), p3V.Get(2));
MessageInterface::ShowMessage(" Spacecraft velocity in FK5 : p4V = (%f, %f, %f)km/s\n", p4V.Get(0), p4V.Get(1), p4V.Get(2));
#endif
// @todo Relative origin velocities need to be subtracted when the origins
// differ; check and fix that part using r12_j2k_vel here. It's not yet
// incorporated because we need to handle the different epochs for the
// bodies, and we ought to do this part in barycentric coordinates
Rvector upRRateVec = p4V - p3V /* - r12_j2k_vel*/ ;
rangeUnit = uplinkVector.GetUnitVector();
uplinkRangeRate = upRRateVec * rangeUnit;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink Range Rate: %.12lf km/s\n",
uplinkRangeRate);
#endif
// 4.1. Target range rate: Do we need this as well?
targetRangeRate = (downlinkRangeRate + uplinkRangeRate) / 2.0;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Target Range Rate: %.12lf km/s\n",
targetRangeRate);
#endif
// 5. Get sensors used in DSN 2-ways range
if (participantHardware.empty()||
((!participantHardware.empty())&&
participantHardware[0].empty()&&
participantHardware[1].empty()
)
)
{
// DO NOT LEAVE THIS RAW IN A SOURCE FILE!!!
// MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
// Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Ideal measurement (no hardware delay and no media correction involve):\n");
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// Calculate real range
Real freqFactor = GetFrequencyFactor(frequency); // Notice that: unit of "frequency" varaibel is Hz (not MHz)
Real realTravelTime = uplinkTime + downlinkTime + receiveDelay + transmitDelay + targetDelay; // unit: second
Real realRangeKm = 0.5 *realTravelTime * GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/1000.0; // unit: km
Real realRange = realTravelTime * freqFactor; // unit: no unit
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Frequency = %.12lf MHz (This value is set to the default value in PhysicalMeasurement class due to no hardware used.)\n", frequency/1.0e6);
MessageInterface::ShowMessage(" Frequency factor = %.12lf MHz\n", freqFactor/1.0e6);
MessageInterface::ShowMessage(" Range in km = %.12lf km\n", realRangeKm);
MessageInterface::ShowMessage(" uplinkRange = %lfkm downlinkRange = %lfkm\n", uplinkRange, downlinkRange);
MessageInterface::ShowMessage(" receiveDelay = %lfm transmitDelay = %lfm targetDelay = %lfm\n", receiveDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM, transmitDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM, targetDelay*GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM);
MessageInterface::ShowMessage(" Range = %.12lf (It has no unit)\n", realRange);
#endif
// Set value for currentMeasurement
currentMeasurement.value[0] = realRange;
currentMeasurement.isFeasible = true;
return true;
}
ObjectArray objList1;
ObjectArray objList2;
ObjectArray objList3;
// objList1 := all transmitters in participantHardware list
// objList2 := all receivers in participantHardware list
// objList3 := all transponders in participantHardware list
for(std::vector<Hardware*>::iterator hw = this->participantHardware[0].begin();
hw != this->participantHardware[0].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transmitter")
objList1.push_back(*hw);
if ((*hw)->GetTypeName() == "Receiver")
objList2.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
for(std::vector<Hardware*>::iterator hw = this->participantHardware[1].begin();
hw != this->participantHardware[1].end(); ++hw)
{
if ((*hw) != NULL)
{
if ((*hw)->GetTypeName() == "Transponder")
objList3.push_back(*hw);
}
else
MessageInterface::ShowMessage(" sensor = NULL\n");
}
if (objList1.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 transmitter to send signal.\n");
throw new MeasurementException("The first participant does not have only 1 transmitter to send signal.\n");
}
if (objList2.size() != 1)
{
MessageInterface::ShowMessage("The first participant does not have only 1 receiver to receive signal.\n");
throw new MeasurementException("The first participant does not have only 1 receiver to receive signal.\n");
}
if (objList3.size() != 1)
{
MessageInterface::ShowMessage("The second participant does not have only 1 transponder to transpond signal.\n");
throw new MeasurementException("The second participant does not have only 1 transponder to transpond signal.\n");
}
Transmitter* gsTransmitter = (Transmitter*)objList1[0];
Receiver* gsReceiver = (Receiver*)objList2[0];
Transponder* scTransponder = (Transponder*)objList3[0];
if (gsTransmitter == NULL)
{
MessageInterface::ShowMessage("Transmitter is NULL object.\n");
throw new GmatBaseException("Transmitter is NULL object.\n");
}
if (gsReceiver == NULL)
{
MessageInterface::ShowMessage("Receiver is NULL object.\n");
throw new GmatBaseException("Receiver is NULL object.\n");
}
if (scTransponder == NULL)
{
MessageInterface::ShowMessage("Transponder is NULL object.\n");
throw new GmatBaseException("Transponder is NULL object.\n");
}
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("5. Sensors, delays, and signals:\n");
MessageInterface::ShowMessage(" List of sensors: %s, %s, %s\n",
gsTransmitter->GetName().c_str(), gsReceiver->GetName().c_str(),
scTransponder->GetName().c_str());
#endif
// 6. Get transmitter, receiver, and transponder delays:
transmitDelay = gsTransmitter->GetDelay();
receiveDelay = gsReceiver->GetDelay();
targetDelay = scTransponder->GetDelay();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Transmitter delay = %le s\n", gsTransmitter->GetDelay());
MessageInterface::ShowMessage(" Receiver delay = %le s\n", gsReceiver->GetDelay());
MessageInterface::ShowMessage(" Transponder delay = %le s\n", scTransponder->GetDelay());
#endif
// 7. Get frequency from transmitter of ground station (participants[0])
Signal* uplinkSignal = gsTransmitter->GetSignal();
Real uplinkFreq = uplinkSignal->GetValue();
// 8. Calculate media correction for uplink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("6. Media correction for uplink leg\n");
MessageInterface::ShowMessage(" UpLink signal frequency = %.12lf MHz\n", uplinkFreq);
#endif
Real roundTripTime = ((uplinkRange + downlinkRange)*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)/GmatTimeConstants::SECS_PER_DAY;
RealArray uplinkCorrection = CalculateMediaCorrection(uplinkFreq, r1, r2, currentMeasurement.epoch - roundTripTime);
Real uplinkRangeCorrection = uplinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real uplinkRealRange = uplinkRange + uplinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Uplink range correction = %.12lf km\n",uplinkRangeCorrection);
MessageInterface::ShowMessage(" Uplink real range = %.12lf km\n",uplinkRealRange);
#endif
// 9. Doppler shift the frequency from the transmitter using uplinkRangeRate:
Real uplinkDSFreq = (1 - uplinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*uplinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("7. Transponder input and output frequencies\n");
MessageInterface::ShowMessage(" Uplink Doppler shift frequency = %.12lf MHz\n", uplinkDSFreq);
#endif
// 10.Set frequency for the input signal of transponder
Signal* inputSignal = scTransponder->GetSignal(0);
inputSignal->SetValue(uplinkDSFreq);
scTransponder->SetSignal(inputSignal, 0);
// 11. Check the transponder feasibility to receive the input signal:
if (scTransponder->IsFeasible(0) == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The transponder is unfeasible to receive uplink signal.\n");
throw new GmatBaseException("The transponder is unfeasible to receive uplink signal.\n");
}
// 12. Get frequency of transponder output signal
Signal* outputSignal = scTransponder->GetSignal(1);
Real downlinkFreq = outputSignal->GetValue();
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink frequency = %.12lf Mhz\n", downlinkFreq);
#endif
// 13. Doppler shift the transponder output frequency by the downlinkRangeRate:
Real downlinkDSFreq = (1 - downlinkRangeRate*GmatMathConstants::KM_TO_M/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM)*downlinkFreq;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink Doppler shift frequency = %.12lf MHz\n", downlinkDSFreq);
#endif
// 14. Set frequency on receiver
Signal* downlinkSignal = gsReceiver->GetSignal();
downlinkSignal->SetValue(downlinkDSFreq);
// 15. Check the receiver feasibility to receive the downlink signal
if (gsReceiver->IsFeasible() == false)
{
currentMeasurement.isFeasible = false;
currentMeasurement.value[0] = 0;
MessageInterface::ShowMessage("The receiver is unfeasible to receive downlink signal.\n");
throw new MeasurementException("The receiver is unfeasible to receive downlink signal.\n");
}
// 16. Calculate media correction for downlink leg:
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("8. Media correction for downlink leg\n");
#endif
RealArray downlinkCorrection = CalculateMediaCorrection(downlinkDSFreq, r3, r4, currentMeasurement.epoch);
Real downlinkRangeCorrection = downlinkCorrection[0]/GmatMathConstants::KM_TO_M;
Real downlinkRealRange = downlinkRange + downlinkRangeCorrection;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Downlink range correction = %.12lf km\n",downlinkRangeCorrection);
MessageInterface::ShowMessage(" Downlink real range = %.12lf km\n",downlinkRealRange);
#endif
// 17. Calculate uplink time and down link time: (Is it needed???)
uplinkTime = uplinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
downlinkTime = downlinkRealRange*GmatMathConstants::KM_TO_M / GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage("9. Travel time:\n");
MessageInterface::ShowMessage(" Uplink time = %.12lf s\n",uplinkTime);
MessageInterface::ShowMessage(" Downlink time = %.12lf s\n",downlinkTime);
#endif
// 18. Calculate real range
// Real realRange = ((upRange + downRange) /
// (GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M) +
// receiveDelay + transmitDelay + targetDelay) * freqFactor;
// Real freqFactor = GetFrequencyFactor(frequency);
Real freqFactor = GetFrequencyFactor(uplinkFreq*1.0e6); // Notice that: unit of "uplinkFreq" is MHz (not Hz)
Real realTravelTime = uplinkTime + downlinkTime + receiveDelay + transmitDelay + targetDelay; // unit: second
Real realRangeKm = 0.5*realTravelTime * GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/1000.0; // unit: km
Real realRange = realTravelTime * freqFactor; // unit: no unit
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Frequency factor = %.12lf MHz\n", freqFactor/1.0e6);
MessageInterface::ShowMessage(" Calculated real range in km = %.12lf km\n", realRangeKm);
MessageInterface::ShowMessage(" Calculated real range = %.12lf (It has no unit)\n", realRange);
#endif
// 19. Set value for currentMeasurement
// currentMeasurement.value[0] = realRange;
currentMeasurement.value[0] = realRangeKm;
currentMeasurement.isFeasible = true;
retval = true;
}
return retval;
}