44 #include "input_output/FGModelLoader.h"
46 #include "models/flight_control/FGFilter.h"
47 #include "models/flight_control/FGDeadBand.h"
48 #include "models/flight_control/FGGain.h"
49 #include "models/flight_control/FGPID.h"
50 #include "models/flight_control/FGSwitch.h"
51 #include "models/flight_control/FGSummer.h"
52 #include "models/flight_control/FGKinemat.h"
53 #include "models/flight_control/FGFCSFunction.h"
54 #include "models/flight_control/FGActuator.h"
55 #include "models/flight_control/FGAccelerometer.h"
56 #include "models/flight_control/FGMagnetometer.h"
57 #include "models/flight_control/FGGyro.h"
58 #include "models/flight_control/FGWaypoint.h"
59 #include "models/flight_control/FGAngles.h"
60 #include "models/flight_control/FGDistributor.h"
61 #include "models/flight_control/FGLinearActuator.h"
63 #include "FGFCSChannel.h"
80 DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
81 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
82 GearCmd = GearPos = 1;
83 BrakePos.resize(FGLGear::bgNumBrakeGroups);
84 TailhookPos = WingFoldPos = 0.0;
87 for (i=0;i<NForms;i++) {
88 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
89 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
103 PropAdvanceCmd.clear();
105 PropFeatherCmd.clear();
110 for (i=0;i<SystemChannels.size();i++)
delete SystemChannels[i];
111 SystemChannels.clear();
118 bool FGFCS::InitModel(
void)
120 if (!FGModel::InitModel())
return false;
124 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = 0.0;
125 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = 0.0;
126 for (i=0; i<ThrottleCmd.size(); i++) ThrottleCmd[i] = 0.0;
127 for (i=0; i<MixtureCmd.size(); i++) MixtureCmd[i] = 0.0;
128 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = 0.0;
129 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = 0.0;
131 DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = 0;
132 PTrimCmd = YTrimCmd = RTrimCmd = 0.0;
133 TailhookPos = WingFoldPos = 0.0;
135 for (i=0;i<NForms;i++) {
136 DePos[i] = DaLPos[i] = DaRPos[i] = DrPos[i] = 0.0;
137 DfPos[i] = DsbPos[i] = DspPos[i] = 0.0;
141 for (
unsigned int i=0; i<SystemChannels.size(); i++) SystemChannels[i]->Reset();
159 if (Holding)
return false;
163 for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
164 for (i=0; i<MixturePos.size(); i++) MixturePos[i] = MixtureCmd[i];
165 for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
166 for (i=0; i<PropFeather.size(); i++) PropFeather[i] = PropFeatherCmd[i];
169 for (i=0; i<SystemChannels.size(); i++) {
170 if (debug_lvl & 4) cout <<
" Executing System Channel: " << SystemChannels[i]->GetName() << endl;
171 ChannelRate = SystemChannels[i]->GetRate();
172 SystemChannels[i]->Execute();
188 DaLPos[ofDeg] = pos*radtodeg;
191 DaLPos[ofRad] = pos*degtorad;
195 DaLPos[ofNorm] = pos;
197 DaLPos[ofMag] = fabs(DaLPos[ofRad]);
207 DaRPos[ofDeg] = pos*radtodeg;
210 DaRPos[ofRad] = pos*degtorad;
214 DaRPos[ofNorm] = pos;
216 DaRPos[ofMag] = fabs(DaRPos[ofRad]);
226 DePos[ofDeg] = pos*radtodeg;
229 DePos[ofRad] = pos*degtorad;
235 DePos[ofMag] = fabs(DePos[ofRad]);
245 DrPos[ofDeg] = pos*radtodeg;
248 DrPos[ofRad] = pos*degtorad;
254 DrPos[ofMag] = fabs(DrPos[ofRad]);
264 DfPos[ofDeg] = pos*radtodeg;
267 DfPos[ofRad] = pos*degtorad;
273 DfPos[ofMag] = fabs(DfPos[ofRad]);
283 DsbPos[ofDeg] = pos*radtodeg;
286 DsbPos[ofRad] = pos*degtorad;
290 DsbPos[ofNorm] = pos;
292 DsbPos[ofMag] = fabs(DsbPos[ofRad]);
302 DspPos[ofDeg] = pos*radtodeg;
305 DspPos[ofRad] = pos*degtorad;
309 DspPos[ofNorm] = pos;
311 DspPos[ofMag] = fabs(DspPos[ofRad]);
318 if (engineNum < (
int)ThrottlePos.size()) {
320 for (
unsigned int ctr=0; ctr<ThrottleCmd.size(); ctr++)
321 ThrottleCmd[ctr] = setting;
323 ThrottleCmd[engineNum] = setting;
326 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottleCmd.size()
327 <<
" engines exist, but attempted throttle command is for engine "
328 << engineNum << endl;
336 if (engineNum < (
int)ThrottlePos.size()) {
338 for (
unsigned int ctr=0; ctr<ThrottlePos.size(); ctr++)
339 ThrottlePos[ctr] = setting;
341 ThrottlePos[engineNum] = setting;
344 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottlePos.size()
345 <<
" engines exist, but attempted throttle position setting is for engine "
346 << engineNum << endl;
352 double FGFCS::GetThrottleCmd(
int engineNum)
const
354 if (engineNum < (
int)ThrottlePos.size()) {
356 cerr <<
"Cannot get throttle value for ALL engines" << endl;
358 return ThrottleCmd[engineNum];
361 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottleCmd.size()
362 <<
" engines exist, but throttle setting for engine " << engineNum
363 <<
" is selected" << endl;
370 double FGFCS::GetThrottlePos(
int engineNum)
const
372 if (engineNum < (
int)ThrottlePos.size()) {
374 cerr <<
"Cannot get throttle value for ALL engines" << endl;
376 return ThrottlePos[engineNum];
379 cerr <<
"Throttle " << engineNum <<
" does not exist! " << ThrottlePos.size()
380 <<
" engines exist, but attempted throttle position setting is for engine "
381 << engineNum << endl;
390 if (engineNum < (
int)ThrottlePos.size()) {
392 for (
unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
393 MixtureCmd[ctr] = setting;
395 MixtureCmd[engineNum] = setting;
404 if (engineNum < (
int)ThrottlePos.size()) {
406 for (
unsigned int ctr=0; ctr<MixtureCmd.size(); ctr++)
407 MixturePos[ctr] = MixtureCmd[ctr];
409 MixturePos[engineNum] = setting;
418 if (engineNum < (
int)ThrottlePos.size()) {
420 for (
unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
421 PropAdvanceCmd[ctr] = setting;
423 PropAdvanceCmd[engineNum] = setting;
432 if (engineNum < (
int)ThrottlePos.size()) {
434 for (
unsigned int ctr=0; ctr<PropAdvanceCmd.size(); ctr++)
435 PropAdvance[ctr] = PropAdvanceCmd[ctr];
437 PropAdvance[engineNum] = setting;
446 if (engineNum < (
int)ThrottlePos.size()) {
448 for (
unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
449 PropFeatherCmd[ctr] = setting;
451 PropFeatherCmd[engineNum] = setting;
460 if (engineNum < (
int)ThrottlePos.size()) {
462 for (
unsigned int ctr=0; ctr<PropFeatherCmd.size(); ctr++)
463 PropFeather[ctr] = PropFeatherCmd[ctr];
465 PropFeather[engineNum] = setting;
474 if (document->
GetName() ==
"autopilot") {
475 Name =
"Autopilot: ";
476 systype = stAutoPilot;
477 }
else if (document->
GetName() ==
"flight_control") {
480 }
else if (document->
GetName() ==
"system") {
495 while (channel_element) {
507 if (sOnOffProperty.length() > 0) {
508 FGPropertyNode* OnOffPropertyNode = PropertyManager->GetNode(sOnOffProperty);
509 if (OnOffPropertyNode == 0) {
511 <<
"The On/Off property, " << sOnOffProperty <<
" specified for channel "
513 <<
"understood. The simulation will abort" <<
reset << endl;
514 throw(
"Bad system definition");
516 newChannel =
new FGFCSChannel(
this, sChannelName, ChannelRate,
519 newChannel =
new FGFCSChannel(
this, sChannelName, ChannelRate);
521 SystemChannels.push_back(newChannel);
528 while (component_element) {
530 if ((component_element->
GetName() ==
string(
"lag_filter")) ||
531 (component_element->
GetName() ==
string(
"lead_lag_filter")) ||
532 (component_element->
GetName() ==
string(
"washout_filter")) ||
533 (component_element->
GetName() ==
string(
"second_order_filter")) )
535 newChannel->
Add(
new FGFilter(
this, component_element));
536 }
else if ((component_element->
GetName() ==
string(
"pure_gain")) ||
537 (component_element->
GetName() ==
string(
"scheduled_gain")) ||
538 (component_element->
GetName() ==
string(
"aerosurface_scale")))
540 newChannel->
Add(
new FGGain(
this, component_element));
541 }
else if (component_element->
GetName() ==
string(
"summer")) {
542 newChannel->
Add(
new FGSummer(
this, component_element));
543 }
else if (component_element->
GetName() ==
string(
"deadband")) {
545 }
else if (component_element->
GetName() ==
string(
"switch")) {
546 newChannel->
Add(
new FGSwitch(
this, component_element));
547 }
else if (component_element->
GetName() ==
string(
"kinematic")) {
548 newChannel->
Add(
new FGKinemat(
this, component_element));
549 }
else if (component_element->
GetName() ==
string(
"fcs_function")) {
551 }
else if (component_element->
GetName() ==
string(
"pid")) {
552 newChannel->
Add(
new FGPID(
this, component_element));
553 }
else if (component_element->
GetName() ==
string(
"integrator")) {
557 cerr << component_element->
ReadFrom();
559 +
" does not provide the parameter <c1>");
561 c1_el->ChangeName(
"ki");
564 newChannel->
Add(
new FGPID(
this, component_element));
565 }
else if (component_element->
GetName() ==
string(
"actuator")) {
567 }
else if (component_element->
GetName() ==
string(
"sensor")) {
568 newChannel->
Add(
new FGSensor(
this, component_element));
569 }
else if (component_element->
GetName() ==
string(
"accelerometer")) {
571 }
else if (component_element->
GetName() ==
string(
"magnetometer")) {
573 }
else if (component_element->
GetName() ==
string(
"gyro")) {
574 newChannel->
Add(
new FGGyro(
this, component_element));
575 }
else if ((component_element->
GetName() ==
string(
"waypoint_heading")) ||
576 (component_element->
GetName() ==
string(
"waypoint_distance")))
579 }
else if (component_element->
GetName() ==
string(
"angle")) {
580 newChannel->
Add(
new FGAngles(
this, component_element));
581 }
else if (component_element->
GetName() ==
string(
"distributor")) {
583 }
else if (component_element->
GetName() ==
string(
"linear_actuator")) {
586 cerr <<
"Unknown FCS component: " << component_element->
GetName() << endl;
590 cerr <<
reset << endl;
598 PostLoad(document, FDMExec);
612 SGPath FGFCS::FindFullPathName(
const SGPath& path)
const
614 SGPath name = FGModel::FindFullPathName(path);
615 if (systype != stSystem || !name.isNull())
return name;
618 const array<string, 1> dir_names = {
"Systems"};
621 const array<string, 2> dir_names = {
"Systems",
"systems"};
624 for(
const string& dir_name: dir_names) {
626 if (!name.isNull())
return name;
636 string CompStrings =
"";
637 bool firstime =
true;
640 for (
unsigned int i=0; i<SystemChannels.size(); i++)
642 for (
unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
644 if (firstime) firstime =
false;
645 else CompStrings += delimiter;
647 CompStrings += SystemChannels[i]->GetComponent(c)->GetName();
659 std::ostringstream buf;
661 bool firstime =
true;
664 for (
unsigned int i=0; i<SystemChannels.size(); i++)
666 for (
unsigned int c=0; c<SystemChannels[i]->GetNumComponents(); c++)
668 if (firstime) firstime =
false;
669 else buf << delimiter;
671 buf << setprecision(9) << SystemChannels[i]->GetComponent(c)->GetOutput();
681 void FGFCS::AddThrottle(
void)
683 ThrottleCmd.push_back(0.0);
684 ThrottlePos.push_back(0.0);
685 MixtureCmd.push_back(0.0);
686 MixturePos.push_back(0.0);
687 PropAdvanceCmd.push_back(0.0);
688 PropAdvance.push_back(0.0);
689 PropFeatherCmd.push_back(
false);
690 PropFeather.push_back(
false);
692 unsigned int num = (
unsigned int)ThrottleCmd.size()-1;
698 double FGFCS::GetDt(
void)
const
705 void FGFCS::bind(
void)
720 PropertyManager->Tie(
"fcs/mag-left-aileron-pos-rad",
this, ofMag, &
FGFCS::GetDaLPos);
725 PropertyManager->Tie(
"fcs/mag-right-aileron-pos-rad",
this, ofMag, &
FGFCS::GetDaRPos);
730 PropertyManager->Tie(
"fcs/mag-elevator-pos-rad",
this, ofMag, &
FGFCS::GetDePos);
735 PropertyManager->Tie(
"fcs/mag-rudder-pos-rad",
this,ofMag, &
FGFCS::GetDrPos);
744 PropertyManager->Tie(
"fcs/mag-speedbrake-pos-rad",
this,ofMag, &
FGFCS::GetDsbPos);
749 PropertyManager->Tie(
"fcs/mag-spoiler-pos-rad",
this, ofMag, &
FGFCS::GetDspPos);
759 PropertyManager->Tie(
"simulation/channel-dt",
this, &FGFCS::GetChannelDeltaT);
766 void FGFCS::bindThrottle(
unsigned int num)
770 tmp = CreateIndexedPropertyName(
"fcs/throttle-cmd-norm", num);
771 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetThrottleCmd,
773 tmp = CreateIndexedPropertyName(
"fcs/throttle-pos-norm", num);
774 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetThrottlePos,
776 tmp = CreateIndexedPropertyName(
"fcs/mixture-cmd-norm", num);
777 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetMixtureCmd,
779 tmp = CreateIndexedPropertyName(
"fcs/mixture-pos-norm", num);
780 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetMixturePos,
782 tmp = CreateIndexedPropertyName(
"fcs/advance-cmd-norm", num);
785 tmp = CreateIndexedPropertyName(
"fcs/advance-pos-norm", num);
786 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetPropAdvance,
788 tmp = CreateIndexedPropertyName(
"fcs/feather-cmd-norm", num);
791 tmp = CreateIndexedPropertyName(
"fcs/feather-pos-norm", num);
792 PropertyManager->Tie( tmp.c_str(),
this, num, &FGFCS::GetPropFeather,
815 void FGFCS::Debug(
int from)
817 if (debug_lvl <= 0)
return;
821 cout << endl <<
" " << Name << endl;
824 if (debug_lvl & 2 ) {
825 if (from == 0) cout <<
"Instantiated: FGFCS" << endl;
826 if (from == 1) cout <<
"Destroyed: FGFCS" << endl;
828 if (debug_lvl & 4 ) {
830 if (debug_lvl & 8 ) {
832 if (debug_lvl & 16) {
834 if (debug_lvl & 64) {
Element * FindElement(const std::string &el="")
Searches for a specified element.
double GetAttributeValueAsNumber(const std::string &key)
Retrieves an attribute value as a double precision real number.
Element * GetElement(unsigned int el=0)
Returns a pointer to the element requested by index.
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
void AddAttribute(const std::string &name, const std::string &value)
Stores an attribute belonging to this element.
std::string ReadFrom(void) const
Return a string that contains a description of the location where the current XML element was read fr...
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
bool HasAttribute(const std::string &key)
Determines if an element has the supplied attribute.
Element * GetNextElement(void)
Returns a pointer to the next element in the list.
const std::string & GetName(void) const
Retrieves the element name.
Encapsulates a Accelerometer component for the flight control system.
Encapsulates an Actuator component for the flight control system.
Provides a way to determine the smallest included angle.
Models a deadband object.
Encapsulates a distributor for the flight control system.
void Add(FGFCSComponent *comp)
Adds a component to a channel.
Models a FCSFunction object.
void SetThrottlePos(int engine, double cmd)
Sets the actual throttle setting for the specified engine.
void SetPitchTrimCmd(double cmd)
Sets the pitch trim command.
void SetDaLPos(int form, double pos)
Sets the left aileron position.
bool Load(Element *el) override
Loads the Flight Control System.
void SetGearPos(double gearpos)
Set the gear extend/retract position, defaults to down.
double GetCBrake(void) const
Gets the center brake.
void SetDsbPos(int form, double pos)
Sets the speedbrake position.
double GetLBrake(void) const
Gets the left brake.
double GetDfPos(int form=ofRad) const
Gets the flaps position.
void SetDsbCmd(double cmd)
Sets the speedbrake command.
bool GetFeatherCmd(int engine) const
Gets the prop feather command.
double GetDfCmd(void) const
Gets the flaps command.
double GetDeCmd(void) const
Gets the elevator command.
std::string GetComponentStrings(const std::string &delimiter) const
Retrieves all component names for inclusion in output stream.
void SetMixtureCmd(int engine, double cmd)
Sets the mixture command for the specified engine.
double GetTailhookPos(void) const
Gets the tailhook position (0 up, 1 down)
void SetPropFeather(int engine, bool cmd)
Sets the actual prop feather setting for the specified engine.
double GetPropAdvanceCmd(int engine) const
Gets the prop pitch command.
void SetFeatherCmd(int engine, bool cmd)
Sets the propeller feather command for the specified engine.
void SetDeCmd(double cmd)
Sets the elevator command.
double GetDePos(int form=ofRad) const
Gets the elevator position.
void SetDrPos(int form, double pos)
Sets the rudder position.
void SetMixturePos(int engine, double cmd)
Sets the actual mixture setting for the specified engine.
void SetDaCmd(double cmd)
Sets the aileron command.
double GetDsbPos(int form=ofRad) const
Gets the speedbrake position.
double GetRBrake(void) const
Gets the right brake.
void SetDaRPos(int form, double pos)
Sets the right aileron position.
void SetDePos(int form, double pos)
Sets the elevator position.
void SetRBrake(double cmd)
Sets the right brake group.
void SetDrCmd(double cmd)
Sets the rudder command.
double GetDrCmd(void) const
Gets the rudder command.
bool Run(bool Holding) override
Runs the Flight Controls model; called by the Executive Can pass in a value indicating if the executi...
double GetDaCmd(void) const
Gets the aileron command.
double GetDaLPos(int form=ofRad) const
Gets the left aileron position.
double GetRollTrimCmd(void) const
Gets the aileron trim command.
~FGFCS() override
Destructor.
void SetDfPos(int form, double pos)
Sets the flaps position.
double GetDaRPos(int form=ofRad) const
Gets the right aileron position.
void SetDspPos(int form, double pos)
Sets the spoiler position.
void SetTailhookPos(double hookpos)
Set the tailhook position.
void SetDfCmd(double cmd)
Sets the flaps command.
double GetBrake(FGLGear::BrakeGroup bg)
Gets the brake for a specified group.
double GetWingFoldPos(void) const
Gets the wing fold position (0 unfolded, 1 folded)
double GetDspCmd(void) const
Gets the spoiler command.
void SetPropAdvance(int engine, double cmd)
Sets the actual prop pitch setting for the specified engine.
std::string GetComponentValues(const std::string &delimiter) const
Retrieves all component outputs for inclusion in output stream.
double GetPitchTrimCmd(void) const
Gets the pitch trim command.
void SetThrottleCmd(int engine, double cmd)
Sets the throttle command for the specified engine.
double GetGearPos(void) const
Gets the gear position (0 up, 1 down), defaults to down.
double GetDsbCmd(void) const
Gets the speedbrake command.
void SetDspCmd(double cmd)
Sets the spoilers command.
void SetLBrake(double cmd)
Sets the left brake group.
double GetDspPos(int form=ofRad) const
Gets the spoiler position.
void SetCBrake(double cmd)
Sets the center brake group.
double GetYawTrimCmd(void) const
Gets the rudder trim command.
void SetPropAdvanceCmd(int engine, double cmd)
Sets the propeller pitch command for the specified engine.
void SetRollTrimCmd(double cmd)
Sets the aileron trim command.
void SetYawTrimCmd(double cmd)
Sets the rudder trim command.
double GetGearCmd(void) const
Get the gear extend/retract command.
void SetWingFoldPos(double foldpos)
Set the wing fold position.
double GetDrPos(int form=ofRad) const
Gets the rudder position.
void SetGearCmd(double gearcmd)
Set the gear extend/retract command, defaults to down.
Encapsulates the JSBSim simulation executive.
const SGPath & GetFullAircraftPath(void)
Retrieves the full aircraft path name.
const SGPath & GetSystemsPath(void)
Retrieves the systems path.
double GetDeltaT(void) const
Returns the simulation delta T.
Encapsulates a filter for the flight control system.
Encapsulates a gain component for the flight control system.
Encapsulates a Gyro component for the flight control system.
static char normint[6]
normal intensity text
static char fgred[6]
red text
static char fgblue[6]
blue text
static char reset[5]
resets text properties
static char highint[5]
highlights text
Encapsulates a kinematic (mechanical) component for the flight control system.
BrakeGroup
Brake grouping enumerators.
Models a flight control system summing component.
Encapsulates a magnetometer component for the flight control system.
Base class for all scheduled JSBSim models.
virtual bool Run(bool Holding)
Runs the model; called by the Executive.
bool Upload(Element *el, bool preLoad)
Uploads this model in memory.
Encapsulates a PID control component for the flight control system.
Class wrapper for property handling.
Encapsulates a Sensor component for the flight control system.
Models a flight control system summing component.
Encapsulates a switch for the flight control system.
Models a Waypoint object.