45#include "FGAuxiliary.h"
46#include "initialization/FGInitialCondition.h"
48#include "input_output/FGPropertyManager.h"
49#include "FGInertial.h"
50#include "FGAtmosphere.h"
64 pt = FGAtmosphere::StdDaySLpressure;
65 tat = FGAtmosphere::StdDaySLtemperature;
69 qbar = qbarUW = qbarUV = 0.0;
73 gamma = Vt = Vground = 0.0;
75 hoverbmac = hoverbcg = 0.0;
79 vPilotAccel.InitMatrix();
80 vPilotAccelN.InitMatrix();
81 vAeroUVW.InitMatrix();
82 vAeroPQR.InitMatrix();
83 vMachUVW.InitMatrix();
84 vEulerRates.InitMatrix();
85 vNEUFromStart.InitMatrix();
95bool FGAuxiliary::InitModel(
void)
97 if (!FGModel::InitModel())
return false;
100 tat = in.Temperature;
104 qbar = qbarUW = qbarUV = 0.0;
108 gamma = Vt = Vground = 0.0;
110 hoverbmac = hoverbcg = 0.0;
114 vPilotAccel.InitMatrix();
115 vPilotAccelN.InitMatrix();
116 vAeroUVW.InitMatrix();
117 vAeroPQR.InitMatrix();
118 vMachUVW.InitMatrix();
119 vEulerRates.InitMatrix();
120 vNEUFromStart.InitMatrix();
121 NEUCalcValid =
false;
128void FGAuxiliary::SetInitialState(
const FGInitialCondition* ic)
130 NEUStartLocation = ic->GetPosition();
146 if (Holding)
return false;
150 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
151 if (in.CosTht != 0.0) {
152 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
153 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
157 vAeroPQR = in.vPQR - in.TurbPQR;
158 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
160 alpha = beta = adot = bdot = 0;
161 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
162 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
163 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
164 double mUW = AeroU2 + AeroW2;
166 double Vt2 = mUW + AeroV2;
170 beta = atan2(vAeroUVW(eV), sqrt(mUW));
173 alpha = atan2(vAeroUVW(eW), vAeroUVW(eU));
174 double Vtdot = (vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eV)*in.vUVWdot(eV) + vAeroUVW(eW)*in.vUVWdot(eW))/Vt;
175 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
176 bdot = (in.vUVWdot(eV)*Vt - vAeroUVW(eV)*Vtdot)/(Vt*sqrt(mUW));
180 UpdateWindMatrices();
182 Re = Vt * in.Wingchord / in.KinematicViscosity;
184 double densityD2 = 0.5*in.Density;
186 qbar = densityD2 * Vt2;
187 qbarUW = densityD2 * (mUW);
188 qbarUV = densityD2 * (AeroU2 + AeroV2);
189 Mach = Vt / in.SoundSpeed;
190 MachU = vMachUVW(eU) = vAeroUVW(eU) / in.SoundSpeed;
191 vMachUVW(eV) = vAeroUVW(eV) / in.SoundSpeed;
192 vMachUVW(eW) = vAeroUVW(eW) / in.SoundSpeed;
194 Vground = sqrt( in.vVel(eNorth)*in.vVel(eNorth) + in.vVel(eEast)*in.vVel(eEast) );
196 psigt = atan2(in.vVel(eEast), in.vVel(eNorth));
197 if (psigt < 0.0) psigt += 2*M_PI;
198 gamma = atan2(-in.vVel(eDown), Vground);
200 tat = in.Temperature*(1 + 0.2*Mach*Mach);
205 if (abs(Mach) > 0.0) {
207 veas = sqrt(2 * qbar / FGAtmosphere::StdDaySLdensity);
212 vPilotAccel.InitMatrix();
213 vNcg = in.vBodyAccel/in.StandardGravity;
218 vPilotAccel = in.vBodyAccel + in.vPQRidot * in.ToEyePt;
219 vPilotAccel += in.vPQRi * (in.vPQRi * in.ToEyePt);
221 vNwcg = mTb2w * vNcg;
222 vNwcg(eZ) = 1.0 - vNwcg(eZ);
224 vPilotAccelN = vPilotAccel / in.StandardGravity;
230 hoverbcg = in.DistanceAGL / in.Wingspan;
233 hoverbmac = (in.DistanceAGL - vMac(3)) / in.Wingspan;
236 NEUCalcValid =
false;
245 constexpr double SHRatio = FGAtmosphere::SHRatio;
246 constexpr double a = (SHRatio-1.0) / 2.0;
247 constexpr double b = SHRatio / (SHRatio-1.0);
248 constexpr double c = 2.0*b;
249 constexpr double d = 1.0 / (SHRatio-1.0);
250 const double coeff = pow(0.5*(SHRatio+1.0), b)
251 * pow((SHRatio+1.0)/(SHRatio-1.0), d);
253 if (mach < 0)
return pressure;
255 return pressure*pow((1.0 + a*mach*mach), b);
271 return pressure*coeff*pow(mach, c)/pow(c*mach*mach-1.0, d);
282 constexpr double SHRatio = FGAtmosphere::SHRatio;
283 constexpr double a = 2.0/(SHRatio-1.0);
284 constexpr double b = (SHRatio-1.0)/SHRatio;
285 constexpr double c = 2.0/b;
286 constexpr double d = 0.5*a;
287 const double coeff = pow(0.5*(SHRatio+1.0), -0.25*c)
288 * pow(0.5*(SHRatio+1.0)/SHRatio, -0.5*d);
290 double A = qc / pressure + 1;
291 double M = sqrt(a*(pow(A, b) - 1.0));
294 for (
unsigned int i = 0; i<10; i++)
295 M = coeff*sqrt(A*pow(1 - 1.0 / (c*M*M), d));
312 constexpr double StdDaySLpressure = FGAtmosphere::StdDaySLpressure;
313 double qc =
PitotTotalPressure(vcas / in.StdDaySLsoundspeed, StdDaySLpressure) - StdDaySLpressure;
333void FGAuxiliary::UpdateWindMatrices(
void)
335 double ca, cb, sa, sb;
357double FGAuxiliary::GetNlf(
void)
const
360 return (in.vFw(3))/(in.Mass*
slugtolb);
367double FGAuxiliary::GetLongitudeRelativePosition(
void)
const
375double FGAuxiliary::GetLatitudeRelativePosition(
void)
const
378 FDMExec->
GetIC()->GetGeodLatitudeRadIC())*fttom;
383double FGAuxiliary::GetDistanceRelativePosition(
void)
const
385 auto ic = FDMExec->
GetIC();
387 ic->GetGeodLatitudeRadIC())*fttom;
392const FGColumnVector3& FGAuxiliary::GetNEUPositionFromStart()
const
398 vNEUFromStart(3) *= -1.0;
402 return vNEUFromStart;
407void FGAuxiliary::bind(
void)
410 PropertyManager->Tie(
"propulsion/tat-c",
this, &FGAuxiliary::GetTAT_C);
419 PropertyManager->Tie(
"velocities/p-aero-rad_sec",
this, eX, &FGAuxiliary::GetAeroPQR);
420 PropertyManager->Tie(
"velocities/q-aero-rad_sec",
this, eY, &FGAuxiliary::GetAeroPQR);
421 PropertyManager->Tie(
"velocities/r-aero-rad_sec",
this, eZ, &FGAuxiliary::GetAeroPQR);
422 PropertyManager->Tie(
"velocities/phidot-rad_sec",
this, ePhi, &FGAuxiliary::GetEulerRates);
423 PropertyManager->Tie(
"velocities/thetadot-rad_sec",
this, eTht, &FGAuxiliary::GetEulerRates);
424 PropertyManager->Tie(
"velocities/psidot-rad_sec",
this, ePsi, &FGAuxiliary::GetEulerRates);
425 PropertyManager->Tie(
"velocities/u-aero-fps",
this, eU, &FGAuxiliary::GetAeroUVW);
426 PropertyManager->Tie(
"velocities/v-aero-fps",
this, eV, &FGAuxiliary::GetAeroUVW);
427 PropertyManager->Tie(
"velocities/w-aero-fps",
this, eW, &FGAuxiliary::GetAeroUVW);
431 PropertyManager->Tie(
"accelerations/a-pilot-x-ft_sec2",
this, eX, &FGAuxiliary::GetPilotAccel);
432 PropertyManager->Tie(
"accelerations/a-pilot-y-ft_sec2",
this, eY, &FGAuxiliary::GetPilotAccel);
433 PropertyManager->Tie(
"accelerations/a-pilot-z-ft_sec2",
this, eZ, &FGAuxiliary::GetPilotAccel);
434 PropertyManager->Tie(
"accelerations/n-pilot-x-norm",
this, eX, &FGAuxiliary::GetNpilot);
435 PropertyManager->Tie(
"accelerations/n-pilot-y-norm",
this, eY, &FGAuxiliary::GetNpilot);
436 PropertyManager->Tie(
"accelerations/n-pilot-z-norm",
this, eZ, &FGAuxiliary::GetNpilot);
440 PropertyManager->Tie(
"forces/load-factor",
this, &FGAuxiliary::GetNlf);
441 PropertyManager->Tie(
"aero/alpha-rad",
this, &FGAuxiliary::Getalpha);
442 PropertyManager->Tie(
"aero/beta-rad",
this, &FGAuxiliary::Getbeta);
443 PropertyManager->Tie(
"aero/mag-beta-rad",
this, &FGAuxiliary::GetMagBeta);
444 PropertyManager->Tie(
"aero/alpha-deg",
this, inDegrees, &FGAuxiliary::Getalpha);
445 PropertyManager->Tie(
"aero/beta-deg",
this, inDegrees, &FGAuxiliary::Getbeta);
446 PropertyManager->Tie(
"aero/mag-beta-deg",
this, inDegrees, &FGAuxiliary::GetMagBeta);
447 PropertyManager->Tie(
"aero/Re",
this, &FGAuxiliary::GetReynoldsNumber);
448 PropertyManager->Tie(
"aero/qbar-psf",
this, &FGAuxiliary::Getqbar);
449 PropertyManager->Tie(
"aero/qbarUW-psf",
this, &FGAuxiliary::GetqbarUW);
450 PropertyManager->Tie(
"aero/qbarUV-psf",
this, &FGAuxiliary::GetqbarUV);
451 PropertyManager->Tie(
"aero/alphadot-rad_sec",
this, &FGAuxiliary::Getadot);
452 PropertyManager->Tie(
"aero/betadot-rad_sec",
this, &FGAuxiliary::Getbdot);
453 PropertyManager->Tie(
"aero/alphadot-deg_sec",
this, inDegrees, &FGAuxiliary::Getadot);
454 PropertyManager->Tie(
"aero/betadot-deg_sec",
this, inDegrees, &FGAuxiliary::Getbdot);
455 PropertyManager->Tie(
"aero/h_b-cg-ft",
this, &FGAuxiliary::GetHOverBCG);
456 PropertyManager->Tie(
"aero/h_b-mac-ft",
this, &FGAuxiliary::GetHOverBMAC);
457 PropertyManager->Tie(
"flight-path/gamma-rad",
this, &FGAuxiliary::GetGamma);
458 PropertyManager->Tie(
"flight-path/gamma-deg",
this, inDegrees, &FGAuxiliary::GetGamma);
459 PropertyManager->Tie(
"flight-path/psi-gt-rad",
this, &FGAuxiliary::GetGroundTrack);
461 PropertyManager->Tie(
"position/distance-from-start-lon-mt",
this, &FGAuxiliary::GetLongitudeRelativePosition);
462 PropertyManager->Tie(
"position/distance-from-start-lat-mt",
this, &FGAuxiliary::GetLatitudeRelativePosition);
463 PropertyManager->Tie(
"position/distance-from-start-mag-mt",
this, &FGAuxiliary::GetDistanceRelativePosition);
468 PropertyManager->Tie(
"position/from-start-neu-n-ft",
this, eX, &FGAuxiliary::GetNEUPositionFromStart);
469 PropertyManager->Tie(
"position/from-start-neu-e-ft",
this, eY, &FGAuxiliary::GetNEUPositionFromStart);
470 PropertyManager->Tie(
"position/from-start-neu-u-ft",
this, eZ, &FGAuxiliary::GetNEUPositionFromStart);
475double FGAuxiliary::BadUnits(
void)
const
477 cerr <<
"Bad units" << endl;
return 0.0;
499void FGAuxiliary::Debug(
int from)
501 if (debug_lvl <= 0)
return;
508 if (debug_lvl & 2 ) {
509 if (from == 0) cout <<
"Instantiated: FGAuxiliary" << endl;
510 if (from == 1) cout <<
"Destroyed: FGAuxiliary" << endl;
512 if (debug_lvl & 4 ) {
514 if (debug_lvl & 8 ) {
516 if (debug_lvl & 16) {
517 if (Mach > 100 || Mach < 0.00)
518 cout <<
"FGPropagate::Mach is out of bounds: " << Mach << endl;
519 if (qbar > 1e6 || qbar < 0.00)
520 cout <<
"FGPropagate::qbar is out of bounds: " << qbar << endl;
522 if (debug_lvl & 64) {
double PitotTotalPressure(double mach, double pressure) const
Compute the total pressure in front of the Pitot tube.
double VcalibratedFromMach(double mach, double pressure) const
Calculate the calibrated airspeed from the Mach number.
double GetVtrueFPS() const
Returns the true airspeed in feet per second.
double GetVcalibratedFPS(void) const
Returns Calibrated airspeed in feet/second.
double GetVground(void) const
Gets the ground speed in feet per second.
double GetVcalibratedKTS(void) const
Returns Calibrated airspeed in knots.
double GetMach(void) const
Gets the Mach number.
double GetTotalPressure(void) const
Returns the total pressure.
double GetVtrueKTS() const
Returns the true airspeed in knots.
double GetNz(void) const
The vertical acceleration in g's of the aircraft center of gravity.
double GetVequivalentFPS(void) const
Returns equivalent airspeed in feet/second.
double GetMachU(void) const
The mach number calculated using the vehicle X axis velocity.
bool Run(bool Holding) override
Runs the Auxiliary routines; called by the Executive Can pass in a value indicating if the executive ...
double GetTotalTemperature(void) const
Returns the total temperature.
double GetVt(void) const
Gets the magnitude of total vehicle velocity including wind effects in feet per second.
double GetNy(void) const
The lateral acceleration in g's of the aircraft center of gravity.
double GetNx(void) const
The longitudinal acceleration in g's of the aircraft center of gravity.
~FGAuxiliary()
Destructor.
double MachFromVcalibrated(double vcas, double pressure) const
Calculate the Mach number from the calibrated airspeed.Based on the formulas in the US Air Force Airc...
double MachFromImpactPressure(double qc, double p) const
Compute the Mach number from the differential pressure (qc) and the static pressure.
FGAuxiliary(FGFDMExec *Executive)
Constructor.
double GetVequivalentKTS(void) const
Returns equivalent airspeed in knots.
This class implements a 3 element column vector.
Encapsulates the JSBSim simulation executive.
std::shared_ptr< FGInitialCondition > GetIC(void) const
Returns a pointer to the FGInitialCondition object.
static constexpr double slugtolb
Note that definition of lbtoslug by the inverse of slugtolb and not to a different constant you can a...
static constexpr double RankineToCelsius(double rankine)
Converts from degrees Rankine to degrees Celsius.
double GetGeodLatitudeRad(void) const
Get the GEODETIC latitude in radians.
double GetLongitude() const
Get the longitude.
double GetRadius() const
Get the distance from the center of the earth in feet.
FGColumnVector3 LocationToLocal(const FGColumnVector3 &ecvec) const
Conversion from a location in the earth centered and fixed frame to local horizontal frame coordinate...
FGLocation LocalToLocation(const FGColumnVector3 &lvec) const
Conversion from Local frame coordinates to a location in the earth centered and fixed frame.
double GetLatitudeDeg() const
Get the GEOCENTRIC latitude in degrees.
double GetLongitudeDeg() const
Get the longitude.
void SetPositionGeodetic(double lon, double lat, double height)
Sets the longitude, latitude and the distance above the reference spheroid.
double GetDistanceTo(double target_longitude, double target_latitude) const
Get the geodetic distance between the current location and a given location.
FGMatrix33 Transposed(void) const
Transposed matrix.
Base class for all scheduled JSBSim models.
virtual bool Run(bool Holding)
Runs the model; called by the Executive.