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.56 2011/12/11 17:03:05 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;
146 vEulerRates(eTht) = in.vPQR(eQ)*in.CosPhi - in.vPQR(eR)*in.SinPhi;
147 if (in.CosTht != 0.0) {
148 vEulerRates(ePsi) = (in.vPQR(eQ)*in.SinPhi + in.vPQR(eR)*in.CosPhi)/in.CosTht;
149 vEulerRates(ePhi) = in.vPQR(eP) + vEulerRates(ePsi)*in.SinTht;
152 // Combine the wind speed with aircraft speed to obtain wind relative speed
153 vAeroPQR = in.vPQR - in.TurbPQR;
154 vAeroUVW = in.vUVW - in.Tl2b * in.TotalWindNED;
156 Vt = vAeroUVW.Magnitude();
157 alpha = beta = adot = bdot = 0;
158 double AeroU2 = vAeroUVW(eU)*vAeroUVW(eU);
159 double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV);
160 double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW);
161 double mUW = AeroU2 + AeroW2;
166 if (vAeroUVW(eW) != 0.0)
167 alpha = AeroU2 > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0;
168 if (vAeroUVW(eV) != 0.0)
169 beta = mUW > 0.0 ? atan2(vAeroUVW(eV), sqrt(mUW)) : 0.0;
172 if (vAeroUVW(eU) < 0.0) signU=-1;
175 adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW;
176 bdot = (signU*mUW*in.vUVWdot(eV)
177 - vAeroUVW(eV)*(vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eW)*in.vUVWdot(eW)))/(Vt2*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;
194 double MachU2 = MachU * MachU;
198 Vground = sqrt( in.vVel(eNorth)*in.vVel(eNorth) + in.vVel(eEast)*in.vVel(eEast) );
200 psigt = atan2(in.vVel(eEast), in.vVel(eNorth));
201 if (psigt < 0.0) psigt += 2*M_PI;
202 gamma = atan2(-in.vVel(eDown), Vground);
204 tat = in.Temperature*(1 + 0.2*Mach*Mach); // Total Temperature, isentropic flow
205 tatc = RankineToCelsius(tat);
207 if (MachU < 1) { // Calculate total pressure assuming isentropic flow
208 pt = in.Pressure*pow((1 + 0.2*MachU2),3.5);
210 // Use Rayleigh pitot tube formula for normal shock in front of pitot tube
211 B = 5.76 * MachU2 / (5.6*MachU2 - 0.8);
212 D = (2.8 * MachU2 - 0.4) * 0.4167;
213 pt = in.Pressure*pow(B,3.5)*D;
216 A = pow(((pt-in.Pressure)/in.PressureSL + 1),0.28571);
218 vcas = sqrt(7 * in.PressureSL / in.DensitySL * (A-1));
219 veas = sqrt(2 * qbar / in.DensitySL);
220 vtrue = 1116.43559 * MachU * sqrt(in.Temperature / 518.67);
222 vcas = veas = vtrue = 0.0;
225 vPilotAccel.InitMatrix();
226 vNcg = in.vBodyAccel/in.SLGravity;
228 // Nz is Acceleration in "g's", along normal axis (-Z body axis)
230 vPilotAccel = in.vBodyAccel + in.vPQRdot * in.ToEyePt;
231 vPilotAccel += in.vPQR * (in.vPQR * in.ToEyePt);
233 // The line below handles low velocity (and on-ground) cases, basically
234 // representing the opposite of the force that the landing gear would
235 // exert on the ground (which is just the total weight). This eliminates
236 // any jitter that could be introduced by the landing gear. Theoretically,
237 // this branch could be eliminated, with a penalty of having a short
238 // transient at startup (lasting only a fraction of a second).
239 vPilotAccel = in.Tl2b * FGColumnVector3( 0.0, 0.0, -in.SLGravity );
240 Nz = -vPilotAccel(eZ) / in.SLGravity;
243 vNwcg = mTb2w * vNcg;
244 vNwcg(eZ) = 1.0 - vNwcg(eZ);
246 vPilotAccelN = vPilotAccel / in.SLGravity;
249 vLocationVRP = in.vLocation.LocalToLocation( in.Tb2l * in.VRPBody );
251 // Recompute some derived values now that we know the dependent parameters values ...
252 hoverbcg = in.DistanceAGL / in.Wingspan;
254 FGColumnVector3 vMac = in.Tb2l * in.RPBody;
255 hoverbmac = (in.DistanceAGL + vMac(3)) / in.Wingspan;
257 // When all models are executed calculate the distance from the initial point.
258 CalculateRelativePosition();
263 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
265 // From Stevens and Lewis, "Aircraft Control and Simulation", 3rd Ed., the
266 // transformation from body to wind axes is defined (where "a" is alpha and "B"
269 // cos(a)*cos(B) sin(B) sin(a)*cos(B)
270 // -cos(a)*sin(B) cos(B) -sin(a)*sin(B)
273 // The transform from wind to body axes is then,
275 // cos(a)*cos(B) -cos(a)*sin(B) -sin(a)
277 // sin(a)*cos(B) -sin(a)*sin(B) cos(a)
279 void FGAuxiliary::UpdateWindMatrices(void)
281 double ca, cb, sa, sb;
298 mTb2w = mTw2b.Transposed();
301 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
303 // A positive headwind is blowing with you, a negative headwind is blowing against you.
304 // psi is the direction the wind is blowing *towards*.
305 // ToDo: should this simply be in the atmosphere class? Same with Get Crosswind.
307 double FGAuxiliary::GetHeadWind(void) const
309 return in.Vwind * cos(in.WindPsi - in.Psi);
312 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
314 // A positive crosswind is blowing towards the right (from teh perspective of the
315 // pilot). A negative crosswind is blowing towards the -Y direction (left).
316 // psi is the direction the wind is blowing *towards*.
318 double FGAuxiliary::GetCrossWind(void) const
320 return in.Vwind * sin(in.WindPsi - in.Psi);
323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 double FGAuxiliary::GethVRP(void) const
327 return vLocationVRP.GetRadius() - in.ReferenceRadius;
330 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
332 double FGAuxiliary::GetNlf(void) const
335 return (-in.vFw(3))/(in.Mass*slugtolb);
340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
342 void FGAuxiliary::CalculateRelativePosition(void) //ToDo: This belongs elsewhere - perhaps in FGPropagate or Exec
344 const double earth_radius_mt = in.ReferenceRadius*fttom;
345 lat_relative_position=(in.Latitude - FDMExec->GetIC()->GetLatitudeDegIC() *degtorad)*earth_radius_mt;
346 lon_relative_position=(in.Longitude - FDMExec->GetIC()->GetLongitudeDegIC()*degtorad)*earth_radius_mt;
347 relative_position = sqrt(lat_relative_position*lat_relative_position + lon_relative_position*lon_relative_position);
350 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
352 void FGAuxiliary::bind(void)
354 typedef double (FGAuxiliary::*PMF)(int) const;
355 typedef double (FGAuxiliary::*PF)(void) const;
356 PropertyManager->Tie("propulsion/tat-r", this, &FGAuxiliary::GetTotalTemperature);
357 PropertyManager->Tie("propulsion/tat-c", this, &FGAuxiliary::GetTAT_C);
358 PropertyManager->Tie("propulsion/pt-lbs_sqft", this, &FGAuxiliary::GetTotalPressure);
359 PropertyManager->Tie("velocities/vc-fps", this, &FGAuxiliary::GetVcalibratedFPS);
360 PropertyManager->Tie("velocities/vc-kts", this, &FGAuxiliary::GetVcalibratedKTS);
361 PropertyManager->Tie("velocities/ve-fps", this, &FGAuxiliary::GetVequivalentFPS);
362 PropertyManager->Tie("velocities/ve-kts", this, &FGAuxiliary::GetVequivalentKTS);
363 PropertyManager->Tie("velocities/vtrue-fps", this, &FGAuxiliary::GetVtrueFPS);
364 PropertyManager->Tie("velocities/vtrue-kts", this, &FGAuxiliary::GetVtrueKTS);
365 PropertyManager->Tie("velocities/machU", this, &FGAuxiliary::GetMachU);
366 PropertyManager->Tie("velocities/p-aero-rad_sec", this, eX, (PMF)&FGAuxiliary::GetAeroPQR);
367 PropertyManager->Tie("velocities/q-aero-rad_sec", this, eY, (PMF)&FGAuxiliary::GetAeroPQR);
368 PropertyManager->Tie("velocities/r-aero-rad_sec", this, eZ, (PMF)&FGAuxiliary::GetAeroPQR);
369 PropertyManager->Tie("velocities/phidot-rad_sec", this, ePhi, (PMF)&FGAuxiliary::GetEulerRates);
370 PropertyManager->Tie("velocities/thetadot-rad_sec", this, eTht, (PMF)&FGAuxiliary::GetEulerRates);
371 PropertyManager->Tie("velocities/psidot-rad_sec", this, ePsi, (PMF)&FGAuxiliary::GetEulerRates);
372 PropertyManager->Tie("velocities/u-aero-fps", this, eU, (PMF)&FGAuxiliary::GetAeroUVW);
373 PropertyManager->Tie("velocities/v-aero-fps", this, eV, (PMF)&FGAuxiliary::GetAeroUVW);
374 PropertyManager->Tie("velocities/w-aero-fps", this, eW, (PMF)&FGAuxiliary::GetAeroUVW);
375 PropertyManager->Tie("velocities/vt-fps", this, &FGAuxiliary::GetVt);
376 PropertyManager->Tie("velocities/mach", this, &FGAuxiliary::GetMach);
377 PropertyManager->Tie("velocities/vg-fps", this, &FGAuxiliary::GetVground);
378 PropertyManager->Tie("accelerations/a-pilot-x-ft_sec2", this, eX, (PMF)&FGAuxiliary::GetPilotAccel);
379 PropertyManager->Tie("accelerations/a-pilot-y-ft_sec2", this, eY, (PMF)&FGAuxiliary::GetPilotAccel);
380 PropertyManager->Tie("accelerations/a-pilot-z-ft_sec2", this, eZ, (PMF)&FGAuxiliary::GetPilotAccel);
381 PropertyManager->Tie("accelerations/n-pilot-x-norm", this, eX, (PMF)&FGAuxiliary::GetNpilot);
382 PropertyManager->Tie("accelerations/n-pilot-y-norm", this, eY, (PMF)&FGAuxiliary::GetNpilot);
383 PropertyManager->Tie("accelerations/n-pilot-z-norm", this, eZ, (PMF)&FGAuxiliary::GetNpilot);
384 PropertyManager->Tie("accelerations/Nz", this, &FGAuxiliary::GetNz);
385 PropertyManager->Tie("forces/load-factor", this, &FGAuxiliary::GetNlf);
386 /* PropertyManager->Tie("atmosphere/headwind-fps", this, &FGAuxiliary::GetHeadWind, true);
387 PropertyManager->Tie("atmosphere/crosswind-fps", this, &FGAuxiliary::GetCrossWind, true); */
388 PropertyManager->Tie("aero/alpha-rad", this, (PF)&FGAuxiliary::Getalpha);
389 PropertyManager->Tie("aero/beta-rad", this, (PF)&FGAuxiliary::Getbeta);
390 PropertyManager->Tie("aero/mag-beta-rad", this, (PF)&FGAuxiliary::GetMagBeta);
391 PropertyManager->Tie("aero/alpha-deg", this, inDegrees, (PMF)&FGAuxiliary::Getalpha);
392 PropertyManager->Tie("aero/beta-deg", this, inDegrees, (PMF)&FGAuxiliary::Getbeta);
393 PropertyManager->Tie("aero/mag-beta-deg", this, inDegrees, (PMF)&FGAuxiliary::GetMagBeta);
394 PropertyManager->Tie("aero/Re", this, &FGAuxiliary::GetReynoldsNumber);
395 PropertyManager->Tie("aero/qbar-psf", this, &FGAuxiliary::Getqbar);
396 PropertyManager->Tie("aero/qbarUW-psf", this, &FGAuxiliary::GetqbarUW);
397 PropertyManager->Tie("aero/qbarUV-psf", this, &FGAuxiliary::GetqbarUV);
398 PropertyManager->Tie("aero/alphadot-rad_sec", this, (PF)&FGAuxiliary::Getadot);
399 PropertyManager->Tie("aero/betadot-rad_sec", this, (PF)&FGAuxiliary::Getbdot);
400 PropertyManager->Tie("aero/alphadot-deg_sec", this, inDegrees, (PMF)&FGAuxiliary::Getadot);
401 PropertyManager->Tie("aero/betadot-deg_sec", this, inDegrees, (PMF)&FGAuxiliary::Getbdot);
402 PropertyManager->Tie("aero/h_b-cg-ft", this, &FGAuxiliary::GetHOverBCG);
403 PropertyManager->Tie("aero/h_b-mac-ft", this, &FGAuxiliary::GetHOverBMAC);
404 PropertyManager->Tie("flight-path/gamma-rad", this, &FGAuxiliary::GetGamma);
405 PropertyManager->Tie("flight-path/psi-gt-rad", this, &FGAuxiliary::GetGroundTrack);
407 PropertyManager->Tie("position/distance-from-start-lon-mt", this, &FGAuxiliary::GetLongitudeRelativePosition);
408 PropertyManager->Tie("position/distance-from-start-lat-mt", this, &FGAuxiliary::GetLatitudeRelativePosition);
409 PropertyManager->Tie("position/distance-from-start-mag-mt", this, &FGAuxiliary::GetDistanceRelativePosition);
410 PropertyManager->Tie("position/vrp-gc-latitude_deg", &vLocationVRP, &FGLocation::GetLatitudeDeg);
411 PropertyManager->Tie("position/vrp-longitude_deg", &vLocationVRP, &FGLocation::GetLongitudeDeg);
412 PropertyManager->Tie("position/vrp-radius-ft", &vLocationVRP, &FGLocation::GetRadius);
415 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
417 double FGAuxiliary::BadUnits(void) const
419 cerr << "Bad units" << endl; return 0.0;
422 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
423 // The bitmasked value choices are as follows:
424 // unset: In this case (the default) JSBSim would only print
425 // out the normally expected messages, essentially echoing
426 // the config files as they are read. If the environment
427 // variable is not set, debug_lvl is set to 1 internally
428 // 0: This requests JSBSim not to output any messages
430 // 1: This value explicity requests the normal JSBSim
432 // 2: This value asks for a message to be printed out when
433 // a class is instantiated
434 // 4: When this value is set, a message is displayed when a
435 // FGModel object executes its Run() method
436 // 8: When this value is set, various runtime state variables
437 // are printed out periodically
438 // 16: When set various parameters are sanity checked and
439 // a message is printed out when they go out of bounds
441 void FGAuxiliary::Debug(int from)
443 if (debug_lvl <= 0) return;
445 if (debug_lvl & 1) { // Standard console startup message output
446 if (from == 0) { // Constructor
450 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
451 if (from == 0) cout << "Instantiated: FGAuxiliary" << endl;
452 if (from == 1) cout << "Destroyed: FGAuxiliary" << endl;
454 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
456 if (debug_lvl & 8 ) { // Runtime state variables
458 if (debug_lvl & 16) { // Sanity checking
459 if (Mach > 100 || Mach < 0.00)
460 cout << "FGPropagate::Mach is out of bounds: " << Mach << endl;
461 if (qbar > 1e6 || qbar < 0.00)
462 cout << "FGPropagate::qbar is out of bounds: " << qbar << endl;
464 if (debug_lvl & 64) {
465 if (from == 0) { // Constructor
466 cout << IdSrc << endl;
467 cout << IdHdr << endl;
472 } // namespace JSBSim