45#include "FGAuxiliary.h"
46#include "initialization/FGInitialCondition.h"
48#include "input_output/FGPropertyManager.h"
49#include "FGInertial.h"
50#include "FGAtmosphere.h"
51#include "input_output/FGLog.h"
65 pt = FGAtmosphere::StdDaySLpressure;
66 tat = FGAtmosphere::StdDaySLtemperature;
70 qbar = qbarUW = qbarUV = 0.0;
74 gamma = Vt = Vground = 0.0;
76 hoverbmac = hoverbcg = 0.0;
80 vPilotAccel.InitMatrix();
81 vPilotAccelN.InitMatrix();
82 vAeroUVW.InitMatrix();
83 vAeroPQR.InitMatrix();
84 vMachUVW.InitMatrix();
85 vEulerRates.InitMatrix();
86 vNEUFromStart.InitMatrix();
96bool FGAuxiliary::InitModel(
void)
98 if (!FGModel::InitModel())
return false;
101 tat = in.Temperature;
105 qbar = qbarUW = qbarUV = 0.0;
109 gamma = Vt = Vground = 0.0;
111 hoverbmac = hoverbcg = 0.0;
115 vPilotAccel.InitMatrix();
116 vPilotAccelN.InitMatrix();
117 vAeroUVW.InitMatrix();
118 vAeroPQR.InitMatrix();
119 vMachUVW.InitMatrix();
120 vEulerRates.InitMatrix();
121 vNEUFromStart.InitMatrix();
122 NEUCalcValid =
false;
129void FGAuxiliary::SetInitialState(
const FGInitialCondition* ic)
131 NEUStartLocation = ic->GetPosition();
147 if (Holding)
return false;
151 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
152 if (in.CosTht != 0.0) {
153 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
154 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
158 vAeroPQR = in.vPQR - in.TurbPQR;
159 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
161 alpha = beta = adot = bdot = 0;
162 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
163 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
164 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
165 double mUW = AeroU2 + AeroW2;
167 double Vt2 = mUW + AeroV2;
171 beta = atan2(vAeroUVW(eV), sqrt(mUW));
174 alpha = atan2(vAeroUVW(eW), vAeroUVW(eU));
175 double Vtdot = (vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eV)*in.vUVWdot(eV) + vAeroUVW(eW)*in.vUVWdot(eW))/Vt;
176 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
177 bdot = (in.vUVWdot(eV)*Vt - vAeroUVW(eV)*Vtdot)/(Vt*sqrt(mUW));
181 UpdateWindMatrices();
183 Re = Vt * in.Wingchord / in.KinematicViscosity;
185 double densityD2 = 0.5*in.Density;
187 qbar = densityD2 * Vt2;
188 qbarUW = densityD2 * (mUW);
189 qbarUV = densityD2 * (AeroU2 + AeroV2);
190 Mach = Vt / in.SoundSpeed;
191 MachU = vMachUVW(eU) = vAeroUVW(eU) / in.SoundSpeed;
192 vMachUVW(eV) = vAeroUVW(eV) / in.SoundSpeed;
193 vMachUVW(eW) = vAeroUVW(eW) / in.SoundSpeed;
195 Vground = sqrt( in.vVel(eNorth)*in.vVel(eNorth) + in.vVel(eEast)*in.vVel(eEast) );
197 psigt = atan2(in.vVel(eEast), in.vVel(eNorth));
198 if (psigt < 0.0) psigt += 2*M_PI;
199 gamma = atan2(-in.vVel(eDown), Vground);
201 tat = in.Temperature*(1 + 0.2*Mach*Mach);
206 if (abs(Mach) > 0.0) {
208 veas = sqrt(2 * qbar / FGAtmosphere::StdDaySLdensity);
213 vPilotAccel.InitMatrix();
214 vNcg = in.vBodyAccel/in.StandardGravity;
219 vPilotAccel = in.vBodyAccel + in.vPQRidot * in.ToEyePt;
220 vPilotAccel += in.vPQRi * (in.vPQRi * in.ToEyePt);
222 vNwcg = mTb2w * vNcg;
223 vNwcg(eZ) = 1.0 - vNwcg(eZ);
225 vPilotAccelN = vPilotAccel / in.StandardGravity;
231 hoverbcg = in.DistanceAGL / in.Wingspan;
234 hoverbmac = (in.DistanceAGL - vMac(3)) / in.Wingspan;
237 NEUCalcValid =
false;
246 constexpr double SHRatio = FGAtmosphere::SHRatio;
247 constexpr double a = (SHRatio-1.0) / 2.0;
248 constexpr double b = SHRatio / (SHRatio-1.0);
249 constexpr double c = 2.0*b;
250 constexpr double d = 1.0 / (SHRatio-1.0);
251 const double coeff = pow(0.5*(SHRatio+1.0), b)
252 * pow((SHRatio+1.0)/(SHRatio-1.0), d);
254 if (mach < 0)
return pressure;
256 return pressure*pow((1.0 + a*mach*mach), b);
272 return pressure*coeff*pow(mach, c)/pow(c*mach*mach-1.0, d);
283 constexpr double SHRatio = FGAtmosphere::SHRatio;
284 constexpr double a = 2.0/(SHRatio-1.0);
285 constexpr double b = (SHRatio-1.0)/SHRatio;
286 constexpr double c = 2.0/b;
287 constexpr double d = 0.5*a;
288 const double coeff = pow(0.5*(SHRatio+1.0), -0.25*c)
289 * pow(0.5*(SHRatio+1.0)/SHRatio, -0.5*d);
291 double A = qc / pressure + 1;
292 double M = sqrt(a*(pow(A, b) - 1.0));
295 for (
unsigned int i = 0; i<10; i++)
296 M = coeff*sqrt(A*pow(1 - 1.0 / (c*M*M), d));
313 constexpr double StdDaySLpressure = FGAtmosphere::StdDaySLpressure;
314 double qc =
PitotTotalPressure(vcas / in.StdDaySLsoundspeed, StdDaySLpressure) - StdDaySLpressure;
334void FGAuxiliary::UpdateWindMatrices(
void)
336 double ca, cb, sa, sb;
358double FGAuxiliary::GetNlf(
void)
const
361 return (in.vFw(3))/(in.Mass*
slugtolb);
368double FGAuxiliary::GetLongitudeRelativePosition(
void)
const
376double FGAuxiliary::GetLatitudeRelativePosition(
void)
const
379 FDMExec->
GetIC()->GetGeodLatitudeRadIC())*fttom;
384double FGAuxiliary::GetDistanceRelativePosition(
void)
const
386 auto ic = FDMExec->
GetIC();
388 ic->GetGeodLatitudeRadIC())*fttom;
393const FGColumnVector3& FGAuxiliary::GetNEUPositionFromStart()
const
399 vNEUFromStart(3) *= -1.0;
403 return vNEUFromStart;
408void FGAuxiliary::bind(
void)
411 PropertyManager->Tie(
"propulsion/tat-c",
this, &FGAuxiliary::GetTAT_C);
420 PropertyManager->Tie(
"velocities/p-aero-rad_sec",
this, eX, &FGAuxiliary::GetAeroPQR);
421 PropertyManager->Tie(
"velocities/q-aero-rad_sec",
this, eY, &FGAuxiliary::GetAeroPQR);
422 PropertyManager->Tie(
"velocities/r-aero-rad_sec",
this, eZ, &FGAuxiliary::GetAeroPQR);
423 PropertyManager->Tie(
"velocities/phidot-rad_sec",
this, ePhi, &FGAuxiliary::GetEulerRates);
424 PropertyManager->Tie(
"velocities/thetadot-rad_sec",
this, eTht, &FGAuxiliary::GetEulerRates);
425 PropertyManager->Tie(
"velocities/psidot-rad_sec",
this, ePsi, &FGAuxiliary::GetEulerRates);
426 PropertyManager->Tie(
"velocities/u-aero-fps",
this, eU, &FGAuxiliary::GetAeroUVW);
427 PropertyManager->Tie(
"velocities/v-aero-fps",
this, eV, &FGAuxiliary::GetAeroUVW);
428 PropertyManager->Tie(
"velocities/w-aero-fps",
this, eW, &FGAuxiliary::GetAeroUVW);
432 PropertyManager->Tie(
"accelerations/a-pilot-x-ft_sec2",
this, eX, &FGAuxiliary::GetPilotAccel);
433 PropertyManager->Tie(
"accelerations/a-pilot-y-ft_sec2",
this, eY, &FGAuxiliary::GetPilotAccel);
434 PropertyManager->Tie(
"accelerations/a-pilot-z-ft_sec2",
this, eZ, &FGAuxiliary::GetPilotAccel);
435 PropertyManager->Tie(
"accelerations/n-pilot-x-norm",
this, eX, &FGAuxiliary::GetNpilot);
436 PropertyManager->Tie(
"accelerations/n-pilot-y-norm",
this, eY, &FGAuxiliary::GetNpilot);
437 PropertyManager->Tie(
"accelerations/n-pilot-z-norm",
this, eZ, &FGAuxiliary::GetNpilot);
441 PropertyManager->Tie(
"forces/load-factor",
this, &FGAuxiliary::GetNlf);
442 PropertyManager->Tie(
"aero/alpha-rad",
this, &FGAuxiliary::Getalpha);
443 PropertyManager->Tie(
"aero/beta-rad",
this, &FGAuxiliary::Getbeta);
444 PropertyManager->Tie(
"aero/mag-beta-rad",
this, &FGAuxiliary::GetMagBeta);
445 PropertyManager->Tie(
"aero/alpha-deg",
this, inDegrees, &FGAuxiliary::Getalpha);
446 PropertyManager->Tie(
"aero/beta-deg",
this, inDegrees, &FGAuxiliary::Getbeta);
447 PropertyManager->Tie(
"aero/mag-beta-deg",
this, inDegrees, &FGAuxiliary::GetMagBeta);
448 PropertyManager->Tie(
"aero/Re",
this, &FGAuxiliary::GetReynoldsNumber);
449 PropertyManager->Tie(
"aero/qbar-psf",
this, &FGAuxiliary::Getqbar);
450 PropertyManager->Tie(
"aero/qbarUW-psf",
this, &FGAuxiliary::GetqbarUW);
451 PropertyManager->Tie(
"aero/qbarUV-psf",
this, &FGAuxiliary::GetqbarUV);
452 PropertyManager->Tie(
"aero/alphadot-rad_sec",
this, &FGAuxiliary::Getadot);
453 PropertyManager->Tie(
"aero/betadot-rad_sec",
this, &FGAuxiliary::Getbdot);
454 PropertyManager->Tie(
"aero/alphadot-deg_sec",
this, inDegrees, &FGAuxiliary::Getadot);
455 PropertyManager->Tie(
"aero/betadot-deg_sec",
this, inDegrees, &FGAuxiliary::Getbdot);
456 PropertyManager->Tie(
"aero/h_b-cg-ft",
this, &FGAuxiliary::GetHOverBCG);
457 PropertyManager->Tie(
"aero/h_b-mac-ft",
this, &FGAuxiliary::GetHOverBMAC);
458 PropertyManager->Tie(
"flight-path/gamma-rad",
this, &FGAuxiliary::GetGamma);
459 PropertyManager->Tie(
"flight-path/gamma-deg",
this, inDegrees, &FGAuxiliary::GetGamma);
460 PropertyManager->Tie(
"flight-path/psi-gt-rad",
this, &FGAuxiliary::GetGroundTrack);
462 PropertyManager->Tie(
"position/distance-from-start-lon-mt",
this, &FGAuxiliary::GetLongitudeRelativePosition);
463 PropertyManager->Tie(
"position/distance-from-start-lat-mt",
this, &FGAuxiliary::GetLatitudeRelativePosition);
464 PropertyManager->Tie(
"position/distance-from-start-mag-mt",
this, &FGAuxiliary::GetDistanceRelativePosition);
469 PropertyManager->Tie(
"position/from-start-neu-n-ft",
this, eX, &FGAuxiliary::GetNEUPositionFromStart);
470 PropertyManager->Tie(
"position/from-start-neu-e-ft",
this, eY, &FGAuxiliary::GetNEUPositionFromStart);
471 PropertyManager->Tie(
"position/from-start-neu-u-ft",
this, eZ, &FGAuxiliary::GetNEUPositionFromStart);
476double FGAuxiliary::BadUnits(
void)
const
478 FGLogging log(LogLevel::ERROR);
479 log <<
"Bad units" << endl;
502void FGAuxiliary::Debug(
int from)
504 if (debug_lvl <= 0)
return;
511 if (debug_lvl & 2 ) {
512 FGLogging log(LogLevel::DEBUG);
513 if (from == 0) log <<
"Instantiated: FGAuxiliary" << endl;
514 if (from == 1) log <<
"Destroyed: FGAuxiliary" << endl;
516 if (debug_lvl & 4 ) {
518 if (debug_lvl & 8 ) {
520 if (debug_lvl & 16) {
521 FGLogging log(LogLevel::DEBUG);
522 if (Mach > 100 || Mach < 0.00)
523 log <<
"FGPropagate::Mach is out of bounds: " << Mach << endl;
524 if (qbar > 1e6 || qbar < 0.00)
525 log <<
"FGPropagate::qbar is out of bounds: " << qbar << endl;
527 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.
Main namespace for the JSBSim Flight Dynamics Model.