45 #include "FGAuxiliary.h" 46 #include "initialization/FGInitialCondition.h" 47 #include "FGFDMExec.h" 48 #include "input_output/FGPropertyManager.h" 54 IDENT(IdSrc,
"$Id: FGAuxiliary.cpp,v 1.72 2016/05/21 11:45:22 bcoconni Exp $");
55 IDENT(IdHdr,ID_AUXILIARY);
70 qbar = qbarUW = qbarUV = 0.0;
71 Mach = MachU = MachPitot = 0.0;
74 gamma = Vt = Vground = Vpitot = 0.0;
78 hoverbmac = hoverbcg = 0.0;
82 vPilotAccel.InitMatrix();
83 vPilotAccelN.InitMatrix();
84 vAeroUVW.InitMatrix();
85 vAeroPQR.InitMatrix();
86 vMachUVW.InitMatrix();
87 vWindUVW.InitMatrix();
88 vPitotUVW.InitMatrix();
90 vEulerRates.InitMatrix();
99 bool FGAuxiliary::InitModel(
void)
101 if (!FGModel::InitModel())
return false;
104 tat = in.Temperature;
108 qbar = qbarUW = qbarUV = 0.0;
109 Mach = MachU = MachPitot = 0.0;
112 gamma = Vt = Vground = Vpitot = 0.0;
115 seconds_in_day = 0.0;
116 hoverbmac = hoverbcg = 0.0;
120 vPilotAccel.InitMatrix();
121 vPilotAccelN.InitMatrix();
122 vAeroUVW.InitMatrix();
123 vAeroPQR.InitMatrix();
124 vMachUVW.InitMatrix();
126 vEulerRates.InitMatrix();
143 if (Holding)
return false;
147 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
148 if (in.CosTht != 0.0) {
149 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
150 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
154 vAeroPQR = in.vPQR - in.TurbPQR;
155 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
158 alpha = beta = adot = bdot = 0;
159 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
160 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
161 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
162 double mUW = AeroU2 + AeroW2;
167 if (vAeroUVW(eW) != 0.0)
168 alpha = AeroU2 > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
169 if (vAeroUVW(eV) != 0.0)
170 beta = mUW > 0.0 ? atan2(vAeroUVW(eV), sqrt(mUW)) : 0.0;
175 if ( mUW >= 0.001 ) {
176 double Vtdot = (vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eV)*in.vUVWdot(eV) + vAeroUVW(eW)*in.vUVWdot(eW))/Vt;
177 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
180 bdot = (in.vUVWdot(eV)*Vt - vAeroUVW(eV)*Vtdot)/(Vt*sqrt(mUW));
184 UpdateWindMatrices();
186 Re = Vt * in.Wingchord / in.KinematicViscosity;
188 double densityD2 = 0.5*in.Density;
190 qbar = densityD2 * Vt2;
191 qbarUW = densityD2 * (mUW);
192 qbarUV = densityD2 * (AeroU2 + AeroV2);
193 Mach = Vt / in.SoundSpeed;
194 MachU = vMachUVW(eU) = vAeroUVW(eU) / in.SoundSpeed;
195 vMachUVW(eV) = vAeroUVW(eV) / in.SoundSpeed;
196 vMachUVW(eW) = vAeroUVW(eW) / in.SoundSpeed;
200 Vground = sqrt( in.vVel(eNorth)*in.vVel(eNorth) + in.vVel(eEast)*in.vVel(eEast) );
202 psigt = atan2(in.vVel(eEast), in.vVel(eNorth));
203 if (psigt < 0.0) psigt += 2*M_PI;
204 gamma = atan2(-in.vVel(eDown), Vground);
206 tat = in.Temperature*(1 + 0.2*Mach*Mach);
212 vPitotUVW = mTw2p * vWindUVW;
213 Vpitot = vPitotUVW(eU);
214 if (Vpitot < 0.0) Vpitot = 0.0;
215 MachPitot = Vpitot / in.SoundSpeed;
218 if (abs(MachPitot) > 0.0) {
220 veas = sqrt(2 * qbar / in.DensitySL);
221 vtrue = 1116.43559 * Mach * sqrt(in.Temperature / 518.67);
223 vcas = veas = vtrue = 0.0;
226 vPilotAccel.InitMatrix();
227 vNcg = in.vBodyAccel/in.SLGravity;
231 vPilotAccel = in.vBodyAccel + in.vPQRidot * in.ToEyePt;
232 vPilotAccel += in.vPQRi * (in.vPQRi * in.ToEyePt);
234 vNwcg = mTb2w * vNcg;
235 vNwcg(eZ) = 1.0 - vNwcg(eZ);
237 vPilotAccelN = vPilotAccel / in.SLGravity;
243 hoverbcg = in.DistanceAGL / in.Wingspan;
246 hoverbmac = (in.DistanceAGL + vMac(3)) / in.Wingspan;
267 void FGAuxiliary::UpdateWindMatrices(
void)
269 double ca, cb, sa, sb;
291 ca = cos(alpha + in.PitotAngle);
292 sa = sin(alpha + in.PitotAngle);
312 double FGAuxiliary::GetHeadWind(
void)
const 314 return in.Vwind * cos(in.WindPsi - in.Psi);
323 double FGAuxiliary::GetCrossWind(
void)
const 325 return in.Vwind * sin(in.WindPsi - in.Psi);
330 double FGAuxiliary::GetNlf(
void)
const 333 return (-in.vFw(3))/(in.Mass*slugtolb);
340 double FGAuxiliary::GetLongitudeRelativePosition(
void)
const 351 double FGAuxiliary::GetLatitudeRelativePosition(
void)
const 362 double FGAuxiliary::GetDistanceRelativePosition(
void)
const 373 void FGAuxiliary::bind(
void)
378 PropertyManager->
Tie(
"propulsion/tat-c",
this, &FGAuxiliary::GetTAT_C);
387 PropertyManager->
Tie(
"velocities/p-aero-rad_sec",
this, eX, (PMF)&FGAuxiliary::GetAeroPQR);
388 PropertyManager->
Tie(
"velocities/q-aero-rad_sec",
this, eY, (PMF)&FGAuxiliary::GetAeroPQR);
389 PropertyManager->
Tie(
"velocities/r-aero-rad_sec",
this, eZ, (PMF)&FGAuxiliary::GetAeroPQR);
390 PropertyManager->
Tie(
"velocities/phidot-rad_sec",
this, ePhi, (PMF)&FGAuxiliary::GetEulerRates);
391 PropertyManager->
Tie(
"velocities/thetadot-rad_sec",
this, eTht, (PMF)&FGAuxiliary::GetEulerRates);
392 PropertyManager->
Tie(
"velocities/psidot-rad_sec",
this, ePsi, (PMF)&FGAuxiliary::GetEulerRates);
393 PropertyManager->
Tie(
"velocities/u-aero-fps",
this, eU, (PMF)&FGAuxiliary::GetAeroUVW);
394 PropertyManager->
Tie(
"velocities/v-aero-fps",
this, eV, (PMF)&FGAuxiliary::GetAeroUVW);
395 PropertyManager->
Tie(
"velocities/w-aero-fps",
this, eW, (PMF)&FGAuxiliary::GetAeroUVW);
399 PropertyManager->
Tie(
"accelerations/a-pilot-x-ft_sec2",
this, eX, (PMF)&FGAuxiliary::GetPilotAccel);
400 PropertyManager->
Tie(
"accelerations/a-pilot-y-ft_sec2",
this, eY, (PMF)&FGAuxiliary::GetPilotAccel);
401 PropertyManager->
Tie(
"accelerations/a-pilot-z-ft_sec2",
this, eZ, (PMF)&FGAuxiliary::GetPilotAccel);
402 PropertyManager->
Tie(
"accelerations/n-pilot-x-norm",
this, eX, (PMF)&FGAuxiliary::GetNpilot);
403 PropertyManager->
Tie(
"accelerations/n-pilot-y-norm",
this, eY, (PMF)&FGAuxiliary::GetNpilot);
404 PropertyManager->
Tie(
"accelerations/n-pilot-z-norm",
this, eZ, (PMF)&FGAuxiliary::GetNpilot);
407 PropertyManager->
Tie(
"forces/load-factor",
this, &FGAuxiliary::GetNlf);
410 PropertyManager->
Tie(
"aero/alpha-rad",
this, (PF)&FGAuxiliary::Getalpha);
411 PropertyManager->
Tie(
"aero/beta-rad",
this, (PF)&FGAuxiliary::Getbeta);
412 PropertyManager->
Tie(
"aero/mag-beta-rad",
this, (PF)&FGAuxiliary::GetMagBeta);
413 PropertyManager->
Tie(
"aero/alpha-deg",
this, inDegrees, (PMF)&FGAuxiliary::Getalpha);
414 PropertyManager->
Tie(
"aero/beta-deg",
this, inDegrees, (PMF)&FGAuxiliary::Getbeta);
415 PropertyManager->
Tie(
"aero/mag-beta-deg",
this, inDegrees, (PMF)&FGAuxiliary::GetMagBeta);
416 PropertyManager->
Tie(
"aero/Re",
this, &FGAuxiliary::GetReynoldsNumber);
417 PropertyManager->
Tie(
"aero/qbar-psf",
this, &FGAuxiliary::Getqbar);
418 PropertyManager->
Tie(
"aero/qbarUW-psf",
this, &FGAuxiliary::GetqbarUW);
419 PropertyManager->
Tie(
"aero/qbarUV-psf",
this, &FGAuxiliary::GetqbarUV);
420 PropertyManager->
Tie(
"aero/alphadot-rad_sec",
this, (PF)&FGAuxiliary::Getadot);
421 PropertyManager->
Tie(
"aero/betadot-rad_sec",
this, (PF)&FGAuxiliary::Getbdot);
422 PropertyManager->
Tie(
"aero/alphadot-deg_sec",
this, inDegrees, (PMF)&FGAuxiliary::Getadot);
423 PropertyManager->
Tie(
"aero/betadot-deg_sec",
this, inDegrees, (PMF)&FGAuxiliary::Getbdot);
424 PropertyManager->
Tie(
"aero/h_b-cg-ft",
this, &FGAuxiliary::GetHOverBCG);
425 PropertyManager->
Tie(
"aero/h_b-mac-ft",
this, &FGAuxiliary::GetHOverBMAC);
426 PropertyManager->
Tie(
"flight-path/gamma-rad",
this, &FGAuxiliary::GetGamma);
427 PropertyManager->
Tie(
"flight-path/psi-gt-rad",
this, &FGAuxiliary::GetGroundTrack);
429 PropertyManager->
Tie(
"position/distance-from-start-lon-mt",
this, &FGAuxiliary::GetLongitudeRelativePosition);
430 PropertyManager->
Tie(
"position/distance-from-start-lat-mt",
this, &FGAuxiliary::GetLatitudeRelativePosition);
431 PropertyManager->
Tie(
"position/distance-from-start-mag-mt",
this, &FGAuxiliary::GetDistanceRelativePosition);
439 double FGAuxiliary::BadUnits(
void)
const 441 cerr <<
"Bad units" << endl;
return 0.0;
463 void FGAuxiliary::Debug(
int from)
465 if (debug_lvl <= 0)
return;
472 if (debug_lvl & 2 ) {
473 if (from == 0) cout <<
"Instantiated: FGAuxiliary" << endl;
474 if (from == 1) cout <<
"Destroyed: FGAuxiliary" << endl;
476 if (debug_lvl & 4 ) {
478 if (debug_lvl & 8 ) {
480 if (debug_lvl & 16) {
481 if (Mach > 100 || Mach < 0.00)
482 cout <<
"FGPropagate::Mach is out of bounds: " << Mach << endl;
483 if (qbar > 1e6 || qbar < 0.00)
484 cout <<
"FGPropagate::qbar is out of bounds: " << qbar << endl;
486 if (debug_lvl & 64) {
488 cout << IdSrc << endl;
489 cout << IdHdr << endl;
double GetLatitudeRadIC(void) const
Gets the initial latitude.
FGInitialCondition * GetIC(void)
Returns a pointer to the FGInitialCondition object.
double GetVequivalentKTS(void) const
Returns equivalent airspeed in knots.
double GetSeaLevelRadius(void) const
Get the local sea level radius.
static double RankineToCelsius(double rankine)
Converts from degrees Rankine to degrees Celsius.
double GetTotalTemperature(void) const
Returns the total temperature.
double GetVcalibratedFPS(void) const
Returns Calibrated airspeed in feet/second.
double GetVtrueKTS() const
Returns the true airspeed in knots.
double GetDistanceTo(double target_longitude, double target_latitude) const
Get the geodetic distance between the current location and a given location.
double GetLatitude() const
Get the latitude.
FGLocation holds an arbitrary location in the Earth centered Earth fixed reference frame (ECEF)...
double GetNy(void) const
The lateral acceleration in g's of the aircraft center of gravity.
double GetLongitudeRadIC(void) const
Gets the initial longitude.
static double PitotTotalPressure(double mach, double p)
Compute the total pressure in front of the Pitot tube.
double GetRadius() const
Get the distance from the center of the earth.
virtual bool Run(bool Holding)
Runs the model; called by the Executive.
static double VcalibratedFromMach(double mach, double p, double psl, double rhosl)
Calculate the calibrated airspeed from the Mach number.
double GetLongitude() const
Get the longitude.
void Tie(const std::string &name, bool *pointer, bool useDefault=true)
Tie a property to an external bool variable.
double GetVtrueFPS() const
Returns the true airspeed in feet per second.
double GetNz(void) const
The vertical acceleration in g's of the aircraft center of gravity.
FGLocation LocalToLocation(const FGColumnVector3 &lvec) const
Conversion from Local frame coordinates to a location in the earth centered and fixed frame...
Base class for all scheduled JSBSim models.
double GetVequivalentFPS(void) const
Returns equivalent airspeed in feet/second.
This class implements a 3 element column vector.
double GetLongitudeDeg() const
Get the longitude.
Encapsulates various uncategorized scheduled functions.
double GetMachU(void) const
The mach number calculated using the vehicle X axis velocity.
FGMatrix33 Transposed(void) const
Transposed matrix.
~FGAuxiliary()
Destructor.
double Magnitude(void) const
Length of the vector.
double GetLatitudeDeg() const
Get the latitude.
bool Run(bool Holding)
Runs the Auxiliary routines; called by the Executive Can pass in a value indicating if the executive ...
double GetMach(void) const
Gets the Mach number.
double GetTotalPressure(void) const
Returns the total pressure.
double GetVcalibratedKTS(void) const
Returns Calibrated airspeed in knots.
Encapsulates the JSBSim simulation executive.
double GetVground(void) const
Gets the ground speed in feet per second.
double GetVt(void) const
Gets the magnitude of total vehicle velocity including wind effects in feet per second.