//------------------------------------------------------------------------ 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(); }