1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGAuxiliary.cpp
4 Author: Tony Peden, Jon Berndt
6 Purpose: Calculates additional parameters needed by the visual system, etc.
9 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU Lesser General Public License as published by the Free Software
13 Foundation; either version 2 of the License, or (at your option) any later
16 This program is distributed in the hope that it will be useful, but WITHOUT
17 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
21 You should have received a copy of the GNU Lesser General Public License along with
22 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 Place - Suite 330, Boston, MA 02111-1307, USA.
25 Further information about the GNU Lesser General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class calculates various auxiliary parameters.
33 Anderson, John D. "Introduction to Flight", 3rd Edition, McGraw-Hill, 1989
36 --------------------------------------------------------------------------------
39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
45 #include "FGAuxiliary.h"
46 #include "FGFDMExec.h"
47 #include "input_output/FGPropertyManager.h"
53 static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.55 2011/11/12 18:59:11 bcoconni Exp $";
54 static const char *IdHdr = ID_AUXILIARY;
56 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
58 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
61 FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex)
66 tatc = RankineToCelsius(tat);
69 qbar = qbarUW = qbarUV = 0.0;
73 gamma = Vt = Vground = 0.0;
77 hoverbmac = hoverbcg = 0.0;
80 lon_relative_position = lat_relative_position = relative_position = 0.0;
82 vPilotAccel.InitMatrix();
83 vPilotAccelN.InitMatrix();
84 vAeroUVW.InitMatrix();
85 vAeroPQR.InitMatrix();
86 vMachUVW.InitMatrix();
88 vEulerRates.InitMatrix();
95 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
97 bool FGAuxiliary::InitModel(void)
100 tat = in.Temperature;
101 tatc = RankineToCelsius(tat);
104 qbar = qbarUW = qbarUV = 0.0;
108 gamma = Vt = Vground = 0.0;
111 seconds_in_day = 0.0;
112 hoverbmac = hoverbcg = 0.0;
115 lon_relative_position = lat_relative_position = relative_position = 0.0;
117 vPilotAccel.InitMatrix();
118 vPilotAccelN.InitMatrix();
119 vAeroUVW.InitMatrix();
120 vAeroPQR.InitMatrix();
121 vMachUVW.InitMatrix();
123 vEulerRates.InitMatrix();
128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130 FGAuxiliary::~FGAuxiliary()
135 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
137 bool FGAuxiliary::Run(bool Holding)
141 if (FGModel::Run(Holding)) return true; // return true if error returned from base class
142 if (Holding) return false;
148 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
149 if (in.CosTht != 0.0) {
150 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
151 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
154 // Combine the wind speed with aircraft speed to obtain wind relative speed
155 vAeroPQR = in.vPQR - in.TurbPQR;
156 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
158 Vt = vAeroUVW.Magnitude();
159 alpha = beta = adot = bdot = 0;
160 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
161 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
162 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
163 double mUW = AeroU2 + AeroW2;
168 if (vAeroUVW(eW) != 0.0)
169 alpha = AeroU2 > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
170 if (vAeroUVW(eV) != 0.0)
171 beta = mUW > 0.0 ? atan2(vAeroUVW(eV), sqrt(mUW)) : 0.0;
174 if (vAeroUVW(eU) < 0.0) signU=-1;
177 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
178 bdot = (signU*mUW*in.vUVWdot(eV)
179 - vAeroUVW(eV)*(vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eW)*in.vUVWdot(eW)))/(Vt2*sqrt(mUW));
183 UpdateWindMatrices();
185 Re = Vt * in.Wingchord / in.KinematicViscosity;
187 double densityD2 = 0.5*in.Density;
189 qbar = densityD2 * Vt2;
190 qbarUW = densityD2 * (mUW);
191 qbarUV = densityD2 * (AeroU2 + AeroV2);
192 Mach = Vt / in.SoundSpeed;
193 MachU = vMachUVW(eU) = vAeroUVW(eU) / in.SoundSpeed;
194 vMachUVW(eV) = vAeroUVW(eV) / in.SoundSpeed;
195 vMachUVW(eW) = vAeroUVW(eW) / in.SoundSpeed;
196 double MachU2 = MachU * MachU;
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); // Total Temperature, isentropic flow
207 tatc = RankineToCelsius(tat);
209 if (MachU < 1) { // Calculate total pressure assuming isentropic flow
210 pt = in.Pressure*pow((1 + 0.2*MachU2),3.5);
212 // Use Rayleigh pitot tube formula for normal shock in front of pitot tube
213 B = 5.76 * MachU2 / (5.6*MachU2 - 0.8);
214 D = (2.8 * MachU2 - 0.4) * 0.4167;
215 pt = in.Pressure*pow(B,3.5)*D;
218 A = pow(((pt-in.Pressure)/in.PressureSL + 1),0.28571);
220 vcas = sqrt(7 * in.PressureSL / in.DensitySL * (A-1));
221 veas = sqrt(2 * qbar / in.DensitySL);
222 vtrue = 1116.43559 * MachU * sqrt(in.Temperature / 518.67);
224 vcas = veas = vtrue = 0.0;
227 vPilotAccel.InitMatrix();
228 vNcg = in.vBodyAccel/in.SLGravity;
230 // Nz is Acceleration in "g's", along normal axis (-Z body axis)
232 vPilotAccel = in.vBodyAccel + in.vPQRdot * in.ToEyePt;
233 vPilotAccel += in.vPQR * (in.vPQR * in.ToEyePt);
235 // The line below handles low velocity (and on-ground) cases, basically
236 // representing the opposite of the force that the landing gear would
237 // exert on the ground (which is just the total weight). This eliminates
238 // any jitter that could be introduced by the landing gear. Theoretically,
239 // this branch could be eliminated, with a penalty of having a short
240 // transient at startup (lasting only a fraction of a second).
241 vPilotAccel = in.Tl2b * FGColumnVector3( 0.0, 0.0, -in.SLGravity );
242 Nz = -vPilotAccel(eZ) / in.SLGravity;
245 vNwcg = mTb2w * vNcg;
246 vNwcg(eZ) = 1.0 - vNwcg(eZ);
248 vPilotAccelN = vPilotAccel / in.SLGravity;
251 vLocationVRP = in.vLocation.LocalToLocation( in.Tb2l * in.VRPBody );
253 // Recompute some derived values now that we know the dependent parameters values ...
254 hoverbcg = in.DistanceAGL / in.Wingspan;
256 FGColumnVector3 vMac = in.Tb2l * in.RPBody;
257 hoverbmac = (in.DistanceAGL + vMac(3)) / in.Wingspan;
259 // When all models are executed calculate the distance from the initial point.
260 CalculateRelativePosition();
267 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
269 // From Stevens and Lewis, "Aircraft Control and Simulation", 3rd Ed., the
270 // transformation from body to wind axes is defined (where "a" is alpha and "B"
273 // cos(a)*cos(B) sin(B) sin(a)*cos(B)
274 // -cos(a)*sin(B) cos(B) -sin(a)*sin(B)
277 // The transform from wind to body axes is then,
279 // cos(a)*cos(B) -cos(a)*sin(B) -sin(a)
281 // sin(a)*cos(B) -sin(a)*sin(B) cos(a)
283 void FGAuxiliary::UpdateWindMatrices(void)
285 double ca, cb, sa, sb;
302 mTb2w = mTw2b.Transposed();
305 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
307 // A positive headwind is blowing with you, a negative headwind is blowing against you.
308 // psi is the direction the wind is blowing *towards*.
309 // ToDo: should this simply be in the atmosphere class? Same with Get Crosswind.
311 double FGAuxiliary::GetHeadWind(void) const
313 return in.Vwind * cos(in.WindPsi - in.Psi);
316 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
318 // A positive crosswind is blowing towards the right (from teh perspective of the
319 // pilot). A negative crosswind is blowing towards the -Y direction (left).
320 // psi is the direction the wind is blowing *towards*.
322 double FGAuxiliary::GetCrossWind(void) const
324 return in.Vwind * sin(in.WindPsi - in.Psi);
327 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
329 double FGAuxiliary::GethVRP(void) const
331 return vLocationVRP.GetRadius() - in.ReferenceRadius;
334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
336 double FGAuxiliary::GetNlf(void) const
339 return (-in.vFw(3))/(in.Mass*slugtolb);
344 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
346 void FGAuxiliary::CalculateRelativePosition(void) //ToDo: This belongs elsewhere - perhaps in FGPropagate or Exec
348 const double earth_radius_mt = in.ReferenceRadius*fttom;
349 lat_relative_position=(in.Latitude - FDMExec->GetIC()->GetLatitudeDegIC() *degtorad)*earth_radius_mt;
350 lon_relative_position=(in.Longitude - FDMExec->GetIC()->GetLongitudeDegIC()*degtorad)*earth_radius_mt;
351 relative_position = sqrt(lat_relative_position*lat_relative_position + lon_relative_position*lon_relative_position);
354 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
356 void FGAuxiliary::bind(void)
358 typedef double (FGAuxiliary::*PMF)(int) const;
359 typedef double (FGAuxiliary::*PF)(void) const;
360 PropertyManager->Tie("propulsion/tat-r", this, &FGAuxiliary::GetTotalTemperature);
361 PropertyManager->Tie("propulsion/tat-c", this, &FGAuxiliary::GetTAT_C);
362 PropertyManager->Tie("propulsion/pt-lbs_sqft", this, &FGAuxiliary::GetTotalPressure);
363 PropertyManager->Tie("velocities/vc-fps", this, &FGAuxiliary::GetVcalibratedFPS);
364 PropertyManager->Tie("velocities/vc-kts", this, &FGAuxiliary::GetVcalibratedKTS);
365 PropertyManager->Tie("velocities/ve-fps", this, &FGAuxiliary::GetVequivalentFPS);
366 PropertyManager->Tie("velocities/ve-kts", this, &FGAuxiliary::GetVequivalentKTS);
367 PropertyManager->Tie("velocities/vtrue-fps", this, &FGAuxiliary::GetVtrueFPS);
368 PropertyManager->Tie("velocities/vtrue-kts", this, &FGAuxiliary::GetVtrueKTS);
369 PropertyManager->Tie("velocities/machU", this, &FGAuxiliary::GetMachU);
370 PropertyManager->Tie("velocities/p-aero-rad_sec", this, eX, (PMF)&FGAuxiliary::GetAeroPQR);
371 PropertyManager->Tie("velocities/q-aero-rad_sec", this, eY, (PMF)&FGAuxiliary::GetAeroPQR);
372 PropertyManager->Tie("velocities/r-aero-rad_sec", this, eZ, (PMF)&FGAuxiliary::GetAeroPQR);
373 PropertyManager->Tie("velocities/phidot-rad_sec", this, ePhi, (PMF)&FGAuxiliary::GetEulerRates);
374 PropertyManager->Tie("velocities/thetadot-rad_sec", this, eTht, (PMF)&FGAuxiliary::GetEulerRates);
375 PropertyManager->Tie("velocities/psidot-rad_sec", this, ePsi, (PMF)&FGAuxiliary::GetEulerRates);
376 PropertyManager->Tie("velocities/u-aero-fps", this, eU, (PMF)&FGAuxiliary::GetAeroUVW);
377 PropertyManager->Tie("velocities/v-aero-fps", this, eV, (PMF)&FGAuxiliary::GetAeroUVW);
378 PropertyManager->Tie("velocities/w-aero-fps", this, eW, (PMF)&FGAuxiliary::GetAeroUVW);
379 PropertyManager->Tie("velocities/vt-fps", this, &FGAuxiliary::GetVt);
380 PropertyManager->Tie("velocities/mach", this, &FGAuxiliary::GetMach);
381 PropertyManager->Tie("velocities/vg-fps", this, &FGAuxiliary::GetVground);
382 PropertyManager->Tie("accelerations/a-pilot-x-ft_sec2", this, eX, (PMF)&FGAuxiliary::GetPilotAccel);
383 PropertyManager->Tie("accelerations/a-pilot-y-ft_sec2", this, eY, (PMF)&FGAuxiliary::GetPilotAccel);
384 PropertyManager->Tie("accelerations/a-pilot-z-ft_sec2", this, eZ, (PMF)&FGAuxiliary::GetPilotAccel);
385 PropertyManager->Tie("accelerations/n-pilot-x-norm", this, eX, (PMF)&FGAuxiliary::GetNpilot);
386 PropertyManager->Tie("accelerations/n-pilot-y-norm", this, eY, (PMF)&FGAuxiliary::GetNpilot);
387 PropertyManager->Tie("accelerations/n-pilot-z-norm", this, eZ, (PMF)&FGAuxiliary::GetNpilot);
388 PropertyManager->Tie("accelerations/Nz", this, &FGAuxiliary::GetNz);
389 PropertyManager->Tie("forces/load-factor", this, &FGAuxiliary::GetNlf);
390 /* PropertyManager->Tie("atmosphere/headwind-fps", this, &FGAuxiliary::GetHeadWind, true);
391 PropertyManager->Tie("atmosphere/crosswind-fps", this, &FGAuxiliary::GetCrossWind, true); */
392 PropertyManager->Tie("aero/alpha-rad", this, (PF)&FGAuxiliary::Getalpha);
393 PropertyManager->Tie("aero/beta-rad", this, (PF)&FGAuxiliary::Getbeta);
394 PropertyManager->Tie("aero/mag-beta-rad", this, (PF)&FGAuxiliary::GetMagBeta);
395 PropertyManager->Tie("aero/alpha-deg", this, inDegrees, (PMF)&FGAuxiliary::Getalpha);
396 PropertyManager->Tie("aero/beta-deg", this, inDegrees, (PMF)&FGAuxiliary::Getbeta);
397 PropertyManager->Tie("aero/mag-beta-deg", this, inDegrees, (PMF)&FGAuxiliary::GetMagBeta);
398 PropertyManager->Tie("aero/Re", this, &FGAuxiliary::GetReynoldsNumber);
399 PropertyManager->Tie("aero/qbar-psf", this, &FGAuxiliary::Getqbar);
400 PropertyManager->Tie("aero/qbarUW-psf", this, &FGAuxiliary::GetqbarUW);
401 PropertyManager->Tie("aero/qbarUV-psf", this, &FGAuxiliary::GetqbarUV);
402 PropertyManager->Tie("aero/alphadot-rad_sec", this, (PF)&FGAuxiliary::Getadot);
403 PropertyManager->Tie("aero/betadot-rad_sec", this, (PF)&FGAuxiliary::Getbdot);
404 PropertyManager->Tie("aero/alphadot-deg_sec", this, inDegrees, (PMF)&FGAuxiliary::Getadot);
405 PropertyManager->Tie("aero/betadot-deg_sec", this, inDegrees, (PMF)&FGAuxiliary::Getbdot);
406 PropertyManager->Tie("aero/h_b-cg-ft", this, &FGAuxiliary::GetHOverBCG);
407 PropertyManager->Tie("aero/h_b-mac-ft", this, &FGAuxiliary::GetHOverBMAC);
408 PropertyManager->Tie("flight-path/gamma-rad", this, &FGAuxiliary::GetGamma);
409 PropertyManager->Tie("flight-path/psi-gt-rad", this, &FGAuxiliary::GetGroundTrack);
411 PropertyManager->Tie("position/distance-from-start-lon-mt", this, &FGAuxiliary::GetLongitudeRelativePosition);
412 PropertyManager->Tie("position/distance-from-start-lat-mt", this, &FGAuxiliary::GetLatitudeRelativePosition);
413 PropertyManager->Tie("position/distance-from-start-mag-mt", this, &FGAuxiliary::GetDistanceRelativePosition);
414 PropertyManager->Tie("position/vrp-gc-latitude_deg", &vLocationVRP, &FGLocation::GetLatitudeDeg);
415 PropertyManager->Tie("position/vrp-longitude_deg", &vLocationVRP, &FGLocation::GetLongitudeDeg);
416 PropertyManager->Tie("position/vrp-radius-ft", &vLocationVRP, &FGLocation::GetRadius);
419 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
421 double FGAuxiliary::BadUnits(void) const
423 cerr << "Bad units" << endl; return 0.0;
426 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
427 // The bitmasked value choices are as follows:
428 // unset: In this case (the default) JSBSim would only print
429 // out the normally expected messages, essentially echoing
430 // the config files as they are read. If the environment
431 // variable is not set, debug_lvl is set to 1 internally
432 // 0: This requests JSBSim not to output any messages
434 // 1: This value explicity requests the normal JSBSim
436 // 2: This value asks for a message to be printed out when
437 // a class is instantiated
438 // 4: When this value is set, a message is displayed when a
439 // FGModel object executes its Run() method
440 // 8: When this value is set, various runtime state variables
441 // are printed out periodically
442 // 16: When set various parameters are sanity checked and
443 // a message is printed out when they go out of bounds
445 void FGAuxiliary::Debug(int from)
447 if (debug_lvl <= 0) return;
449 if (debug_lvl & 1) { // Standard console startup message output
450 if (from == 0) { // Constructor
454 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
455 if (from == 0) cout << "Instantiated: FGAuxiliary" << endl;
456 if (from == 1) cout << "Destroyed: FGAuxiliary" << endl;
458 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
460 if (debug_lvl & 8 ) { // Runtime state variables
462 if (debug_lvl & 16) { // Sanity checking
463 if (Mach > 100 || Mach < 0.00)
464 cout << "FGPropagate::Mach is out of bounds: " << Mach << endl;
465 if (qbar > 1e6 || qbar < 0.00)
466 cout << "FGPropagate::qbar is out of bounds: " << qbar << endl;
468 if (debug_lvl & 64) {
469 if (from == 0) { // Constructor
470 cout << IdSrc << endl;
471 cout << IdHdr << endl;
476 } // namespace JSBSim