//------------------------------------------------------------------------
RealArray PhysicalMeasurement::CalculateMediaCorrection(Real freq, Rvector3 r1, Rvector3 r2, Real epoch)
{
#ifdef DEBUG_MEDIA_CORRECTION
MessageInterface::ShowMessage("start PhysicalMeasurement::CalculateMediaCorrection()\n");
#endif
RealArray mediaCorrection;
// 1. Run Troposphere correction:
UpdateRotationMatrix(epoch, "o_j2k");
Rvector3 rangeVector = r2 - r1;
mediaCorrection = TroposphereCorrection(freq, rangeVector, R_o_j2k);
#ifdef DEBUG_MEDIA_CORRECTION
MessageInterface::ShowMessage(" frequency = %le MHz, epoch = %lf, r2-r1 = ('%s')km\n", freq, epoch, (r2-r1).ToString().c_str());
MessageInterface::ShowMessage(" TroposhereCorrection = (%lf m, %lf arcsec, %le s)\n", mediaCorrection[0], mediaCorrection[1], mediaCorrection[2]);
#endif
#ifdef IONOSPHERE
// 2. Run Ionosphere correction:
RealArray ionoCorrection = this->IonosphereCorrection(freq, r1, r2, epoch);
// 3. Combine effects:
mediaCorrection[0] += ionoCorrection[0];
mediaCorrection[1] += ionoCorrection[1];
mediaCorrection[2] += ionoCorrection[2];
#endif
return mediaCorrection;
}
//------------------------------------------------------------------------------
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 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 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;
}
void Master::Run() {
// Event loop
while(_exitFlag == false) {
// Wait for a new connection from
_userInterfaceServer.Accept();
// Client error
int error = USER_INTERFACE_SERVER_OK;
// Loop getting commands from user as long as a client is connected
while (error != USER_INTERFACE_SERVER_ERROR) {
// Get command from client
error = _userInterfaceServer.GetCommand();
// If client received a command call barrier
if (error == USER_INTERFACE_SERVER_OK) {
_communicator -> Barrier();
}
#ifdef SAGE
// SAGE message
sageMessage msg;
// Check for SAGE message
if (_sageInf.checkMsg(msg, false) > 0) {
// Get SAGE message data
char* data = (char*) msg.getData();
// Determine message
switch(msg.getCode()) {
// Quit
case APP_QUIT:
// Send exit command to UI
_userInterfaceServer.SendCommandExit();
// Send exit command
UpdateCommandExit();
// Stop this loop
error = USER_INTERFACE_SERVER_ERROR;
break;
// Click event
case EVT_CLICK: {
// Click event x and y location normalized to size of window
float clickX, clickY;
// Ckick device Id, button Id, and is down flag
int clickDeviceId, clickButtonId, clickIsDown, clickEvent;
// Parse message
sscanf(data,
"%d %f %f %d %d %d",
&clickDeviceId, &clickX, &clickY,
&clickButtonId, &clickIsDown, &clickEvent);
// If down event for EVT_PAN
if (clickIsDown && clickEvent == EVT_PAN) {
// Set initial position of click event
_translatePosition[0] = clickX;
_translatePosition[1] = clickY;
}
// If down event for EVT_ZOOM
if (clickIsDown && clickEvent == EVT_ZOOM) {
// Set initial position of click event
_scalePosition[0] = clickX;
_scalePosition[1] = clickY;
// Calculate the location of the device in world space
float initX =
(_scalePosition[0] *
(_totalDisplayFrustum[1] - _totalDisplayFrustum[0])) +
_totalDisplayFrustum[0];
float initY =
(_scalePosition[1] *
(_totalDisplayFrustum[3] - _totalDisplayFrustum[2])) +
_totalDisplayFrustum[2];
// Calculate the location of the device with respect to the
// untransfomred object in world space
initX -= _worldTranslate[0];
initY -= _worldTranslate[1];
initX /= _worldScale[0];
initY /= _worldScale[1];
// Keep these values
_scalePositionUntransformed[0] = initX;
_scalePositionUntransformed[1] = initY;
}
// If down event for EVT_ROTATE
if (clickIsDown && clickEvent == EVT_ROTATE) {
// Set inital position
_rotatePosition[0] = clickX;
_rotatePosition[1] = clickY;
// Normalize object's translation to (-1.0, 1.0)
float nTx = (_worldTranslate[0] - _totalDisplayFrustum[0]) /
(_totalDisplayFrustum[1] - _totalDisplayFrustum[0]);
nTx = (nTx * 2.0) - 1.0;
float nTy = (_worldTranslate[1] - _totalDisplayFrustum[2]) /
(_totalDisplayFrustum[3] - _totalDisplayFrustum[2]);
nTy = (nTy * 2.0) - 1.0;
// Normalize puck's position to (-1.0, 1.0)
float nXpos = (clickX * 2.0) - 1.0;
float nYpos = (clickY * 2.0) - 1.0;
// Start trackball
_eventTrackball.Start(nXpos - nTx, nYpos - nTy);
}
// If up event
if (clickIsDown == 0) {
// Force the renderer to update
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
}
// Done with EVT_CLICK case
break;
}
// Pan event
case EVT_PAN: {
// Pan event properties
int panDeviceId;
// Pan event x and y location and change in x, y and z direction
// normalized to size of window
float panX, panY, panDX, panDY, panDZ;
sscanf(data,
"%d %f %f %f %f %f",
&panDeviceId, &panX, &panY, &panDX, &panDY, &panDZ);
// Print event
//fprintf(stderr,
// "PAN MESSAGE: %f %f %f %f %f\n",
// panX, panY, panDX, panDY, panDZ);
// Calculate amount the device has moved in world space since it
// was clicked.
float deltaT[2] = {((_translatePosition[0]-
(_translatePosition[0] + panDX)) *
(_totalDisplayFrustum[1] -
_totalDisplayFrustum[0])),
((_translatePosition[1]-
(_translatePosition[1] + panDY)) *
(_totalDisplayFrustum[3] -
_totalDisplayFrustum[2]))};
// Add the offset
_worldTranslate[0] -= deltaT[0];
_worldTranslate[1] -= deltaT[1];
// Determine translation matrix
float T[16];
calc_trans(T,
_worldTranslate[0],
_worldTranslate[1],
0.0);
// Send translation matrix to UI
_userInterfaceServer.SendTranslationMatrix(T);
// Set the translation matrix
UpdateTranslationMatrix(T);
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
// Update translate device position
_translatePosition[0] += panDX;
_translatePosition[1] += panDY;
// Done with EVT_PAN case
break;
}
// Zoom event
case EVT_ZOOM: {
// Zoom event properties
int zoomDeviceId;
// Zoom event x and y location and change in x, y and z direction
// normalized to size of window
float zoomX, zoomY, zoomDX, zoomDY, zoomDZ;
sscanf(data,
"%d %f %f %f %f %f",
&zoomDeviceId, &zoomX, &zoomY, &zoomDX, &zoomDY, &zoomDZ);
// Print event
//fprintf(stderr,
// "ZOOM MESSAGE: %f %f %f %f %f\n",
// zoomX, zoomY, zoomDX, zoomDY, zoomDZ);
// Calcualte amount the device has moved
float deltaZ[2] = {-zoomDX, -zoomDY};
// Update world scale by delta
_worldScale[0] *= 1.0 - (deltaZ[0] + deltaZ[1]);
_worldScale[1] *= 1.0 - (deltaZ[0] + deltaZ[1]);
_worldScale[2] *= 1.0 - (deltaZ[0] + deltaZ[1]);
// Don't let world scale go below 1.0
if (_worldScale[0] < 1.0) _worldScale[0] = 1.0;
if (_worldScale[1] < 1.0) _worldScale[1] = 1.0;
if (_worldScale[2] < 1.0) _worldScale[2] = 1.0;
// Determine scale matrix
float S[16];
calc_scale(S,
_worldScale[0],
_worldScale[1],
_worldScale[2]);
// Calculate the initial location of the device in world space
float initX =
(zoomX *
(_totalDisplayFrustum[1] - _totalDisplayFrustum[0])) +
_totalDisplayFrustum[0];
float initY =
(zoomY *
(_totalDisplayFrustum[3] - _totalDisplayFrustum[2])) +
_totalDisplayFrustum[2];
// Determine new translation based on scale
_worldTranslate[0] =
initX - (_scalePositionUntransformed[0] * _worldScale[0]);
_worldTranslate[1] =
initY - (_scalePositionUntransformed[1] * _worldScale[1]);
// Determine new translation matrix
float T[16];
calc_trans(T,
_worldTranslate[0],
_worldTranslate[1],
0.0);
// Send scale matrix to UI
_userInterfaceServer.SendScaleMatrix(S);
// Send translation matrix to UI
_userInterfaceServer.SendTranslationMatrix(T);
// Set the scale and translation matrices
UpdateScaleMatrix(S);
_communicator -> Barrier();
UpdateTranslationMatrix(T);
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
// Done with EVT_ZOOM case
break;
}
// Rotate event
case EVT_ROTATE: {
// Rotate event properties
int rotateDeviceId;
// Rotate event x and y location and change in x, y and z direction
// normalized to size of window
float rotateX, rotateY, rotateDX, rotateDY, rotateDZ;
sscanf(data,
"%d %f %f %f %f %f",
&rotateDeviceId, &rotateX, &rotateY,
&rotateDX, &rotateDY, &rotateDZ);
//fprintf(stderr, "EVT_ROTATE: %f %f %f %f %f\n",
// rotateX, rotateY, rotateDX, rotateDY, rotateDZ);
// Normalize object's translation to (-1.0, 1.0)
float nTx = (_worldTranslate[0] - _totalDisplayFrustum[0]) /
(_totalDisplayFrustum[1] - _totalDisplayFrustum[0]);
nTx = (nTx * 2.0) - 1.0;
float nTy = (_worldTranslate[1] - _totalDisplayFrustum[2]) /
(_totalDisplayFrustum[3] - _totalDisplayFrustum[2]);
nTy = (nTy * 2.0) - 1.0;
// Normalize puck's position to (-1.0, 1.0)
float nXpos = (_rotatePosition[0] * 2.0) - 1.0;
float nYpos = (_rotatePosition[1] * 2.0) - 1.0;
// Update trackball
_eventTrackball.Update(nXpos - nTx, nYpos - nTy);
// Get rotation from trackball
float R[16];
_eventTrackball.GetRotationMatrix(R);
// Send rotation matrix to UI
_userInterfaceServer.SendRotationMatrix(R);
// Set the rotation matrix
UpdateRotationMatrix(R);
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
UpdateCommandRender();
_communicator -> Barrier();
_rotatePosition[0] += rotateDX;
_rotatePosition[1] += rotateDY;
// Done with EVT_ZOOM case
break;
}
}
}
#endif
}
}
// One last barrier
_communicator -> Barrier();
}
