Пример #1
0
//------------------------------------------------------------------------
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;
}
Пример #2
0
//------------------------------------------------------------------------------
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;
}
Пример #3
0
//------------------------------------------------------------------------------
bool TDRSSTwoWayRange::Evaluate(bool withEvents)
{
   bool retval = false;

   if (!initialized)
      InitializeMeasurement();

   #ifdef DEBUG_RANGE_CALC
      MessageInterface::ShowMessage("Entered TDRSSTwoWayRange::Evaluate()\n");
      MessageInterface::ShowMessage("  ParticipantCount: %d\n",
            participants.size());
   #endif

   if (withEvents == false)
   {
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation without "
               "events\n");
      #endif

      #ifdef VIEW_PARTICIPANT_STATES
         DumpParticipantStates("++++++++++++++++++++++++++++++++++++++++++++\n"
               "Evaluating TDRSS 2-Way Range without events");
      #endif

      if (CheckLOS(0, 1, NULL) && CheckLOS(1, 2, NULL))
      {
         // Calculate the range vector between the groundstation and the TDRS
         CalculateRangeVectorInertial(0, 1);
         Rvector3 outState;

         // Set feasibility off of topospheric horizon, set by the Z value in topo
         // coords
         std::string updateAll = "All";
         UpdateRotationMatrix(currentMeasurement.epoch, updateAll);
         outState = R_o_j2k * rangeVecInertial;
         currentMeasurement.feasibilityValue = outState[2];

         #ifdef CHECK_PARTICIPANT_LOCATIONS
            MessageInterface::ShowMessage("Evaluating without events\n");
            MessageInterface::ShowMessage("Calculating TDRSS 2-Way Range at epoch "
                  "%.12lf\n", currentMeasurement.epoch);
            MessageInterface::ShowMessage("   J2K Location of %s, id = '%s':  %s",
                  participants[0]->GetName().c_str(),
                  currentMeasurement.participantIDs[0].c_str(),
                  p1Loc.ToString().c_str());
            MessageInterface::ShowMessage("   J2K Location of %s, id = '%s':  %s",
                  participants[1]->GetName().c_str(),
                  currentMeasurement.participantIDs[1].c_str(),
                  p2Loc.ToString().c_str());
            Rvector3 bfLoc = R_o_j2k * p1Loc;
            MessageInterface::ShowMessage("   BodyFixed Location of %s:  %s",
                  participants[0]->GetName().c_str(),
                  bfLoc.ToString().c_str());
            bfLoc = R_o_j2k * p2Loc;
            MessageInterface::ShowMessage("   BodyFixed Location of %s:  %s\n",
                  participants[1]->GetName().c_str(),
                  bfLoc.ToString().c_str());
         #endif

         if (currentMeasurement.feasibilityValue > 0.0)
         {
            currentMeasurement.isFeasible = true;
            currentMeasurement.value[0] = rangeVecInertial.GetMagnitude();
            currentMeasurement.eventCount = 4;

            retval = true;
         }
      }
      else
      {
         currentMeasurement.isFeasible = false;
         currentMeasurement.value[0] = 0.0;
         currentMeasurement.eventCount = 0;
      }

      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("Calculating Range at epoch %.12lf\n",
               currentMeasurement.epoch);
         MessageInterface::ShowMessage("   Location of %s, id = '%s':  %s",
               participants[0]->GetName().c_str(),
               currentMeasurement.participantIDs[0].c_str(),
               p1Loc.ToString().c_str());
         MessageInterface::ShowMessage("   Location of %s, id = '%s':  %s",
               participants[1]->GetName().c_str(),
               currentMeasurement.participantIDs[1].c_str(),
               p2Loc.ToString().c_str());
         MessageInterface::ShowMessage("   Range Vector (inertial):  %s\n",
               rangeVecInertial.ToString().c_str());
         MessageInterface::ShowMessage("   R(Groundstation) dot RangeVec =  %lf\n",
               currentMeasurement.feasibilityValue);
         MessageInterface::ShowMessage("   Feasibility:  %s\n",
               (currentMeasurement.isFeasible ? "true" : "false"));
         MessageInterface::ShowMessage("   Range is %.12lf\n",
               currentMeasurement.value[0]);
         MessageInterface::ShowMessage("   EventCount is %d\n",
               currentMeasurement.eventCount);
      #endif

      #ifdef SHOW_RANGE_CALC
         MessageInterface::ShowMessage("Range at epoch %.12lf is ",
               currentMeasurement.epoch);
         if (currentMeasurement.isFeasible)
            MessageInterface::ShowMessage("feasible, value = %.12lf\n",
               currentMeasurement.value[0]);
         else
            MessageInterface::ShowMessage("not feasible\n");
      #endif
   }
   else
   {
      // Calculate the corrected range measurement
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("TDRSS 2-Way Range Calculation:\n");
      #endif

      #ifdef VIEW_PARTICIPANT_STATES
         DumpParticipantStates("********************************************\n"
               "Evaluating TDRSS 2-Way Range with located events");
         MessageInterface::ShowMessage("Calculating TDRSS 2-Way Range at epoch "
               "%.12lf\n", currentMeasurement.epoch);
      #endif

      // Get the range from the downlink
      Rvector3 r1, r2;
      r1 = downlinkLeg.GetPosition(participants[0]);
      r2 = downlinkLeg.GetPosition(participants[1]);
      Rvector3 rVector = r2 - r1;
      Real realRange = rVector.GetMagnitude();

      #ifdef VIEW_PARTICIPANT_STATES
         MessageInterface::ShowMessage("   %s at downlink: %.12lf %.12lf %.12lf\n",
               participants[0]->GetName().c_str(), r1[0], r1[1], r1[2]);
         MessageInterface::ShowMessage("   %s at downlink: %.12lf %.12lf %.12lf\n",
               participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
      #endif
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   Downlink Range:  %.12lf\n",
               rVector.GetMagnitude());
      #endif

      r1 = backlinkLeg.GetPosition(participants[1]);
      r2 = backlinkLeg.GetPosition(participants[2]);
      if (CheckSat2SatLOS(r1, r2, NULL) == false)
         return false;
      rVector = r2 - r1;
      realRange += rVector.GetMagnitude();
      #ifdef VIEW_PARTICIPANT_STATES
         MessageInterface::ShowMessage("   %s at backlink: %.12lf %.12lf %.12lf\n",
               participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
         MessageInterface::ShowMessage("   %s at backlink: %.12lf %.12lf %.12lf\n",
               participants[2]->GetName().c_str(), r2[0], r2[1], r2[2]);
      #endif

      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   Backlink Range:  %.12lf\n",
                  rVector.GetMagnitude());
      #endif

      // Add the pseudorange from the transponder delay
      targetDelay = GetDelay(2,0);
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   TDRSS target delay is %.12lf\n",
               targetDelay);
      #endif
      realRange += GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M *
            targetDelay;

      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   Delay Range (%le sec delay):  "
               "%.12lf\n", targetDelay,
               (GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM/GmatMathConstants::KM_TO_M * targetDelay));
      #endif

      r1 = forwardlinkLeg.GetPosition(participants[2]);
      r2 = forwardlinkLeg.GetPosition(participants[1]);
      if (CheckSat2SatLOS(r1, r2, NULL) == false)
         return false;
      rVector = r2 - r1;
      realRange += rVector.GetMagnitude();

      #ifdef VIEW_PARTICIPANT_STATES
         MessageInterface::ShowMessage("   %s at frwdlink: %.12lf %.12lf %.12lf\n",
               participants[2]->GetName().c_str(), r1[0], r1[1], r1[2]);
         MessageInterface::ShowMessage("   %s at frwdlink: %.12lf %.12lf %.12lf\n",
               participants[1]->GetName().c_str(), r2[0], r2[1], r2[2]);
      #endif
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   forwardlink Range:  %.12lf\n",
               rVector.GetMagnitude());
      #endif

      r1 = uplinkLeg.GetPosition(participants[1]);
      r2 = uplinkLeg.GetPosition(participants[0]);
      rVector = r2 - r1;
      realRange += rVector.GetMagnitude();

      #ifdef VIEW_PARTICIPANT_STATES
         MessageInterface::ShowMessage("   %s at frwdlink: %.12lf %.12lf %.12lf\n",
               participants[1]->GetName().c_str(), r1[0], r1[1], r1[2]);
         MessageInterface::ShowMessage("   %s at frwdlink: %.12lf %.12lf %.12lf\n",
               participants[0]->GetName().c_str(), r2[0], r2[1], r2[2]);
      #endif
      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   Uplink Range:  %.12lf\n",
               rVector.GetMagnitude());
      #endif

      currentMeasurement.value[0] = realRange / 2.0;

      #ifdef DEBUG_RANGE_CALC
         MessageInterface::ShowMessage("   Calculated Range:  %.12lf\n",
               currentMeasurement.value[0]);
      #endif

      retval = true;
   }

   return retval;
}
Пример #4
0
//------------------------------------------------------------------------------
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;
}
Пример #5
0
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();

}