1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6 Purpose: Model the flight controls
9 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) -------------
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU Lesser General Public License as published by the Free Software
13 Foundation; either version 2 of the License, or (at your option) any later
16 This program is distributed in the hope that it will be useful, but WITHOUT
17 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
21 You should have received a copy of the GNU Lesser General Public License along with
22 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
23 Place - Suite 330, Boston, MA 02111-1307, USA.
25 Further information about the GNU Lesser General Public License can also be found on
26 the world wide web at http://www.gnu.org.
28 FUNCTIONAL DESCRIPTION
29 --------------------------------------------------------------------------------
30 This class models the flight controls for a specific airplane
33 --------------------------------------------------------------------------------
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
41 #include "FGFDMExec.h"
42 #include "FGGroundReactions.h"
43 #include "input_output/FGPropertyManager.h"
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"
66 static const char *IdSrc = "$Id: FGFCS.cpp,v 1.72 2010/11/18 12:38:06 jberndt Exp $";
67 static const char *IdHdr = ID_FCS;
69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
73 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
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;
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;
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
101 PropAdvanceCmd.clear();
104 PropFeatherCmd.clear();
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];
120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122 bool FGFCS::InitModel(void)
126 if (!FGModel::InitModel()) return false;
128 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
129 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
130 for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
131 for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
132 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
133 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
135 DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0;
136 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
137 TailhookPos = WingFoldPos = 0.0;
139 for (i=0;i<NForms;i++) {
140 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
141 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
144 for (unsigned int i=0; i<Systems.size(); i++) {
145 if (Systems[i]->GetType() == "LAG" ||
146 Systems[i]->GetType() == "LEAD_LAG" ||
147 Systems[i]->GetType() == "WASHOUT" ||
148 Systems[i]->GetType() == "SECOND_ORDER_FILTER" ||
149 Systems[i]->GetType() == "INTEGRATOR")
151 ((FGFilter*)Systems[i])->ResetPastStates();
152 } else if (Systems[i]->GetType() == "PID" ) {
153 ((FGPID*)Systems[i])->ResetPastStates();
157 for (unsigned int i=0; i<FCSComponents.size(); i++) {
158 if (FCSComponents[i]->GetType() == "LAG" ||
159 FCSComponents[i]->GetType() == "LEAD_LAG" ||
160 FCSComponents[i]->GetType() == "WASHOUT" ||
161 FCSComponents[i]->GetType() == "SECOND_ORDER_FILTER" ||
162 FCSComponents[i]->GetType() == "INTEGRATOR")
164 ((FGFilter*)FCSComponents[i])->ResetPastStates();
165 } else if (FCSComponents[i]->GetType() == "PID" ) {
166 ((FGPID*)FCSComponents[i])->ResetPastStates();
170 for (unsigned int i=0; i<APComponents.size(); i++) {
171 if (APComponents[i]->GetType() == "LAG" ||
172 APComponents[i]->GetType() == "LEAD_LAG" ||
173 APComponents[i]->GetType() == "WASHOUT" ||
174 APComponents[i]->GetType() == "SECOND_ORDER_FILTER" ||
175 APComponents[i]->GetType() == "INTEGRATOR")
177 ((FGFilter*)APComponents[i])->ResetPastStates();
178 } else if (APComponents[i]->GetType() == "PID" ) {
179 ((FGPID*)APComponents[i])->ResetPastStates();
186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188 void FGFCS::LateBind(void)
192 for (i=0; i<Systems.size(); i++) Systems[i]->LateBind();
193 for (i=0; i<APComponents.size(); i++) APComponents[i]->LateBind();
194 for (i=0; i<FCSComponents.size(); i++) FCSComponents[i]->LateBind();
197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
198 // Notes: In this logic the default engine commands are set. This is simply a
199 // sort of safe-mode method in case the user has not defined control laws for
200 // throttle, mixture, and prop-advance. The throttle, mixture, and prop advance
201 // positions are set equal to the respective commands. Any control logic that is
202 // actually present in the flight_control or autopilot section will override
203 // these simple assignments.
205 bool FGFCS::Run(void)
209 if (FGModel::Run()) return true; // fast exit if nothing to do
210 if (FDMExec->Holding()) return false;
214 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
215 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
216 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
217 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
219 // Set the default steering angle
220 for (i=0; i<SteerPosDeg.size(); i++) {
221 FGLGear* gear = FDMExec->GetGroundReactions()->GetGearUnit(i);
222 SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() );
225 // Execute Systems in order
226 for (i=0; i<Systems.size(); i++) Systems[i]->Run();
229 for (i=0; i<APComponents.size(); i++) APComponents[i]->Run();
231 // Execute Flight Control System
232 for (i=0; i<FCSComponents.size(); i++) FCSComponents[i]->Run();
239 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241 void FGFCS::SetDaLPos( int form , double pos )
246 DaLPos[ofDeg] = pos*radtodeg;
249 DaLPos[ofRad] = pos*degtorad;
253 DaLPos[ofNorm] = pos;
255 DaLPos[ofMag] = fabs(DaLPos[ofRad]);
258 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
260 void FGFCS::SetDaRPos( int form , double pos )
265 DaRPos[ofDeg] = pos*radtodeg;
268 DaRPos[ofRad] = pos*degtorad;
272 DaRPos[ofNorm] = pos;
274 DaRPos[ofMag] = fabs(DaRPos[ofRad]);
277 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
279 void FGFCS::SetDePos( int form , double pos )
284 DePos[ofDeg] = pos*radtodeg;
287 DePos[ofRad] = pos*degtorad;
293 DePos[ofMag] = fabs(DePos[ofRad]);
296 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
298 void FGFCS::SetDrPos( int form , double pos )
303 DrPos[ofDeg] = pos*radtodeg;
306 DrPos[ofRad] = pos*degtorad;
312 DrPos[ofMag] = fabs(DrPos[ofRad]);
315 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
317 void FGFCS::SetDfPos( int form , double pos )
322 DfPos[ofDeg] = pos*radtodeg;
325 DfPos[ofRad] = pos*degtorad;
331 DfPos[ofMag] = fabs(DfPos[ofRad]);
334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
336 void FGFCS::SetDsbPos( int form , double pos )
341 DsbPos[ofDeg] = pos*radtodeg;
344 DsbPos[ofRad] = pos*degtorad;
348 DsbPos[ofNorm] = pos;
350 DsbPos[ofMag] = fabs(DsbPos[ofRad]);
353 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
355 void FGFCS::SetDspPos( int form , double pos )
360 DspPos[ofDeg] = pos*radtodeg;
363 DspPos[ofRad] = pos*degtorad;
367 DspPos[ofNorm] = pos;
369 DspPos[ofMag] = fabs(DspPos[ofRad]);
372 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
374 void FGFCS::SetThrottleCmd(int engineNum, double setting)
378 if (engineNum < (int)ThrottlePos.size()) {
380 for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
382 ThrottleCmd[engineNum] = setting;
385 cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
386 << " engines exist, but attempted throttle command is for engine "
387 << engineNum << endl;
391 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
393 void FGFCS::SetThrottlePos(int engineNum, double setting)
397 if (engineNum < (int)ThrottlePos.size()) {
399 for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
401 ThrottlePos[engineNum] = setting;
404 cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
405 << " engines exist, but attempted throttle position setting is for engine "
406 << engineNum << endl;
410 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
412 double FGFCS::GetThrottleCmd(int engineNum) const
414 if (engineNum < (int)ThrottlePos.size()) {
416 cerr << "Cannot get throttle value for ALL engines" << endl;
418 return ThrottleCmd[engineNum];
421 cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
422 << " engines exist, but throttle setting for engine " << engineNum
423 << " is selected" << endl;
428 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
430 double FGFCS::GetThrottlePos(int engineNum) const
432 if (engineNum < (int)ThrottlePos.size()) {
434 cerr << "Cannot get throttle value for ALL engines" << endl;
436 return ThrottlePos[engineNum];
439 cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
440 << " engines exist, but attempted throttle position setting is for engine "
441 << engineNum << endl;
446 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
448 void FGFCS::SetMixtureCmd(int engineNum, double setting)
452 if (engineNum < (int)ThrottlePos.size()) {
454 for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
456 MixtureCmd[engineNum] = setting;
461 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
463 void FGFCS::SetMixturePos(int engineNum, double setting)
467 if (engineNum < (int)ThrottlePos.size()) {
469 for (ctr=0;ctr<MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
471 MixturePos[engineNum] = setting;
476 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
478 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
482 if (engineNum < (int)ThrottlePos.size()) {
484 for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
486 PropAdvanceCmd[engineNum] = setting;
491 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
493 void FGFCS::SetPropAdvance(int engineNum, double setting)
497 if (engineNum < (int)ThrottlePos.size()) {
499 for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
501 PropAdvance[engineNum] = setting;
506 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
508 void FGFCS::SetFeatherCmd(int engineNum, bool setting)
512 if (engineNum < (int)ThrottlePos.size()) {
514 for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeatherCmd[ctr] = setting;
516 PropFeatherCmd[engineNum] = setting;
521 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
523 void FGFCS::SetPropFeather(int engineNum, bool setting)
527 if (engineNum < (int)ThrottlePos.size()) {
529 for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeather[ctr] = PropFeatherCmd[ctr];
531 PropFeather[engineNum] = setting;
536 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
538 bool FGFCS::Load(Element* el, SystemType systype)
540 string name, file, fname="", interface_property_string, parent_name;
541 vector <FGFCSComponent*> *Components;
542 Element *component_element;
543 Element *channel_element;
547 // ToDo: The handling of name and file attributes could be improved, here,
548 // considering that a name can be in the external file, as well.
550 name = el->GetAttributeValue("name");
552 if (name.empty() || !el->GetAttributeValue("file").empty()) {
553 fname = el->GetAttributeValue("file");
554 if (systype == stSystem) {
555 file = FindSystemFullPathname(fname);
557 file = FDMExec->GetFullAircraftPath() + "/" + fname + ".xml";
560 cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl;
563 document = LoadXMLDocument(file);
565 cerr << "Error loading file " << file << endl;
568 name = document->GetAttributeValue("name");
574 if (document->GetName() == "autopilot") {
575 Components = &APComponents;
576 Name = "Autopilot: " + document->GetAttributeValue("name");
577 } else if (document->GetName() == "flight_control") {
578 Components = &FCSComponents;
579 Name = "FCS: " + document->GetAttributeValue("name");
580 } else if (document->GetName() == "system") {
581 Components = &Systems;
582 Name = "System: " + document->GetAttributeValue("name");
586 if (document->GetName() == "flight_control") bindModel();
588 FGModel::Load(document); // Load interface properties from document
590 // After reading interface properties in a file, read properties in the local
591 // flight_control, autopilot, or system element. This allows general-purpose
592 // systems to be defined in a file, with overrides or initial loaded constants
593 // supplied in the relevant element of the aircraft configuration file.
595 Element* property_element = 0;
597 if (!fname.empty()) {
598 property_element = el->FindElement("property");
599 if (property_element && debug_lvl > 0) cout << endl << " Overriding properties" << endl << endl;
600 while (property_element) {
602 if ( ! property_element->GetAttributeValue("value").empty())
603 value = property_element->GetAttributeValueAsNumber("value");
605 interface_property_string = property_element->GetDataLine();
606 if (PropertyManager->HasNode(interface_property_string)) {
607 FGPropertyManager* node = PropertyManager->GetNode(interface_property_string);
609 cout << " " << "Overriding value for property " << interface_property_string
610 << " (old value: " << node->getDoubleValue() << " new value: " << value << ")" << endl;
611 node->setDoubleValue(value);
613 interface_properties.push_back(new double(value));
614 PropertyManager->Tie(interface_property_string, interface_properties.back());
616 cout << " " << interface_property_string << " (initial value: " << value << ")" << endl;
619 property_element = el->FindNextElement("property");
623 channel_element = document->FindElement("channel");
624 while (channel_element) {
627 cout << endl << highint << fgblue << " Channel "
628 << normint << channel_element->GetAttributeValue("name") << reset << endl;
630 component_element = channel_element->GetElement();
631 while (component_element) {
633 if ((component_element->GetName() == string("lag_filter")) ||
634 (component_element->GetName() == string("lead_lag_filter")) ||
635 (component_element->GetName() == string("washout_filter")) ||
636 (component_element->GetName() == string("second_order_filter")) ||
637 (component_element->GetName() == string("integrator")) )
639 Components->push_back(new FGFilter(this, component_element));
640 } else if ((component_element->GetName() == string("pure_gain")) ||
641 (component_element->GetName() == string("scheduled_gain")) ||
642 (component_element->GetName() == string("aerosurface_scale")))
644 Components->push_back(new FGGain(this, component_element));
645 } else if (component_element->GetName() == string("summer")) {
646 Components->push_back(new FGSummer(this, component_element));
647 } else if (component_element->GetName() == string("deadband")) {
648 Components->push_back(new FGDeadBand(this, component_element));
649 } else if (component_element->GetName() == string("switch")) {
650 Components->push_back(new FGSwitch(this, component_element));
651 } else if (component_element->GetName() == string("kinematic")) {
652 Components->push_back(new FGKinemat(this, component_element));
653 } else if (component_element->GetName() == string("fcs_function")) {
654 Components->push_back(new FGFCSFunction(this, component_element));
655 } else if (component_element->GetName() == string("pid")) {
656 Components->push_back(new FGPID(this, component_element));
657 } else if (component_element->GetName() == string("actuator")) {
658 Components->push_back(new FGActuator(this, component_element));
659 } else if (component_element->GetName() == string("sensor")) {
660 Components->push_back(new FGSensor(this, component_element));
661 } else if (component_element->GetName() == string("accelerometer")) {
662 Components->push_back(new FGAccelerometer(this, component_element));
663 } else if (component_element->GetName() == string("magnetometer")) {
664 Components->push_back(new FGMagnetometer(this, component_element));
665 } else if (component_element->GetName() == string("gyro")) {
666 Components->push_back(new FGGyro(this, component_element));
668 cerr << "Unknown FCS component: " << component_element->GetName() << endl;
671 cerr << highint << fgred << endl << " " << s << endl;
672 cerr << reset << endl;
675 component_element = channel_element->GetNextElement();
677 channel_element = document->FindNextElement("channel");
685 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
687 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
690 case FGLGear::bgLeft:
692 case FGLGear::bgRight:
694 case FGLGear::bgCenter:
697 cerr << "GetBrake asked to return a bogus brake value" << endl;
702 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
704 string FGFCS::FindSystemFullPathname(const string& sysfilename)
706 string fullpath, localpath;
707 string system_filename = sysfilename;
708 string systemPath = FDMExec->GetSystemsPath();
709 string aircraftPath = FDMExec->GetFullAircraftPath();
710 ifstream system_file;
712 fullpath = systemPath + "/";
713 localpath = aircraftPath + "/Systems/";
715 if (system_filename.length() <=4 || system_filename.substr(system_filename.length()-4, 4) != ".xml") {
716 system_filename.append(".xml");
719 system_file.open(string(localpath + system_filename).c_str());
720 if ( !system_file.is_open()) {
721 system_file.open(string(fullpath + system_filename).c_str());
722 if ( !system_file.is_open()) {
723 cerr << " Could not open system file: " << system_filename << " in path "
724 << fullpath << " or " << localpath << endl;
727 return string(fullpath + system_filename);
730 return string(localpath + system_filename);
733 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
735 ifstream* FGFCS::FindSystemFile(const string& sysfilename)
737 string fullpath, localpath;
738 string system_filename = sysfilename;
739 string systemPath = FDMExec->GetSystemsPath();
740 string aircraftPath = FDMExec->GetFullAircraftPath();
741 ifstream* system_file = new ifstream();
743 fullpath = systemPath + "/";
744 localpath = aircraftPath + "/Systems/";
746 if (system_filename.substr(system_filename.length()-4, 4) != ".xml") {
747 system_filename.append(".xml");
750 system_file->open(string(localpath + system_filename).c_str());
751 if ( !system_file->is_open()) {
752 system_file->open(string(fullpath + system_filename).c_str());
753 if ( !system_file->is_open()) {
754 cerr << " Could not open system file: " << system_filename << " in path "
755 << fullpath << " or " << localpath << endl;
761 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
763 string FGFCS::GetComponentStrings(const string& delimiter) const
766 string CompStrings = "";
767 bool firstime = true;
770 for (unsigned int i=0; i<Systems.size(); i++) {
771 if (firstime) firstime = false;
772 else CompStrings += delimiter;
774 CompStrings += Systems[i]->GetName();
778 for (comp = 0; comp < APComponents.size(); comp++)
780 if (firstime) firstime = false;
781 else CompStrings += delimiter;
783 CompStrings += APComponents[comp]->GetName();
787 for (comp = 0; comp < FCSComponents.size(); comp++) {
788 if (firstime) firstime = false;
789 else CompStrings += delimiter;
791 CompStrings += FCSComponents[comp]->GetName();
798 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
800 string FGFCS::GetComponentValues(const string& delimiter) const
802 std::ostringstream buf;
805 bool firstime = true;
808 for (unsigned int i=0; i<Systems.size(); i++) {
809 if (firstime) firstime = false;
810 else buf << delimiter;
812 buf << setprecision(9) << Systems[i]->GetOutput();
816 for (comp = 0; comp < APComponents.size(); comp++) {
817 if (firstime) firstime = false;
818 else buf << delimiter;
820 buf << setprecision(9) << APComponents[comp]->GetOutput();
824 for (comp = 0; comp < FCSComponents.size(); comp++) {
825 if (firstime) firstime = false;
826 else buf << delimiter;
828 buf << setprecision(9) << FCSComponents[comp]->GetOutput();
835 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
837 void FGFCS::AddThrottle(void)
839 ThrottleCmd.push_back(0.0);
840 ThrottlePos.push_back(0.0);
841 MixtureCmd.push_back(0.0); // assume throttle and mixture are coupled
842 MixturePos.push_back(0.0);
843 PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
844 PropAdvance.push_back(0.0);
845 PropFeatherCmd.push_back(false);
846 PropFeather.push_back(false);
848 unsigned int num = (unsigned int)ThrottleCmd.size()-1;
852 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
854 void FGFCS::AddGear(void)
856 SteerPosDeg.push_back(0.0);
859 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
861 double FGFCS::GetDt(void)
863 return FDMExec->GetDeltaT()*rate;
866 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
868 void FGFCS::bind(void)
870 PropertyManager->Tie("fcs/aileron-cmd-norm", this, &FGFCS::GetDaCmd, &FGFCS::SetDaCmd);
871 PropertyManager->Tie("fcs/elevator-cmd-norm", this, &FGFCS::GetDeCmd, &FGFCS::SetDeCmd);
872 PropertyManager->Tie("fcs/rudder-cmd-norm", this, &FGFCS::GetDrCmd, &FGFCS::SetDrCmd);
873 PropertyManager->Tie("fcs/flap-cmd-norm", this, &FGFCS::GetDfCmd, &FGFCS::SetDfCmd);
874 PropertyManager->Tie("fcs/speedbrake-cmd-norm", this, &FGFCS::GetDsbCmd, &FGFCS::SetDsbCmd);
875 PropertyManager->Tie("fcs/spoiler-cmd-norm", this, &FGFCS::GetDspCmd, &FGFCS::SetDspCmd);
876 PropertyManager->Tie("fcs/pitch-trim-cmd-norm", this, &FGFCS::GetPitchTrimCmd, &FGFCS::SetPitchTrimCmd);
877 PropertyManager->Tie("fcs/roll-trim-cmd-norm", this, &FGFCS::GetRollTrimCmd, &FGFCS::SetRollTrimCmd);
878 PropertyManager->Tie("fcs/yaw-trim-cmd-norm", this, &FGFCS::GetYawTrimCmd, &FGFCS::SetYawTrimCmd);
880 PropertyManager->Tie("fcs/left-aileron-pos-rad", this, ofRad, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
881 PropertyManager->Tie("fcs/left-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
882 PropertyManager->Tie("fcs/left-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaLPos, &FGFCS::SetDaLPos);
883 PropertyManager->Tie("fcs/mag-left-aileron-pos-rad", this, ofMag, &FGFCS::GetDaLPos);
885 PropertyManager->Tie("fcs/right-aileron-pos-rad", this, ofRad, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
886 PropertyManager->Tie("fcs/right-aileron-pos-deg", this, ofDeg, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
887 PropertyManager->Tie("fcs/right-aileron-pos-norm", this, ofNorm, &FGFCS::GetDaRPos, &FGFCS::SetDaRPos);
888 PropertyManager->Tie("fcs/mag-right-aileron-pos-rad", this, ofMag, &FGFCS::GetDaRPos);
890 PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad, &FGFCS::GetDePos, &FGFCS::SetDePos);
891 PropertyManager->Tie("fcs/elevator-pos-deg", this, ofDeg, &FGFCS::GetDePos, &FGFCS::SetDePos);
892 PropertyManager->Tie("fcs/elevator-pos-norm", this, ofNorm, &FGFCS::GetDePos, &FGFCS::SetDePos);
893 PropertyManager->Tie("fcs/mag-elevator-pos-rad", this, ofMag, &FGFCS::GetDePos);
895 PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
896 PropertyManager->Tie("fcs/rudder-pos-deg", this,ofDeg, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
897 PropertyManager->Tie("fcs/rudder-pos-norm", this,ofNorm, &FGFCS::GetDrPos, &FGFCS::SetDrPos);
898 PropertyManager->Tie("fcs/mag-rudder-pos-rad", this,ofMag, &FGFCS::GetDrPos);
900 PropertyManager->Tie("fcs/flap-pos-rad", this,ofRad, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
901 PropertyManager->Tie("fcs/flap-pos-deg", this,ofDeg, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
902 PropertyManager->Tie("fcs/flap-pos-norm", this,ofNorm, &FGFCS::GetDfPos, &FGFCS::SetDfPos);
904 PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
905 PropertyManager->Tie("fcs/speedbrake-pos-deg", this,ofDeg, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
906 PropertyManager->Tie("fcs/speedbrake-pos-norm", this,ofNorm, &FGFCS::GetDsbPos, &FGFCS::SetDsbPos);
907 PropertyManager->Tie("fcs/mag-speedbrake-pos-rad", this,ofMag, &FGFCS::GetDsbPos);
909 PropertyManager->Tie("fcs/spoiler-pos-rad", this, ofRad, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
910 PropertyManager->Tie("fcs/spoiler-pos-deg", this, ofDeg, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
911 PropertyManager->Tie("fcs/spoiler-pos-norm", this, ofNorm, &FGFCS::GetDspPos, &FGFCS::SetDspPos);
912 PropertyManager->Tie("fcs/mag-spoiler-pos-rad", this, ofMag, &FGFCS::GetDspPos);
914 PropertyManager->Tie("gear/gear-pos-norm", this, &FGFCS::GetGearPos, &FGFCS::SetGearPos);
915 PropertyManager->Tie("gear/gear-cmd-norm", this, &FGFCS::GetGearCmd, &FGFCS::SetGearCmd);
916 PropertyManager->Tie("fcs/left-brake-cmd-norm", this, &FGFCS::GetLBrake, &FGFCS::SetLBrake);
917 PropertyManager->Tie("fcs/right-brake-cmd-norm", this, &FGFCS::GetRBrake, &FGFCS::SetRBrake);
918 PropertyManager->Tie("fcs/center-brake-cmd-norm", this, &FGFCS::GetCBrake, &FGFCS::SetCBrake);
919 PropertyManager->Tie("fcs/steer-cmd-norm", this, &FGFCS::GetDsCmd, &FGFCS::SetDsCmd);
921 PropertyManager->Tie("gear/tailhook-pos-norm", this, &FGFCS::GetTailhookPos, &FGFCS::SetTailhookPos);
922 PropertyManager->Tie("fcs/wing-fold-pos-norm", this, &FGFCS::GetWingFoldPos, &FGFCS::SetWingFoldPos);
925 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
926 // Technically, this function should probably bind propulsion type specific controls
927 // rather than mixture and prop-advance.
929 void FGFCS::bindThrottle(unsigned int num)
933 tmp = CreateIndexedPropertyName("fcs/throttle-cmd-norm", num);
934 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottleCmd,
935 &FGFCS::SetThrottleCmd);
936 tmp = CreateIndexedPropertyName("fcs/throttle-pos-norm", num);
937 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottlePos,
938 &FGFCS::SetThrottlePos);
939 tmp = CreateIndexedPropertyName("fcs/mixture-cmd-norm", num);
940 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixtureCmd,
941 &FGFCS::SetMixtureCmd);
942 tmp = CreateIndexedPropertyName("fcs/mixture-pos-norm", num);
943 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixturePos,
944 &FGFCS::SetMixturePos);
945 tmp = CreateIndexedPropertyName("fcs/advance-cmd-norm", num);
946 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvanceCmd,
947 &FGFCS::SetPropAdvanceCmd);
948 tmp = CreateIndexedPropertyName("fcs/advance-pos-norm", num);
949 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvance,
950 &FGFCS::SetPropAdvance);
951 tmp = CreateIndexedPropertyName("fcs/feather-cmd-norm", num);
952 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetFeatherCmd,
953 &FGFCS::SetFeatherCmd);
954 tmp = CreateIndexedPropertyName("fcs/feather-pos-norm", num);
955 PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropFeather,
956 &FGFCS::SetPropFeather);
959 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
961 void FGFCS::bindModel(void)
966 for (i=0; i<SteerPosDeg.size(); i++) {
967 if (FDMExec->GetGroundReactions()->GetGearUnit(i)->GetSteerable()) {
968 tmp = CreateIndexedPropertyName("fcs/steer-pos-deg", i);
969 PropertyManager->Tie( tmp.c_str(), this, i, &FGFCS::GetSteerPosDeg, &FGFCS::SetSteerPosDeg);
974 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
975 // The bitmasked value choices are as follows:
976 // unset: In this case (the default) JSBSim would only print
977 // out the normally expected messages, essentially echoing
978 // the config files as they are read. If the environment
979 // variable is not set, debug_lvl is set to 1 internally
980 // 0: This requests JSBSim not to output any messages
982 // 1: This value explicity requests the normal JSBSim
984 // 2: This value asks for a message to be printed out when
985 // a class is instantiated
986 // 4: When this value is set, a message is displayed when a
987 // FGModel object executes its Run() method
988 // 8: When this value is set, various runtime state variables
989 // are printed out periodically
990 // 16: When set various parameters are sanity checked and
991 // a message is printed out when they go out of bounds
993 void FGFCS::Debug(int from)
995 if (debug_lvl <= 0) return;
997 if (debug_lvl & 1) { // Standard console startup message output
998 if (from == 2) { // Loader
999 cout << endl << " " << Name << endl;
1002 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
1003 if (from == 0) cout << "Instantiated: FGFCS" << endl;
1004 if (from == 1) cout << "Destroyed: FGFCS" << endl;
1006 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
1008 if (debug_lvl & 8 ) { // Runtime state variables
1010 if (debug_lvl & 16) { // Sanity checking
1012 if (debug_lvl & 64) {
1013 if (from == 0) { // Constructor
1014 cout << IdSrc << endl;
1015 cout << IdHdr << endl;