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
13  Software Foundation; either version 2 of the License, or (at your option) any
14  later 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
22  with this program; if not, write to the Free Software Foundation, Inc., 59
23  Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24 
25  Further information about the GNU Lesser General Public License can also be
26  found on 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 <iomanip>
41 
42 #include "FGFCS.h"
43 #include "input_output/FGModelLoader.h"
44 
45 #include "models/flight_control/FGFilter.h"
46 #include "models/flight_control/FGDeadBand.h"
47 #include "models/flight_control/FGGain.h"
48 #include "models/flight_control/FGPID.h"
49 #include "models/flight_control/FGSwitch.h"
50 #include "models/flight_control/FGSummer.h"
51 #include "models/flight_control/FGKinemat.h"
52 #include "models/flight_control/FGFCSFunction.h"
53 #include "models/flight_control/FGActuator.h"
54 #include "models/flight_control/FGAccelerometer.h"
55 #include "models/flight_control/FGMagnetometer.h"
56 #include "models/flight_control/FGGyro.h"
57 #include "models/flight_control/FGWaypoint.h"
58 #include "models/flight_control/FGAngles.h"
59 #include "models/flight_control/FGDistributor.h"
60 #include "models/flight_control/FGLinearActuator.h"
61 
62 #include "FGFCSChannel.h"
63 
64 using namespace std;
65 
66 namespace JSBSim {
67 
68 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
69 CLASS IMPLEMENTATION
70 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
71 
FGFCS(FGFDMExec * fdm)72 FGFCS::FGFCS(FGFDMExec* fdm) : FGModel(fdm), ChannelRate(1)
73 {
74   int i;
75   Name = "FGFCS";
76   systype = stFCS;
77 
78   fdmex = fdm;
79   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
80   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
81   GearCmd = GearPos = 1; // default to gear down
82   BrakePos.resize(FGLGear::bgNumBrakeGroups);
83   TailhookPos = WingFoldPos = 0.0;
84 
85   bind();
86   for (i=0;i<NForms;i++) {
87     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
88     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
89   }
90 
91   Debug(0);
92 }
93 
94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95 
~FGFCS()96 FGFCS::~FGFCS()
97 {
98   ThrottleCmd.clear();
99   ThrottlePos.clear();
100   MixtureCmd.clear();
101   MixturePos.clear();
102   PropAdvanceCmd.clear();
103   PropAdvance.clear();
104   PropFeatherCmd.clear();
105   PropFeather.clear();
106 
107   unsigned int i;
108 
109   for (i=0;i<SystemChannels.size();i++) delete SystemChannels[i];
110   SystemChannels.clear();
111 
112   Debug(1);
113 }
114 
115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
116 
InitModel(void)117 bool FGFCS::InitModel(void)
118 {
119   if (!FGModel::InitModel()) return false;
120 
121   unsigned int i;
122 
123   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
124   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
125   for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
126   for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
127   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
128   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
129 
130   DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
131   PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
132   TailhookPos = WingFoldPos = 0.0;
133 
134   for (i=0;i<NForms;i++) {
135     DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
136     DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
137   }
138 
139   // Reset the channels components.
140   for (unsigned int i=0; i<SystemChannels.size(); i++) SystemChannels[i]->Reset();
141 
142   return true;
143 }
144 
145 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
146 // Notes: In this logic the default engine commands are set. This is simply a
147 // sort of safe-mode method in case the user has not defined control laws for
148 // throttle, mixture, and prop-advance. The throttle, mixture, and prop advance
149 // positions are set equal to the respective commands. Any control logic that is
150 // actually present in the flight_control or autopilot section will override
151 // these simple assignments.
152 
Run(bool Holding)153 bool FGFCS::Run(bool Holding)
154 {
155   unsigned int i;
156 
157   if (FGModel::Run(Holding)) return true; // fast exit if nothing to do
158   if (Holding) return false;
159 
160   RunPreFunctions();
161 
162   for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
163   for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
164   for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
165   for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
166 
167   // Execute system channels in order
168   for (i=0; i<SystemChannels.size(); i++) {
169     if (debug_lvl & 4) cout << "    Executing System Channel: " << SystemChannels[i]->GetName() << endl;
170     ChannelRate = SystemChannels[i]->GetRate();
171     SystemChannels[i]->Execute();
172   }
173   ChannelRate = 1;
174 
175   RunPostFunctions();
176 
177   return false;
178 }
179 
180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
181 
SetDaLPos(int form,double pos)182 void FGFCS::SetDaLPos( int form , double pos )
183 {
184   switch(form) {
185   case ofRad:
186     DaLPos[ofRad] = pos;
187     DaLPos[ofDeg] = pos*radtodeg;
188     break;
189   case ofDeg:
190     DaLPos[ofRad] = pos*degtorad;
191     DaLPos[ofDeg] = pos;
192     break;
193   case ofNorm:
194     DaLPos[ofNorm] = pos;
195   }
196   DaLPos[ofMag] = fabs(DaLPos[ofRad]);
197 }
198 
199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
200 
SetDaRPos(int form,double pos)201 void FGFCS::SetDaRPos( int form , double pos )
202 {
203   switch(form) {
204   case ofRad:
205     DaRPos[ofRad] = pos;
206     DaRPos[ofDeg] = pos*radtodeg;
207     break;
208   case ofDeg:
209     DaRPos[ofRad] = pos*degtorad;
210     DaRPos[ofDeg] = pos;
211     break;
212   case ofNorm:
213     DaRPos[ofNorm] = pos;
214   }
215   DaRPos[ofMag] = fabs(DaRPos[ofRad]);
216 }
217 
218 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
219 
SetDePos(int form,double pos)220 void FGFCS::SetDePos( int form , double pos )
221 {
222   switch(form) {
223   case ofRad:
224     DePos[ofRad] = pos;
225     DePos[ofDeg] = pos*radtodeg;
226     break;
227   case ofDeg:
228     DePos[ofRad] = pos*degtorad;
229     DePos[ofDeg] = pos;
230     break;
231   case ofNorm:
232     DePos[ofNorm] = pos;
233   }
234   DePos[ofMag] = fabs(DePos[ofRad]);
235 }
236 
237 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
238 
SetDrPos(int form,double pos)239 void FGFCS::SetDrPos( int form , double pos )
240 {
241   switch(form) {
242   case ofRad:
243     DrPos[ofRad] = pos;
244     DrPos[ofDeg] = pos*radtodeg;
245     break;
246   case ofDeg:
247     DrPos[ofRad] = pos*degtorad;
248     DrPos[ofDeg] = pos;
249     break;
250   case ofNorm:
251     DrPos[ofNorm] = pos;
252   }
253   DrPos[ofMag] = fabs(DrPos[ofRad]);
254 }
255 
256 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
257 
SetDfPos(int form,double pos)258 void FGFCS::SetDfPos( int form , double pos )
259 {
260   switch(form) {
261   case ofRad:
262     DfPos[ofRad] = pos;
263     DfPos[ofDeg] = pos*radtodeg;
264     break;
265   case ofDeg:
266     DfPos[ofRad] = pos*degtorad;
267     DfPos[ofDeg] = pos;
268     break;
269   case ofNorm:
270     DfPos[ofNorm] = pos;
271   }
272   DfPos[ofMag] = fabs(DfPos[ofRad]);
273 }
274 
275 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
276 
SetDsbPos(int form,double pos)277 void FGFCS::SetDsbPos( int form , double pos )
278 {
279   switch(form) {
280   case ofRad:
281     DsbPos[ofRad] = pos;
282     DsbPos[ofDeg] = pos*radtodeg;
283     break;
284   case ofDeg:
285     DsbPos[ofRad] = pos*degtorad;
286     DsbPos[ofDeg] = pos;
287     break;
288   case ofNorm:
289     DsbPos[ofNorm] = pos;
290   }
291   DsbPos[ofMag] = fabs(DsbPos[ofRad]);
292 }
293 
294 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
295 
SetDspPos(int form,double pos)296 void FGFCS::SetDspPos( int form , double pos )
297 {
298   switch(form) {
299   case ofRad:
300     DspPos[ofRad] = pos;
301     DspPos[ofDeg] = pos*radtodeg;
302     break;
303   case ofDeg:
304     DspPos[ofRad] = pos*degtorad;
305     DspPos[ofDeg] = pos;
306     break;
307   case ofNorm:
308     DspPos[ofNorm] = pos;
309   }
310   DspPos[ofMag] = fabs(DspPos[ofRad]);
311 }
312 
313 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
314 
SetThrottleCmd(int engineNum,double setting)315 void FGFCS::SetThrottleCmd(int engineNum, double setting)
316 {
317   if (engineNum < (int)ThrottlePos.size()) {
318     if (engineNum < 0) {
319       for (unsigned int ctr=0; ctr<ThrottleCmd.size(); ctr++)
320         ThrottleCmd[ctr] = setting;
321     } else {
322       ThrottleCmd[engineNum] = setting;
323     }
324   } else {
325     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
326          << " engines exist, but attempted throttle command is for engine "
327          << engineNum << endl;
328   }
329 }
330 
331 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
332 
SetThrottlePos(int engineNum,double setting)333 void FGFCS::SetThrottlePos(int engineNum, double setting)
334 {
335   if (engineNum < (int)ThrottlePos.size()) {
336     if (engineNum < 0) {
337       for (unsigned int ctr=0; ctr<ThrottlePos.size(); ctr++)
338         ThrottlePos[ctr] = setting;
339     } else {
340       ThrottlePos[engineNum] = setting;
341     }
342   } else {
343     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
344          << " engines exist, but attempted throttle position setting is for engine "
345          << engineNum << endl;
346   }
347 }
348 
349 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
350 
GetThrottleCmd(int engineNum) const351 double FGFCS::GetThrottleCmd(int engineNum) const
352 {
353   if (engineNum < (int)ThrottlePos.size()) {
354     if (engineNum < 0) {
355        cerr << "Cannot get throttle value for ALL engines" << endl;
356     } else {
357       return ThrottleCmd[engineNum];
358     }
359   } else {
360     cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
361          << " engines exist, but throttle setting for engine " << engineNum
362          << " is selected" << endl;
363   }
364   return 0.0;
365 }
366 
367 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
368 
GetThrottlePos(int engineNum) const369 double FGFCS::GetThrottlePos(int engineNum) const
370 {
371   if (engineNum < (int)ThrottlePos.size()) {
372     if (engineNum < 0) {
373        cerr << "Cannot get throttle value for ALL engines" << endl;
374     } else {
375       return ThrottlePos[engineNum];
376     }
377   } else {
378     cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
379          << " engines exist, but attempted throttle position setting is for engine "
380          << engineNum << endl;
381   }
382   return 0.0;
383 }
384 
385 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
386 
SetMixtureCmd(int engineNum,double setting)387 void FGFCS::SetMixtureCmd(int engineNum, double setting)
388 {
389   if (engineNum < (int)ThrottlePos.size()) {
390     if (engineNum < 0) {
391       for (unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
392         MixtureCmd[ctr] = setting;
393     } else {
394       MixtureCmd[engineNum] = setting;
395     }
396   }
397 }
398 
399 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
400 
SetMixturePos(int engineNum,double setting)401 void FGFCS::SetMixturePos(int engineNum, double setting)
402 {
403   if (engineNum < (int)ThrottlePos.size()) {
404     if (engineNum < 0) {
405       for (unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
406         MixturePos[ctr] = MixtureCmd[ctr];
407     } else {
408       MixturePos[engineNum] = setting;
409     }
410   }
411 }
412 
413 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
414 
SetPropAdvanceCmd(int engineNum,double setting)415 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
416 {
417   if (engineNum < (int)ThrottlePos.size()) {
418     if (engineNum < 0) {
419       for (unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
420         PropAdvanceCmd[ctr] = setting;
421     } else {
422       PropAdvanceCmd[engineNum] = setting;
423     }
424   }
425 }
426 
427 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
428 
SetPropAdvance(int engineNum,double setting)429 void FGFCS::SetPropAdvance(int engineNum, double setting)
430 {
431   if (engineNum < (int)ThrottlePos.size()) {
432     if (engineNum < 0) {
433       for (unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
434         PropAdvance[ctr] = PropAdvanceCmd[ctr];
435     } else {
436       PropAdvance[engineNum] = setting;
437     }
438   }
439 }
440 
441 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
442 
SetFeatherCmd(int engineNum,bool setting)443 void FGFCS::SetFeatherCmd(int engineNum, bool setting)
444 {
445   if (engineNum < (int)ThrottlePos.size()) {
446     if (engineNum < 0) {
447       for (unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
448         PropFeatherCmd[ctr] = setting;
449     } else {
450       PropFeatherCmd[engineNum] = setting;
451     }
452   }
453 }
454 
455 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
456 
SetPropFeather(int engineNum,bool setting)457 void FGFCS::SetPropFeather(int engineNum, bool setting)
458 {
459   if (engineNum < (int)ThrottlePos.size()) {
460     if (engineNum < 0) {
461       for (unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
462         PropFeather[ctr] = PropFeatherCmd[ctr];
463     } else {
464       PropFeather[engineNum] = setting;
465     }
466   }
467 }
468 
469 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
470 
Load(Element * document)471 bool FGFCS::Load(Element* document)
472 {
473   if (document->GetName() == "autopilot") {
474     Name = "Autopilot: ";
475     systype = stAutoPilot;
476   } else if (document->GetName() == "flight_control") {
477     Name = "FCS: ";
478     systype = stFCS;
479   } else if (document->GetName() == "system") {
480     Name = "System: ";
481     systype = stSystem;
482   }
483 
484   // Load interface properties from document
485   if (!FGModel::Load(document, true))
486     return false;
487 
488   Name += document->GetAttributeValue("name");
489 
490   Debug(2);
491 
492   Element* channel_element = document->FindElement("channel");
493 
494   while (channel_element) {
495 
496     FGFCSChannel* newChannel = 0;
497 
498     string sOnOffProperty = channel_element->GetAttributeValue("execute");
499     string sChannelName = channel_element->GetAttributeValue("name");
500 
501     if (!channel_element->GetAttributeValue("execrate").empty())
502       ChannelRate = channel_element->GetAttributeValueAsNumber("execrate");
503     else
504       ChannelRate = 1;
505 
506     if (sOnOffProperty.length() > 0) {
507       FGPropertyNode* OnOffPropertyNode = PropertyManager->GetNode(sOnOffProperty);
508       if (OnOffPropertyNode == 0) {
509         cerr << channel_element->ReadFrom() << highint << fgred
510              << "The On/Off property, " << sOnOffProperty << " specified for channel "
511              << channel_element->GetAttributeValue("name") << " is undefined or not "
512              << "understood. The simulation will abort" << reset << endl;
513         throw("Bad system definition");
514       } else
515         newChannel = new FGFCSChannel(this, sChannelName, ChannelRate,
516                                       OnOffPropertyNode);
517     } else
518       newChannel = new FGFCSChannel(this, sChannelName, ChannelRate);
519 
520     SystemChannels.push_back(newChannel);
521 
522     if (debug_lvl > 0)
523       cout << endl << highint << fgblue << "    Channel "
524          << normint << channel_element->GetAttributeValue("name") << reset << endl;
525 
526     Element* component_element = channel_element->GetElement();
527     while (component_element) {
528       try {
529         if ((component_element->GetName() == string("lag_filter")) ||
530             (component_element->GetName() == string("lead_lag_filter")) ||
531             (component_element->GetName() == string("washout_filter")) ||
532             (component_element->GetName() == string("second_order_filter")) )
533         {
534           newChannel->Add(new FGFilter(this, component_element));
535         } else if ((component_element->GetName() == string("pure_gain")) ||
536                    (component_element->GetName() == string("scheduled_gain")) ||
537                    (component_element->GetName() == string("aerosurface_scale")))
538         {
539           newChannel->Add(new FGGain(this, component_element));
540         } else if (component_element->GetName() == string("summer")) {
541           newChannel->Add(new FGSummer(this, component_element));
542         } else if (component_element->GetName() == string("deadband")) {
543           newChannel->Add(new FGDeadBand(this, component_element));
544         } else if (component_element->GetName() == string("switch")) {
545           newChannel->Add(new FGSwitch(this, component_element));
546         } else if (component_element->GetName() == string("kinematic")) {
547           newChannel->Add(new FGKinemat(this, component_element));
548         } else if (component_element->GetName() == string("fcs_function")) {
549           newChannel->Add(new FGFCSFunction(this, component_element));
550         } else if (component_element->GetName() == string("pid")) {
551           newChannel->Add(new FGPID(this, component_element));
552         } else if (component_element->GetName() == string("integrator")) {
553           // <integrator> is equivalent to <pid type="trap">
554           Element* c1_el = component_element->FindElement("c1");
555           if (!c1_el) {
556             cerr << component_element->ReadFrom();
557             throw("INTEGRATOR component " + component_element->GetAttributeValue("name")
558                   + " does not provide the parameter <c1>");
559           }
560           c1_el->ChangeName("ki");
561           if (!c1_el->HasAttribute("type"))
562             c1_el->AddAttribute("type", "trap");
563           newChannel->Add(new FGPID(this, component_element));
564         } else if (component_element->GetName() == string("actuator")) {
565           newChannel->Add(new FGActuator(this, component_element));
566         } else if (component_element->GetName() == string("sensor")) {
567           newChannel->Add(new FGSensor(this, component_element));
568         } else if (component_element->GetName() == string("accelerometer")) {
569           newChannel->Add(new FGAccelerometer(this, component_element));
570         } else if (component_element->GetName() == string("magnetometer")) {
571           newChannel->Add(new FGMagnetometer(this, component_element));
572         } else if (component_element->GetName() == string("gyro")) {
573           newChannel->Add(new FGGyro(this, component_element));
574         } else if ((component_element->GetName() == string("waypoint_heading")) ||
575                    (component_element->GetName() == string("waypoint_distance")))
576         {
577           newChannel->Add(new FGWaypoint(this, component_element));
578         } else if (component_element->GetName() == string("angle")) {
579           newChannel->Add(new FGAngles(this, component_element));
580         } else if (component_element->GetName() == string("distributor")) {
581           newChannel->Add(new FGDistributor(this, component_element));
582         } else if (component_element->GetName() == string("linear_actuator")) {
583           newChannel->Add(new FGLinearActuator(this, component_element));
584         } else {
585           cerr << "Unknown FCS component: " << component_element->GetName() << endl;
586         }
587       } catch(string& s) {
588         cerr << highint << fgred << endl << "  " << s << endl;
589         cerr << reset << endl;
590         return false;
591       }
592       component_element = channel_element->GetNextElement();
593     }
594     channel_element = document->FindNextElement("channel");
595   }
596 
597   PostLoad(document, FDMExec);
598 
599   return true;
600 }
601 
602 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
603 
GetBrake(FGLGear::BrakeGroup bg)604 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
605 {
606   return BrakePos[bg];
607 }
608 
609 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
610 
FindFullPathName(const SGPath & path) const611 SGPath FGFCS::FindFullPathName(const SGPath& path) const
612 {
613   SGPath name = FGModel::FindFullPathName(path);
614   if (systype != stSystem || !name.isNull()) return name;
615 
616   name = CheckPathName(FDMExec->GetFullAircraftPath()/string("Systems"), path);
617   if (!name.isNull()) return name;
618 
619   return CheckPathName(FDMExec->GetSystemsPath(), path);
620 }
621 
622 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
623 
GetComponentStrings(const string & delimiter) const624 string FGFCS::GetComponentStrings(const string& delimiter) const
625 {
626   string CompStrings = "";
627   bool firstime = true;
628   int total_count=0;
629 
630   for (unsigned int i=0; i<SystemChannels.size(); i++)
631   {
632     for (unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
633     {
634       if (firstime) firstime = false;
635       else          CompStrings += delimiter;
636 
637       CompStrings += SystemChannels[i]->GetComponent(c)->GetName();
638       total_count++;
639     }
640   }
641 
642   return CompStrings;
643 }
644 
645 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
646 
GetComponentValues(const string & delimiter) const647 string FGFCS::GetComponentValues(const string& delimiter) const
648 {
649   std::ostringstream buf;
650 
651   bool firstime = true;
652   int total_count=0;
653 
654   for (unsigned int i=0; i<SystemChannels.size(); i++)
655   {
656     for (unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
657     {
658       if (firstime) firstime = false;
659       else          buf << delimiter;
660 
661       buf << setprecision(9) << SystemChannels[i]->GetComponent(c)->GetOutput();
662       total_count++;
663     }
664   }
665 
666   return buf.str();
667 }
668 
669 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
670 
AddThrottle(void)671 void FGFCS::AddThrottle(void)
672 {
673   ThrottleCmd.push_back(0.0);
674   ThrottlePos.push_back(0.0);
675   MixtureCmd.push_back(0.0);     // assume throttle and mixture are coupled
676   MixturePos.push_back(0.0);
677   PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
678   PropAdvance.push_back(0.0);
679   PropFeatherCmd.push_back(false);
680   PropFeather.push_back(false);
681 
682   unsigned int num = (unsigned int)ThrottleCmd.size()-1;
683   bindThrottle(num);
684 }
685 
686 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
687 
GetDt(void) const688 double FGFCS::GetDt(void) const
689 {
690   return FDMExec->GetDeltaT()*rate;
691 }
692 
693 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
694 
bind(void)695 void FGFCS::bind(void)
696 {
697   PropertyManager->Tie("fcs/aileron-cmd-norm", this, &FGFCS::GetDaCmd, &FGFCS::SetDaCmd);
698   PropertyManager->Tie("fcs/elevator-cmd-norm", this, &FGFCS::GetDeCmd, &FGFCS::SetDeCmd);
699   PropertyManager->Tie("fcs/rudder-cmd-norm", this, &FGFCS::GetDrCmd, &FGFCS::SetDrCmd);
700   PropertyManager->Tie("fcs/flap-cmd-norm", this, &FGFCS::GetDfCmd, &FGFCS::SetDfCmd);
701   PropertyManager->Tie("fcs/speedbrake-cmd-norm", this, &FGFCS::GetDsbCmd, &FGFCS::SetDsbCmd);
702   PropertyManager->Tie("fcs/spoiler-cmd-norm", this, &FGFCS::GetDspCmd, &FGFCS::SetDspCmd);
703   PropertyManager->Tie("fcs/pitch-trim-cmd-norm", this, &FGFCS::GetPitchTrimCmd, &FGFCS::SetPitchTrimCmd);
704   PropertyManager->Tie("fcs/roll-trim-cmd-norm", this, &FGFCS::GetRollTrimCmd, &FGFCS::SetRollTrimCmd);
705   PropertyManager->Tie("fcs/yaw-trim-cmd-norm", this, &FGFCS::GetYawTrimCmd, &FGFCS::SetYawTrimCmd);
706 
707   PropertyManager->Tie("fcs/left-aileron-pos-rad", this, ofRad, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
708   PropertyManager->Tie("fcs/left-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
709   PropertyManager->Tie("fcs/left-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
710   PropertyManager->Tie("fcs/mag-left-aileron-pos-rad", this, ofMag, &FGFCS::GetDaLPos);
711 
712   PropertyManager->Tie("fcs/right-aileron-pos-rad", this, ofRad, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
713   PropertyManager->Tie("fcs/right-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
714   PropertyManager->Tie("fcs/right-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
715   PropertyManager->Tie("fcs/mag-right-aileron-pos-rad", this, ofMag, &FGFCS::GetDaRPos);
716 
717   PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad, &FGFCS::GetDePos, &FGFCS::SetDePos);
718   PropertyManager->Tie("fcs/elevator-pos-deg", this, ofDeg, &FGFCS::GetDePos, &FGFCS::SetDePos);
719   PropertyManager->Tie("fcs/elevator-pos-norm", this, ofNorm, &FGFCS::GetDePos, &FGFCS::SetDePos);
720   PropertyManager->Tie("fcs/mag-elevator-pos-rad", this, ofMag, &FGFCS::GetDePos);
721 
722   PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
723   PropertyManager->Tie("fcs/rudder-pos-deg", this,ofDeg, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
724   PropertyManager->Tie("fcs/rudder-pos-norm", this,ofNorm, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
725   PropertyManager->Tie("fcs/mag-rudder-pos-rad", this,ofMag, &FGFCS::GetDrPos);
726 
727   PropertyManager->Tie("fcs/flap-pos-rad", this,ofRad, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
728   PropertyManager->Tie("fcs/flap-pos-deg", this,ofDeg, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
729   PropertyManager->Tie("fcs/flap-pos-norm", this,ofNorm, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
730 
731   PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
732   PropertyManager->Tie("fcs/speedbrake-pos-deg", this,ofDeg, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
733   PropertyManager->Tie("fcs/speedbrake-pos-norm", this,ofNorm, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
734   PropertyManager->Tie("fcs/mag-speedbrake-pos-rad", this,ofMag, &FGFCS::GetDsbPos);
735 
736   PropertyManager->Tie("fcs/spoiler-pos-rad", this, ofRad, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
737   PropertyManager->Tie("fcs/spoiler-pos-deg", this, ofDeg, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
738   PropertyManager->Tie("fcs/spoiler-pos-norm", this, ofNorm, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
739   PropertyManager->Tie("fcs/mag-spoiler-pos-rad", this, ofMag, &FGFCS::GetDspPos);
740 
741   PropertyManager->Tie("gear/gear-pos-norm", this, &FGFCS::GetGearPos, &FGFCS::SetGearPos);
742   PropertyManager->Tie("gear/gear-cmd-norm", this, &FGFCS::GetGearCmd, &FGFCS::SetGearCmd);
743   PropertyManager->Tie("fcs/left-brake-cmd-norm", this, &FGFCS::GetLBrake, &FGFCS::SetLBrake);
744   PropertyManager->Tie("fcs/right-brake-cmd-norm", this, &FGFCS::GetRBrake, &FGFCS::SetRBrake);
745   PropertyManager->Tie("fcs/center-brake-cmd-norm", this, &FGFCS::GetCBrake, &FGFCS::SetCBrake);
746 
747   PropertyManager->Tie("gear/tailhook-pos-norm", this, &FGFCS::GetTailhookPos, &FGFCS::SetTailhookPos);
748   PropertyManager->Tie("fcs/wing-fold-pos-norm", this, &FGFCS::GetWingFoldPos, &FGFCS::SetWingFoldPos);
749   PropertyManager->Tie("simulation/channel-dt", this, &FGFCS::GetChannelDeltaT);
750 }
751 
752 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
753 // Technically, this function should probably bind propulsion type specific controls
754 // rather than mixture and prop-advance.
755 
bindThrottle(unsigned int num)756 void FGFCS::bindThrottle(unsigned int num)
757 {
758   string tmp;
759 
760   tmp = CreateIndexedPropertyName("fcs/throttle-cmd-norm", num);
761   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottleCmd,
762                                         &FGFCS::SetThrottleCmd);
763   tmp = CreateIndexedPropertyName("fcs/throttle-pos-norm", num);
764   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottlePos,
765                                         &FGFCS::SetThrottlePos);
766   tmp = CreateIndexedPropertyName("fcs/mixture-cmd-norm", num);
767   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixtureCmd,
768                                         &FGFCS::SetMixtureCmd);
769   tmp = CreateIndexedPropertyName("fcs/mixture-pos-norm", num);
770   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixturePos,
771                                         &FGFCS::SetMixturePos);
772   tmp = CreateIndexedPropertyName("fcs/advance-cmd-norm", num);
773   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvanceCmd,
774                                         &FGFCS::SetPropAdvanceCmd);
775   tmp = CreateIndexedPropertyName("fcs/advance-pos-norm", num);
776   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvance,
777                                         &FGFCS::SetPropAdvance);
778   tmp = CreateIndexedPropertyName("fcs/feather-cmd-norm", num);
779   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetFeatherCmd,
780                                         &FGFCS::SetFeatherCmd);
781   tmp = CreateIndexedPropertyName("fcs/feather-pos-norm", num);
782   PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropFeather,
783                                         &FGFCS::SetPropFeather);
784 }
785 
786 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
787 //    The bitmasked value choices are as follows:
788 //    unset: In this case (the default) JSBSim would only print
789 //       out the normally expected messages, essentially echoing
790 //       the config files as they are read. If the environment
791 //       variable is not set, debug_lvl is set to 1 internally
792 //    0: This requests JSBSim not to output any messages
793 //       whatsoever.
794 //    1: This value explicity requests the normal JSBSim
795 //       startup messages
796 //    2: This value asks for a message to be printed out when
797 //       a class is instantiated
798 //    4: When this value is set, a message is displayed when a
799 //       FGModel object executes its Run() method
800 //    8: When this value is set, various runtime state variables
801 //       are printed out periodically
802 //    16: When set various parameters are sanity checked and
803 //       a message is printed out when they go out of bounds
804 
Debug(int from)805 void FGFCS::Debug(int from)
806 {
807   if (debug_lvl <= 0) return;
808 
809   if (debug_lvl & 1) { // Standard console startup message output
810     if (from == 2) { // Loader
811       cout << endl << "  " << Name << endl;
812     }
813   }
814   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
815     if (from == 0) cout << "Instantiated: FGFCS" << endl;
816     if (from == 1) cout << "Destroyed:    FGFCS" << endl;
817   }
818   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
819   }
820   if (debug_lvl & 8 ) { // Runtime state variables
821   }
822   if (debug_lvl & 16) { // Sanity checking
823   }
824   if (debug_lvl & 64) {
825     if (from == 0) { // Constructor
826     }
827   }
828 }
829 
830 }
831