45 #include "FGAuxiliary.h"
46 #include "initialization/FGInitialCondition.h"
47 #include "FGFDMExec.h"
48 #include "input_output/FGPropertyManager.h"
49 #include "FGInertial.h"
68 qbar = qbarUW = qbarUV = 0.0;
72 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();
94 bool FGAuxiliary::InitModel(
void)
96 if (!FGModel::InitModel())
return false;
103 qbar = qbarUW = qbarUV = 0.0;
107 gamma = Vt = Vground = 0.0;
110 seconds_in_day = 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();
137 if (Holding)
return false;
141 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
142 if (in.CosTht != 0.0) {
143 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
144 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
148 vAeroPQR = in.vPQR - in.TurbPQR;
149 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
151 alpha = beta = adot = bdot = 0;
152 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
153 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
154 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
155 double mUW = AeroU2 + AeroW2;
157 double Vt2 = mUW + AeroV2;
161 beta = atan2(vAeroUVW(eV), sqrt(mUW));
164 alpha = atan2(vAeroUVW(eW), vAeroUVW(eU));
165 double Vtdot = (vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eV)*in.vUVWdot(eV) + vAeroUVW(eW)*in.vUVWdot(eW))/Vt;
166 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
167 bdot = (in.vUVWdot(eV)*Vt - vAeroUVW(eV)*Vtdot)/(Vt*sqrt(mUW));
171 UpdateWindMatrices();
173 Re = Vt * in.Wingchord / in.KinematicViscosity;
175 double densityD2 = 0.5*in.Density;
177 qbar = densityD2 * Vt2;
178 qbarUW = densityD2 * (mUW);
179 qbarUV = densityD2 * (AeroU2 + AeroV2);
180 Mach = Vt / in.SoundSpeed;
181 MachU = vMachUVW(eU) = vAeroUVW(eU) / in.SoundSpeed;
182 vMachUVW(eV) = vAeroUVW(eV) / in.SoundSpeed;
183 vMachUVW(eW) = vAeroUVW(eW) / in.SoundSpeed;
187 Vground = sqrt( in.vVel(eNorth)*in.vVel(eNorth) + in.vVel(eEast)*in.vVel(eEast) );
189 psigt = atan2(in.vVel(eEast), in.vVel(eNorth));
190 if (psigt < 0.0) psigt += 2*M_PI;
191 gamma = atan2(-in.vVel(eDown), Vground);
193 tat = in.Temperature*(1 + 0.2*Mach*Mach);
198 if (abs(Mach) > 0.0) {
200 veas = sqrt(2 * qbar / in.DensitySL);
205 vPilotAccel.InitMatrix();
206 vNcg = in.vBodyAccel/in.StandardGravity;
211 vPilotAccel = in.vBodyAccel + in.vPQRidot * in.ToEyePt;
212 vPilotAccel += in.vPQRi * (in.vPQRi * in.ToEyePt);
214 vNwcg = mTb2w * vNcg;
215 vNwcg(eZ) = 1.0 - vNwcg(eZ);
217 vPilotAccelN = vPilotAccel / in.StandardGravity;
223 hoverbcg = in.DistanceAGL / in.Wingspan;
226 hoverbmac = (in.DistanceAGL - vMac(3)) / in.Wingspan;
247 void FGAuxiliary::UpdateWindMatrices(
void)
249 double ca, cb, sa, sb;
271 double FGAuxiliary::GetNlf(
void)
const
274 return (in.vFw(3))/(in.Mass*
slugtolb);
281 double FGAuxiliary::GetLongitudeRelativePosition(
void)
const
289 double FGAuxiliary::GetLatitudeRelativePosition(
void)
const
297 double FGAuxiliary::GetDistanceRelativePosition(
void)
const
299 FGInitialCondition *ic = FDMExec->
GetIC();
301 ic->GetLatitudeRadIC())* fttom;
306 void FGAuxiliary::bind(
void)
311 PropertyManager->
Tie(
"propulsion/tat-c",
this, &FGAuxiliary::GetTAT_C);
320 PropertyManager->
Tie(
"velocities/p-aero-rad_sec",
this, eX, (PMF)&FGAuxiliary::GetAeroPQR);
321 PropertyManager->
Tie(
"velocities/q-aero-rad_sec",
this, eY, (PMF)&FGAuxiliary::GetAeroPQR);
322 PropertyManager->
Tie(
"velocities/r-aero-rad_sec",
this, eZ, (PMF)&FGAuxiliary::GetAeroPQR);
323 PropertyManager->
Tie(
"velocities/phidot-rad_sec",
this, ePhi, (PMF)&FGAuxiliary::GetEulerRates);
324 PropertyManager->
Tie(
"velocities/thetadot-rad_sec",
this, eTht, (PMF)&FGAuxiliary::GetEulerRates);
325 PropertyManager->
Tie(
"velocities/psidot-rad_sec",
this, ePsi, (PMF)&FGAuxiliary::GetEulerRates);
326 PropertyManager->
Tie(
"velocities/u-aero-fps",
this, eU, (PMF)&FGAuxiliary::GetAeroUVW);
327 PropertyManager->
Tie(
"velocities/v-aero-fps",
this, eV, (PMF)&FGAuxiliary::GetAeroUVW);
328 PropertyManager->
Tie(
"velocities/w-aero-fps",
this, eW, (PMF)&FGAuxiliary::GetAeroUVW);
332 PropertyManager->
Tie(
"accelerations/a-pilot-x-ft_sec2",
this, eX, (PMF)&FGAuxiliary::GetPilotAccel);
333 PropertyManager->
Tie(
"accelerations/a-pilot-y-ft_sec2",
this, eY, (PMF)&FGAuxiliary::GetPilotAccel);
334 PropertyManager->
Tie(
"accelerations/a-pilot-z-ft_sec2",
this, eZ, (PMF)&FGAuxiliary::GetPilotAccel);
335 PropertyManager->
Tie(
"accelerations/n-pilot-x-norm",
this, eX, (PMF)&FGAuxiliary::GetNpilot);
336 PropertyManager->
Tie(
"accelerations/n-pilot-y-norm",
this, eY, (PMF)&FGAuxiliary::GetNpilot);
337 PropertyManager->
Tie(
"accelerations/n-pilot-z-norm",
this, eZ, (PMF)&FGAuxiliary::GetNpilot);
341 PropertyManager->
Tie(
"forces/load-factor",
this, &FGAuxiliary::GetNlf);
342 PropertyManager->
Tie(
"aero/alpha-rad",
this, (PF)&FGAuxiliary::Getalpha);
343 PropertyManager->
Tie(
"aero/beta-rad",
this, (PF)&FGAuxiliary::Getbeta);
344 PropertyManager->
Tie(
"aero/mag-beta-rad",
this, (PF)&FGAuxiliary::GetMagBeta);
345 PropertyManager->
Tie(
"aero/alpha-deg",
this, inDegrees, (PMF)&FGAuxiliary::Getalpha);
346 PropertyManager->
Tie(
"aero/beta-deg",
this, inDegrees, (PMF)&FGAuxiliary::Getbeta);
347 PropertyManager->
Tie(
"aero/mag-beta-deg",
this, inDegrees, (PMF)&FGAuxiliary::GetMagBeta);
348 PropertyManager->
Tie(
"aero/Re",
this, &FGAuxiliary::GetReynoldsNumber);
349 PropertyManager->
Tie(
"aero/qbar-psf",
this, &FGAuxiliary::Getqbar);
350 PropertyManager->
Tie(
"aero/qbarUW-psf",
this, &FGAuxiliary::GetqbarUW);
351 PropertyManager->
Tie(
"aero/qbarUV-psf",
this, &FGAuxiliary::GetqbarUV);
352 PropertyManager->
Tie(
"aero/alphadot-rad_sec",
this, (PF)&FGAuxiliary::Getadot);
353 PropertyManager->
Tie(
"aero/betadot-rad_sec",
this, (PF)&FGAuxiliary::Getbdot);
354 PropertyManager->
Tie(
"aero/alphadot-deg_sec",
this, inDegrees, (PMF)&FGAuxiliary::Getadot);
355 PropertyManager->
Tie(
"aero/betadot-deg_sec",
this, inDegrees, (PMF)&FGAuxiliary::Getbdot);
356 PropertyManager->
Tie(
"aero/h_b-cg-ft",
this, &FGAuxiliary::GetHOverBCG);
357 PropertyManager->
Tie(
"aero/h_b-mac-ft",
this, &FGAuxiliary::GetHOverBMAC);
358 PropertyManager->
Tie(
"flight-path/gamma-rad",
this, &FGAuxiliary::GetGamma);
359 PropertyManager->
Tie(
"flight-path/gamma-deg",
this, inDegrees, (PMF)&FGAuxiliary::GetGamma);
360 PropertyManager->
Tie(
"flight-path/psi-gt-rad",
this, &FGAuxiliary::GetGroundTrack);
362 PropertyManager->
Tie(
"position/distance-from-start-lon-mt",
this, &FGAuxiliary::GetLongitudeRelativePosition);
363 PropertyManager->
Tie(
"position/distance-from-start-lat-mt",
this, &FGAuxiliary::GetLatitudeRelativePosition);
364 PropertyManager->
Tie(
"position/distance-from-start-mag-mt",
this, &FGAuxiliary::GetDistanceRelativePosition);
372 double FGAuxiliary::BadUnits(
void)
const
374 cerr <<
"Bad units" << endl;
return 0.0;
396 void FGAuxiliary::Debug(
int from)
398 if (debug_lvl <= 0)
return;
405 if (debug_lvl & 2 ) {
406 if (from == 0) cout <<
"Instantiated: FGAuxiliary" << endl;
407 if (from == 1) cout <<
"Destroyed: FGAuxiliary" << endl;
409 if (debug_lvl & 4 ) {
411 if (debug_lvl & 8 ) {
413 if (debug_lvl & 16) {
414 if (Mach > 100 || Mach < 0.00)
415 cout <<
"FGPropagate::Mach is out of bounds: " << Mach << endl;
416 if (qbar > 1e6 || qbar < 0.00)
417 cout <<
"FGPropagate::qbar is out of bounds: " << qbar << endl;
419 if (debug_lvl & 64) {