//------------------------------------------------------------------------
RealArray PhysicalMeasurement::TroposphereCorrection(Real freq, Rvector3 rVec, Rmatrix Ro_j2k)
{
   RealArray tropoCorrection;

   if (troposphere != NULL)
   {
   	Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6);
   	troposphere->SetWaveLength(wavelength);
   	Real elevationAngle = asin((Ro_j2k*rVec.GetUnitVector()).GetElement(2));
   	troposphere->SetElevationAngle(elevationAngle);
   	troposphere->SetRange(rVec.GetMagnitude()*GmatMathConstants::KM_TO_M);
   	tropoCorrection = troposphere->Correction();
//   	Real rangeCorrection = tropoCorrection[0]/GmatMathConstants::KM_TO_M;

		#ifdef DEBUG_RANGE_CALC_WITH_EVENTS
			MessageInterface::ShowMessage("       Apply Troposphere media correction:\n");
			MessageInterface::ShowMessage("         .Wave length = %.12lf m\n", wavelength);
			MessageInterface::ShowMessage("         .Elevation angle = %.12lf degree\n", elevationAngle*GmatMathConstants::DEG_PER_RAD);
			MessageInterface::ShowMessage("         .Range correction = %.12lf m\n", tropoCorrection[0]);
		#endif
   }
   else
   {
   	tropoCorrection.push_back(0.0);
   	tropoCorrection.push_back(0.0);
   	tropoCorrection.push_back(0.0);
   }

   return tropoCorrection;
}
Beispiel #2
0
//------------------------------------------------------------------------------
Real AngleUtil::ComputeAngleInDeg(const Rvector3 &vecA, const Rvector3 &vecB,
                                  Real tol)
{
   Rvector3 uvecA = vecA.GetUnitVector();
   Rvector3 uvecB = vecB.GetUnitVector();
   Real aDotB = uvecA * uvecB;
   Real angRad = 0.0;
   Real angDeg = 0.0;
   
   // if dot-product is less than or equal to tolerance, the angle is arccos of
   // the dot-product, otherwise, the angle is arcsin of the magnitude of the
   // cross-product.
   
   if (Abs(aDotB) <= Abs(tol))
   {
      angRad = ACos(aDotB);
      angDeg = DEG_PER_RAD * angRad;
   }
   else
   {
      // compute cross-product of two vectors
      Rvector3 aCrossB = Cross(uvecA, uvecB);
      Real crossMag = aCrossB.GetMagnitude();
      angRad = ASin(crossMag);

      if (aDotB < 0.0)
      {
         angRad = PI_OVER_TWO - angRad;
         angDeg = DEG_PER_RAD * angRad;
      }
   }
   
   return angDeg;
}
Beispiel #3
0
//------------------------------------------------------------------------------
void GeocentricSolarEclipticAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
                                                          bool forceComputation) 
{
   Rvector6 rvSun = secondary->GetMJ2000State(atEpoch) -
                    primary->GetMJ2000State(atEpoch);
   Rvector3 rSun  = rvSun.GetR();
   Rvector3 vSun  = rvSun.GetV();

   Real     rMag  = rSun.GetMagnitude();
   Real     vMag  = vSun.GetMagnitude();

   Rvector3 rUnit = rSun / rMag;
   Rvector3 vUnit = vSun / vMag;
   
   Rvector3 rxv   = Cross(rSun,vSun);
   Real     rxvM  = rxv.GetMagnitude();

   Rvector3 x     = rUnit;
   Rvector3 z     = rxv / rxvM;
   Rvector3 y     = Cross(z,x);

   rotMatrix(0,0) = x(0);
   rotMatrix(0,1) = y(0);
   rotMatrix(0,2) = z(0);
   rotMatrix(1,0) = x(1);
   rotMatrix(1,1) = y(1);
   rotMatrix(1,2) = z(1);
   rotMatrix(2,0) = x(2);
   rotMatrix(2,1) = y(2);
   rotMatrix(2,2) = z(2);

   Rvector3 vR    = vSun / rMag;
   Rvector3 xDot  = vR - x * (x * vR);
   Rvector3 zDot;     // zero vector by default
   Rvector3 yDot  = Cross(z, xDot);

   rotDotMatrix(0,0) = xDot(0);
   rotDotMatrix(0,1) = yDot(0);
   rotDotMatrix(0,2) = zDot(0);
   rotDotMatrix(1,0) = xDot(1);
   rotDotMatrix(1,1) = yDot(1);
   rotDotMatrix(1,2) = zDot(1);
   rotDotMatrix(2,0) = xDot(2);
   rotDotMatrix(2,1) = yDot(2);
   rotDotMatrix(2,2) = zDot(2);

}
Beispiel #4
0
//--------------------------------------------------------------------
void Structure::CalcCenter ()
// Find the center of the object
   {
   ZMinMax minmax;
   for (Integer ix=0;  ix<=Appendages.Size()-1;  ++ix)
      minmax.Broaden(Appendages[ix]->Body.MinMax());
   for (int i=0;  i<=3-1;  ++i)
      Center[i] = (minmax.Min.V[i]+minmax.Max.V[i])/2;
   Rvector3 size;
   for (int i=0;  i<=3-1;  ++i)
      //size[i] = std::max(Center[i]-minmax.Min.V[i],minmax.Max.V[i]-Center[i]);
      size[i] = GmatMathUtil::Max(Center[i]-minmax.Min.V[i],minmax.Max.V[i]-Center[i]);
   Radius = size.GetMagnitude();
   }
Beispiel #5
0
//--------------------------------------------------------------------
void SurfaceMesh::BuildNormals ()
// Create the normal vectors for the faces
   {
   for (Integer i=0;  i<=FacesSize-1;  ++i)
      {
      ZFace& face (Faces[i]);
      Rvector3 x = Vectors[face.VertexIndex[0]].WrtBody.ConvertToRvector3();
      Rvector3 y = Vectors[face.VertexIndex[1]].WrtBody.ConvertToRvector3();
      Rvector3 z = Vectors[face.VertexIndex[2]].WrtBody.ConvertToRvector3();
      Rvector3 xy = y-x;
      Rvector3 yz = z-y;
      Rvector3 n = Cross(xy,yz);
      if (n.GetMagnitude() != 0)
         n = n.Normalize();
      int index = MakeVector (n,false);
      for (Integer j=0;  j<=3-1;  ++j)
         face.NormalIndex[j] = index;
      }
   }
Beispiel #6
0
void NadirPointing::ComputeCosineMatrixAndAngularVelocity(Real atTime)
{
   //#ifdef DEBUG_NADIRPOINTING
   //#endif

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("In NadirPointing, computing at time %le\n",
            atTime);
      MessageInterface::ShowMessage("ModeOfConstraint = %s\n", modeOfConstraint.c_str());
      if (!owningSC) MessageInterface::ShowMessage("--- owningSC is NULL!!!!\n");
      if (!refBody) MessageInterface::ShowMessage("--- refBody is NULL!!!!\n");
   #endif

   if (!isInitialized || needsReinit)
   {
      Initialize();
      // Check for unset reference body pointer
      if (!refBody)
      {
         std::string attEx  = "Reference body ";
         attEx             += refBodyName + " not defined for attitude of type \"";
         attEx             += "NadirPointing\"";
         throw AttitudeException(attEx);
      }
   }

   /// using a spacecraft object, *owningSC
   A1Mjd theTime(atTime);

   Rvector6 scState = ((SpaceObject*) owningSC)->GetMJ2000State(theTime);
   Rvector6 refState = refBody->GetMJ2000State(theTime);
    
   Rvector3 pos(scState[0] - refState[0], scState[1] - refState[1],  scState[2] - refState[2]);
   Rvector3 vel(scState[3] - refState[3], scState[4] - refState[4],  scState[5] - refState[5]);

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("   scState  = %s\n", scState.ToString().c_str());
      MessageInterface::ShowMessage("   refState = %s\n", refState.ToString().c_str());
      MessageInterface::ShowMessage("   pos      = %s\n", pos.ToString().c_str());
      MessageInterface::ShowMessage("   vel      = %s\n", vel.ToString().c_str());
   #endif

   // Error message
   if ( bodyAlignmentVector.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("The size of BodyAlignmentVector is too small.");
   }
   else if ( bodyConstraintVector.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("The size of BodyConstraintVector is too small.");
   }
   else if ( bodyAlignmentVector.GetUnitVector()*bodyConstraintVector.GetUnitVector() > ( 1.0 - 1.0e-5) )
   {
	   throw AttitudeException("BodyAlignmentVector and BodyConstraintVector are the same.");
   }
   else if ( pos.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("A position vector is zero.");
   }
   else if ( vel.GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("A velocity vector is zero.");
   }
   else if ( Cross(pos,vel).GetMagnitude() < 1.0e-5 )
   {
	   throw AttitudeException("Cross product of position and velocity is zero: LVLH frame cannot be defined.");
   }
   
   Rvector3 normal = Cross(pos,vel);

   Rvector3 xhat = pos/pos.GetMagnitude();
   Rvector3 yhat = normal/normal.GetMagnitude();
   Rvector3 zhat = Cross(xhat,yhat);

   #ifdef DEBUG_NADIR_POINTING_COMPUTE
      MessageInterface::ShowMessage("   normal  = %s\n", normal.ToString().c_str());
      MessageInterface::ShowMessage("   xhat    = %s\n", xhat.ToString().c_str());
      MessageInterface::ShowMessage("   yhat    = %s\n", yhat.ToString().c_str());
      MessageInterface::ShowMessage("   zhat    = %s\n", zhat.ToString().c_str());
   #endif

   // RiI calculation (from inertial to LVLH)
   Rmatrix33 RIi;
   RIi(0,0) = xhat(0);
   RIi(1,0) = xhat(1);
   RIi(2,0) = xhat(2);
   RIi(0,1) = yhat(0);
   RIi(1,1) = yhat(1);
   RIi(2,1) = yhat(2);
   RIi(0,2) = zhat(0);
   RIi(1,2) = zhat(1);
   RIi(2,2) = zhat(2);
   
   // Set alignment/constraint vector in body frame
   if ( modeOfConstraint == "OrbitNormal" )
   {
	   referenceVector[0] = -1;
	   referenceVector[1] = 0;
	   referenceVector[2] = 0;

	   constraintVector[0] = 0;
	   constraintVector[1] = 1;
	   constraintVector[2] = 0;
   }
   else if ( modeOfConstraint == "VelocityConstraint" )
   {
	   referenceVector[0] = -1;
	   referenceVector[1] = 0;
	   referenceVector[2] = 0;

	   constraintVector[0] = 0;
	   constraintVector[1] = 0;
	   constraintVector[2] = 1;
   }

   // RBi calculation using TRIAD (from LVLH to body frame)
   Rmatrix33 RiB = TRIAD( bodyAlignmentVector, bodyConstraintVector, referenceVector, constraintVector );

   // the rotation matrix (from body to inertial)
   dcm = RIi*RiB;
   // the final rotation matrix (from inertial to body)
   dcm =  dcm.Transpose();

   // We do NOT calculate angular velocity for Nadir.
   // TODO : Throw AttitudeException??
   /*
   Rmatrix33 wxIBB = - RBi*RiIDot*dcm.Transpose();
   Rvector3 wIBB;
   wIBB.Set(wxIBB(2,1),wxIBB(0,2),wxIBB(1,0));
   */
}
Beispiel #7
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;
}
Beispiel #8
0
//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
                                                   bool forceComputation)
{
   if (!primary)
      throw CoordinateSystemException("Primary \"" + primaryName +
         "\" is not yet set in object referenced coordinate system!");

   if (!secondary)
      throw CoordinateSystemException("Secondary \"" + secondaryName +
         "\" is not yet set in object referenced coordinate system!");

   
   if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
   {
      CoordinateSystemException cse;
      cse.SetDetails("For object referenced axes, axes are improperly "
                     "defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
                     xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
      throw cse;
   }
   
   if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
   {
      CoordinateSystemException cse;
      cse.SetDetails("For object referenced axes, too many axes are defined.\n"
                     "XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
                     xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
      throw cse;
   }
   
   SpacePoint *useAsSecondary = secondary;
//   if (!useAsSecondary)  useAsSecondary = origin;
   Rvector6 rv     = useAsSecondary->GetMJ2000State(atEpoch) -
                     primary->GetMJ2000State(atEpoch);
   #ifdef DEBUG_ROT_MATRIX
      if (visitCount == 0)
      {
         MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
               primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
         visitCount++;
      }
   #endif

   #ifdef DEBUG_ROT_MATRIX
      if (visitCount == 0)
      {
         std::stringstream ss;
         ss.precision(30);
         ss << " ----------------- rv Earth to Moon (truncated)    = "
              << rv << std::endl;

         MessageInterface::ShowMessage("%s\n", ss.str().c_str());
         visitCount++;
      }
   #endif

   Rvector3 a     =  useAsSecondary->GetMJ2000Acceleration(atEpoch) -
                     primary->GetMJ2000Acceleration(atEpoch);
   
   Rvector3 r      = rv.GetR();
   Rvector3 v      = rv.GetV();
   Rvector3 n     =  Cross(r,v);
   Rvector3 rUnit = r.GetUnitVector();
   Rvector3 vUnit = v.GetUnitVector();
   Rvector3 nUnit = n.GetUnitVector();
   Real     rMag  = r.GetMagnitude();
   Real     vMag  = v.GetMagnitude();
   Real     nMag  = n.GetMagnitude();
   // check for divide-by-zero
   if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
   {
      std::string errmsg = "Object referenced axis system named \"";
      errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
      throw CoordinateSystemException(errmsg);
   }

   Rvector3 rDot  = (v / rMag) - (rUnit / rMag) * (rUnit * v);
   Rvector3 vDot  = (a / vMag) - (vUnit / vMag) * (vUnit * a);
   Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
   Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
   bool     xUsed = true, yUsed = true, zUsed = true;


   // determine the x-axis
   if ((xAxis == "R") || (xAxis == "r"))
   {
      xUnit = rUnit;
      xDot  = rDot;
   }
   else if ((xAxis == "-R") || (xAxis == "-r"))
   {
      xUnit = -rUnit;
      xDot  = -rDot;
   }
   else if ((xAxis == "V") || (xAxis == "v"))
   {
      xUnit = vUnit;
      xDot  = vDot;
   }
   else if ((xAxis == "-V") || (xAxis == "-v"))
   {
      xUnit = -vUnit;
      xDot  = -vDot;
   }
   else if ((xAxis == "N") || (xAxis == "n"))
   {
      xUnit = nUnit;
      xDot  = nDot;
   }
   else if ((xAxis == "-N") || (xAxis == "-n"))
   {
      xUnit = -nUnit;
      xDot  = -nDot;
   }
   else
   {
      xUsed = false;
   }
   // determine the y-axis
   if ((yAxis == "R") || (yAxis == "r"))
   {
      yUnit = rUnit;
      yDot  = rDot;
   }
   else if ((yAxis == "-R") || (yAxis == "-r"))
   {
      yUnit = -rUnit;
      yDot  = -rDot;
   }
   else if ((yAxis == "V") || (yAxis == "v"))
   {
      yUnit = vUnit;
      yDot  = vDot;
   }
   else if ((yAxis == "-V") || (yAxis == "-v"))
   {
      yUnit = -vUnit;
      yDot  = -vDot;
   }
   else if ((yAxis == "N") || (yAxis == "n"))
   {
      yUnit = nUnit;
      yDot  = nDot;
   }
   else if ((yAxis == "-N") || (yAxis == "-n"))
   {
      yUnit = -nUnit;
      yDot  = -nDot;
   }
   else
   {
      yUsed = false;
   }
   // determine the z-axis
   if ((zAxis == "R") || (zAxis == "r"))
   {
      zUnit = rUnit;
      zDot  = rDot;
   }
   else if ((zAxis == "-R") || (zAxis == "-r"))
   {
      zUnit = -rUnit;
      zDot  = -rDot;
   }
   else if ((zAxis == "V") || (zAxis == "v"))
   {
      zUnit = vUnit;
      zDot  = vDot;
   }
   else if ((zAxis == "-V") || (zAxis == "-v"))
   {
      zUnit = -vUnit;
      zDot  = -vDot;
   }
   else if ((zAxis == "N") || (zAxis == "n"))
   {
      zUnit = nUnit;
      zDot  = nDot;
   }
   else if ((zAxis == "-N") || (zAxis == "-n"))
   {
      zUnit = -nUnit;
      zDot  = -nDot;
   }
   else
   {
      zUsed = false;
   }
   // determine the third axis
   if (xUsed && yUsed && !zUsed)
   {
      zUnit = Cross(xUnit, yUnit);
      zDot  = Cross(xDot, yUnit) + Cross(xUnit, yDot);
   }
   else if (xUsed && zUsed && !yUsed)
   {
      yUnit = Cross(zUnit,xUnit);
      yDot  = Cross(zDot, xUnit) + Cross(zUnit, xDot);
   }
   else if (yUsed && zUsed && !xUsed)
   {
      xUnit = Cross(yUnit,zUnit);
      xDot  = Cross(yDot, zUnit) + Cross(yUnit, zDot);
   }
   else
   {
      throw CoordinateSystemException(
            "Object referenced axes are improperly defined.");
   }
   // Compute the rotation matrix
   rotMatrix(0,0) = xUnit(0);
   rotMatrix(0,1) = yUnit(0);
   rotMatrix(0,2) = zUnit(0);
   rotMatrix(1,0) = xUnit(1);
   rotMatrix(1,1) = yUnit(1);
   rotMatrix(1,2) = zUnit(1);
   rotMatrix(2,0) = xUnit(2);
   rotMatrix(2,1) = yUnit(2);
   rotMatrix(2,2) = zUnit(2);

   // Compute the rotation derivative matrix
   rotDotMatrix(0,0) = xDot(0);
   rotDotMatrix(0,1) = yDot(0);
   rotDotMatrix(0,2) = zDot(0);
   rotDotMatrix(1,0) = xDot(1);
   rotDotMatrix(1,1) = yDot(1);
   rotDotMatrix(1,2) = zDot(1);
   rotDotMatrix(2,0) = xDot(2);
   rotDotMatrix(2,1) = yDot(2);
   rotDotMatrix(2,2) = zDot(2);

   #ifdef DEBUG_ROT_MATRIX
      MessageInterface::ShowMessage
         ("rotMatrix=%s\n", rotMatrix.ToString().c_str());

      std::stringstream ss;

      ss.setf(std::ios::fixed);
      ss.precision(30);
      ss << " ----------------- rotMatrix    = " << rotMatrix << std::endl;
      ss.setf(std::ios::scientific);
      ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;

      MessageInterface::ShowMessage("%s\n", ss.str().c_str());
   #endif

   if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
   {
      std::stringstream errmsg("");
      errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
      errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
   }
}
Beispiel #9
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;
}
Beispiel #10
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;
}
Beispiel #11
0
//---------------------------------------------------------------------------
float Ionosphere::ElectronDensity(Rvector3 pos2, Rvector3 pos1)
{
   // the fisrt position's latitude and longitude (unit: degree):
   //real latitude = (real)(GmatMathConstants::PI_OVER_TWO_DEG - acos(pos1.Get(2)/pos1.GetMagnitude())*GmatMathConstants::DEG_PER_RAD);	// unit: degree
   real latitude = (real)(asin(pos1.Get(2)/pos1.GetMagnitude())*GmatMathConstants::DEG_PER_RAD);	// unit: degree
   real longitude = (real)(atan2(pos1.Get(1),pos1.Get(0))*GmatMathConstants::DEG_PER_RAD);			// unit: degree
   
   // mmag  = 0 geographic   =1 geomagnetic coordinates
   integer jmag = 0;	// 1;
   
   // jf(1:30)     =.true./.false. flags; explained in IRISUB.FOR
   logical jf[31];
   for (int i=1; i <= 30; ++i)
      jf[i] = TRUE_;
   
   jf[5] = FALSE_;
   jf[6] = FALSE_;
   jf[23] = FALSE_;
   jf[29] = FALSE_;
   jf[30] = FALSE_;
   
//   jf[21] = FALSE_;
//   jf[28] = FALSE_;
   
   // iy,md        date as yyyy and mmdd (or -ddd)
   // hour         decimal hours LT (or UT+25)
   integer iy = (integer)yyyy;
   integer md = (integer)mmdd;
   real hour = (real)hours;
   
   // Upper and lower integration limits
   real hbeg = (real)(pos1.GetMagnitude() - earthRadius); // 0
   real hend = hbeg;
   real hstp = 1.0;
   
   integer error = 0;

   real outf[20*501+1];
   real oarr[51];


//   iri_sub(&jf[1], &jmag, &latitude, &longitude, &iy, &md, &hour,
//           &hbeg, &hend, &hstp, &outf[21], &oarr[1], &error);

   integer ivar = 1;		// get attitude result
   integer iut = 1;			// 1 for universal time; 0 for local time

# ifdef DEBUG_IONOSPHERE_ELECT_DENSITY
   MessageInterface::ShowMessage("           .At time = %lf A1Mjd:",epoch);
   MessageInterface::ShowMessage("         year = %d   md = %d   hour = %lf h,   time type = %s,\n", iy, md, hour, (iut?"Universal":"Local"));
   MessageInterface::ShowMessage("              At position (x,y,z) = (%lf,  %lf,  %lf)km in Earth fixed coordinate system: ", pos1[0], pos1[1], pos1[2]);
   MessageInterface::ShowMessage("(latitude = %lf degree,  longitude = %lf degree,  attitude = %lf km,  ", latitude, longitude, hbeg);
   MessageInterface::ShowMessage("coordinate system type = %s)\n",(jmag?"Geomagetic":"Geographic"));
#endif
   iri_web(&jmag, &jf[1], &latitude, &longitude, &iy, &md, &iut, &hour, &hbeg, &hbeg, 
	   &ivar, &hbeg, &hend, &hstp, &outf[21], &oarr[1], &error);

   if (error != 0)
   {
      MessageInterface::ShowMessage("Ionosphere data files not found\n");
      throw new MeasurementException("Ionosphere data files not found\n");
   }

   real density = outf[20+1];
   if (density < 0)
      density = 0;

#ifdef DEBUG_IONOSPHERE_ELECT_DENSITY
   MessageInterface::ShowMessage("              Electron density at that time and location = %le electrons per m3.\n", density);
#endif

   return density;         //*(pos2-pos1).GetMagnitude();
}
Beispiel #12
0
//------------------------------------------------------------------------------
Integer CoordUtil::ComputeCartToKepl(Real grav, Real r[3], Real v[3], Real *tfp,
                                     Real elem[6], Real *ma)
{
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(wxT("CoordUtil::ComputeCartToKepl called ... \n"));
      MessageInterface::ShowMessage(wxT("   grav = %12.10f\n"), grav);
      MessageInterface::ShowMessage(wxT("   KEP_ECC_TOL = %12.10f\n"), GmatOrbitConstants::KEP_ECC_TOL);
   #endif

   if (Abs(grav) < 1E-30)
      return(2);
   
   Rvector3 pos(r[0], r[1], r[2]);
   Rvector3 vel(v[0], v[1], v[2]);
   
   // eqn 4.1
   Rvector3 angMomentum = Cross(pos, vel);
   
   // eqn 4.2
   Real h = angMomentum.GetMagnitude();
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, pos = %12.10f  %12.10f  %12.10f \n"), pos[0], pos[1], pos[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, vel = %12.10f  %12.10f  %12.10f \n"), vel[0], vel[1], vel[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, angMomentum = %12.10f  %12.10f  %12.10f\n"),
            angMomentum[0], angMomentum[1], angMomentum[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, h = %12.10f\n"), h);
   #endif
   
//   if (h < 1E-30)
//   {
//      return 1;
//   }
   
   // eqn 4.3
   Rvector3 nodeVec = Cross(Rvector3(0,0,1), angMomentum);
   
   // eqn 4.4
   Real n = nodeVec.GetMagnitude();
   
   // eqn 4.5 - 4.6
   Real posMag = pos.GetMagnitude();
   Real velMag = vel.GetMagnitude();
   
   // eqn 4.7 - 4.8
   Rvector3 eccVec = (1/grav)*((velMag*velMag - grav/posMag)*pos - (pos * vel) * vel);
   Real e = eccVec.GetMagnitude();
   
   // eqn 4.9
   Real zeta = 0.5*velMag*velMag - grav/posMag;
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, nodeVec = %12.10f  %12.10f  %12.10f \n"),
            nodeVec[0], nodeVec[1], nodeVec[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, n = %12.10f\n"), n);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, posMag = %12.10f\n"), posMag);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, velMag = %12.10f\n"), velMag);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, eccVec = %12.10f  %12.10f  %12.10f \n"), eccVec[0], eccVec[1], eccVec[2]);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, e = %12.10f\n"), e);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, zeta = %12.10f\n"), zeta);
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, Abs(1.0 - e) = %12.10f\n"), Abs(1.0 - e));
   #endif
   
   if ((Abs(1.0 - e)) <= GmatOrbitConstants::KEP_ECC_TOL)
   {
      wxString errmsg = wxT("Error in conversion to Keplerian state: ");
      errmsg += wxT("The state results in an orbit that is nearly parabolic.\n");
      throw UtilityException(errmsg);
   } 
   
   // eqn 4.10
   Real sma = -grav/(2*zeta);
   #ifdef DEBUG_CART_TO_KEPL
      MessageInterface::ShowMessage(
            wxT("   in ComputeCartToKepl, sma = %12.10f\n"), sma);
   #endif


   if (Abs(sma*(1 - e)) < .001)
   {
      throw UtilityException
      (wxT("Error in conversion from Cartesian to Keplerian state: ")
       wxT("The state results in a singular conic section with radius of periapsis less than 1 m.\n"));
//         (wxT("CoordUtil::CartesianToKeplerian() ")
//          wxT("Warning: A nearly singular conic section was encountered while ")
//          wxT("converting from the Cartesian state to the Keplerian elements.  The radius of ")
//          wxT("periapsis must be greater than 1 meter.\n"));

   }
   // eqn 4.11
   Real i = ACos( angMomentum.Get(2)/h );
   if (i >= PI - GmatOrbitConstants::KEP_TOL)
   {
      throw UtilityException
         (wxT("Error in conversion to Keplerian state: ")
          wxT("GMAT does not currently support orbits with inclination of 180 degrees.\n"));
   } 
   Real raan, argPeriapsis, trueAnom;
   raan=argPeriapsis=trueAnom=0;
   if ( e >= 1E-11 && i >= 1E-11 )  // CASE 1: Non-circular, Inclined Orbit
   {
      raan = ACos( nodeVec.Get(0)/n );
      if (nodeVec.Get(1) < 0)
         raan = TWO_PI - raan;
           
      argPeriapsis = ACos( (nodeVec*eccVec)/(n*e) );
      if (eccVec.Get(2) < 0)
         argPeriapsis = TWO_PI - argPeriapsis;
        
      trueAnom = ACos( (eccVec*pos)/(e*posMag) );
      if (pos*vel < 0)
         trueAnom = TWO_PI - trueAnom;
   }
   if ( e >= 1E-11 && i < 1E-11 )  // CASE 2: Non-circular, Equatorial Orbit
   {
      raan = 0;
      argPeriapsis = ACos(eccVec.Get(0)/e);
      if (eccVec.Get(1) < 0)
         argPeriapsis = TWO_PI - argPeriapsis;
           
      trueAnom = ACos( (eccVec*pos)/(e*posMag) );
      if (pos*vel < 0)
         trueAnom = TWO_PI - trueAnom;
   }
   if ( e < 1E-11 && i >= 1E-11 )  // CASE 3: Circular, Inclined Orbit
   {
      raan = ACos( nodeVec.Get(0)/n );
      if (nodeVec.Get(1) < 0)
         raan = TWO_PI - raan;
           
      argPeriapsis = 0;
        
      trueAnom = ACos( (nodeVec*pos)/(n*posMag) );
      if (pos.Get(2) < 0)
         trueAnom = TWO_PI - trueAnom;
   }
   if ( e < 1E-11 && i < 1E-11 )  // CASE 4: Circular, Equatorial Orbit
   {
      raan = 0;
      argPeriapsis = 0;
      trueAnom = ACos(pos.Get(0)/posMag);
      if (pos.Get(1) < 0)
         trueAnom = TWO_PI - trueAnom;
   }
   
   elem[0] = sma;
   elem[1] = e;
   elem[2] = i*DEG_PER_RAD;
   elem[3] = raan*DEG_PER_RAD;
   elem[4] = argPeriapsis*DEG_PER_RAD;
   elem[5] = trueAnom*DEG_PER_RAD;
 
   return 0;  
}