FlightGear next
FGAerodynamics.cpp
Go to the documentation of this file.
1/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGAerodynamics.cpp
4 Author: Jon S. Berndt
5 Date started: 09/13/00
6 Purpose: Encapsulates the aerodynamic forces
7
8 ------------- Copyright (C) 2000 Jon S. Berndt (jon@jsbsim.org) -------------
9
10 This program is free software; you can redistribute it and/or modify it under
11 the terms of the GNU Lesser General Public License as published by the Free
12 Software Foundation; either version 2 of the License, or (at your option) any
13 later version.
14
15 This program is distributed in the hope that it will be useful, but WITHOUT
16 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
18 details.
19
20 You should have received a copy of the GNU Lesser General Public License along
21 with this program; if not, write to the Free Software Foundation, Inc., 59
22 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23
24 Further information about the GNU Lesser General Public License can also be
25 found on the world wide web at http://www.gnu.org.
26
27FUNCTIONAL DESCRIPTION
28--------------------------------------------------------------------------------
29
30HISTORY
31--------------------------------------------------------------------------------
3209/13/00 JSB Created
3304/22/01 JSB Moved code into here from FGAircraft
34
35%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36INCLUDES
37%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
38
39#include "FGAerodynamics.h"
41
42using namespace std;
43
44namespace JSBSim {
45
46/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
47CLASS IMPLEMENTATION
48%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
49
50
52{
53 Name = "FGAerodynamics";
54
55 AxisIdx["DRAG"] = 0;
56 AxisIdx["SIDE"] = 1;
57 AxisIdx["LIFT"] = 2;
58 AxisIdx["ROLL"] = 3;
59 AxisIdx["PITCH"] = 4;
60 AxisIdx["YAW"] = 5;
61
62 AxisIdx["AXIAL"] = 0;
63 AxisIdx["NORMAL"] = 2;
64
65 AxisIdx["X"] = 0;
66 AxisIdx["Y"] = 1;
67 AxisIdx["Z"] = 2;
68
69 forceAxisType = atNone;
70 momentAxisType = atNone;
71
72 AeroFunctions = new AeroFunctionArray[6];
73 AeroFunctionsAtCG = new AeroFunctionArray[6];
74
75 impending_stall = stall_hyst = 0.0;
76 alphaclmin = alphaclmax = 0.0;
77 alphaclmin0 = alphaclmax0 = 0.0;
78 alphahystmin = alphahystmax = 0.0;
79 clsq = lod = 0.0;
80 alphaw = 0.0;
81 bi2vel = ci2vel = 0.0;
82 AeroRPShift = 0;
83 vDeltaRP.InitMatrix();
84
85 bind();
86
87 Debug(0);
88}
89
90//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91
93{
94 unsigned int i,j;
95
96 for (i=0; i<6; i++)
97 for (j=0; j<AeroFunctions[i].size(); j++)
98 delete AeroFunctions[i][j];
99 for (i=0; i<6; i++)
100 for (j=0; j<AeroFunctionsAtCG[i].size(); j++)
101 delete AeroFunctionsAtCG[i][j];
102
103 delete[] AeroFunctions;
104 delete[] AeroFunctionsAtCG;
105
106 delete AeroRPShift;
107
108 Debug(1);
109}
110
111//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
112
114{
115 if (!FGModel::InitModel()) return false;
116
117 impending_stall = stall_hyst = 0.0;
118 alphaclmin = alphaclmin0;
119 alphaclmax = alphaclmax0;
120 alphahystmin = alphahystmax = 0.0;
121 clsq = lod = 0.0;
122 alphaw = 0.0;
123 bi2vel = ci2vel = 0.0;
124 AeroRPShift = 0;
125 vDeltaRP.InitMatrix();
126 vForces.InitMatrix();
127 vMoments.InitMatrix();
128 return true;
129}
130//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131
132bool FGAerodynamics::Run(bool Holding)
133{
134
135 if (FGModel::Run(Holding)) return true;
136 if (Holding) return false; // if paused don't execute
137
138 unsigned int axis_ctr;
139 const double twovel=2*in.Vt;
140
141 // The lift coefficient squared (property aero/cl-squared) is computed before
142 // the aero functions are called to make sure that they use the same value for
143 // qbar.
144 if ( in.Qbar > 1.0) {
145 // Skip the computation if qbar is close to zero to avoid huge values for
146 // aero/cl-squared when a non-null lift coincides with a very small aero
147 // velocity (i.e. when qbar is close to zero).
148 clsq = vFw(eLift) / (in.Wingarea*in.Qbar);
149 clsq *= clsq;
150 }
151
153
154 // calculate some oft-used quantities for speed
155
156 if (twovel != 0) {
157 bi2vel = in.Wingspan / twovel;
158 ci2vel = in.Wingchord / twovel;
159 }
160 alphaw = in.Alpha + in.Wingincidence;
161 qbar_area = in.Wingarea * in.Qbar;
162
163 if (alphaclmax != 0) {
164 if (in.Alpha > 0.85*alphaclmax) {
165 impending_stall = 10*(in.Alpha/alphaclmax - 0.85);
166 } else {
167 impending_stall = 0;
168 }
169 }
170
171 if (alphahystmax != 0.0 && alphahystmin != 0.0) {
172 if (in.Alpha > alphahystmax) {
173 stall_hyst = 1;
174 } else if (in.Alpha < alphahystmin) {
175 stall_hyst = 0;
176 }
177 }
178
179 vFw.InitMatrix();
180 vFnative.InitMatrix();
181 vFnativeAtCG.InitMatrix();
182
183 BuildStabilityTransformMatrices();
184
185 for (axis_ctr = 0; axis_ctr < 3; ++axis_ctr) {
186 AeroFunctionArray::iterator f;
187
188 AeroFunctionArray* array = &AeroFunctions[axis_ctr];
189 for (f=array->begin(); f != array->end(); ++f) {
190 // Tell the Functions to cache values, so when the function values are
191 // being requested for output, the functions do not get calculated again
192 // in a context that might have changed, but instead use the values that
193 // have already been calculated for this frame.
194 (*f)->cacheValue(true);
195 vFnative(axis_ctr+1) += (*f)->GetValue();
196 }
197
198 array = &AeroFunctionsAtCG[axis_ctr];
199 for (f=array->begin(); f != array->end(); ++f) {
200 (*f)->cacheValue(true); // Same as above
201 vFnativeAtCG(axis_ctr+1) += (*f)->GetValue();
202 }
203 }
204
205 switch (forceAxisType) {
206 case atBodyXYZ: // Forces already in body axes; no manipulation needed
207 vForces = vFnative;
208 vForcesAtCG = vFnativeAtCG;
209 break;
210 case atWind: // Copy forces into wind axes
211 vFnative(eDrag)*=-1; vFnative(eLift)*=-1;
212 vForces = in.Tw2b*vFnative;
213
214 vFnativeAtCG(eDrag)*=-1; vFnativeAtCG(eLift)*=-1;
215 vForcesAtCG = in.Tw2b*vFnativeAtCG;
216 break;
217 case atBodyAxialNormal: // Convert native forces into Axial|Normal|Side system
218 vFnative(eX)*=-1; vFnative(eZ)*=-1;
219 vForces = vFnative;
220
221 vFnativeAtCG(eX)*=-1; vFnativeAtCG(eZ)*=-1;
222 vForcesAtCG = vFnativeAtCG;
223 break;
224 case atStability: // Convert from stability axes to both body and wind axes
225 vFnative(eDrag) *= -1; vFnative(eLift) *= -1;
226 vForces = Ts2b*vFnative;
227
228 vFnativeAtCG(eDrag) *= -1; vFnativeAtCG(eLift) *= -1;
229 vForcesAtCG = Ts2b*vFnativeAtCG;
230 break;
231 default:
232 {
233 stringstream s;
234 s << " A proper axis type has NOT been selected. Check "
235 << "your aerodynamics definition.";
236 cerr << endl << s.str() << endl;
237 throw BaseException(s.str());
238 }
239 }
240 // Calculate aerodynamic reference point shift, if any. The shift takes place
241 // in the structual axis. That is, if the shift is positive, it is towards the
242 // back (tail) of the vehicle. The AeroRPShift function should be
243 // non-dimensionalized by the wing chord. The calculated vDeltaRP will be in
244 // feet.
245 if (AeroRPShift) vDeltaRP(eX) = AeroRPShift->GetValue()*in.Wingchord;
246
247 vDXYZcg(eX) = in.RPBody(eX) - vDeltaRP(eX); // vDeltaRP is given in the
248 vDXYZcg(eY) = in.RPBody(eY) + vDeltaRP(eY); // structural frame.
249 vDXYZcg(eZ) = in.RPBody(eZ) - vDeltaRP(eZ);
250
251 vMomentsMRC.InitMatrix();
252
253 for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
254 AeroFunctionArray* array = &AeroFunctions[axis_ctr+3];
255 for (AeroFunctionArray::iterator f=array->begin(); f != array->end(); ++f) {
256 // Tell the Functions to cache values, so when the function values are
257 // being requested for output, the functions do not get calculated again
258 // in a context that might have changed, but instead use the values that
259 // have already been calculated for this frame.
260 (*f)->cacheValue(true);
261 vMomentsMRC(axis_ctr+1) += (*f)->GetValue();
262 }
263 }
264
265 // Transform moments to bodyXYZ if the moments are specified in stability or
266 // wind axes
267 vMomentsMRCBodyXYZ.InitMatrix();
268 switch (momentAxisType) {
269 case atBodyXYZ:
270 vMomentsMRCBodyXYZ = vMomentsMRC;
271 break;
272 case atStability:
273 vMomentsMRCBodyXYZ = Ts2b*vMomentsMRC;
274 break;
275 case atWind:
276 vMomentsMRCBodyXYZ = in.Tw2b*vMomentsMRC;
277 break;
278 default:
279 {
280 stringstream s;
281 s << " A proper axis type has NOT been selected. Check "
282 << "your aerodynamics definition.";
283 cerr << endl << s.str() << endl;
284 throw BaseException(s.str());
285 }
286 }
287
288 vMoments = vMomentsMRCBodyXYZ + vDXYZcg*vForces; // M = r X F
289
290 // Now add the "at CG" values to base forces - after the moments have been
291 // transferred.
292 vForces += vForcesAtCG;
293
294 // Note that we still need to convert to wind axes here, because it is used in
295 // the L/D calculation, and we still may want to look at Lift and Drag.
296 //
297 // JSB 4/27/12 - After use, convert wind axes to produce normal lift and drag
298 // values - not negative ones!
299 //
300 // As a clarification, JSBSim assumes that drag and lift values are defined in
301 // wind axes - BUT with a 180 rotation about the Y axis. That is, lift and
302 // drag will be positive up and aft, respectively, so that they are reported
303 // as positive numbers. However, the wind axes themselves assume that the X
304 // and Z forces are positive forward and down. Same applies to the stability
305 // axes.
306 vFw = in.Tb2w * vForces;
307 vFw(eDrag) *= -1; vFw(eLift) *= -1;
308
309 // Calculate Lift over Drag
310 if ( fabs(vFw(eDrag)) > 0.0)
311 lod = fabs( vFw(eLift) / vFw(eDrag));
312
314
315 return false;
316}
317
318//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
319
321{
322 FGColumnVector3 vFs = Tb2s*vForces;
323 // Need sign flips since drag is positive and lift is positive in stability axes
324 vFs(eDrag) *= -1; vFs(eLift) *= -1;
325
326 return vFs;
327}
328
329//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330
332{
333 string axis;
334 string scratch_unit="";
335 Element *temp_element, *axis_element, *function_element;
336
337 Name = "Aerodynamics Model: " + document->GetAttributeValue("name");
338
339 // Perform base class Pre-Load
340 if (!FGModel::Upload(document, true))
341 return false;
342
343 DetermineAxisSystem(document); // Determine if Lift/Side/Drag, etc. is used.
344
345 Debug(2);
346
347 if ((temp_element = document->FindElement("alphalimits"))) {
348 scratch_unit = temp_element->GetAttributeValue("unit");
349 if (scratch_unit.empty()) scratch_unit = "RAD";
350 alphaclmin0 = temp_element->FindElementValueAsNumberConvertFromTo("min", scratch_unit, "RAD");
351 alphaclmax0 = temp_element->FindElementValueAsNumberConvertFromTo("max", scratch_unit, "RAD");
352 alphaclmin = alphaclmin0;
353 alphaclmax = alphaclmax0;
354 }
355
356 if ((temp_element = document->FindElement("hysteresis_limits"))) {
357 scratch_unit = temp_element->GetAttributeValue("unit");
358 if (scratch_unit.empty()) scratch_unit = "RAD";
359 alphahystmin = temp_element->FindElementValueAsNumberConvertFromTo("min", scratch_unit, "RAD");
360 alphahystmax = temp_element->FindElementValueAsNumberConvertFromTo("max", scratch_unit, "RAD");
361 }
362
363 if ((temp_element = document->FindElement("aero_ref_pt_shift_x"))) {
364 function_element = temp_element->FindElement("function");
365 AeroRPShift = new FGFunction(FDMExec, function_element);
366 }
367
368 axis_element = document->FindElement("axis");
369 while (axis_element) {
370 AeroFunctionArray ca;
371 AeroFunctionArray ca_atCG;
372 axis = axis_element->GetAttributeValue("name");
373 function_element = axis_element->FindElement("function");
374 while (function_element) {
375 string current_func_name = function_element->GetAttributeValue("name");
376 bool apply_at_cg = false;
377 if (function_element->HasAttribute("apply_at_cg")) {
378 if (function_element->GetAttributeValue("apply_at_cg") == "true") apply_at_cg = true;
379 }
380 if (!apply_at_cg) {
381 try {
382 ca.push_back( new FGFunction(FDMExec, function_element) );
383 } catch (const string& str) {
384 cerr << endl << axis_element->ReadFrom()
385 << endl << fgred << "Error loading aerodynamic function in "
386 << current_func_name << ":" << str << " Aborting." << reset << endl;
387 return false;
388 }
389 } else {
390 try {
391 ca_atCG.push_back( new FGFunction(FDMExec, function_element) );
392 } catch (const string& str) {
393 cerr << endl << axis_element->ReadFrom()
394 << endl << fgred << "Error loading aerodynamic function in "
395 << current_func_name << ":" << str << " Aborting." << reset << endl;
396 return false;
397 }
398 }
399 function_element = axis_element->FindNextElement("function");
400 }
401 AeroFunctions[AxisIdx[axis]] = ca;
402 AeroFunctionsAtCG[AxisIdx[axis]] = ca_atCG;
403 axis_element = document->FindNextElement("axis");
404 }
405
406 PostLoad(document, FDMExec); // Perform base class Post-Load
407
408 return true;
409}
410
411//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
412//
413// This private class function checks to verify consistency in the choice of
414// aerodynamic axes used in the config file. One set of LIFT|DRAG|SIDE, or
415// X|Y|Z, or AXIAL|NORMAL|SIDE must be chosen; mixed system axes are not allowed.
416// Note that if the "SIDE" axis specifier is entered first in a config file,
417// a warning message will be given IF the AXIAL|NORMAL specifiers are also given.
418// This is OK, and the warning is due to the SIDE specifier used for both
419// the Lift/Drag and Axial/Normal axis systems.
420// Alternatively the axis name 'X|Y|Z or ROLL|PITCH|YAW' can be specified in
421// conjunction with a frame 'BODY|STABILITY|WIND', for example:
422// <axis name="X" frame="STABILITY"/>
423
424void FGAerodynamics::DetermineAxisSystem(Element* document)
425{
426 Element* axis_element = document->FindElement("axis");
427 string axis;
428 while (axis_element) {
429 axis = axis_element->GetAttributeValue("name");
430 string frame = axis_element->GetAttributeValue("frame");
431 if (axis == "X" || axis == "Y" || axis == "Z") {
432 ProcessAxesNameAndFrame(forceAxisType, axis, frame, axis_element,
433 "(X Y Z)");
434 } else if (axis == "ROLL" || axis == "PITCH" || axis == "YAW") {
435 ProcessAxesNameAndFrame(momentAxisType, axis, frame, axis_element,
436 "(ROLL PITCH YAW)");
437 } else if (axis == "LIFT" || axis == "DRAG") {
438 if (forceAxisType == atNone) forceAxisType = atWind;
439 else if (forceAxisType != atWind) {
440 cerr << endl << axis_element->ReadFrom()
441 << endl << " Mixed aerodynamic axis systems have been used in the"
442 << " aircraft config file. (LIFT DRAG)" << endl;
443 }
444 } else if (axis == "SIDE") {
445 if (forceAxisType != atNone && forceAxisType != atWind && forceAxisType != atBodyAxialNormal) {
446 cerr << endl << axis_element->ReadFrom()
447 << endl << " Mixed aerodynamic axis systems have been used in the"
448 << " aircraft config file. (SIDE)" << endl;
449 }
450 } else if (axis == "AXIAL" || axis == "NORMAL") {
451 if (forceAxisType == atNone) forceAxisType = atBodyAxialNormal;
452 else if (forceAxisType != atBodyAxialNormal) {
453 cerr << endl << axis_element->ReadFrom()
454 << endl << " Mixed aerodynamic axis systems have been used in the"
455 << " aircraft config file. (NORMAL AXIAL)" << endl;
456 }
457 } else { // error
458 stringstream s;
459 s << axis_element->ReadFrom()
460 << endl << " An unknown axis type, " << axis << " has been specified"
461 << " in the aircraft configuration file.";
462 cerr << endl << s.str() << endl;
463 throw BaseException(s.str());
464 }
465 axis_element = document->FindNextElement("axis");
466 }
467
468 if (forceAxisType == atNone) {
469 forceAxisType = atWind;
470 cerr << endl << " The aerodynamic axis system has been set by default"
471 << " to the Lift/Side/Drag system." << endl;
472 }
473 if (momentAxisType == atNone) {
474 momentAxisType = atBodyXYZ;
475 cerr << endl << " The aerodynamic moment axis system has been set by default"
476 << " to the bodyXYZ system." << endl;
477 }
478}
479
480//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
481
482void FGAerodynamics::ProcessAxesNameAndFrame(eAxisType& axisType, const string& name,
483 const string& frame, Element* el,
484 const string& validNames)
485{
486 if (frame == "BODY" || frame.empty()) {
487 if (axisType == atNone) axisType = atBodyXYZ;
488 else if (axisType != atBodyXYZ)
489 cerr << endl << el->ReadFrom()
490 << endl << " Mixed aerodynamic axis systems have been used in the "
491 << " aircraft config file." << validNames << " - BODY" << endl;
492 }
493 else if (frame == "STABILITY") {
494 if (axisType == atNone) axisType = atStability;
495 else if (axisType != atStability)
496 cerr << endl << el->ReadFrom()
497 << endl << " Mixed aerodynamic axis systems have been used in the "
498 << " aircraft config file." << validNames << " - STABILITY" << endl;
499 }
500 else if (frame == "WIND") {
501 if (axisType == atNone) axisType = atWind;
502 else if (axisType != atWind)
503 cerr << endl << el->ReadFrom()
504 << endl << " Mixed aerodynamic axis systems have been used in the "
505 << " aircraft config file." << validNames << " - WIND" << endl;
506 }
507 else {
508 stringstream s;
509 s << " Unknown axis frame type of - " << frame;
510 cerr << endl << s.str() << endl;
511 throw BaseException(s.str());
512 }
513}
514
515//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
516
517string FGAerodynamics::GetAeroFunctionStrings(const string& delimeter) const
518{
519 string AeroFunctionStrings = "";
520 bool firstime = true;
521 unsigned int axis, sd;
522
523 for (axis = 0; axis < 6; axis++) {
524 for (sd = 0; sd < AeroFunctions[axis].size(); sd++) {
525 if (firstime) {
526 firstime = false;
527 } else {
528 AeroFunctionStrings += delimeter;
529 }
530 AeroFunctionStrings += AeroFunctions[axis][sd]->GetName();
531 }
532 }
533
534 string FunctionStrings = FGModelFunctions::GetFunctionStrings(delimeter);
535
536 if (FunctionStrings.size() > 0) {
537 if (AeroFunctionStrings.size() > 0) {
538 AeroFunctionStrings += delimeter + FunctionStrings;
539 } else {
540 AeroFunctionStrings = FunctionStrings;
541 }
542 }
543
544 return AeroFunctionStrings;
545}
546
547//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
548
549string FGAerodynamics::GetAeroFunctionValues(const string& delimeter) const
550{
551 ostringstream buf;
552
553 for (unsigned int axis = 0; axis < 6; axis++) {
554 for (unsigned int sd = 0; sd < AeroFunctions[axis].size(); sd++) {
555 if (buf.tellp() > 0) buf << delimeter;
556 buf << AeroFunctions[axis][sd]->GetValue();
557 }
558 }
559
560 string FunctionValues = FGModelFunctions::GetFunctionValues(delimeter);
561
562 if (FunctionValues.size() > 0) {
563 if (buf.str().size() > 0) {
564 buf << delimeter << FunctionValues;
565 } else {
566 buf << FunctionValues;
567 }
568 }
569
570 return buf.str();
571}
572
573//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
574
575void FGAerodynamics::bind(void)
576{
577 typedef double (FGAerodynamics::*PMF)(int) const;
578
579 PropertyManager->Tie("forces/fbx-aero-lbs", this, eX, (PMF)&FGAerodynamics::GetForces);
580 PropertyManager->Tie("forces/fby-aero-lbs", this, eY, (PMF)&FGAerodynamics::GetForces);
581 PropertyManager->Tie("forces/fbz-aero-lbs", this, eZ, (PMF)&FGAerodynamics::GetForces);
582 PropertyManager->Tie("moments/l-aero-lbsft", this, eL, (PMF)&FGAerodynamics::GetMoments);
583 PropertyManager->Tie("moments/m-aero-lbsft", this, eM, (PMF)&FGAerodynamics::GetMoments);
584 PropertyManager->Tie("moments/n-aero-lbsft", this, eN, (PMF)&FGAerodynamics::GetMoments);
585 PropertyManager->Tie("forces/fwx-aero-lbs", this, eDrag, (PMF)&FGAerodynamics::GetvFw);
586 PropertyManager->Tie("forces/fwy-aero-lbs", this, eSide, (PMF)&FGAerodynamics::GetvFw);
587 PropertyManager->Tie("forces/fwz-aero-lbs", this, eLift, (PMF)&FGAerodynamics::GetvFw);
588 PropertyManager->Tie("forces/fsx-aero-lbs", this, eX, (PMF)&FGAerodynamics::GetForcesInStabilityAxes);
589 PropertyManager->Tie("forces/fsy-aero-lbs", this, eY, (PMF)&FGAerodynamics::GetForcesInStabilityAxes);
590 PropertyManager->Tie("forces/fsz-aero-lbs", this, eZ, (PMF)&FGAerodynamics::GetForcesInStabilityAxes);
591 PropertyManager->Tie("moments/roll-stab-aero-lbsft", this, eRoll, (PMF)&FGAerodynamics::GetMomentsInStabilityAxes);
592 PropertyManager->Tie("moments/pitch-stab-aero-lbsft", this, ePitch, (PMF)&FGAerodynamics::GetMomentsInStabilityAxes);
593 PropertyManager->Tie("moments/yaw-stab-aero-lbsft", this, eYaw, (PMF)&FGAerodynamics::GetMomentsInStabilityAxes);
594 PropertyManager->Tie("moments/roll-wind-aero-lbsft", this, eRoll, (PMF)&FGAerodynamics::GetMomentsInWindAxes);
595 PropertyManager->Tie("moments/pitch-wind-aero-lbsft", this, ePitch, (PMF)&FGAerodynamics::GetMomentsInWindAxes);
596 PropertyManager->Tie("moments/yaw-wind-aero-lbsft", this, eYaw, (PMF)&FGAerodynamics::GetMomentsInWindAxes);
597 PropertyManager->Tie("forces/lod-norm", this, &FGAerodynamics::GetLoD);
598 PropertyManager->Tie("aero/cl-squared", this, &FGAerodynamics::GetClSquared);
599 PropertyManager->Tie("aero/qbar-area", &qbar_area);
602 PropertyManager->Tie("aero/bi2vel", this, &FGAerodynamics::GetBI2Vel);
603 PropertyManager->Tie("aero/ci2vel", this, &FGAerodynamics::GetCI2Vel);
604 PropertyManager->Tie("aero/alpha-wing-rad", this, &FGAerodynamics::GetAlphaW);
605 PropertyManager->Tie("systems/stall-warn-norm", this, &FGAerodynamics::GetStallWarn);
606 PropertyManager->Tie("aero/stall-hyst-norm", this, &FGAerodynamics::GetHysteresisParm);
607}
608
609//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
610//
611// Build transformation matrices for transforming from stability axes to
612// body axes and to wind axes. Where "a" is alpha and "B" is beta:
613//
614// The transform from body to stability axes is:
615//
616// cos(a) 0 sin(a)
617// 0 1 0
618// -sin(a) 0 cos(a)
619//
620// The transform from stability to body axes is:
621//
622// cos(a) 0 -sin(a)
623// 0 1 0
624// sin(a) 0 cos(a)
625//
626//
627
628void FGAerodynamics::BuildStabilityTransformMatrices(void)
629{
630 double ca = cos(in.Alpha);
631 double sa = sin(in.Alpha);
632
633 // Stability-to-body
634 Ts2b(1, 1) = ca;
635 Ts2b(1, 2) = 0.0;
636 Ts2b(1, 3) = -sa;
637 Ts2b(2, 1) = 0.0;
638 Ts2b(2, 2) = 1.0;
639 Ts2b(2, 3) = 0.0;
640 Ts2b(3, 1) = sa;
641 Ts2b(3, 2) = 0.0;
642 Ts2b(3, 3) = ca;
643
644 Tb2s = Ts2b.Transposed();
645}
646
647//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
648// The bitmasked value choices are as follows:
649// unset: In this case (the default) JSBSim would only print
650// out the normally expected messages, essentially echoing
651// the config files as they are read. If the environment
652// variable is not set, debug_lvl is set to 1 internally
653// 0: This requests JSBSim not to output any messages
654// whatsoever.
655// 1: This value explicity requests the normal JSBSim
656// startup messages
657// 2: This value asks for a message to be printed out when
658// a class is instantiated
659// 4: When this value is set, a message is displayed when a
660// FGModel object executes its Run() method
661// 8: When this value is set, various runtime state variables
662// are printed out periodically
663// 16: When set various parameters are sanity checked and
664// a message is printed out when they go out of bounds
665
666void FGAerodynamics::Debug(int from)
667{
668 if (debug_lvl <= 0) return;
669
670 if (debug_lvl & 1) { // Standard console startup message output
671 if (from == 2) { // Loader
672 switch (forceAxisType) {
673 case (atWind):
674 cout << endl << " Aerodynamics (Lift|Side|Drag axes):" << endl << endl;
675 break;
676 case (atBodyAxialNormal):
677 cout << endl << " Aerodynamics (Axial|Side|Normal axes):" << endl << endl;
678 break;
679 case (atBodyXYZ):
680 cout << endl << " Aerodynamics (Body X|Y|Z axes):" << endl << endl;
681 break;
682 case (atStability):
683 cout << endl << " Aerodynamics (Stability X|Y|Z axes):" << endl << endl;
684 break;
685 case (atNone):
686 cout << endl << " Aerodynamics (undefined axes):" << endl << endl;
687 break;
688 }
689 }
690 }
691 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
692 if (from == 0) cout << "Instantiated: FGAerodynamics" << endl;
693 if (from == 1) cout << "Destroyed: FGAerodynamics" << endl;
694 }
695 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
696 }
697 if (debug_lvl & 8 ) { // Runtime state variables
698 }
699 if (debug_lvl & 16) { // Sanity checking
700 }
701 if (debug_lvl & 64) {
702 if (from == 0) { // Constructor
703 }
704 }
705}
706
707} // namespace JSBSim
#define i(x)
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
double FindElementValueAsNumberConvertFromTo(const std::string &el, const std::string &supplied_units, const std::string &target_units)
Searches for the named element and converts and returns the data belonging to it.
bool HasAttribute(const std::string &key)
Determines if an element has the supplied attribute.
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
Element * FindElement(const std::string &el="")
Searches for a specified element.
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
Encapsulates the aerodynamic calculations.
void SetAlphaCLMin(double tt)
FGAerodynamics(FGFDMExec *Executive)
Constructor.
bool InitModel(void) override
~FGAerodynamics() override
Destructor.
const FGColumnVector3 & GetMoments(void) const
Gets the total aerodynamic moment vector about the CG.
FGColumnVector3 GetForcesInStabilityAxes(void) const
Retrieves the aerodynamic forces in the stability axes.
double GetAlphaCLMin(void) const
const FGColumnVector3 & GetvFw(void) const
Retrieves the aerodynamic forces in the wind axes.
double GetStallWarn(void) const
bool Load(Element *element) override
Loads the Aerodynamics model.
double GetAlphaW(void) const
void SetAlphaCLMax(double tt)
double GetClSquared(void) const
Retrieves the square of the lift coefficient.
double GetHysteresisParm(void) const
bool Run(bool Holding) override
Runs the Aerodynamics model; called by the Executive Can pass in a value indicating if the executive ...
double GetLoD(void) const
Retrieves the lift over drag ratio.
std::string GetAeroFunctionValues(const std::string &delimeter) const
Gets the aero function values.
FGColumnVector3 GetMomentsInWindAxes(void) const
Gets the total aerodynamic moment vector about the CG in the wind axes.
const FGColumnVector3 & GetForces(void) const
Gets the total aerodynamic force vector.
double GetCI2Vel(void) const
double GetAlphaCLMax(void) const
struct JSBSim::FGAerodynamics::Inputs in
double GetBI2Vel(void) const
std::string GetAeroFunctionStrings(const std::string &delimeter) const
Gets the strings for the current set of aero functions.
FGColumnVector3 GetMomentsInStabilityAxes(void) const
Gets the total aerodynamic moment vector about the CG in the stability axes.
This class implements a 3 element column vector.
Represents a mathematical function.
Definition FGFunction.h:753
static char reset[5]
resets text properties
Definition FGJSBBase.h:129
static char fgred[6]
red text
Definition FGJSBBase.h:139
static short debug_lvl
Definition FGJSBBase.h:190
FGMatrix33 Transposed(void) const
Transposed matrix.
Definition FGMatrix33.h:221
std::string GetFunctionValues(const std::string &delimeter) const
Gets the function values.
std::string GetFunctionStrings(const std::string &delimeter) const
Gets the strings for the current set of functions.
void PostLoad(Element *el, FGFDMExec *fdmex, std::string prefix="")
FGPropertyManager * PropertyManager
Definition FGModel.h:117
bool InitModel(void) override
Definition FGModel.cpp:81
FGFDMExec * FDMExec
Definition FGModel.h:116
FGModel(FGFDMExec *)
Constructor.
Definition FGModel.cpp:57
bool Upload(Element *el, bool preLoad)
Uploads this model in memory.
Definition FGModel.cpp:110
std::string Name
Definition FGModel.h:103
virtual bool Run(bool Holding)
Runs the model; called by the Executive.
Definition FGModel.cpp:89
void Tie(const std::string &name, T *pointer)
Tie a property to an external variable.
const char * name