68 #include "FGPropagate.h"
69 #include "initialization/FGInitialCondition.h"
70 #include "FGFDMExec.h"
71 #include "simgear/io/iostreams/sgstream.hxx"
72 #include "FGInertial.h"
93 integrator_rotational_rate = eRectEuler;
94 integrator_translational_rate = eAdamsBashforth2;
95 integrator_rotational_position = eRectEuler;
96 integrator_translational_position = eAdamsBashforth3;
120 if (!FGModel::InitModel())
return false;
124 Inertial->SetAltitudeAGL(VState.
vLocation, 4.0);
131 integrator_rotational_rate = eRectEuler;
132 integrator_translational_rate = eAdamsBashforth2;
133 integrator_rotational_position = eRectEuler;
134 integrator_translational_position = eAdamsBashforth3;
151 Ti2ec = { cos(epa), sin(epa), 0.0,
152 -sin(epa), cos(epa), 0.0,
156 VState.vInertialPosition = Tec2i * VState.
vLocation;
158 UpdateLocationMatrices();
166 UpdateBodyMatrices();
172 vVel = Tb2l * VState.
vUVW;
175 RecomputeLocalTerrainVelocity();
181 VState.
vPQRi = VState.
vPQR + Ti2b * in.vOmegaPlanet;
183 CalculateInertialVelocity();
190 void FGPropagate::InitializeDerivatives()
192 VState.dqPQRidot.assign(5, in.vPQRidot);
193 VState.dqUVWidot.assign(5, in.vUVWidot);
194 VState.dqInertialVelocity.assign(5, VState.vInertialVelocity);
195 VState.dqQtrndot.assign(5, VState.vQtrndot);
221 if (Holding)
return false;
223 double dt = in.DeltaT * rate;
228 Integrate(VState.
qAttitudeECI, VState.vQtrndot, VState.dqQtrndot, dt, integrator_rotational_position);
229 Integrate(VState.
vPQRi, in.vPQRidot, VState.dqPQRidot, dt, integrator_rotational_rate);
230 Integrate(VState.vInertialPosition, VState.vInertialVelocity, VState.dqInertialVelocity, dt, integrator_translational_position);
231 Integrate(VState.vInertialVelocity, in.vUVWidot, VState.dqUVWidot, dt, integrator_translational_rate);
239 epa += in.vOmegaPlanet(eZ)*dt;
242 double cos_epa = cos(epa);
243 double sin_epa = sin(epa);
244 Ti2ec = { cos_epa, sin_epa, 0.0,
245 -sin_epa, cos_epa, 0.0,
250 VState.
vLocation = Ti2ec*VState.vInertialPosition;
254 UpdateLocationMatrices();
258 UpdateBodyMatrices();
265 RecomputeLocalTerrainVelocity();
267 VState.
vPQR = VState.
vPQRi - Ti2b * in.vOmegaPlanet;
276 vVel = Tb2l * VState.
vUVW;
279 ComputeOrbitalParameters();
290 VState.
vUVW.InitMatrix();
291 CalculateInertialVelocity();
292 VState.
vPQR.InitMatrix();
293 VState.
vPQRi = Ti2b * in.vOmegaPlanet;
295 InitializeDerivatives();
306 void FGPropagate::CalculateQuatdot(
void)
319 void FGPropagate::CalculateInertialVelocity(
void)
321 VState.vInertialVelocity = Tb2i * VState.
vUVW + (in.vOmegaPlanet * VState.vInertialPosition);
329 void FGPropagate::CalculateUVW(
void)
331 VState.
vUVW = Ti2b * (VState.vInertialVelocity - (in.vOmegaPlanet * VState.vInertialPosition));
336 void FGPropagate::Integrate( FGColumnVector3& Integrand,
337 FGColumnVector3& Val,
338 deque <FGColumnVector3>& ValDot,
340 eIntegrateType integration_type)
342 ValDot.push_front(Val);
345 switch(integration_type) {
346 case eRectEuler: Integrand += dt*ValDot[0];
348 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
350 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
352 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
354 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
356 case eAdamsBashforth5: Integrand += dt*((1901./720.)*ValDot[0] - (1387./360.)*ValDot[1] + (109./30.)*ValDot[2] - (637./360.)*ValDot[3] + (251./720.)*ValDot[4]);
362 case eLocalLinearization:
363 throw(
"Can only use Buss (1 & 2) or local linearization integration methods in for rotational position!");
371 void FGPropagate::Integrate( FGQuaternion& Integrand,
373 deque <FGQuaternion>& ValDot,
375 eIntegrateType integration_type)
377 ValDot.push_front(Val);
380 switch(integration_type) {
381 case eRectEuler: Integrand += dt*ValDot[0];
383 case eTrapezoidal: Integrand += 0.5*dt*(ValDot[0] + ValDot[1]);
385 case eAdamsBashforth2: Integrand += dt*(1.5*ValDot[0] - 0.5*ValDot[1]);
387 case eAdamsBashforth3: Integrand += (1/12.0)*dt*(23.0*ValDot[0] - 16.0*ValDot[1] + 5.0*ValDot[2]);
389 case eAdamsBashforth4: Integrand += (1/24.0)*dt*(55.0*ValDot[0] - 59.0*ValDot[1] + 37.0*ValDot[2] - 9.0*ValDot[3]);
391 case eAdamsBashforth5: Integrand += dt*((1901./720.)*ValDot[0] - (1387./360.)*ValDot[1] + (109./30.)*ValDot[2] - (637./360.)*ValDot[3] + (251./720.)*ValDot[4]);
399 Integrand = Integrand * QExp(0.5 * dt * VState.
vPQRi);
407 FGColumnVector3 wi = VState.
vPQRi;
408 FGColumnVector3 wdoti = in.vPQRidot;
409 FGColumnVector3 omega = wi + 0.5*dt*wdoti + dt*dt/12.*wdoti*wi;
410 Integrand = Integrand * QExp(0.5 * dt * omega);
413 case eLocalLinearization:
419 FGColumnVector3 wi = 0.5 * VState.
vPQRi;
420 FGColumnVector3 wdoti = 0.5 * in.vPQRidot;
421 double omegak2 = DotProduct(VState.
vPQRi, VState.
vPQRi);
422 double omegak = omegak2 > 1E-6 ? sqrt(omegak2) : 1E-6;
423 double rhok = 0.5 * dt * omegak;
424 double C1 = cos(rhok);
425 double C2 = 2.0 * sin(rhok) / omegak;
426 double C3 = 4.0 * (1.0 - C1) / (omegak*omegak);
427 double C4 = 4.0 * (dt - C2) / (omegak*omegak);
428 FGColumnVector3 Omega = C2*wi + C3*wdoti + C4*wi*wdoti;
431 q(1) = C1 - C4*DotProduct(wi, wdoti);
436 Integrand = Integrand * q;
470 Integrand.Normalize();
475 void FGPropagate::UpdateLocationMatrices(
void)
479 Ti2l = Tec2l * Ti2ec;
485 void FGPropagate::UpdateBodyMatrices(
void)
491 Tec2b = Ti2b * Tec2i;
499 void FGPropagate::ComputeOrbitalParameters(
void)
501 const FGColumnVector3 Z{0., 0., 1.};
502 FGColumnVector3 R = VState.vInertialPosition;
503 FGColumnVector3 angularMomentum = R * VState.vInertialVelocity;
504 h = angularMomentum.Magnitude();
505 Inclination = acos(angularMomentum(eZ)/h)*radtodeg;
507 if (abs(Inclination) > 1E-8) {
508 N = Z * angularMomentum;
509 RightAscension = atan2(N(eY), N(eX))*radtodeg;
513 RightAscension = 0.0;
517 double vr = DotProduct(R, VState.vInertialVelocity);
518 FGColumnVector3 eVector = (VState.vInertialVelocity*angularMomentum/in.GM - R);
519 Eccentricity = eVector.Magnitude();
520 if (Eccentricity > 1E-8) {
521 eVector /= Eccentricity;
522 PerigeeArgument = acos(DotProduct(N, eVector))*radtodeg;
523 if (eVector(eZ) < 0) PerigeeArgument = 360. - PerigeeArgument;
527 eVector = {1., 0., 0.};
528 PerigeeArgument = 0.0;
531 TrueAnomaly = acos(
Constrain(-1.0, DotProduct(eVector, R), 1.0)) * radtodeg;
532 if (vr < 0.0) TrueAnomaly = 360. - TrueAnomaly;
533 ApoapsisRadius = h*h / (in.GM * (1-Eccentricity));
534 PeriapsisRadius = h*h / (in.GM * (1+Eccentricity));
536 if (Eccentricity < 1.0) {
537 double semimajor = 0.5*(ApoapsisRadius + PeriapsisRadius);
538 OrbitalPeriod = 2.*M_PI*pow(semimajor, 1.5)/sqrt(in.GM);
546 void FGPropagate::SetInertialOrientation(
const FGQuaternion& Qi)
550 UpdateBodyMatrices();
557 void FGPropagate::SetInertialVelocity(
const FGColumnVector3& Vi) {
558 VState.vInertialVelocity = Vi;
560 vVel = Tb2l * VState.
vUVW;
565 void FGPropagate::SetInertialRates(
const FGColumnVector3& vRates) {
566 VState.
vPQRi = Ti2b * vRates;
567 VState.
vPQR = VState.
vPQRi - Ti2b * in.vOmegaPlanet;
580 void FGPropagate::SetAltitudeASL(
double altASL)
584 UpdateVehicleState();
589 void FGPropagate::RecomputeLocalTerrainVelocity()
592 FGColumnVector3 normal;
593 Inertial->GetContactPoint(VState.
vLocation, contact, normal,
594 LocalTerrainVelocity, LocalTerrainAngularVelocity);
599 double FGPropagate::GetTerrainElevation(
void)
const
601 FGColumnVector3 vDummy;
603 contact.SetEllipse(in.SemiMajor, in.SemiMinor);
604 Inertial->GetContactPoint(VState.
vLocation, contact, vDummy, vDummy, vDummy);
605 return contact.GetGeodAltitude();
610 void FGPropagate::SetTerrainElevation(
double terrainElev)
612 Inertial->SetTerrainElevation(terrainElev);
621 Inertial->GetContactPoint(VState.
vLocation, contact, vDummy, vDummy, vDummy);
627 double FGPropagate::GetDistanceAGL(
void)
const
629 return Inertial->GetAltitudeAGL(VState.
vLocation);
634 double FGPropagate::GetDistanceAGLKm(
void)
const
636 return GetDistanceAGL()*0.0003048;
641 void FGPropagate::SetDistanceAGL(
double tt)
643 Inertial->SetAltitudeAGL(VState.
vLocation, tt);
644 UpdateVehicleState();
649 void FGPropagate::SetDistanceAGLKm(
double tt)
651 SetDistanceAGL(tt*3280.8399);
656 void FGPropagate::SetVState(
const VehicleState& vstate)
660 UpdateLocationMatrices();
661 SetInertialOrientation(vstate.qAttitudeECI);
662 RecomputeLocalTerrainVelocity();
663 VState.
vUVW = vstate.vUVW;
664 vVel = Tb2l * VState.
vUVW;
665 VState.
vPQR = vstate.vPQR;
666 VState.
vPQRi = VState.
vPQR + Ti2b * in.vOmegaPlanet;
667 VState.vInertialPosition = vstate.vInertialPosition;
673 void FGPropagate::UpdateVehicleState(
void)
675 RecomputeLocalTerrainVelocity();
676 VState.vInertialPosition = Tec2i * VState.
vLocation;
677 UpdateLocationMatrices();
678 UpdateBodyMatrices();
679 vVel = Tb2l * VState.
vUVW;
685 void FGPropagate::SetLocation(
const FGLocation& l)
688 UpdateVehicleState();
700 void FGPropagate::DumpState(
void)
704 <<
"------------------------------------------------------------------" <<
reset << endl;
706 <<
"State Report at sim time: " << FDMExec->
GetSimTime() <<
" seconds" <<
reset << endl;
709 cout <<
" ECI: " << VState.vInertialPosition.
Dump(
", ") <<
" (x,y,z, in ft)" << endl;
710 cout <<
" ECEF: " << VState.
vLocation <<
" (x,y,z, in ft)" << endl;
713 <<
", " <<
GetAltitudeASL() <<
" (geodetic lat, lon, alt ASL in deg and ft)" << endl;
716 <<
"Orientation" <<
underoff << endl;
722 cout <<
" ECI: " << VState.vInertialVelocity.
Dump(
", ") <<
" (x,y,z in ft/s)" << endl;
723 cout <<
" ECEF: " << (Tb2ec * VState.
vUVW).Dump(
", ") <<
" (x,y,z in ft/s)" << endl;
724 cout <<
" Local: " <<
GetVel() <<
" (n,e,d in ft/sec)" << endl;
725 cout <<
" Body: " <<
GetUVW() <<
" (u,v,w in ft/sec)" << endl;
728 <<
"Body Rates (relative to given frame, expressed in body frame)" <<
underoff << endl;
729 cout <<
" ECI: " << (VState.
vPQRi*radtodeg).Dump(
", ") <<
" (p,q,r in deg/s)" << endl;
730 cout <<
" ECEF: " << (VState.
vPQR*radtodeg).Dump(
", ") <<
" (p,q,r in deg/s)" << endl;
735 void FGPropagate::WriteStateFile(
int num)
739 if (num == 0)
return;
743 if (path.isNull()) path = SGPath(
"initfile.");
744 else path.append(
"initfile.");
747 path.concat(to_string((
double)FDMExec->
GetSimTime())+
".xml");
752 if (outfile.is_open()) {
753 outfile <<
"<?xml version=\"1.0\"?>" << endl;
754 outfile <<
"<initialize name=\"reset00\">" << endl;
755 outfile <<
" <ubody unit=\"FT/SEC\"> " << VState.
vUVW(eU) <<
" </ubody> " << endl;
756 outfile <<
" <vbody unit=\"FT/SEC\"> " << VState.
vUVW(eV) <<
" </vbody> " << endl;
757 outfile <<
" <wbody unit=\"FT/SEC\"> " << VState.
vUVW(eW) <<
" </wbody> " << endl;
759 outfile <<
" <theta unit=\"DEG\"> " << VState.
qAttitudeLocal.
GetEuler(eTht)*radtodeg <<
" </theta>" << endl;
763 outfile <<
" <altitude unit=\"FT\"> " << GetDistanceAGL() <<
" </altitude>" << endl;
764 outfile <<
"</initialize>" << endl;
767 cerr <<
"Could not open and/or write the state to the initial conditions file: "
773 if (outfile.is_open()) {
774 outfile <<
"<?xml version=\"1.0\"?>" << endl;
775 outfile <<
"<initialize name=\"IC File\" version=\"2.0\">" << endl;
776 outfile <<
"" << endl;
777 outfile <<
" <position frame=\"ECEF\">" << endl;
780 outfile <<
" <altitudeMSL unit=\"FT\"> " <<
GetAltitudeASL() <<
" </altitudeMSL>" << endl;
781 outfile <<
" </position>" << endl;
782 outfile <<
"" << endl;
783 outfile <<
" <orientation unit=\"DEG\" frame=\"LOCAL\">" << endl;
787 outfile <<
" </orientation>" << endl;
788 outfile <<
"" << endl;
789 outfile <<
" <velocity unit=\"FT/SEC\" frame=\"LOCAL\">" << endl;
790 outfile <<
" <x> " <<
GetVel(eNorth) <<
" </x>" << endl;
791 outfile <<
" <y> " <<
GetVel(eEast) <<
" </y>" << endl;
792 outfile <<
" <z> " <<
GetVel(eDown) <<
" </z>" << endl;
793 outfile <<
" </velocity>" << endl;
794 outfile <<
"" << endl;
795 outfile <<
" <attitude_rate unit=\"DEG/SEC\" frame=\"BODY\">" << endl;
796 outfile <<
" <roll> " << (VState.
vPQR*radtodeg)(eRoll) <<
" </roll>" << endl;
797 outfile <<
" <pitch> " << (VState.
vPQR*radtodeg)(ePitch) <<
" </pitch>" << endl;
798 outfile <<
" <yaw> " << (VState.
vPQR*radtodeg)(eYaw) <<
" </yaw>" << endl;
799 outfile <<
" </attitude_rate>" << endl;
800 outfile <<
"" << endl;
801 outfile <<
"</initialize>" << endl;
804 cerr <<
"Could not open and/or write the state to the initial conditions file: "
809 cerr <<
"When writing a state file, the supplied value must be 1 or 2 for the version number of the resulting IC file" << endl;
815 void FGPropagate::bind(
void)
847 PropertyManager->Tie(
"position/lat-gc-rad",
this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
848 PropertyManager->Tie(
"position/long-gc-rad",
this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
849 PropertyManager->Tie(
"position/lat-gc-deg",
this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
850 PropertyManager->Tie(
"position/long-gc-deg",
this, &FGPropagate::GetLongitudeDeg, &FGPropagate::SetLongitudeDeg);
851 PropertyManager->Tie(
"position/lat-geod-rad",
this, &FGPropagate::GetGeodLatitudeRad);
852 PropertyManager->Tie(
"position/lat-geod-deg",
this, &FGPropagate::GetGeodLatitudeDeg);
853 PropertyManager->Tie(
"position/geod-alt-ft",
this, &FGPropagate::GetGeodeticAltitude);
854 PropertyManager->Tie(
"position/h-agl-ft",
this, &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
855 PropertyManager->Tie(
"position/geod-alt-km",
this, &FGPropagate::GetGeodeticAltitudeKm);
856 PropertyManager->Tie(
"position/h-agl-km",
this, &FGPropagate::GetDistanceAGLKm, &FGPropagate::SetDistanceAGLKm);
857 PropertyManager->Tie(
"position/radius-to-vehicle-ft",
this, &FGPropagate::GetRadius);
858 PropertyManager->Tie(
"position/terrain-elevation-asl-ft",
this,
859 &FGPropagate::GetTerrainElevation,
860 &FGPropagate::SetTerrainElevation);
866 PropertyManager->Tie(
"position/ecef-x-ft",
this, eX, (PMF)&FGPropagate::GetLocation);
867 PropertyManager->Tie(
"position/ecef-y-ft",
this, eY, (PMF)&FGPropagate::GetLocation);
868 PropertyManager->Tie(
"position/ecef-z-ft",
this, eZ, (PMF)&FGPropagate::GetLocation);
885 PropertyManager->Tie(
"orbital/specific-angular-momentum-ft2_sec", &h);
886 PropertyManager->Tie(
"orbital/inclination-deg", &Inclination);
887 PropertyManager->Tie(
"orbital/right-ascension-deg", &RightAscension);
888 PropertyManager->Tie(
"orbital/eccentricity", &Eccentricity);
889 PropertyManager->Tie(
"orbital/argument-of-perigee-deg", &PerigeeArgument);
890 PropertyManager->Tie(
"orbital/true-anomaly-deg", &TrueAnomaly);
891 PropertyManager->Tie(
"orbital/apoapsis-radius-ft", &ApoapsisRadius);
892 PropertyManager->Tie(
"orbital/periapsis-radius-ft", &PeriapsisRadius);
893 PropertyManager->Tie(
"orbital/period-sec", &OrbitalPeriod);
895 PropertyManager->Tie(
"simulation/integrator/rate/rotational", (
int*)&integrator_rotational_rate);
896 PropertyManager->Tie(
"simulation/integrator/rate/translational", (
int*)&integrator_translational_rate);
897 PropertyManager->Tie(
"simulation/integrator/position/rotational", (
int*)&integrator_rotational_position);
898 PropertyManager->Tie(
"simulation/integrator/position/translational", (
int*)&integrator_translational_position);
900 PropertyManager->Tie(
"simulation/write-state-file",
this, (iPMF)0, &FGPropagate::WriteStateFile);
922 void FGPropagate::Debug(
int from)
924 if (debug_lvl <= 0)
return;
931 if (debug_lvl & 2 ) {
932 if (from == 0) cout <<
"Instantiated: FGPropagate" << endl;
933 if (from == 1) cout <<
"Destroyed: FGPropagate" << endl;
935 if (debug_lvl & 4 ) {
937 if (debug_lvl & 8 && from == 2) {
939 <<
" Propagation Report (English units: ft, degrees) at simulation time " << FDMExec->
GetSimTime() <<
" seconds"
942 cout <<
highint <<
" Earth Position Angle (deg): " << setw(8) << setprecision(3) <<
reset
945 cout <<
highint <<
" Body velocity (ft/sec): " << setw(8) << setprecision(3) <<
reset << VState.
vUVW << endl;
946 cout <<
highint <<
" Local velocity (ft/sec): " << setw(8) << setprecision(3) <<
reset << vVel << endl;
947 cout <<
highint <<
" Inertial velocity (ft/sec): " << setw(8) << setprecision(3) <<
reset << VState.vInertialVelocity << endl;
948 cout <<
highint <<
" Inertial Position (ft): " << setw(10) << setprecision(3) <<
reset << VState.vInertialPosition << endl;
954 cout <<
highint <<
" Matrix ECEF to Body (Orientation of Body with respect to ECEF): "
955 <<
reset << endl << Tec2b.
Dump(
"\t",
" ") << endl;
956 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
960 cout <<
highint <<
" Matrix Body to ECEF (Orientation of ECEF with respect to Body):"
961 <<
reset << endl << Tb2ec.
Dump(
"\t",
" ") << endl;
962 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
966 cout <<
highint <<
" Matrix Local to Body (Orientation of Body with respect to Local):"
967 <<
reset << endl << Tl2b.
Dump(
"\t",
" ") << endl;
968 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
972 cout <<
highint <<
" Matrix Body to Local (Orientation of Local with respect to Body):"
973 <<
reset << endl << Tb2l.
Dump(
"\t",
" ") << endl;
974 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
978 cout <<
highint <<
" Matrix Local to ECEF (Orientation of ECEF with respect to Local):"
979 <<
reset << endl << Tl2ec.
Dump(
"\t",
" ") << endl;
980 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
984 cout <<
highint <<
" Matrix ECEF to Local (Orientation of Local with respect to ECEF):"
985 <<
reset << endl << Tec2l.
Dump(
"\t",
" ") << endl;
986 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
990 cout <<
highint <<
" Matrix ECEF to Inertial (Orientation of Inertial with respect to ECEF):"
991 <<
reset << endl << Tec2i.Dump(
"\t",
" ") << endl;
992 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
993 << setprecision(3) <<
reset << (Tec2i.GetQuaternion().GetEuler()*radtodeg)
996 cout <<
highint <<
" Matrix Inertial to ECEF (Orientation of ECEF with respect to Inertial):"
997 <<
reset << endl << Ti2ec.Dump(
"\t",
" ") << endl;
998 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
999 << setprecision(3) <<
reset << (Ti2ec.GetQuaternion().GetEuler()*radtodeg)
1002 cout <<
highint <<
" Matrix Inertial to Body (Orientation of Body with respect to Inertial):"
1003 <<
reset << endl << Ti2b.
Dump(
"\t",
" ") << endl;
1004 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
1008 cout <<
highint <<
" Matrix Body to Inertial (Orientation of Inertial with respect to Body):"
1009 <<
reset << endl << Tb2i.
Dump(
"\t",
" ") << endl;
1010 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
1014 cout <<
highint <<
" Matrix Inertial to Local (Orientation of Local with respect to Inertial):"
1015 <<
reset << endl << Ti2l.
Dump(
"\t",
" ") << endl;
1016 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
1020 cout <<
highint <<
" Matrix Local to Inertial (Orientation of Inertial with respect to Local):"
1021 <<
reset << endl << Tl2i.Dump(
"\t",
" ") << endl;
1022 cout <<
highint <<
" Associated Euler angles (deg): " << setw(8)
1023 << setprecision(3) <<
reset << (Tl2i.GetQuaternion().GetEuler()*radtodeg)
1026 cout << setprecision(6);
1028 if (debug_lvl & 16) {
1032 s <<
"Vehicle rotation rate is excessive (>1000 rad/sec): " << VState.
vPQR.
Magnitude();
1033 cerr << endl << s.str() << endl;
1034 throw BaseException(s.str());
1038 s <<
"Vehicle velocity is excessive (>1e10 ft/sec): " << VState.
vUVW.
Magnitude();
1039 cerr << endl << s.str() << endl;
1040 throw BaseException(s.str());
1042 if (fabs(GetDistanceAGL()) > 1e10) {
1044 s <<
"Vehicle altitude is excessive (>1e10 ft): " << GetDistanceAGL();
1045 cerr << endl << s.str() << endl;
1046 throw BaseException(s.str());
1050 if (debug_lvl & 64) {
This class implements a 3 element column vector.
double Magnitude(void) const
Length of the vector.
std::string Dump(const std::string &delimeter) const
Prints the contents of the vector.
Encapsulates the JSBSim simulation executive.
bool IntegrationSuspended(void) const
Returns the simulation suspension state.
double GetSimTime(void) const
Returns the cumulative simulation time in seconds.
std::shared_ptr< FGInertial > GetInertial(void) const
Returns the FGInertial pointer.
const SGPath & GetOutputPath(void)
Retrieves the path to the output files.
Initializes the simulation run.
const FGQuaternion & GetOrientation(void) const
Gets the initial orientation.
const FGLocation & GetPosition(void) const
Gets the initial position.
const FGColumnVector3 GetUVWFpsIC(void) const
Gets the initial body velocity.
double GetEarthPositionAngleIC(void) const
Gets the initial Earth position angle.
const FGColumnVector3 GetPQRRadpsIC(void) const
Gets the initial body rotation rate.
static constexpr double Constrain(double min, double value, double max)
Constrain a value between a minimum and a maximum value.
static char fgblue[6]
blue text
static char underon[5]
underlines text
static char reset[5]
resets text properties
static char underoff[6]
underline off
static char highint[5]
highlights text
FGLocation holds an arbitrary location in the Earth centered Earth fixed reference frame (ECEF).
void SetEllipse(double semimajor, double semiminor)
Sets the semimajor and semiminor axis lengths for this planet.
const FGMatrix33 & GetTl2ec(void) const
Transform matrix from local horizontal to earth centered frame.
double GetGeodLatitudeDeg(void) const
Get the GEODETIC latitude in degrees.
double GetRadius() const
Get the distance from the center of the earth in feet.
double GetSeaLevelRadius(void) const
Get the sea level radius in feet below the current location.
double GetLatitudeDeg() const
Get the GEOCENTRIC latitude in degrees.
void SetRadius(double radius)
Set the distance from the center of the earth.
double GetLongitudeDeg() const
Get the longitude.
FGMatrix33 Transposed(void) const
Transposed matrix.
std::string Dump(const std::string &delimeter) const
Prints the contents of the matrix.
FGQuaternion GetQuaternion(void) const
Returns the quaternion associated with this direction cosine (rotation) matrix.
Base class for all scheduled JSBSim models.
virtual bool Run(bool Holding)
Runs the model; called by the Executive.
const FGColumnVector3 & GetEuler(void) const
Retrieves the Euler angles that define the vehicle orientation.
const FGColumnVector3 & GetUVW(void) const
Retrieves the body frame vehicle velocity vector.
double GetNEDVelocityMagnitude(void) const
Retrieves the total local NED velocity in ft/sec.
const FGColumnVector3 & GetVel(void) const
Retrieves the velocity vector.
double GetInertialVelocityMagnitude(void) const
Retrieves the total inertial velocity in ft/sec.
double Gethdot(void) const
Returns the current altitude rate.
FGColumnVector3 GetEulerDeg(void) const
Retrieves the Euler angles (in degrees) that define the vehicle orientation.
double GetLocalTerrainRadius(void) const
Returns the "constant" LocalTerrainRadius.
bool Run(bool Holding)
Runs the state propagation model; called by the Executive Can pass in a value indicating if the execu...
const FGColumnVector3 & GetPQR(void) const
Retrieves the body angular rates vector, relative to the ECEF frame.
double GetEarthPositionAngleDeg(void) const
Returns the Earth position angle in degrees.
const FGColumnVector3 & GetInertialVelocity(void) const
Retrieves the inertial velocity vector in ft/sec.
double GetAltitudeASL(void) const
Returns the current altitude above sea level.
const FGColumnVector3 & GetInertialPosition(void) const
Retrieves the inertial position vector.
~FGPropagate()
Destructor.
bool InitModel(void)
Initializes the FGPropagate class after instantiation and prior to first execution.
FGPropagate(FGFDMExec *Executive)
Constructor.
const FGColumnVector3 & GetPQRi(void) const
Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
double GetEarthPositionAngle(void) const
Returns the Earth position angle.
void SetHoldDown(bool hd)
Sets the property forces/hold-down.
double GetAltitudeASLmeters(void) const
Returns the current altitude above sea level.
Models the Quaternion representation of rotations.
const FGColumnVector3 & GetEuler(void) const
Retrieves the Euler angles.
void Normalize(void)
Normalize.
const FGMatrix33 & GetT(void) const
Transformation matrix.
double GetEulerDeg(int i) const
Retrieves the Euler angles.
FGQuaternion GetQDot(const FGColumnVector3 &PQR) const
Quaternion derivative for given angular rates.
FGColumnVector3 vUVW
The velocity vector of the vehicle with respect to the ECEF frame, expressed in the body system.
FGQuaternion qAttitudeECI
The current orientation of the vehicle, that is, the orientation of the body frame relative to the in...
FGColumnVector3 vPQRi
The angular velocity vector for the vehicle body frame relative to the ECI frame, expressed in the bo...
FGColumnVector3 vPQR
The angular velocity vector for the vehicle relative to the ECEF frame, expressed in the body frame.
FGLocation vLocation
Represents the current location of the vehicle in Earth centered Earth fixed (ECEF) frame.
FGQuaternion qAttitudeLocal
The current orientation of the vehicle, that is, the orientation of the body frame relative to the lo...