X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2Fmodels%2FFGFCS.cpp;h=3f659831f5c03944085cc9eb75ae589ed4e657db;hb=642735ab18421db87a07d6841dd720fd4615bfff;hp=cf41c57555baf6a7ed3365ece6449323cf530df4;hpb=3cda82e0a9b3ff5eb6d21408328c5f864be31e0d;p=flightgear.git diff --git a/src/FDM/JSBSim/models/FGFCS.cpp b/src/FDM/JSBSim/models/FGFCS.cpp index cf41c5755..3f659831f 100644 --- a/src/FDM/JSBSim/models/FGFCS.cpp +++ b/src/FDM/JSBSim/models/FGFCS.cpp @@ -1,12 +1,12 @@ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - Module: FGFCS.cpp + Module: FGFCS.cpp Author: Jon Berndt Date started: 12/12/98 Purpose: Model the flight controls Called by: FDMExec - ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- + ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------- This program is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software @@ -37,32 +37,36 @@ HISTORY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include "FGFCS.h" -#include -#include #include +#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "FGFCS.h" +#include "FGFDMExec.h" +#include "FGGroundReactions.h" +#include "input_output/FGPropertyManager.h" + +#include "models/flight_control/FGFilter.h" +#include "models/flight_control/FGDeadBand.h" +#include "models/flight_control/FGGain.h" +#include "models/flight_control/FGPID.h" +#include "models/flight_control/FGSwitch.h" +#include "models/flight_control/FGSummer.h" +#include "models/flight_control/FGKinemat.h" +#include "models/flight_control/FGFCSFunction.h" +#include "models/flight_control/FGSensor.h" +#include "models/flight_control/FGActuator.h" +#include "models/flight_control/FGAccelerometer.h" +#include "models/flight_control/FGMagnetometer.h" +#include "models/flight_control/FGGyro.h" + +using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id$"; +static const char *IdSrc = "$Id: FGFCS.cpp,v 1.77 2011/09/25 14:05:40 bcoconni Exp $"; static const char *IdHdr = ID_FCS; -#if defined(WIN32) && !defined(__CYGWIN__) -#define snprintf _snprintf -#endif - /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -75,7 +79,8 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) DaCmd = DeCmd = DrCmd = DsCmd = DfCmd = DsbCmd = DspCmd = 0; PTrimCmd = YTrimCmd = RTrimCmd = 0.0; GearCmd = GearPos = 1; // default to gear down - LeftBrake = RightBrake = CenterBrake = 0.0; + BrakePos.resize(FGLGear::bgNumBrakeGroups); + TailhookPos = WingFoldPos = 0.0; bind(); for (i=0;iHasNode("fcs")) unbind( PropertyManager->GetNode("fcs") ); - if (PropertyManager->HasNode("ap")) unbind( PropertyManager->GetNode("ap") ); - PropertyManager->Untie( "gear/gear-cmd-norm" ); - PropertyManager->Untie( "gear/gear-pos-norm" ); - ThrottleCmd.clear(); ThrottlePos.clear(); MixtureCmd.clear(); @@ -108,17 +108,80 @@ FGFCS::~FGFCS() unsigned int i; for (i=0;iGetType() == "LAG" || + Systems[i]->GetType() == "LEAD_LAG" || + Systems[i]->GetType() == "WASHOUT" || + Systems[i]->GetType() == "SECOND_ORDER_FILTER" || + Systems[i]->GetType() == "INTEGRATOR") + { + ((FGFilter*)Systems[i])->ResetPastStates(); + } else if (Systems[i]->GetType() == "PID" ) { + ((FGPID*)Systems[i])->ResetPastStates(); + } + } + + for (unsigned int i=0; iGetType() == "LAG" || + FCSComponents[i]->GetType() == "LEAD_LAG" || + FCSComponents[i]->GetType() == "WASHOUT" || + FCSComponents[i]->GetType() == "SECOND_ORDER_FILTER" || + FCSComponents[i]->GetType() == "INTEGRATOR") + { + ((FGFilter*)FCSComponents[i])->ResetPastStates(); + } else if (FCSComponents[i]->GetType() == "PID" ) { + ((FGPID*)FCSComponents[i])->ResetPastStates(); + } + } + + for (unsigned int i=0; iGetType() == "LAG" || + APComponents[i]->GetType() == "LEAD_LAG" || + APComponents[i]->GetType() == "WASHOUT" || + APComponents[i]->GetType() == "SECOND_ORDER_FILTER" || + APComponents[i]->GetType() == "INTEGRATOR") + { + ((FGFilter*)APComponents[i])->ResetPastStates(); + } else if (APComponents[i]->GetType() == "PID" ) { + ((FGPID*)APComponents[i])->ResetPastStates(); + } + } + + return true; +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Notes: In this logic the default engine commands are set. This is simply a // sort of safe-mode method in case the user has not defined control laws for @@ -127,32 +190,37 @@ FGFCS::~FGFCS() // actually present in the flight_control or autopilot section will override // these simple assignments. -bool FGFCS::Run(void) +bool FGFCS::Run(bool Holding) { unsigned int i; - if (FGModel::Run()) return true; // fast exit if nothing to do - if (FDMExec->Holding()) return false; + if (FGModel::Run(Holding)) return true; // fast exit if nothing to do + if (Holding) return false; + + RunPreFunctions(); for (i=0; iGetGearUnit(i); + FGLGear* gear = FDMExec->GetGroundReactions()->GetGearUnit(i); SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() ); } - // Cycle through the sensor, autopilot, and flight control components - for (i=0; iRun(); - for (i=0; iRun(); - } + // Execute Systems in order + for (i=0; iRun(); + + // Execute Autopilot + for (i=0; iRun(); + + // Execute Flight Control System for (i=0; iRun(); + RunPostFunctions(); + return false; } @@ -386,7 +454,7 @@ void FGFCS::SetMixturePos(int engineNum, double setting) if (engineNum < (int)ThrottlePos.size()) { if (engineNum < 0) { - for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr]; + for (ctr=0;ctr *Components; - Element *component_element, *property_element, *sensor_element; + Element *component_element; Element *channel_element; Components=0; - // Determine if the FCS/Autopilot is defined inline in the aircraft configuration - // file or in a separate file. Set up the Element pointer as appropriate. - string separator = "/"; -#ifdef macintosh - separator = ";"; -#endif +// ToDo: The handling of name and file attributes could be improved, here, +// considering that a name can be in the external file, as well. name = el->GetAttributeValue("name"); - if (name.empty()) { + if (name.empty() || !el->GetAttributeValue("file").empty()) { fname = el->GetAttributeValue("file"); - file = FDMExec->GetFullAircraftPath() + separator + fname + ".xml"; + if (systype == stSystem) { + file = FindSystemFullPathname(fname); + } else { + file = FDMExec->GetFullAircraftPath() + "/" + fname + ".xml"; + } if (fname.empty()) { - cerr << "FCS/Autopilot does not appear to be defined inline nor in a file" << endl; + cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl; return false; } else { document = LoadXMLDocument(file); + if (!document) { + cerr << "Error loading file " << file << endl; + return false; + } + name = document->GetAttributeValue("name"); } } else { document = el; @@ -492,39 +565,56 @@ bool FGFCS::Load(Element* el) } else if (document->GetName() == "flight_control") { Components = &FCSComponents; Name = "FCS: " + document->GetAttributeValue("name"); + } else if (document->GetName() == "system") { + Components = &Systems; + Name = "System: " + document->GetAttributeValue("name"); } - Debug(2); - // ToDo: How do these get untied? - // ToDo: Consider having INPUT and OUTPUT interface properties. Would then - // have to duplicate this block of code after channel read code. - // Input properties could be write only (nah), and output could be read - // only. - if (document->GetName() == "flight_control") bindModel(); - property_element = document->FindElement("property"); - while (property_element) { - interface_properties.push_back(new double(0)); - interface_property_string = property_element->GetDataLine(); - PropertyManager->Tie(interface_property_string, interface_properties.back()); - property_element = document->FindNextElement("property"); - } - - sensor_element = document->FindElement("sensor"); - while (sensor_element) { - try { - sensors.push_back(new FGSensor(this, sensor_element)); - } catch (string s) { - cerr << highint << fgred << endl << " " << s << endl; - return false; + FGModel::Load(document); // Load interface properties from document + + // After reading interface properties in a file, read properties in the local + // flight_control, autopilot, or system element. This allows general-purpose + // systems to be defined in a file, with overrides or initial loaded constants + // supplied in the relevant element of the aircraft configuration file. + + Element* property_element = 0; + + if (!fname.empty()) { + property_element = el->FindElement("property"); + if (property_element && debug_lvl > 0) cout << endl << " Overriding properties" << endl << endl; + while (property_element) { + double value=0.0; + if ( ! property_element->GetAttributeValue("value").empty()) + value = property_element->GetAttributeValueAsNumber("value"); + + interface_property_string = property_element->GetDataLine(); + if (PropertyManager->HasNode(interface_property_string)) { + FGPropertyManager* node = PropertyManager->GetNode(interface_property_string); + if (debug_lvl > 0) + cout << " " << "Overriding value for property " << interface_property_string + << " (old value: " << node->getDoubleValue() << " new value: " << value << ")" << endl; + node->setDoubleValue(value); + } else { + interface_properties.push_back(new double(value)); + PropertyManager->Tie(interface_property_string, interface_properties.back()); + if (debug_lvl > 0) + cout << " " << interface_property_string << " (initial value: " << value << ")" << endl; + } + + property_element = el->FindNextElement("property"); } - sensor_element = document->FindNextElement("sensor"); } channel_element = document->FindElement("channel"); while (channel_element) { + + if (debug_lvl > 0) + cout << endl << highint << fgblue << " Channel " + << normint << channel_element->GetAttributeValue("name") << reset << endl; + component_element = channel_element->GetElement(); while (component_element) { try { @@ -554,6 +644,14 @@ bool FGFCS::Load(Element* el) Components->push_back(new FGPID(this, component_element)); } else if (component_element->GetName() == string("actuator")) { Components->push_back(new FGActuator(this, component_element)); + } else if (component_element->GetName() == string("sensor")) { + Components->push_back(new FGSensor(this, component_element)); + } else if (component_element->GetName() == string("accelerometer")) { + Components->push_back(new FGAccelerometer(this, component_element)); + } else if (component_element->GetName() == string("magnetometer")) { + Components->push_back(new FGMagnetometer(this, component_element)); + } else if (component_element->GetName() == string("gyro")) { + Components->push_back(new FGGyro(this, component_element)); } else { cerr << "Unknown FCS component: " << component_element->GetName() << endl; } @@ -567,6 +665,10 @@ bool FGFCS::Load(Element* el) channel_element = document->FindNextElement("channel"); } + PostLoad(document, PropertyManager); + + ResetParser(); + return true; } @@ -574,40 +676,100 @@ bool FGFCS::Load(Element* el) double FGFCS::GetBrake(FGLGear::BrakeGroup bg) { - switch (bg) { - case FGLGear::bgLeft: - return LeftBrake; - case FGLGear::bgRight: - return RightBrake; - case FGLGear::bgCenter: - return CenterBrake; - default: - cerr << "GetBrake asked to return a bogus brake value" << endl; + return BrakePos[bg]; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +string FGFCS::FindSystemFullPathname(const string& sysfilename) +{ + string fullpath, localpath; + string system_filename = sysfilename; + string systemPath = FDMExec->GetSystemsPath(); + string aircraftPath = FDMExec->GetFullAircraftPath(); + ifstream system_file; + + fullpath = systemPath + "/"; + localpath = aircraftPath + "/Systems/"; + + if (system_filename.length() <=4 || system_filename.substr(system_filename.length()-4, 4) != ".xml") { + system_filename.append(".xml"); } - return 0.0; + + system_file.open(string(localpath + system_filename).c_str()); + if ( !system_file.is_open()) { + system_file.open(string(fullpath + system_filename).c_str()); + if ( !system_file.is_open()) { + cerr << " Could not open system file: " << system_filename << " in path " + << fullpath << " or " << localpath << endl; + return string(""); + } else { + return string(fullpath + system_filename); + } + } + return string(localpath + system_filename); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +ifstream* FGFCS::FindSystemFile(const string& sysfilename) +{ + string fullpath, localpath; + string system_filename = sysfilename; + string systemPath = FDMExec->GetSystemsPath(); + string aircraftPath = FDMExec->GetFullAircraftPath(); + ifstream* system_file = new ifstream(); + + fullpath = systemPath + "/"; + localpath = aircraftPath + "/Systems/"; + + if (system_filename.substr(system_filename.length()-4, 4) != ".xml") { + system_filename.append(".xml"); + } + + system_file->open(string(localpath + system_filename).c_str()); + if ( !system_file->is_open()) { + system_file->open(string(fullpath + system_filename).c_str()); + if ( !system_file->is_open()) { + cerr << " Could not open system file: " << system_filename << " in path " + << fullpath << " or " << localpath << endl; + } + } + return system_file; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGFCS::GetComponentStrings(string delimeter) +string FGFCS::GetComponentStrings(const string& delimiter) const { unsigned int comp; string CompStrings = ""; bool firstime = true; + int total_count=0; - for (comp = 0; comp < FCSComponents.size(); comp++) { + for (unsigned int i=0; iGetName(); + CompStrings += Systems[i]->GetName(); + total_count++; } for (comp = 0; comp < APComponents.size(); comp++) { if (firstime) firstime = false; - else CompStrings += delimeter; + else CompStrings += delimiter; CompStrings += APComponents[comp]->GetName(); + total_count++; + } + + for (comp = 0; comp < FCSComponents.size(); comp++) { + if (firstime) firstime = false; + else CompStrings += delimiter; + + CompStrings += FCSComponents[comp]->GetName(); + total_count++; } return CompStrings; @@ -615,31 +777,39 @@ string FGFCS::GetComponentStrings(string delimeter) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGFCS::GetComponentValues(string delimeter) +string FGFCS::GetComponentValues(const string& delimiter) const { + std::ostringstream buf; + unsigned int comp; - string CompValues = ""; - char buffer[17]; bool firstime = true; + int total_count=0; - for (comp = 0; comp < FCSComponents.size(); comp++) { + for (unsigned int i=0; iGetOutput()); - CompValues += string(buffer); + buf << setprecision(9) << Systems[i]->GetOutput(); + total_count++; } for (comp = 0; comp < APComponents.size(); comp++) { if (firstime) firstime = false; - else CompValues += delimeter; + else buf << delimiter; + + buf << setprecision(9) << APComponents[comp]->GetOutput(); + total_count++; + } + + for (comp = 0; comp < FCSComponents.size(); comp++) { + if (firstime) firstime = false; + else buf << delimiter; - sprintf(buffer, "%9.6f", APComponents[comp]->GetOutput()); - CompValues += string(buffer); + buf << setprecision(9) << FCSComponents[comp]->GetOutput(); + total_count++; } - CompValues += "\0"; - return CompValues; + return buf.str(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -661,9 +831,10 @@ void FGFCS::AddThrottle(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGFCS::AddGear(void) +void FGFCS::AddGear(unsigned int NumGear) { - SteerPosDeg.push_back(0.0); + SteerPosDeg.clear(); + for (unsigned int i=0; iTie("fcs/right-brake-cmd-norm", this, &FGFCS::GetRBrake, &FGFCS::SetRBrake); PropertyManager->Tie("fcs/center-brake-cmd-norm", this, &FGFCS::GetCBrake, &FGFCS::SetCBrake); PropertyManager->Tie("fcs/steer-cmd-norm", this, &FGFCS::GetDsCmd, &FGFCS::SetDsCmd); + + PropertyManager->Tie("gear/tailhook-pos-norm", this, &FGFCS::GetTailhookPos, &FGFCS::SetTailhookPos); + PropertyManager->Tie("fcs/wing-fold-pos-norm", this, &FGFCS::GetWingFoldPos, &FGFCS::SetWingFoldPos); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Technically, this function should probably bind propulsion type specific controls // rather than mixture and prop-advance. -// void FGFCS::bindThrottle(unsigned int num) { - char tmp[80]; + string tmp; - snprintf(tmp, 80, "fcs/throttle-cmd-norm[%u]",num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetThrottleCmd, + tmp = CreateIndexedPropertyName("fcs/throttle-cmd-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottleCmd, &FGFCS::SetThrottleCmd); - snprintf(tmp, 80, "fcs/throttle-pos-norm[%u]",num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetThrottlePos, + tmp = CreateIndexedPropertyName("fcs/throttle-pos-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetThrottlePos, &FGFCS::SetThrottlePos); - snprintf(tmp, 80, "fcs/mixture-cmd-norm[%u]",num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetMixtureCmd, + tmp = CreateIndexedPropertyName("fcs/mixture-cmd-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixtureCmd, &FGFCS::SetMixtureCmd); - snprintf(tmp, 80, "fcs/mixture-pos-norm[%u]",num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetMixturePos, + tmp = CreateIndexedPropertyName("fcs/mixture-pos-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetMixturePos, &FGFCS::SetMixturePos); - snprintf(tmp, 80, "fcs/advance-cmd-norm[%u]",num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropAdvanceCmd, + tmp = CreateIndexedPropertyName("fcs/advance-cmd-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvanceCmd, &FGFCS::SetPropAdvanceCmd); - snprintf(tmp, 80, "fcs/advance-pos-norm[%u]", num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropAdvance, + tmp = CreateIndexedPropertyName("fcs/advance-pos-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropAdvance, &FGFCS::SetPropAdvance); - snprintf(tmp, 80, "fcs/feather-cmd-norm[%u]", num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetFeatherCmd, + tmp = CreateIndexedPropertyName("fcs/feather-cmd-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetFeatherCmd, &FGFCS::SetFeatherCmd); - snprintf(tmp, 80, "fcs/feather-pos-norm[%u]", num); - PropertyManager->Tie( tmp, this, num, &FGFCS::GetPropFeather, + tmp = CreateIndexedPropertyName("fcs/feather-pos-norm", num); + PropertyManager->Tie( tmp.c_str(), this, num, &FGFCS::GetPropFeather, &FGFCS::SetPropFeather); } @@ -769,26 +942,12 @@ void FGFCS::bindThrottle(unsigned int num) void FGFCS::bindModel(void) { unsigned int i; - char tmp[80]; + string tmp; for (i=0; iGetGearUnit(i)->GetSteerable()) { - snprintf(tmp,80,"fcs/steer-pos-deg[%u]",i); - PropertyManager->Tie( tmp, this, i, &FGFCS::GetSteerPosDeg, &FGFCS::SetSteerPosDeg); - } - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGFCS::unbind(FGPropertyManager *node) -{ - int N = node->nChildren(); - for (int i=0; igetChild(i)->nChildren() ) { - unbind( (FGPropertyManager*)node->getChild(i) ); - } else if ( node->getChild(i)->isTied() ) { - node->getChild(i)->untie(); + if (FDMExec->GetGroundReactions()->GetGearUnit(i)->GetSteerable()) { + tmp = CreateIndexedPropertyName("fcs/steer-pos-deg", i); + PropertyManager->Tie( tmp.c_str(), this, i, &FGFCS::GetSteerPosDeg, &FGFCS::SetSteerPosDeg); } } } @@ -818,7 +977,7 @@ void FGFCS::Debug(int from) if (debug_lvl & 1) { // Standard console startup message output if (from == 2) { // Loader - cout << endl << " Flight Control (" << Name << ")" << endl; + cout << endl << " " << Name << endl; } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification