]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/FGFCS.cpp
67c02d698781d1759471d4f7600b37ab678ba171
[flightgear.git] / src / FDM / JSBSim / models / FGFCS.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3  Module:       FGFCS.cpp 
4  Author:       Jon Berndt
5  Date started: 12/12/98
6  Purpose:      Model the flight controls
7  Called by:    FDMExec
8
9  ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
10
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
14  version.
15
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
19  details.
20
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.
24
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.
27
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class models the flight controls for a specific airplane
31
32 HISTORY
33 --------------------------------------------------------------------------------
34 12/12/98   JSB   Created
35
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 INCLUDES
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39
40 #include "FGFCS.h"
41 #include "FGFDMExec.h"
42 #include "FGGroundReactions.h"
43 #include "input_output/FGPropertyManager.h"
44 #include <fstream>
45 #include <sstream>
46 #include <iomanip>
47
48 #include "models/flight_control/FGFilter.h"
49 #include "models/flight_control/FGDeadBand.h"
50 #include "models/flight_control/FGGain.h"
51 #include "models/flight_control/FGPID.h"
52 #include "models/flight_control/FGSwitch.h"
53 #include "models/flight_control/FGSummer.h"
54 #include "models/flight_control/FGKinemat.h"
55 #include "models/flight_control/FGFCSFunction.h"
56 #include "models/flight_control/FGSensor.h"
57 #include "models/flight_control/FGActuator.h"
58 #include "models/flight_control/FGAccelerometer.h"
59 #include "models/flight_control/FGMagnetometer.h"
60 #include "models/flight_control/FGGyro.h"
61
62 using namespace std;
63
64 namespace JSBSim {
65
66 static const char *IdSrc = "$Id: FGFCS.cpp,v 1.74 2011/05/20 03:18:36 jberndt Exp $";
67 static const char *IdHdr = ID_FCS;
68
69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
70 CLASS IMPLEMENTATION
71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
72
73 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
74 {
75   int i;
76   Name = "FGFCS";
77
78   DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0;
79   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
80   GearCmd = GearPos = 1; // default to gear down
81   LeftBrake = RightBrake = CenterBrake = 0.0;
82   TailhookPos = WingFoldPos = 0.0; 
83
84   bind();
85   for (i=0;i<NForms;i++) {
86     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
87     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
88   }
89
90   Debug(0);
91 }
92
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94
95 FGFCS::~FGFCS()
96 {
97   ThrottleCmd.clear();
98   ThrottlePos.clear();
99   MixtureCmd.clear();
100   MixturePos.clear();
101   PropAdvanceCmd.clear();
102   PropAdvance.clear();
103   SteerPosDeg.clear();
104   PropFeatherCmd.clear();
105   PropFeather.clear();
106
107   unsigned int i;
108
109   for (i=0;i<APComponents.size();i++) delete APComponents[i];
110   APComponents.clear();
111   for (i=0;i<FCSComponents.size();i++) delete FCSComponents[i];
112   FCSComponents.clear();
113   for (i=0;i<Systems.size();i++) delete Systems[i];
114   Systems.clear();
115
116
117   Debug(1);
118 }
119
120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
121
122 bool FGFCS::InitModel(void)
123 {
124   unsigned int i;
125
126   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
127   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
128   for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
129   for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
130   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
131   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
132
133   DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0;
134   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
135   TailhookPos = WingFoldPos = 0.0;
136
137   for (i=0;i<NForms;i++) {
138     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
139     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
140   }
141
142   for (unsigned int i=0; i<Systems.size(); i++) {
143     if (Systems[i]->GetType() == "LAG" ||
144         Systems[i]->GetType() == "LEAD_LAG" ||
145         Systems[i]->GetType() == "WASHOUT" ||
146         Systems[i]->GetType() == "SECOND_ORDER_FILTER" ||
147         Systems[i]->GetType() == "INTEGRATOR")
148     {
149       ((FGFilter*)Systems[i])->ResetPastStates();
150     } else if (Systems[i]->GetType() == "PID" ) {
151       ((FGPID*)Systems[i])->ResetPastStates();
152     }
153   }
154
155   for (unsigned int i=0; i<FCSComponents.size(); i++) {
156     if (FCSComponents[i]->GetType() == "LAG" ||
157         FCSComponents[i]->GetType() == "LEAD_LAG" ||
158         FCSComponents[i]->GetType() == "WASHOUT" ||
159         FCSComponents[i]->GetType() == "SECOND_ORDER_FILTER" ||
160         FCSComponents[i]->GetType() == "INTEGRATOR")
161     {
162       ((FGFilter*)FCSComponents[i])->ResetPastStates();
163     } else if (FCSComponents[i]->GetType() == "PID" ) {
164       ((FGPID*)FCSComponents[i])->ResetPastStates();
165     }
166   }
167
168   for (unsigned int i=0; i<APComponents.size(); i++) {
169     if (APComponents[i]->GetType() == "LAG" ||
170         APComponents[i]->GetType() == "LEAD_LAG" ||
171         APComponents[i]->GetType() == "WASHOUT" ||
172         APComponents[i]->GetType() == "SECOND_ORDER_FILTER" ||
173         APComponents[i]->GetType() == "INTEGRATOR")
174     {
175       ((FGFilter*)APComponents[i])->ResetPastStates();
176     } else if (APComponents[i]->GetType() == "PID" ) {
177       ((FGPID*)APComponents[i])->ResetPastStates();
178     }
179   }
180
181   return true;
182 }
183
184 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
185 // Notes: In this logic the default engine commands are set. This is simply a
186 // sort of safe-mode method in case the user has not defined control laws for
187 // throttle, mixture, and prop-advance. The throttle, mixture, and prop advance
188 // positions are set equal to the respective commands. Any control logic that is
189 // actually present in the flight_control or autopilot section will override
190 // these simple assignments.
191
192 bool FGFCS::Run(bool Holding)
193 {
194   unsigned int i;
195
196   if (FGModel::Run(Holding)) return true; // fast exit if nothing to do
197   if (Holding) return false;
198
199   RunPreFunctions();
200
201   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
202   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
203   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
204   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
205
206   // Set the default steering angle
207   for (i=0; i<SteerPosDeg.size(); i++) {
208     FGLGear* gear = FDMExec->GetGroundReactions()->GetGearUnit(i);
209     SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() );
210   }
211
212   // Execute Systems in order
213   for (i=0; i<Systems.size(); i++) Systems[i]->Run();
214
215   // Execute Autopilot
216   for (i=0; i<APComponents.size(); i++) APComponents[i]->Run();
217
218   // Execute Flight Control System
219   for (i=0; i<FCSComponents.size(); i++) FCSComponents[i]->Run();
220
221   RunPostFunctions();
222
223   return false;
224 }
225
226 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
227
228 void FGFCS::SetDaLPos( int form , double pos )
229 {
230   switch(form) {
231   case ofRad:
232     DaLPos[ofRad] = pos;
233     DaLPos[ofDeg] = pos*radtodeg;
234     break;
235   case ofDeg:
236     DaLPos[ofRad] = pos*degtorad;
237     DaLPos[ofDeg] = pos;
238     break;
239   case ofNorm:
240     DaLPos[ofNorm] = pos;
241   }
242   DaLPos[ofMag] = fabs(DaLPos[ofRad]);
243 }
244
245 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
246
247 void FGFCS::SetDaRPos( int form , double pos )
248 {
249   switch(form) {
250   case ofRad:
251     DaRPos[ofRad] = pos;
252     DaRPos[ofDeg] = pos*radtodeg;
253     break;
254   case ofDeg:
255     DaRPos[ofRad] = pos*degtorad;
256     DaRPos[ofDeg] = pos;
257     break;
258   case ofNorm:
259     DaRPos[ofNorm] = pos;
260   }
261   DaRPos[ofMag] = fabs(DaRPos[ofRad]);
262 }
263
264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
265
266 void FGFCS::SetDePos( int form , double pos )
267 {
268   switch(form) {
269   case ofRad:
270     DePos[ofRad] = pos;
271     DePos[ofDeg] = pos*radtodeg;
272     break;
273   case ofDeg:
274     DePos[ofRad] = pos*degtorad;
275     DePos[ofDeg] = pos;
276     break;
277   case ofNorm:
278     DePos[ofNorm] = pos;
279   }
280   DePos[ofMag] = fabs(DePos[ofRad]);
281 }
282
283 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
284
285 void FGFCS::SetDrPos( int form , double pos )
286 {
287   switch(form) {
288   case ofRad:
289     DrPos[ofRad] = pos;
290     DrPos[ofDeg] = pos*radtodeg;
291     break;
292   case ofDeg:
293     DrPos[ofRad] = pos*degtorad;
294     DrPos[ofDeg] = pos;
295     break;
296   case ofNorm:
297     DrPos[ofNorm] = pos;
298   }
299   DrPos[ofMag] = fabs(DrPos[ofRad]);
300 }
301
302 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
303
304 void FGFCS::SetDfPos( int form , double pos )
305 {
306   switch(form) {
307   case ofRad:
308     DfPos[ofRad] = pos;
309     DfPos[ofDeg] = pos*radtodeg;
310     break;
311   case ofDeg:
312     DfPos[ofRad] = pos*degtorad;
313     DfPos[ofDeg] = pos;
314     break;
315   case ofNorm:
316     DfPos[ofNorm] = pos;
317   }
318   DfPos[ofMag] = fabs(DfPos[ofRad]);
319 }
320
321 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
322
323 void FGFCS::SetDsbPos( int form , double pos )
324 {
325   switch(form) {
326   case ofRad:
327     DsbPos[ofRad] = pos;
328     DsbPos[ofDeg] = pos*radtodeg;
329     break;
330   case ofDeg:
331     DsbPos[ofRad] = pos*degtorad;
332     DsbPos[ofDeg] = pos;
333     break;
334   case ofNorm:
335     DsbPos[ofNorm] = pos;
336   }
337   DsbPos[ofMag] = fabs(DsbPos[ofRad]);
338 }
339
340 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
341
342 void FGFCS::SetDspPos( int form , double pos )
343 {
344   switch(form) {
345   case ofRad:
346     DspPos[ofRad] = pos;
347     DspPos[ofDeg] = pos*radtodeg;
348     break;
349   case ofDeg:
350     DspPos[ofRad] = pos*degtorad;
351     DspPos[ofDeg] = pos;
352     break;
353   case ofNorm:
354     DspPos[ofNorm] = pos;
355   }
356   DspPos[ofMag] = fabs(DspPos[ofRad]);
357 }
358
359 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
360
361 void FGFCS::SetThrottleCmd(int engineNum, double setting)
362 {
363   unsigned int ctr;
364
365   if (engineNum < (int)ThrottlePos.size()) {
366     if (engineNum < 0) {
367       for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
368     } else {
369       ThrottleCmd[engineNum] = setting;
370     }
371   } else {
372     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
373          << " engines exist, but attempted throttle command is for engine "
374          << engineNum << endl;
375   }
376 }
377
378 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
379
380 void FGFCS::SetThrottlePos(int engineNum, double setting)
381 {
382   unsigned int ctr;
383
384   if (engineNum < (int)ThrottlePos.size()) {
385     if (engineNum < 0) {
386       for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
387     } else {
388       ThrottlePos[engineNum] = setting;
389     }
390   } else {
391     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
392          << " engines exist, but attempted throttle position setting is for engine "
393          << engineNum << endl;
394   }
395 }
396
397 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
398
399 double FGFCS::GetThrottleCmd(int engineNum) const
400 {
401   if (engineNum < (int)ThrottlePos.size()) {
402     if (engineNum < 0) {
403        cerr << "Cannot get throttle value for ALL engines" << endl;
404     } else {
405       return ThrottleCmd[engineNum];
406     }
407   } else {
408     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
409          << " engines exist, but throttle setting for engine " << engineNum
410          << " is selected" << endl;
411   }
412   return 0.0;
413 }
414
415 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
416
417 double FGFCS::GetThrottlePos(int engineNum) const
418 {
419   if (engineNum < (int)ThrottlePos.size()) {
420     if (engineNum < 0) {
421        cerr << "Cannot get throttle value for ALL engines" << endl;
422     } else {
423       return ThrottlePos[engineNum];
424     }
425   } else {
426     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
427          << " engines exist, but attempted throttle position setting is for engine "
428          << engineNum << endl;
429   }
430   return 0.0;
431 }
432
433 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
434
435 void FGFCS::SetMixtureCmd(int engineNum, double setting)
436 {
437   unsigned int ctr;
438
439   if (engineNum < (int)ThrottlePos.size()) {
440     if (engineNum < 0) {
441       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
442     } else {
443       MixtureCmd[engineNum] = setting;
444     }
445   }
446 }
447
448 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
449
450 void FGFCS::SetMixturePos(int engineNum, double setting)
451 {
452   unsigned int ctr;
453
454   if (engineNum < (int)ThrottlePos.size()) {
455     if (engineNum < 0) {
456       for (ctr=0;ctr<MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
457     } else {
458       MixturePos[engineNum] = setting;
459     }
460   }
461 }
462
463 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
464
465 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
466 {
467   unsigned int ctr;
468
469   if (engineNum < (int)ThrottlePos.size()) {
470     if (engineNum < 0) {
471       for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
472     } else {
473       PropAdvanceCmd[engineNum] = setting;
474     }
475   }
476 }
477
478 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
479
480 void FGFCS::SetPropAdvance(int engineNum, double setting)
481 {
482   unsigned int ctr;
483
484   if (engineNum < (int)ThrottlePos.size()) {
485     if (engineNum < 0) {
486       for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
487     } else {
488       PropAdvance[engineNum] = setting;
489     }
490   }
491 }
492
493 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
494
495 void FGFCS::SetFeatherCmd(int engineNum, bool setting)
496 {
497   unsigned int ctr;
498
499   if (engineNum < (int)ThrottlePos.size()) {
500     if (engineNum < 0) {
501       for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeatherCmd[ctr] = setting;
502     } else {
503       PropFeatherCmd[engineNum] = setting;
504     }
505   }
506 }
507
508 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
509
510 void FGFCS::SetPropFeather(int engineNum, bool setting)
511 {
512   unsigned int ctr;
513
514   if (engineNum < (int)ThrottlePos.size()) {
515     if (engineNum < 0) {
516       for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeather[ctr] = PropFeatherCmd[ctr];
517     } else {
518       PropFeather[engineNum] = setting;
519     }
520   }
521 }
522
523 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
524
525 bool FGFCS::Load(Element* el, SystemType systype)
526 {
527   string name, file, fname="", interface_property_string, parent_name;
528   vector <FGFCSComponent*> *Components;
529   Element *component_element;
530   Element *channel_element;
531
532   Components=0;
533
534 // ToDo: The handling of name and file attributes could be improved, here,
535 //       considering that a name can be in the external file, as well.
536
537   name = el->GetAttributeValue("name");
538
539   if (name.empty() || !el->GetAttributeValue("file").empty()) {
540     fname = el->GetAttributeValue("file");
541     if (systype == stSystem) {
542       file = FindSystemFullPathname(fname);
543     } else { 
544       file = FDMExec->GetFullAircraftPath() + "/" + fname + ".xml";
545     }
546     if (fname.empty()) {
547       cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl;
548       return false;
549     } else {
550       document = LoadXMLDocument(file);
551       if (!document) {
552         cerr << "Error loading file " << file << endl;
553         return false;
554       }
555       name = document->GetAttributeValue("name");
556     }
557   } else {
558     document = el;
559   }
560
561   if (document->GetName() == "autopilot") {
562     Components = &APComponents;
563     Name = "Autopilot: " + document->GetAttributeValue("name");
564   } else if (document->GetName() == "flight_control") {
565     Components = &FCSComponents;
566     Name = "FCS: " + document->GetAttributeValue("name");
567   } else if (document->GetName() == "system") {
568     Components = &Systems;
569     Name = "System: " + document->GetAttributeValue("name");
570   }
571   Debug(2);
572
573   if (document->GetName() == "flight_control") bindModel();
574
575   FGModel::Load(document); // Load interface properties from document
576
577   // After reading interface properties in a file, read properties in the local
578   // flight_control, autopilot, or system element. This allows general-purpose
579   // systems to be defined in a file, with overrides or initial loaded constants
580   // supplied in the relevant element of the aircraft configuration file.
581
582   Element* property_element = 0;
583
584   if (!fname.empty()) {
585     property_element = el->FindElement("property");
586     if (property_element && debug_lvl > 0) cout << endl << "    Overriding properties" << endl << endl;
587     while (property_element) {
588       double value=0.0;
589       if ( ! property_element->GetAttributeValue("value").empty())
590         value = property_element->GetAttributeValueAsNumber("value");
591
592       interface_property_string = property_element->GetDataLine();
593       if (PropertyManager->HasNode(interface_property_string)) {
594         FGPropertyManager* node = PropertyManager->GetNode(interface_property_string);
595         if (debug_lvl > 0)
596           cout << "      " << "Overriding value for property " << interface_property_string
597                << " (old value: " << node->getDoubleValue() << "  new value: " << value << ")" << endl;
598         node->setDoubleValue(value);
599       } else {
600         interface_properties.push_back(new double(value));
601         PropertyManager->Tie(interface_property_string, interface_properties.back());
602         if (debug_lvl > 0)
603           cout << "      " << interface_property_string << " (initial value: " << value << ")" << endl;
604       }
605       
606       property_element = el->FindNextElement("property");
607     }
608   }
609
610   channel_element = document->FindElement("channel");
611   while (channel_element) {
612   
613     if (debug_lvl > 0)
614       cout << endl << highint << fgblue << "    Channel " 
615          << normint << channel_element->GetAttributeValue("name") << reset << endl;
616   
617     component_element = channel_element->GetElement();
618     while (component_element) {
619       try {
620         if ((component_element->GetName() == string("lag_filter")) ||
621             (component_element->GetName() == string("lead_lag_filter")) ||
622             (component_element->GetName() == string("washout_filter")) ||
623             (component_element->GetName() == string("second_order_filter")) ||
624             (component_element->GetName() == string("integrator")) )
625         {
626           Components->push_back(new FGFilter(this, component_element));
627         } else if ((component_element->GetName() == string("pure_gain")) ||
628                    (component_element->GetName() == string("scheduled_gain")) ||
629                    (component_element->GetName() == string("aerosurface_scale")))
630         {
631           Components->push_back(new FGGain(this, component_element));
632         } else if (component_element->GetName() == string("summer")) {
633           Components->push_back(new FGSummer(this, component_element));
634         } else if (component_element->GetName() == string("deadband")) {
635           Components->push_back(new FGDeadBand(this, component_element));
636         } else if (component_element->GetName() == string("switch")) {
637           Components->push_back(new FGSwitch(this, component_element));
638         } else if (component_element->GetName() == string("kinematic")) {
639           Components->push_back(new FGKinemat(this, component_element));
640         } else if (component_element->GetName() == string("fcs_function")) {
641           Components->push_back(new FGFCSFunction(this, component_element));
642         } else if (component_element->GetName() == string("pid")) {
643           Components->push_back(new FGPID(this, component_element));
644         } else if (component_element->GetName() == string("actuator")) {
645           Components->push_back(new FGActuator(this, component_element));
646         } else if (component_element->GetName() == string("sensor")) {
647           Components->push_back(new FGSensor(this, component_element));
648         } else if (component_element->GetName() == string("accelerometer")) {
649           Components->push_back(new FGAccelerometer(this, component_element));
650         } else if (component_element->GetName() == string("magnetometer")) {
651           Components->push_back(new FGMagnetometer(this, component_element));
652         } else if (component_element->GetName() == string("gyro")) {
653           Components->push_back(new FGGyro(this, component_element));
654         } else {
655           cerr << "Unknown FCS component: " << component_element->GetName() << endl;
656         }
657       } catch(string s) {
658         cerr << highint << fgred << endl << "  " << s << endl;
659         cerr << reset << endl;
660         return false;
661       }
662       component_element = channel_element->GetNextElement();
663     }
664     channel_element = document->FindNextElement("channel");
665   }
666
667   ResetParser();
668
669   return true;
670 }
671
672 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
673
674 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
675 {
676   switch (bg) {
677   case FGLGear::bgLeft:
678     return LeftBrake;
679   case FGLGear::bgRight:
680     return RightBrake;
681   case FGLGear::bgCenter:
682     return CenterBrake;
683   default:
684     cerr << "GetBrake asked to return a bogus brake value" << endl;
685   }
686   return 0.0;
687 }
688
689 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
690
691 string FGFCS::FindSystemFullPathname(const string& sysfilename)
692 {
693   string fullpath, localpath;
694   string system_filename = sysfilename;
695   string systemPath = FDMExec->GetSystemsPath();
696   string aircraftPath = FDMExec->GetFullAircraftPath();
697   ifstream system_file;
698
699   fullpath = systemPath + "/";
700   localpath = aircraftPath + "/Systems/";
701
702   if (system_filename.length() <=4 || system_filename.substr(system_filename.length()-4, 4) != ".xml") {
703     system_filename.append(".xml");
704   }
705
706   system_file.open(string(localpath + system_filename).c_str());
707   if ( !system_file.is_open()) {
708     system_file.open(string(fullpath + system_filename).c_str());
709       if ( !system_file.is_open()) {
710         cerr << " Could not open system file: " << system_filename << " in path "
711              << fullpath << " or " << localpath << endl;
712         return string("");
713       } else {
714         return string(fullpath + system_filename);
715       }
716   }
717   return string(localpath + system_filename);
718 }
719
720 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
721
722 ifstream* FGFCS::FindSystemFile(const string& sysfilename)
723 {
724   string fullpath, localpath;
725   string system_filename = sysfilename;
726   string systemPath = FDMExec->GetSystemsPath();
727   string aircraftPath = FDMExec->GetFullAircraftPath();
728   ifstream* system_file = new ifstream();
729
730   fullpath = systemPath + "/";
731   localpath = aircraftPath + "/Systems/";
732
733   if (system_filename.substr(system_filename.length()-4, 4) != ".xml") {
734     system_filename.append(".xml");
735   }
736
737   system_file->open(string(localpath + system_filename).c_str());
738   if ( !system_file->is_open()) {
739     system_file->open(string(fullpath + system_filename).c_str());
740       if ( !system_file->is_open()) {
741         cerr << " Could not open system file: " << system_filename << " in path "
742              << fullpath << " or " << localpath << endl;
743       }
744   }
745   return system_file;
746 }
747
748 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
749
750 string FGFCS::GetComponentStrings(const string& delimiter) const
751 {
752   unsigned int comp;
753   string CompStrings = "";
754   bool firstime = true;
755   int total_count=0;
756
757   for (unsigned int i=0; i<Systems.size(); i++) {
758     if (firstime) firstime = false;
759     else          CompStrings += delimiter;
760
761     CompStrings += Systems[i]->GetName();
762     total_count++;
763   }
764
765   for (comp = 0; comp < APComponents.size(); comp++)
766   {
767     if (firstime) firstime = false;
768     else          CompStrings += delimiter;
769
770     CompStrings += APComponents[comp]->GetName();
771     total_count++;
772   }
773
774   for (comp = 0; comp < FCSComponents.size(); comp++) {
775     if (firstime) firstime = false;
776     else          CompStrings += delimiter;
777
778     CompStrings += FCSComponents[comp]->GetName();
779     total_count++;
780   }
781
782   return CompStrings;
783 }
784
785 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
786
787 string FGFCS::GetComponentValues(const string& delimiter) const
788 {
789   std::ostringstream buf;
790
791   unsigned int comp;
792   bool firstime = true;
793   int total_count=0;
794
795   for (unsigned int i=0; i<Systems.size(); i++) {
796     if (firstime) firstime = false;
797     else          buf << delimiter;
798
799     buf << setprecision(9) << Systems[i]->GetOutput();
800     total_count++;
801   }
802
803   for (comp = 0; comp < APComponents.size(); comp++) {
804     if (firstime) firstime = false;
805     else          buf << delimiter;
806
807     buf << setprecision(9) << APComponents[comp]->GetOutput();
808     total_count++;
809   }
810
811   for (comp = 0; comp < FCSComponents.size(); comp++) {
812     if (firstime) firstime = false;
813     else          buf << delimiter;
814
815     buf << setprecision(9) << FCSComponents[comp]->GetOutput();
816     total_count++;
817   }
818
819   return buf.str();
820 }
821
822 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
823
824 void FGFCS::AddThrottle(void)
825 {
826   ThrottleCmd.push_back(0.0);
827   ThrottlePos.push_back(0.0);
828   MixtureCmd.push_back(0.0);     // assume throttle and mixture are coupled
829   MixturePos.push_back(0.0);
830   PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
831   PropAdvance.push_back(0.0);
832   PropFeatherCmd.push_back(false);
833   PropFeather.push_back(false);
834
835   unsigned int num = (unsigned int)ThrottleCmd.size()-1;
836   bindThrottle(num);
837 }
838
839 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
840
841 void FGFCS::AddGear(void)
842 {
843   SteerPosDeg.push_back(0.0);
844 }
845
846 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
847
848 double FGFCS::GetDt(void)
849 {
850   return FDMExec->GetDeltaT()*rate;
851 }
852
853 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
854
855 void FGFCS::bind(void)
856 {
857   PropertyManager->Tie("fcs/aileron-cmd-norm", this, &FGFCS::GetDaCmd, &FGFCS::SetDaCmd);
858   PropertyManager->Tie("fcs/elevator-cmd-norm", this, &FGFCS::GetDeCmd, &FGFCS::SetDeCmd);
859   PropertyManager->Tie("fcs/rudder-cmd-norm", this, &FGFCS::GetDrCmd, &FGFCS::SetDrCmd);
860   PropertyManager->Tie("fcs/flap-cmd-norm", this, &FGFCS::GetDfCmd, &FGFCS::SetDfCmd);
861   PropertyManager->Tie("fcs/speedbrake-cmd-norm", this, &FGFCS::GetDsbCmd, &FGFCS::SetDsbCmd);
862   PropertyManager->Tie("fcs/spoiler-cmd-norm", this, &FGFCS::GetDspCmd, &FGFCS::SetDspCmd);
863   PropertyManager->Tie("fcs/pitch-trim-cmd-norm", this, &FGFCS::GetPitchTrimCmd, &FGFCS::SetPitchTrimCmd);
864   PropertyManager->Tie("fcs/roll-trim-cmd-norm", this, &FGFCS::GetRollTrimCmd, &FGFCS::SetRollTrimCmd);
865   PropertyManager->Tie("fcs/yaw-trim-cmd-norm", this, &FGFCS::GetYawTrimCmd, &FGFCS::SetYawTrimCmd);
866
867   PropertyManager->Tie("fcs/left-aileron-pos-rad", this, ofRad, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
868   PropertyManager->Tie("fcs/left-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
869   PropertyManager->Tie("fcs/left-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
870   PropertyManager->Tie("fcs/mag-left-aileron-pos-rad", this, ofMag, &FGFCS::GetDaLPos);
871
872   PropertyManager->Tie("fcs/right-aileron-pos-rad", this, ofRad, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
873   PropertyManager->Tie("fcs/right-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
874   PropertyManager->Tie("fcs/right-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
875   PropertyManager->Tie("fcs/mag-right-aileron-pos-rad", this, ofMag, &FGFCS::GetDaRPos);
876
877   PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad, &FGFCS::GetDePos, &FGFCS::SetDePos);
878   PropertyManager->Tie("fcs/elevator-pos-deg", this, ofDeg, &FGFCS::GetDePos, &FGFCS::SetDePos);
879   PropertyManager->Tie("fcs/elevator-pos-norm", this, ofNorm, &FGFCS::GetDePos, &FGFCS::SetDePos);
880   PropertyManager->Tie("fcs/mag-elevator-pos-rad", this, ofMag, &FGFCS::GetDePos);
881
882   PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
883   PropertyManager->Tie("fcs/rudder-pos-deg", this,ofDeg, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
884   PropertyManager->Tie("fcs/rudder-pos-norm", this,ofNorm, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
885   PropertyManager->Tie("fcs/mag-rudder-pos-rad", this,ofMag, &FGFCS::GetDrPos);
886
887   PropertyManager->Tie("fcs/flap-pos-rad", this,ofRad, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
888   PropertyManager->Tie("fcs/flap-pos-deg", this,ofDeg, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
889   PropertyManager->Tie("fcs/flap-pos-norm", this,ofNorm, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
890
891   PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
892   PropertyManager->Tie("fcs/speedbrake-pos-deg", this,ofDeg, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
893   PropertyManager->Tie("fcs/speedbrake-pos-norm", this,ofNorm, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
894   PropertyManager->Tie("fcs/mag-speedbrake-pos-rad", this,ofMag, &FGFCS::GetDsbPos);
895
896   PropertyManager->Tie("fcs/spoiler-pos-rad", this, ofRad, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
897   PropertyManager->Tie("fcs/spoiler-pos-deg", this, ofDeg, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
898   PropertyManager->Tie("fcs/spoiler-pos-norm", this, ofNorm, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
899   PropertyManager->Tie("fcs/mag-spoiler-pos-rad", this, ofMag, &FGFCS::GetDspPos);
900
901   PropertyManager->Tie("gear/gear-pos-norm", this, &FGFCS::GetGearPos, &FGFCS::SetGearPos);
902   PropertyManager->Tie("gear/gear-cmd-norm", this, &FGFCS::GetGearCmd, &FGFCS::SetGearCmd);
903   PropertyManager->Tie("fcs/left-brake-cmd-norm", this, &FGFCS::GetLBrake, &FGFCS::SetLBrake);
904   PropertyManager->Tie("fcs/right-brake-cmd-norm", this, &FGFCS::GetRBrake, &FGFCS::SetRBrake);
905   PropertyManager->Tie("fcs/center-brake-cmd-norm", this, &FGFCS::GetCBrake, &FGFCS::SetCBrake);
906   PropertyManager->Tie("fcs/steer-cmd-norm", this, &FGFCS::GetDsCmd, &FGFCS::SetDsCmd);
907
908   PropertyManager->Tie("gear/tailhook-pos-norm", this, &FGFCS::GetTailhookPos, &FGFCS::SetTailhookPos);
909   PropertyManager->Tie("fcs/wing-fold-pos-norm", this, &FGFCS::GetWingFoldPos, &FGFCS::SetWingFoldPos);
910 }
911
912 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
913 // Technically, this function should probably bind propulsion type specific controls
914 // rather than mixture and prop-advance.
915
916 void FGFCS::bindThrottle(unsigned int num)
917 {
918   string tmp;
919
920   tmp = CreateIndexedPropertyName("fcs/throttle-cmd-norm", num);
921   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottleCmd,
922                                         &FGFCS::SetThrottleCmd);
923   tmp = CreateIndexedPropertyName("fcs/throttle-pos-norm", num);
924   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottlePos,
925                                         &FGFCS::SetThrottlePos);
926   tmp = CreateIndexedPropertyName("fcs/mixture-cmd-norm", num);
927   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixtureCmd,
928                                         &FGFCS::SetMixtureCmd);
929   tmp = CreateIndexedPropertyName("fcs/mixture-pos-norm", num);
930   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixturePos,
931                                         &FGFCS::SetMixturePos);
932   tmp = CreateIndexedPropertyName("fcs/advance-cmd-norm", num);
933   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvanceCmd,
934                                         &FGFCS::SetPropAdvanceCmd);
935   tmp = CreateIndexedPropertyName("fcs/advance-pos-norm", num);
936   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvance,
937                                         &FGFCS::SetPropAdvance);
938   tmp = CreateIndexedPropertyName("fcs/feather-cmd-norm", num);
939   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetFeatherCmd,
940                                         &FGFCS::SetFeatherCmd);
941   tmp = CreateIndexedPropertyName("fcs/feather-pos-norm", num);
942   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropFeather,
943                                         &FGFCS::SetPropFeather);
944 }
945
946 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
947
948 void FGFCS::bindModel(void)
949 {
950   unsigned int i;
951   string tmp;
952
953   for (i=0; i<SteerPosDeg.size(); i++) {
954     if (FDMExec->GetGroundReactions()->GetGearUnit(i)->GetSteerable()) {
955       tmp = CreateIndexedPropertyName("fcs/steer-pos-deg", i);
956       PropertyManager->Tie( tmp.c_str(), this, i, &FGFCS::GetSteerPosDeg, &FGFCS::SetSteerPosDeg);
957     }
958   }
959 }
960
961 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
962 //    The bitmasked value choices are as follows:
963 //    unset: In this case (the default) JSBSim would only print
964 //       out the normally expected messages, essentially echoing
965 //       the config files as they are read. If the environment
966 //       variable is not set, debug_lvl is set to 1 internally
967 //    0: This requests JSBSim not to output any messages
968 //       whatsoever.
969 //    1: This value explicity requests the normal JSBSim
970 //       startup messages
971 //    2: This value asks for a message to be printed out when
972 //       a class is instantiated
973 //    4: When this value is set, a message is displayed when a
974 //       FGModel object executes its Run() method
975 //    8: When this value is set, various runtime state variables
976 //       are printed out periodically
977 //    16: When set various parameters are sanity checked and
978 //       a message is printed out when they go out of bounds
979
980 void FGFCS::Debug(int from)
981 {
982   if (debug_lvl <= 0) return;
983
984   if (debug_lvl & 1) { // Standard console startup message output
985     if (from == 2) { // Loader
986       cout << endl << "  " << Name << endl;
987     }
988   }
989   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
990     if (from == 0) cout << "Instantiated: FGFCS" << endl;
991     if (from == 1) cout << "Destroyed:    FGFCS" << endl;
992   }
993   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
994   }
995   if (debug_lvl & 8 ) { // Runtime state variables
996   }
997   if (debug_lvl & 16) { // Sanity checking
998   }
999   if (debug_lvl & 64) {
1000     if (from == 0) { // Constructor
1001       cout << IdSrc << endl;
1002       cout << IdHdr << endl;
1003     }
1004   }
1005 }
1006
1007 }