//------------------------------------------------------------------------
RealArray PhysicalMeasurement::TroposphereCorrection(Real freq, Rvector3 rVec, Rmatrix Ro_j2k)
{
RealArray tropoCorrection;
if (troposphere != NULL)
{
Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6);
troposphere->SetWaveLength(wavelength);
Real elevationAngle = asin((Ro_j2k*rVec.GetUnitVector()).GetElement(2));
troposphere->SetElevationAngle(elevationAngle);
troposphere->SetRange(rVec.GetMagnitude()*GmatMathConstants::KM_TO_M);
tropoCorrection = troposphere->Correction();
// Real rangeCorrection = tropoCorrection[0]/GmatMathConstants::KM_TO_M;
#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
MessageInterface::ShowMessage(" Apply Troposphere media correction:\n");
MessageInterface::ShowMessage(" .Wave length = %.12lf m\n", wavelength);
MessageInterface::ShowMessage(" .Elevation angle = %.12lf degree\n", elevationAngle*GmatMathConstants::DEG_PER_RAD);
MessageInterface::ShowMessage(" .Range correction = %.12lf m\n", tropoCorrection[0]);
#endif
}
else
{
tropoCorrection.push_back(0.0);
tropoCorrection.push_back(0.0);
tropoCorrection.push_back(0.0);
}
return tropoCorrection;
}
Rmatrix33 NadirPointing::TRIAD(Rvector3& V1, Rvector3& V2, Rvector3& W1, Rvector3& W2)
{
// V1, V2 : defined in frame A
// W1, W2 : defined in frame B
// TRIAD algorithm calculates the rotation matrix from A to B
Rvector3 r1 = V1.GetUnitVector();
Rvector3 temp = Cross(V1,V2);
Rvector3 r2 = temp.GetUnitVector();
Rvector3 r3 = Cross(V1,temp);
r3 = r3.GetUnitVector();
Rvector3 s1 = W1.GetUnitVector();
temp = Cross(W1,W2);
Rvector3 s2 = temp.GetUnitVector();
Rvector3 s3 = Cross(W1,temp);
s3 = s3.GetUnitVector();
//MessageInterface::LogMessage("s3 = %f\t%f\t%f\n", s3(0), s3(1), s3(2) );
// YRL, calculate resultRotMatrix
Rmatrix33 resultRotMatrix = Outerproduct(s1,r1) + Outerproduct(s2,r2) + Outerproduct(s3,r3) ;
return resultRotMatrix;
}
//------------------------------------------------------------------------------
Real AngleUtil::ComputeAngleInDeg(const Rvector3 &vecA, const Rvector3 &vecB,
Real tol)
{
Rvector3 uvecA = vecA.GetUnitVector();
Rvector3 uvecB = vecB.GetUnitVector();
Real aDotB = uvecA * uvecB;
Real angRad = 0.0;
Real angDeg = 0.0;
// if dot-product is less than or equal to tolerance, the angle is arccos of
// the dot-product, otherwise, the angle is arcsin of the magnitude of the
// cross-product.
if (Abs(aDotB) <= Abs(tol))
{
angRad = ACos(aDotB);
angDeg = DEG_PER_RAD * angRad;
}
else
{
// compute cross-product of two vectors
Rvector3 aCrossB = Cross(uvecA, uvecB);
Real crossMag = aCrossB.GetMagnitude();
angRad = ASin(crossMag);
if (aDotB < 0.0)
{
angRad = PI_OVER_TWO - angRad;
angDeg = DEG_PER_RAD * angRad;
}
}
return angDeg;
}
//------------------------------------------------------------------------------
bool Periapsis::Evaluate()
{
if (mOrigin == NULL)
OrbitData::InitializeRefObjects();
Rvector6 cartState = OrbitData::GetRelativeCartState(mOrigin);
//MessageInterface::ShowMessage
// (wxT("===> Periapsis::Evaluate() mOrigin=%s, cartState=%s\n"),
// mOrigin->GetName().c_str(), cartState.ToString().c_str());
if (cartState == Rvector6::RVECTOR6_UNDEFINED)
return false;
// compute position and velocity unit vectors
Rvector3 pos = Rvector3(cartState[0], cartState[1], cartState[2]);
Rvector3 vel = Rvector3(cartState[3], cartState[4], cartState[5]);
Rvector3 R = pos.GetUnitVector();
Rvector3 V = vel.GetUnitVector();
// compute cos(90 - beta) as the dot product of the R and V vectors
// Real rdotv = R*V;
// mRealValue = rdotv;
mRealValue = R*V;
// Changed to use IsEqual() (LOJ: 2010.02.02)
//if (mRealValue == 0.0)
if (GmatMathUtil::IsEqual(mRealValue, 0.0))
mRealValue = 1.0e-40;
return true;
}
//------------------------------------------------------------------------------
bool Apoapsis::Evaluate()
{
if (mOrigin == NULL)
OrbitData::InitializeRefObjects();
Rvector6 cartState = OrbitData::GetRelativeCartState(mOrigin);
if (cartState == Rvector6::RVECTOR6_UNDEFINED)
return false;
// compute position and velocity unit vectors
Rvector3 pos = Rvector3(cartState[0], cartState[1], cartState[2]);
Rvector3 vel = Rvector3(cartState[3], cartState[4], cartState[5]);
Rvector3 R = pos.GetUnitVector();
Rvector3 V = vel.GetUnitVector();
// compute cos(90 - beta) as the dot product of the R and V vectors
Real rdotv = R*V;
mRealValue = rdotv;
if (mRealValue == 0.0)
mRealValue = -1.0e-40;
//MessageInterface::ShowMessage(wxT("Apoapsis::Evaluate() r=%f,%f,%f, v=%f,%f,%f, r.v=%f\n"),
// R[0], R[1], R[2], V[0], V[1], V[2], rdotv);
return true;
}
//---------------------------------------------------------------------------
// Real Ionosphere::BendingAngle()
//---------------------------------------------------------------------------
Real Ionosphere::BendingAngle()
{
Rvector3 rangeVec = spacecraftLoc - stationLoc;
Rvector3 dR = rangeVec / NUM_OF_INTERVALS;
Rvector3 p1 = stationLoc;
Rvector3 p2, delta;
Real n1, n2, dn_drho, de1, de2, integrant;
Real gammar = 0.0;
//Real beta0 = GmatConstants::PI/2 - acos(rangeVec.GetUnitVector()*p1.GetUnitVector());
Real beta0 = GmatMathConstants::PI_OVER_TWO - acos(rangeVec.GetUnitVector()*p1.GetUnitVector());
//MessageInterface::ShowMessage("Elevation angle = %f\n", beta0*180/GmatConstants::PI);
MessageInterface::ShowMessage("Elevation angle = %f\n", beta0*GmatMathConstants::DEG_PER_RAD);
Real beta = beta0;
Real freq = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / waveLength;
for(int i = 0; i < NUM_OF_INTERVALS; ++i)
{
p2 = p1 + dR;
delta = dR/100;
de1 = ElectronDensity(p1+delta, p1);
de2 = ElectronDensity(p1+2*delta,p1+delta);
n1 = 1 - 40.3*de1/(freq*freq);
n2 = 1 - 40.3*de2/(freq*freq);
dn_drho = -40.3*(de2 - de1)/ (freq*freq) / (((p1+delta).GetMagnitude() - p1.GetMagnitude())*GmatMathConstants::KM_TO_M);
integrant = dn_drho/(n1*tan(beta));
gammar += integrant*(p2.GetMagnitude() - p1.GetMagnitude())*GmatMathConstants::KM_TO_M;
//MessageInterface::ShowMessage("de1 = %.12lf, de2 = %.12lf, rho1 = %f, "
//"rho2 = %f, integrant = %.12lf, gammar =%.12lf\n",de1, de2, p1.GetMagnitude(),
//p2.GetMagnitude(), integrant, gammar*180/GmatConstants::PI);
p1 = p2;
beta = beta0 + gammar;
}
return gammar;
}
//------------------------------------------------------------------------------
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;
}
//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
bool forceComputation)
{
if (!primary)
throw CoordinateSystemException("Primary \"" + primaryName +
"\" is not yet set in object referenced coordinate system!");
if (!secondary)
throw CoordinateSystemException("Secondary \"" + secondaryName +
"\" is not yet set in object referenced coordinate system!");
if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, axes are improperly "
"defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, too many axes are defined.\n"
"XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
SpacePoint *useAsSecondary = secondary;
// if (!useAsSecondary) useAsSecondary = origin;
Rvector6 rv = useAsSecondary->GetMJ2000State(atEpoch) -
primary->GetMJ2000State(atEpoch);
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
visitCount++;
}
#endif
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
std::stringstream ss;
ss.precision(30);
ss << " ----------------- rv Earth to Moon (truncated) = "
<< rv << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
visitCount++;
}
#endif
Rvector3 a = useAsSecondary->GetMJ2000Acceleration(atEpoch) -
primary->GetMJ2000Acceleration(atEpoch);
Rvector3 r = rv.GetR();
Rvector3 v = rv.GetV();
Rvector3 n = Cross(r,v);
Rvector3 rUnit = r.GetUnitVector();
Rvector3 vUnit = v.GetUnitVector();
Rvector3 nUnit = n.GetUnitVector();
Real rMag = r.GetMagnitude();
Real vMag = v.GetMagnitude();
Real nMag = n.GetMagnitude();
// check for divide-by-zero
if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
{
std::string errmsg = "Object referenced axis system named \"";
errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
throw CoordinateSystemException(errmsg);
}
Rvector3 rDot = (v / rMag) - (rUnit / rMag) * (rUnit * v);
Rvector3 vDot = (a / vMag) - (vUnit / vMag) * (vUnit * a);
Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
bool xUsed = true, yUsed = true, zUsed = true;
// determine the x-axis
if ((xAxis == "R") || (xAxis == "r"))
{
xUnit = rUnit;
xDot = rDot;
}
else if ((xAxis == "-R") || (xAxis == "-r"))
{
xUnit = -rUnit;
xDot = -rDot;
}
else if ((xAxis == "V") || (xAxis == "v"))
{
xUnit = vUnit;
xDot = vDot;
}
else if ((xAxis == "-V") || (xAxis == "-v"))
{
xUnit = -vUnit;
xDot = -vDot;
}
else if ((xAxis == "N") || (xAxis == "n"))
{
xUnit = nUnit;
xDot = nDot;
}
else if ((xAxis == "-N") || (xAxis == "-n"))
{
xUnit = -nUnit;
xDot = -nDot;
}
else
{
xUsed = false;
}
// determine the y-axis
if ((yAxis == "R") || (yAxis == "r"))
{
yUnit = rUnit;
yDot = rDot;
}
else if ((yAxis == "-R") || (yAxis == "-r"))
{
yUnit = -rUnit;
yDot = -rDot;
}
else if ((yAxis == "V") || (yAxis == "v"))
{
yUnit = vUnit;
yDot = vDot;
}
else if ((yAxis == "-V") || (yAxis == "-v"))
{
yUnit = -vUnit;
yDot = -vDot;
}
else if ((yAxis == "N") || (yAxis == "n"))
{
yUnit = nUnit;
yDot = nDot;
}
else if ((yAxis == "-N") || (yAxis == "-n"))
{
yUnit = -nUnit;
yDot = -nDot;
}
else
{
yUsed = false;
}
// determine the z-axis
if ((zAxis == "R") || (zAxis == "r"))
{
zUnit = rUnit;
zDot = rDot;
}
else if ((zAxis == "-R") || (zAxis == "-r"))
{
zUnit = -rUnit;
zDot = -rDot;
}
else if ((zAxis == "V") || (zAxis == "v"))
{
zUnit = vUnit;
zDot = vDot;
}
else if ((zAxis == "-V") || (zAxis == "-v"))
{
zUnit = -vUnit;
zDot = -vDot;
}
else if ((zAxis == "N") || (zAxis == "n"))
{
zUnit = nUnit;
zDot = nDot;
}
else if ((zAxis == "-N") || (zAxis == "-n"))
{
zUnit = -nUnit;
zDot = -nDot;
}
else
{
zUsed = false;
}
// determine the third axis
if (xUsed && yUsed && !zUsed)
{
zUnit = Cross(xUnit, yUnit);
zDot = Cross(xDot, yUnit) + Cross(xUnit, yDot);
}
else if (xUsed && zUsed && !yUsed)
{
yUnit = Cross(zUnit,xUnit);
yDot = Cross(zDot, xUnit) + Cross(zUnit, xDot);
}
else if (yUsed && zUsed && !xUsed)
{
xUnit = Cross(yUnit,zUnit);
xDot = Cross(yDot, zUnit) + Cross(yUnit, zDot);
}
else
{
throw CoordinateSystemException(
"Object referenced axes are improperly defined.");
}
// Compute the rotation matrix
rotMatrix(0,0) = xUnit(0);
rotMatrix(0,1) = yUnit(0);
rotMatrix(0,2) = zUnit(0);
rotMatrix(1,0) = xUnit(1);
rotMatrix(1,1) = yUnit(1);
rotMatrix(1,2) = zUnit(1);
rotMatrix(2,0) = xUnit(2);
rotMatrix(2,1) = yUnit(2);
rotMatrix(2,2) = zUnit(2);
// Compute the rotation derivative matrix
rotDotMatrix(0,0) = xDot(0);
rotDotMatrix(0,1) = yDot(0);
rotDotMatrix(0,2) = zDot(0);
rotDotMatrix(1,0) = xDot(1);
rotDotMatrix(1,1) = yDot(1);
rotDotMatrix(1,2) = zDot(1);
rotDotMatrix(2,0) = xDot(2);
rotDotMatrix(2,1) = yDot(2);
rotDotMatrix(2,2) = zDot(2);
#ifdef DEBUG_ROT_MATRIX
MessageInterface::ShowMessage
("rotMatrix=%s\n", rotMatrix.ToString().c_str());
std::stringstream ss;
ss.setf(std::ios::fixed);
ss.precision(30);
ss << " ----------------- rotMatrix = " << rotMatrix << std::endl;
ss.setf(std::ios::scientific);
ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
#endif
if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
{
std::stringstream errmsg("");
errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
}
}
//------------------------------------------------------------------------------
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;
}