45 #include "FGFDMExec.h" 46 #include "FGGroundReactions.h" 47 #include "input_output/FGXMLElement.h" 48 #include "input_output/FGModelLoader.h" 50 #include "models/flight_control/FGFilter.h" 51 #include "models/flight_control/FGDeadBand.h" 52 #include "models/flight_control/FGGain.h" 53 #include "models/flight_control/FGPID.h" 54 #include "models/flight_control/FGSwitch.h" 55 #include "models/flight_control/FGSummer.h" 56 #include "models/flight_control/FGKinemat.h" 57 #include "models/flight_control/FGFCSFunction.h" 58 #include "models/flight_control/FGSensor.h" 59 #include "models/flight_control/FGActuator.h" 60 #include "models/flight_control/FGAccelerometer.h" 61 #include "models/flight_control/FGMagnetometer.h" 62 #include "models/flight_control/FGGyro.h" 63 #include "models/flight_control/FGWaypoint.h" 64 #include "models/flight_control/FGAngles.h" 65 #include "models/flight_control/FGDistributor.h" 67 #include "FGFCSChannel.h" 73 IDENT(IdSrc,
"$Id: FGFCS.cpp,v 1.98 2017/02/25 14:23:18 bcoconni Exp $");
87 DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
88 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
89 GearCmd = GearPos = 1;
90 BrakePos.resize(FGLGear::bgNumBrakeGroups);
91 TailhookPos = WingFoldPos = 0.0;
94 for (i=0;i<NForms;i++) {
95 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
96 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
110 PropAdvanceCmd.clear();
112 PropFeatherCmd.clear();
117 for (i=0;i<SystemChannels.size();i++)
delete SystemChannels[i];
118 SystemChannels.clear();
125 bool FGFCS::InitModel(
void)
127 if (!FGModel::InitModel())
return false;
131 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
132 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
133 for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
134 for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
135 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
136 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
138 DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
139 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
140 TailhookPos = WingFoldPos = 0.0;
142 for (i=0;i<NForms;i++) {
143 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
144 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
148 for (
unsigned int i=0; i<SystemChannels.size(); i++) SystemChannels[i]->Reset();
166 if (Holding)
return false;
170 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
171 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
172 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
173 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
176 for (i=0; i<SystemChannels.size(); i++) {
177 if (debug_lvl & 4) cout <<
" Executing System Channel: " << SystemChannels[i]->GetName() << endl;
178 ChannelRate = SystemChannels[i]->GetRate();
179 SystemChannels[i]->Execute();
195 DaLPos[ofDeg] = pos*radtodeg;
198 DaLPos[ofRad] = pos*degtorad;
202 DaLPos[ofNorm] = pos;
204 DaLPos[ofMag] = fabs(DaLPos[ofRad]);
214 DaRPos[ofDeg] = pos*radtodeg;
217 DaRPos[ofRad] = pos*degtorad;
221 DaRPos[ofNorm] = pos;
223 DaRPos[ofMag] = fabs(DaRPos[ofRad]);
233 DePos[ofDeg] = pos*radtodeg;
236 DePos[ofRad] = pos*degtorad;
242 DePos[ofMag] = fabs(DePos[ofRad]);
252 DrPos[ofDeg] = pos*radtodeg;
255 DrPos[ofRad] = pos*degtorad;
261 DrPos[ofMag] = fabs(DrPos[ofRad]);
271 DfPos[ofDeg] = pos*radtodeg;
274 DfPos[ofRad] = pos*degtorad;
280 DfPos[ofMag] = fabs(DfPos[ofRad]);
290 DsbPos[ofDeg] = pos*radtodeg;
293 DsbPos[ofRad] = pos*degtorad;
297 DsbPos[ofNorm] = pos;
299 DsbPos[ofMag] = fabs(DsbPos[ofRad]);
309 DspPos[ofDeg] = pos*radtodeg;
312 DspPos[ofRad] = pos*degtorad;
316 DspPos[ofNorm] = pos;
318 DspPos[ofMag] = fabs(DspPos[ofRad]);
325 if (engineNum < (
int)ThrottlePos.size()) {
327 for (
unsigned int ctr=0; ctr<ThrottleCmd.size(); ctr++)
328 ThrottleCmd[ctr] = setting;
330 ThrottleCmd[engineNum] = setting;
333 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottleCmd.size()
334 <<
" engines exist, but attempted throttle command is for engine " 335 << engineNum << endl;
343 if (engineNum < (
int)ThrottlePos.size()) {
345 for (
unsigned int ctr=0; ctr<ThrottlePos.size(); ctr++)
346 ThrottlePos[ctr] = setting;
348 ThrottlePos[engineNum] = setting;
351 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottlePos.size()
352 <<
" engines exist, but attempted throttle position setting is for engine " 353 << engineNum << endl;
359 double FGFCS::GetThrottleCmd(
int engineNum)
const 361 if (engineNum < (
int)ThrottlePos.size()) {
363 cerr <<
"Cannot get throttle value for ALL engines" << endl;
365 return ThrottleCmd[engineNum];
368 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottleCmd.size()
369 <<
" engines exist, but throttle setting for engine " << engineNum
370 <<
" is selected" << endl;
377 double FGFCS::GetThrottlePos(
int engineNum)
const 379 if (engineNum < (
int)ThrottlePos.size()) {
381 cerr <<
"Cannot get throttle value for ALL engines" << endl;
383 return ThrottlePos[engineNum];
386 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottlePos.size()
387 <<
" engines exist, but attempted throttle position setting is for engine " 388 << engineNum << endl;
397 if (engineNum < (
int)ThrottlePos.size()) {
399 for (
unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
400 MixtureCmd[ctr] = setting;
402 MixtureCmd[engineNum] = setting;
411 if (engineNum < (
int)ThrottlePos.size()) {
413 for (
unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
414 MixturePos[ctr] = MixtureCmd[ctr];
416 MixturePos[engineNum] = setting;
425 if (engineNum < (
int)ThrottlePos.size()) {
427 for (
unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
428 PropAdvanceCmd[ctr] = setting;
430 PropAdvanceCmd[engineNum] = setting;
439 if (engineNum < (
int)ThrottlePos.size()) {
441 for (
unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
442 PropAdvance[ctr] = PropAdvanceCmd[ctr];
444 PropAdvance[engineNum] = setting;
453 if (engineNum < (
int)ThrottlePos.size()) {
455 for (
unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
456 PropFeatherCmd[ctr] = setting;
458 PropFeatherCmd[engineNum] = setting;
467 if (engineNum < (
int)ThrottlePos.size()) {
469 for (
unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
470 PropFeather[ctr] = PropFeatherCmd[ctr];
472 PropFeather[engineNum] = setting;
481 if (document->
GetName() ==
"autopilot") {
482 Name =
"Autopilot: ";
483 systype = stAutoPilot;
484 }
else if (document->
GetName() ==
"flight_control") {
487 }
else if (document->
GetName() ==
"system") {
502 while (channel_element) {
513 if (sOnOffProperty.length() > 0) {
514 FGPropertyNode* OnOffPropertyNode = PropertyManager->GetNode(sOnOffProperty);
515 if (OnOffPropertyNode == 0) {
517 <<
"The On/Off property, " << sOnOffProperty <<
" specified for channel " 519 <<
"understood. The simulation will abort" <<
reset << endl;
520 throw(
"Bad system definition");
525 newChannel =
new FGFCSChannel(
this, sChannelName, Rate);
527 SystemChannels.push_back(newChannel);
534 while (component_element) {
536 if ((component_element->
GetName() == string(
"lag_filter")) ||
537 (component_element->
GetName() == string(
"lead_lag_filter")) ||
538 (component_element->
GetName() == string(
"washout_filter")) ||
539 (component_element->
GetName() == string(
"second_order_filter")) ||
540 (component_element->
GetName() == string(
"integrator")) )
542 newChannel->
Add(
new FGFilter(
this, component_element));
543 }
else if ((component_element->
GetName() == string(
"pure_gain")) ||
544 (component_element->
GetName() == string(
"scheduled_gain")) ||
545 (component_element->
GetName() == string(
"aerosurface_scale")))
547 newChannel->
Add(
new FGGain(
this, component_element));
548 }
else if (component_element->
GetName() == string(
"summer")) {
549 newChannel->
Add(
new FGSummer(
this, component_element));
550 }
else if (component_element->
GetName() == string(
"deadband")) {
552 }
else if (component_element->
GetName() == string(
"switch")) {
553 newChannel->
Add(
new FGSwitch(
this, component_element));
554 }
else if (component_element->
GetName() == string(
"kinematic")) {
555 newChannel->
Add(
new FGKinemat(
this, component_element));
556 }
else if (component_element->
GetName() == string(
"fcs_function")) {
558 }
else if (component_element->
GetName() == string(
"pid")) {
559 newChannel->
Add(
new FGPID(
this, component_element));
560 }
else if (component_element->
GetName() == string(
"actuator")) {
562 }
else if (component_element->
GetName() == string(
"sensor")) {
563 newChannel->
Add(
new FGSensor(
this, component_element));
564 }
else if (component_element->
GetName() == string(
"accelerometer")) {
566 }
else if (component_element->
GetName() == string(
"magnetometer")) {
568 }
else if (component_element->
GetName() == string(
"gyro")) {
569 newChannel->
Add(
new FGGyro(
this, component_element));
570 }
else if ((component_element->
GetName() == string(
"waypoint_heading")) ||
571 (component_element->
GetName() == string(
"waypoint_distance")))
574 }
else if (component_element->
GetName() == string(
"angle")) {
575 newChannel->
Add(
new FGAngles(
this, component_element));
576 }
else if (component_element->
GetName() == string(
"distributor")) {
579 cerr <<
"Unknown FCS component: " << component_element->
GetName() << endl;
583 cerr <<
reset << endl;
591 PostLoad(document, PropertyManager);
605 SGPath FGFCS::FindFullPathName(
const SGPath& path)
const 607 SGPath name = FGModel::FindFullPathName(path);
608 if (systype != stSystem || !name.isNull())
return name;
611 if (!name.isNull())
return name;
620 string CompStrings =
"";
621 bool firstime =
true;
624 for (
unsigned int i=0; i<SystemChannels.size(); i++)
626 for (
unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
628 if (firstime) firstime =
false;
629 else CompStrings += delimiter;
631 CompStrings += SystemChannels[i]->GetComponent(c)->GetName();
643 std::ostringstream buf;
645 bool firstime =
true;
648 for (
unsigned int i=0; i<SystemChannels.size(); i++)
650 for (
unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
652 if (firstime) firstime =
false;
653 else buf << delimiter;
655 buf << setprecision(9) << SystemChannels[i]->GetComponent(c)->GetOutput();
665 void FGFCS::AddThrottle(
void)
667 ThrottleCmd.push_back(0.0);
668 ThrottlePos.push_back(0.0);
669 MixtureCmd.push_back(0.0);
670 MixturePos.push_back(0.0);
671 PropAdvanceCmd.push_back(0.0);
672 PropAdvance.push_back(0.0);
673 PropFeatherCmd.push_back(
false);
674 PropFeather.push_back(
false);
676 unsigned int num = (
unsigned int)ThrottleCmd.size()-1;
682 double FGFCS::GetDt(
void)
const 689 void FGFCS::bind(
void)
743 PropertyManager->
Tie(
"simulation/channel-dt",
this, &FGFCS::GetChannelDeltaT);
750 void FGFCS::bindThrottle(
unsigned int num)
754 tmp = CreateIndexedPropertyName(
"fcs/throttle-cmd-norm", num);
755 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetThrottleCmd,
757 tmp = CreateIndexedPropertyName(
"fcs/throttle-pos-norm", num);
758 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetThrottlePos,
760 tmp = CreateIndexedPropertyName(
"fcs/mixture-cmd-norm", num);
761 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetMixtureCmd,
763 tmp = CreateIndexedPropertyName(
"fcs/mixture-pos-norm", num);
764 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetMixturePos,
766 tmp = CreateIndexedPropertyName(
"fcs/advance-cmd-norm", num);
769 tmp = CreateIndexedPropertyName(
"fcs/advance-pos-norm", num);
770 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetPropAdvance,
772 tmp = CreateIndexedPropertyName(
"fcs/feather-cmd-norm", num);
775 tmp = CreateIndexedPropertyName(
"fcs/feather-pos-norm", num);
776 PropertyManager->
Tie( tmp.c_str(),
this, num, &FGFCS::GetPropFeather,
799 void FGFCS::Debug(
int from)
801 if (debug_lvl <= 0)
return;
805 cout << endl <<
" " << Name << endl;
808 if (debug_lvl & 2 ) {
809 if (from == 0) cout <<
"Instantiated: FGFCS" << endl;
810 if (from == 1) cout <<
"Destroyed: FGFCS" << endl;
812 if (debug_lvl & 4 ) {
814 if (debug_lvl & 8 ) {
816 if (debug_lvl & 16) {
818 if (debug_lvl & 64) {
820 cout << IdSrc << endl;
821 cout << IdHdr << endl;
void SetDePos(int form, double pos)
Sets the elevator position.
Provides a way to determine the smallest included angle.
void SetDsbCmd(double cmd)
Sets the speedbrake command.
double GetRBrake(void) const
Gets the right brake.
void SetRollTrimCmd(double cmd)
Sets the aileron trim command.
Encapsulates a Accelerometer component for the flight control system.
double GetDsbCmd(void) const
Gets the speedbrake command.
double GetDfPos(int form=ofRad) const
Gets the flaps position.
double GetDeCmd(void) const
Gets the elevator command.
void SetLBrake(double cmd)
Sets the left brake group.
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
Encapsulates a PID control component for the flight control system.
double GetDePos(int form=ofRad) const
Gets the elevator position.
double GetDfCmd(void) const
Gets the flaps command.
void SetPropAdvanceCmd(int engine, double cmd)
Sets the propeller pitch command for the specified engine.
void SetDrPos(int form, double pos)
Sets the rudder position.
double GetDaLPos(int form=ofRad) const
Gets the left aileron position.
void SetDspPos(int form, double pos)
Sets the spoiler position.
BrakeGroup
Brake grouping enumerators.
void SetRBrake(double cmd)
Sets the right brake group.
const SGPath & GetFullAircraftPath(void)
Retrieves the full aircraft path name.
Encapsulates a magnetometer component for the flight control system.
Class wrapper for property handling.
static char reset[5]
resets text properties
double GetRollTrimCmd(void) const
Gets the aileron trim command.
Element * FindElement(const std::string &el="")
Searches for a specified element.
void SetDsbPos(int form, double pos)
Sets the speedbrake position.
Encapsulates a gain component for the flight control system.
static char normint[6]
normal intensity text
void SetPropFeather(int engine, bool cmd)
Sets the actual prop feather setting for the specified engine.
void SetThrottleCmd(int engine, double cmd)
Sets the throttle command for the specified engine.
void SetDaCmd(double cmd)
Sets the aileron command.
bool Run(bool Holding)
Runs the Flight Controls model; called by the Executive Can pass in a value indicating if the executi...
void SetDaRPos(int form, double pos)
Sets the right aileron position.
Encapsulates an Actuator component for the flight control system.
void SetDfPos(int form, double pos)
Sets the flaps position.
void SetPitchTrimCmd(double cmd)
Sets the pitch trim command.
double GetCBrake(void) const
Gets the center brake.
Encapsulates a Sensor component for the flight control system.
static char fgred[6]
red text
double GetPitchTrimCmd(void) const
Gets the pitch trim command.
void SetDaLPos(int form, double pos)
Sets the left aileron position.
virtual bool Run(bool Holding)
Runs the model; called by the Executive.
Models a flight control system summing component.
void Tie(const std::string &name, bool *pointer, bool useDefault=true)
Tie a property to an external bool variable.
void Add(FGFCSComponent *comp)
Adds a component to a channel.
void SetYawTrimCmd(double cmd)
Sets the rudder trim command.
bool GetFeatherCmd(int engine) const
Gets the prop feather command.
void SetCBrake(double cmd)
Sets the center brake group.
void SetDspCmd(double cmd)
Sets the spoilers command.
double GetAttributeValueAsNumber(const std::string &key)
Retrieves an attribute value as a double precision real number.
const std::string & GetName(void) const
Retrieves the element name.
std::string GetComponentValues(const std::string &delimiter) const
Retrieves all component outputs for inclusion in output stream.
Base class for all scheduled JSBSim models.
Encapsulates a filter for the flight control system.
void SetDrCmd(double cmd)
Sets the rudder command.
double GetDsbPos(int form=ofRad) const
Gets the speedbrake position.
double GetYawTrimCmd(void) const
Gets the rudder trim command.
void SetDeCmd(double cmd)
Sets the elevator command.
std::string GetComponentStrings(const std::string &delimiter) const
Retrieves all component names for inclusion in output stream.
void SetGearPos(double gearpos)
Set the gear extend/retract position, defaults to down.
void SetMixturePos(int engine, double cmd)
Sets the actual mixture setting for the specified engine.
bool Load(Element *el)
Loads the Flight Control System.
void SetTailhookPos(double hookpos)
Set the tailhook position.
double GetDaCmd(void) const
Gets the aileron command.
static char fgblue[6]
blue text
double GetGearCmd(void) const
Get the gear extend/retract command.
void SetDfCmd(double cmd)
Sets the flaps command.
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
void SetWingFoldPos(double foldpos)
Set the wing fold position.
Encapsulates a kinematic (mechanical) component for the flight control system.
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
static char highint[5]
highlights text
double GetPropAdvanceCmd(int engine) const
Gets the prop pitch command.
void SetThrottlePos(int engine, double cmd)
Sets the actual throttle setting for the specified engine.
void SetPropAdvance(int engine, double cmd)
Sets the actual prop pitch setting for the specified engine.
void SetMixtureCmd(int engine, double cmd)
Sets the mixture command for the specified engine.
double GetDspPos(int form=ofRad) const
Gets the spoiler position.
Element * GetElement(unsigned int el=0)
Returns a pointer to the element requested by index.
double GetDeltaT(void) const
Returns the simulation delta T.
Encapsulates a distributor for the flight control system.
Models a deadband object.
double GetGearPos(void) const
Gets the gear position (0 up, 1 down), defaults to down.
Encapsulates the JSBSim simulation executive.
double GetBrake(FGLGear::BrakeGroup bg)
Gets the brake for a specified group.
const SGPath & GetSystemsPath(void)
Retrieves the systems path.
Encapsulates a Gyro component for the flight control system.
double GetDspCmd(void) const
Gets the spoiler command.
double GetLBrake(void) const
Gets the left brake.
Encapsulates a switch for the flight control system.
double GetDrCmd(void) const
Gets the rudder command.
void SetGearCmd(double gearcmd)
Set the gear extend/retract command, defaults to down.
Models a FCSFunction object.
void SetFeatherCmd(int engine, bool cmd)
Sets the propeller feather command for the specified engine.
Element * GetNextElement(void)
Returns a pointer to the next element in the list.
virtual bool Load(Element *el)
Loads this model.
Models a Waypoint object.
double GetDaRPos(int form=ofRad) const
Gets the right aileron position.
double GetTailhookPos(void) const
Gets the tailhook position (0 up, 1 down)
double GetWingFoldPos(void) const
Gets the wing fold position (0 unfolded, 1 folded)
double GetDrPos(int form=ofRad) const
Gets the rudder position.