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