1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6 Purpose: Model the flight controls
9 ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
11 This program is free software; you can redistribute it and/or modify it under
12 the terms of the GNU 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 General Public License for more
21 You should have received a copy of the GNU 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 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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
42 #include "FGFDMExec.h"
43 #include "FGAtmosphere.h"
44 #include "FGAircraft.h"
45 #include "FGTranslation.h"
46 #include "FGRotation.h"
47 #include "FGPosition.h"
48 #include "FGAuxiliary.h"
50 #include "FGPropertyManager.h"
52 #include "filtersjb/FGFilter.h"
53 #include "filtersjb/FGDeadBand.h"
54 #include "filtersjb/FGGain.h"
55 #include "filtersjb/FGGradient.h"
56 #include "filtersjb/FGSwitch.h"
57 #include "filtersjb/FGSummer.h"
58 #include "filtersjb/FGKinemat.h"
60 static const char *IdSrc = "$Id$";
61 static const char *IdHdr = ID_FCS;
63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
67 FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
72 DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0.0;
73 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
74 GearCmd = GearPos = 1; // default to gear down
75 LeftBrake = RightBrake = CenterBrake = 0.0;
79 for(i=0;i<=NForms;i++) {
80 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
81 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
84 for(i=0;i<NNorm;i++) { ToNormalize[i]=-1;}
88 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
96 PropAdvanceCmd.clear();
103 for (i=0;i<Components.size();i++) delete Components[i];
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 bool FGFCS::Run(void)
113 if (!FGModel::Run()) {
114 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
115 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
116 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
117 for (i=0; i<Components.size(); i++) Components[i]->Run();
118 if(DoNormalize) Normalize();
125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
127 void FGFCS::SetThrottleCmd(int engineNum, double setting)
131 if (engineNum < (int)ThrottlePos.size()) {
133 for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
135 ThrottleCmd[engineNum] = setting;
138 cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
139 << " engines exist, but attempted throttle command is for engine "
140 << engineNum << endl;
144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
146 void FGFCS::SetThrottlePos(int engineNum, double setting)
150 if (engineNum < (int)ThrottlePos.size()) {
152 for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
154 ThrottlePos[engineNum] = setting;
157 cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
158 << " engines exist, but attempted throttle position setting is for engine "
159 << engineNum << endl;
163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
165 double FGFCS::GetThrottleCmd(int engineNum)
167 if (engineNum < (int)ThrottlePos.size()) {
169 cerr << "Cannot get throttle value for ALL engines" << endl;
171 return ThrottleCmd[engineNum];
174 cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
175 << " engines exist, but throttle setting for engine " << engineNum
176 << " is selected" << endl;
181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183 double FGFCS::GetThrottlePos(int engineNum)
185 if (engineNum < (int)ThrottlePos.size()) {
187 cerr << "Cannot get throttle value for ALL engines" << endl;
189 return ThrottlePos[engineNum];
192 cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
193 << " engines exist, but attempted throttle position setting is for engine "
194 << engineNum << endl;
199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
201 void FGFCS::SetMixtureCmd(int engineNum, double setting)
205 if (engineNum < (int)ThrottlePos.size()) {
207 for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
209 MixtureCmd[engineNum] = setting;
214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
216 void FGFCS::SetMixturePos(int engineNum, double setting)
220 if (engineNum < (int)ThrottlePos.size()) {
222 for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
224 MixturePos[engineNum] = setting;
229 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
231 void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
235 if (engineNum < (int)ThrottlePos.size()) {
237 for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
239 PropAdvanceCmd[engineNum] = setting;
244 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
246 void FGFCS::SetPropAdvance(int engineNum, double setting)
250 if (engineNum < (int)ThrottlePos.size()) {
252 for (ctr=0;ctr<=PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
254 PropAdvance[engineNum] = setting;
259 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
261 bool FGFCS::Load(FGConfigFile* AC_cfg)
266 Name = Name + ":" + AC_cfg->GetValue("NAME");
267 if (debug_lvl > 0) cout << " Control System Name: " << Name << endl;
268 if( AC_cfg->GetValue("NORMALIZE") == "FALSE") {
270 cout << " Automatic Control Surface Normalization Disabled" << endl;
272 AC_cfg->GetNextConfigLine();
273 while ((token = AC_cfg->GetValue()) != string("/FLIGHT_CONTROL")) {
274 if (token == "COMPONENT") {
275 token = AC_cfg->GetValue("TYPE");
276 if (debug_lvl > 0) cout << endl << " Loading Component \""
277 << AC_cfg->GetValue("NAME")
278 << "\" of type: " << token << endl;
279 if ((token == "LAG_FILTER") ||
280 (token == "LEAD_LAG_FILTER") ||
281 (token == "SECOND_ORDER_FILTER") ||
282 (token == "WASHOUT_FILTER") ||
283 (token == "INTEGRATOR") ) {
284 Components.push_back(new FGFilter(this, AC_cfg));
285 } else if ((token == "PURE_GAIN") ||
286 (token == "SCHEDULED_GAIN") ||
287 (token == "AEROSURFACE_SCALE") ) {
289 Components.push_back(new FGGain(this, AC_cfg));
291 } else if (token == "SUMMER") {
292 Components.push_back(new FGSummer(this, AC_cfg));
293 } else if (token == "DEADBAND") {
294 Components.push_back(new FGDeadBand(this, AC_cfg));
295 } else if (token == "GRADIENT") {
296 Components.push_back(new FGGradient(this, AC_cfg));
297 } else if (token == "SWITCH") {
298 Components.push_back(new FGSwitch(this, AC_cfg));
299 } else if (token == "KINEMAT") {
300 Components.push_back(new FGKinemat(this, AC_cfg));
302 cerr << "Unknown token [" << token << "] in FCS portion of config file" << endl;
305 AC_cfg->GetNextConfigLine();
308 //collect information for normalizing control surfaces
310 for(i=0;i<Components.size();i++) {
312 if(Components[i]->GetType() == "AEROSURFACE_SCALE"
313 || Components[i]->GetType() == "KINEMAT" ) {
314 if( Components[i]->GetOutputIdx() == FG_ELEVATOR_POS ) {
316 } else if ( Components[i]->GetOutputIdx() == FG_LEFT_AILERON_POS
317 || Components[i]->GetOutputIdx() == FG_AILERON_POS ) {
319 } else if ( Components[i]->GetOutputIdx() == FG_RIGHT_AILERON_POS ) {
321 } else if ( Components[i]->GetOutputIdx() == FG_RUDDER_POS ) {
323 } else if ( Components[i]->GetOutputIdx() == FG_SPDBRAKE_POS ) {
325 } else if ( Components[i]->GetOutputIdx() == FG_SPOILERS_POS ) {
327 } else if ( Components[i]->GetOutputIdx() == FG_FLAPS_POS ) {
336 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
338 double FGFCS::GetComponentOutput(eParam idx) {
339 return Components[idx]->GetOutput();
342 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
344 string FGFCS::GetComponentName(int idx) {
345 return Components[idx]->GetName();
348 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
350 double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
353 case FGLGear::bgLeft:
355 case FGLGear::bgRight:
357 case FGLGear::bgCenter:
360 cerr << "GetBrake asked to return a bogus brake value" << endl;
365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 string FGFCS::GetComponentStrings(void)
370 string CompStrings = "";
371 bool firstime = true;
373 for (comp = 0; comp < Components.size(); comp++) {
374 if (firstime) firstime = false;
375 else CompStrings += ", ";
377 CompStrings += Components[comp]->GetName();
383 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
385 string FGFCS::GetComponentValues(void)
388 string CompValues = "";
390 bool firstime = true;
392 for (comp = 0; comp < Components.size(); comp++) {
393 if (firstime) firstime = false;
394 else CompValues += ", ";
396 sprintf(buffer, "%9.6f", Components[comp]->GetOutput());
397 CompValues += string(buffer);
403 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
405 void FGFCS::AddThrottle(void)
407 ThrottleCmd.push_back(0.0);
408 ThrottlePos.push_back(0.0);
409 MixtureCmd.push_back(0.0); // assume throttle and mixture are coupled
410 MixturePos.push_back(0.0);
411 PropAdvanceCmd.push_back(0.0); // assume throttle and prop pitch are coupled
412 PropAdvance.push_back(0.0);
415 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
417 void FGFCS::Normalize(void) {
419 //not all of these are guaranteed to be defined for every model
420 //those that are have an index >=0 in the ToNormalize array
421 //ToNormalize is filled in Load()
423 if( ToNormalize[iDe] > -1 ) {
424 DePos[ofNorm] = Components[ToNormalize[iDe]]->GetOutputPct();
427 if( ToNormalize[iDaL] > -1 ) {
428 DaLPos[ofNorm] = Components[ToNormalize[iDaL]]->GetOutputPct();
431 if( ToNormalize[iDaR] > -1 ) {
432 DaRPos[ofNorm] = Components[ToNormalize[iDaR]]->GetOutputPct();
435 if( ToNormalize[iDr] > -1 ) {
436 DrPos[ofNorm] = Components[ToNormalize[iDr]]->GetOutputPct();
439 if( ToNormalize[iDsb] > -1 ) {
440 DsbPos[ofNorm] = Components[ToNormalize[iDsb]]->GetOutputPct();
443 if( ToNormalize[iDsp] > -1 ) {
444 DspPos[ofNorm] = Components[ToNormalize[iDsp]]->GetOutputPct();
447 if( ToNormalize[iDf] > -1 ) {
448 DfPos[ofNorm] = Components[ToNormalize[iDf]]->GetOutputPct();
451 DePos[ofMag] = fabs(DePos[ofRad]);
452 DaLPos[ofMag] = fabs(DaLPos[ofRad]);
453 DaRPos[ofMag] = fabs(DaRPos[ofRad]);
454 DrPos[ofMag] = fabs(DrPos[ofRad]);
455 DsbPos[ofMag] = fabs(DsbPos[ofRad]);
456 DspPos[ofMag] = fabs(DspPos[ofRad]);
457 DfPos[ofMag] = fabs(DfPos[ofRad]);
461 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
463 void FGFCS::bind(void){
464 PropertyManager->Tie("fcs/aileron-cmd-norm", this,
468 PropertyManager->Tie("fcs/elevator-cmd-norm", this,
472 PropertyManager->Tie("fcs/rudder-cmd-norm", this,
476 PropertyManager->Tie("fcs/flap-cmd-norm", this,
480 PropertyManager->Tie("fcs/speedbrake-cmd-norm", this,
484 PropertyManager->Tie("fcs/spoiler-cmd-norm", this,
488 PropertyManager->Tie("fcs/pitch-trim-cmd-norm", this,
489 &FGFCS::GetPitchTrimCmd,
490 &FGFCS::SetPitchTrimCmd,
492 PropertyManager->Tie("fcs/roll-trim-cmd-norm", this,
493 &FGFCS::GetYawTrimCmd,
494 &FGFCS::SetYawTrimCmd,
496 PropertyManager->Tie("fcs/yaw-trim-cmd-norm", this,
497 &FGFCS::GetRollTrimCmd,
498 &FGFCS::SetRollTrimCmd,
500 PropertyManager->Tie("gear/gear-cmd-norm", this,
505 PropertyManager->Tie("fcs/left-aileron-pos-rad", this,ofRad,
509 PropertyManager->Tie("fcs/left-aileron-pos-norm", this,ofNorm,
513 PropertyManager->Tie("fcs/mag-left-aileron-pos-rad", this,ofMag,
518 PropertyManager->Tie("fcs/right-aileron-pos-rad", this,ofRad,
522 PropertyManager->Tie("fcs/right-aileron-pos-norm", this,ofNorm,
526 PropertyManager->Tie("fcs/mag-right-aileron-pos-rad", this,ofMag,
531 PropertyManager->Tie("fcs/elevator-pos-rad", this, ofRad,
535 PropertyManager->Tie("fcs/elevator-pos-norm", this,ofNorm,
539 PropertyManager->Tie("fcs/mag-elevator-pos-rad", this,ofMag,
544 PropertyManager->Tie("fcs/rudder-pos-rad", this,ofRad,
548 PropertyManager->Tie("fcs/rudder-pos-norm", this,ofNorm,
552 PropertyManager->Tie("fcs/mag-rudder-pos-rad", this,ofMag,
557 PropertyManager->Tie("fcs/flap-pos-deg", this,ofRad,
561 PropertyManager->Tie("fcs/flap-pos-norm", this,ofNorm,
566 PropertyManager->Tie("fcs/speedbrake-pos-rad", this,ofRad,
570 PropertyManager->Tie("fcs/speedbrake-pos-norm", this,ofNorm,
574 PropertyManager->Tie("fcs/mag-speedbrake-pos-rad", this,ofMag,
579 PropertyManager->Tie("fcs/spoiler-pos-rad", this,ofRad,
583 PropertyManager->Tie("fcs/spoiler-pos-norm", this,ofNorm,
587 PropertyManager->Tie("fcs/mag-spoiler-pos-rad", this,ofMag,
592 PropertyManager->Tie("gear/gear-pos-norm", this,
598 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
600 void FGFCS::unbind(void){
601 PropertyManager->Untie("fcs/aileron-cmd-norm");
602 PropertyManager->Untie("fcs/elevator-cmd-norm");
603 PropertyManager->Untie("fcs/rudder-cmd-norm");
604 PropertyManager->Untie("fcs/flap-cmd-norm");
605 PropertyManager->Untie("fcs/speedbrake-cmd-norm");
606 PropertyManager->Untie("fcs/spoiler-cmd-norm");
607 PropertyManager->Untie("fcs/pitch-trim-cmd-norm");
608 PropertyManager->Untie("fcs/roll-trim-cmd-norm");
609 PropertyManager->Untie("fcs/yaw-trim-cmd-norm");
610 PropertyManager->Untie("gear/gear-cmd-norm");
611 PropertyManager->Untie("fcs/left-aileron-pos-rad");
612 PropertyManager->Untie("fcs/mag-left-aileron-pos-rad");
613 PropertyManager->Untie("fcs/left-aileron-pos-norm");
614 PropertyManager->Untie("fcs/right-aileron-pos-rad");
615 PropertyManager->Untie("fcs/mag-right-aileron-pos-rad");
616 PropertyManager->Untie("fcs/right-aileron-pos-norm");
617 PropertyManager->Untie("fcs/elevator-pos-rad");
618 PropertyManager->Untie("fcs/mag-elevator-pos-rad");
619 PropertyManager->Untie("fcs/elevator-pos-norm");
620 PropertyManager->Untie("fcs/rudder-pos-rad");
621 PropertyManager->Untie("fcs/mag-rudder-pos-rad");
622 PropertyManager->Untie("fcs/rudder-pos-norm");
623 PropertyManager->Untie("fcs/flap-pos-deg");
624 PropertyManager->Untie("fcs/flap-pos-norm");
625 PropertyManager->Untie("fcs/speedbrake-pos-rad");
626 PropertyManager->Untie("fcs/mag-speedbrake-pos-rad");
627 PropertyManager->Untie("fcs/speedbrake-pos-norm");
628 PropertyManager->Untie("fcs/spoiler-pos-rad");
629 PropertyManager->Untie("fcs/mag-spoiler-pos-rad");
630 PropertyManager->Untie("fcs/spoiler-pos-norm");
631 PropertyManager->Untie("gear/gear-pos-norm");
636 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
637 // The bitmasked value choices are as follows:
638 // unset: In this case (the default) JSBSim would only print
639 // out the normally expected messages, essentially echoing
640 // the config files as they are read. If the environment
641 // variable is not set, debug_lvl is set to 1 internally
642 // 0: This requests JSBSim not to output any messages
644 // 1: This value explicity requests the normal JSBSim
646 // 2: This value asks for a message to be printed out when
647 // a class is instantiated
648 // 4: When this value is set, a message is displayed when a
649 // FGModel object executes its Run() method
650 // 8: When this value is set, various runtime state variables
651 // are printed out periodically
652 // 16: When set various parameters are sanity checked and
653 // a message is printed out when they go out of bounds
655 void FGFCS::Debug(int from)
657 if (debug_lvl <= 0) return;
659 if (debug_lvl & 1) { // Standard console startup message output
660 if (from == 0) { // Constructor
664 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
665 if (from == 0) cout << "Instantiated: FGFCS" << endl;
666 if (from == 1) cout << "Destroyed: FGFCS" << endl;
668 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
670 if (debug_lvl & 8 ) { // Runtime state variables
672 if (debug_lvl & 16) { // Sanity checking
674 if (debug_lvl & 64) {
675 if (from == 0) { // Constructor
676 cout << IdSrc << endl;
677 cout << IdHdr << endl;