fgfcs.cpp

来自「6 DOF Missle Simulation」· C++ 代码 · 共 992 行 · 第 1/3 页

    } else {
      ThrottleCmd[engineNum] = setting;
    }
  } else {
    cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
         << " engines exist, but attempted throttle command is for engine "
         << engineNum << endl;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetThrottlePos(int engineNum, double setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<ThrottlePos.size();ctr++) ThrottlePos[ctr] = setting;
    } else {
      ThrottlePos[engineNum] = setting;
    }
  } else {
    cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
         << " engines exist, but attempted throttle position setting is for engine "
         << engineNum << endl;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double FGFCS::GetThrottleCmd(int engineNum) const
{
  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
       cerr << "Cannot get throttle value for ALL engines" << endl;
    } else {
      return ThrottleCmd[engineNum];
    }
  } else {
    cerr << "Throttle " << engineNum << " does not exist! " << ThrottleCmd.size()
         << " engines exist, but throttle setting for engine " << engineNum
         << " is selected" << endl;
  }
  return 0.0;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double FGFCS::GetThrottlePos(int engineNum) const
{
  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
       cerr << "Cannot get throttle value for ALL engines" << endl;
    } else {
      return ThrottlePos[engineNum];
    }
  } else {
    cerr << "Throttle " << engineNum << " does not exist! " << ThrottlePos.size()
         << " engines exist, but attempted throttle position setting is for engine "
         << engineNum << endl;
  }
  return 0.0;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetMixtureCmd(int engineNum, double setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<MixtureCmd.size();ctr++) MixtureCmd[ctr] = setting;
    } else {
      MixtureCmd[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetMixturePos(int engineNum, double setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<=MixtureCmd.size();ctr++) MixturePos[ctr] = MixtureCmd[ctr];
    } else {
      MixturePos[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetPropAdvanceCmd(int engineNum, double setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<PropAdvanceCmd.size();ctr++) PropAdvanceCmd[ctr] = setting;
    } else {
      PropAdvanceCmd[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetPropAdvance(int engineNum, double setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<=PropAdvanceCmd.size();ctr++) PropAdvance[ctr] = PropAdvanceCmd[ctr];
    } else {
      PropAdvance[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetFeatherCmd(int engineNum, bool setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<PropFeatherCmd.size();ctr++) PropFeatherCmd[ctr] = setting;
    } else {
      PropFeatherCmd[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGFCS::SetPropFeather(int engineNum, bool setting)
{
  unsigned int ctr;

  if (engineNum < (int)ThrottlePos.size()) {
    if (engineNum < 0) {
      for (ctr=0;ctr<=PropFeatherCmd.size();ctr++) PropFeather[ctr] = PropFeatherCmd[ctr];
    } else {
      PropFeather[engineNum] = setting;
    }
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGFCS::Load(Element* el, SystemType systype)
{
  string name, file, fname="", interface_property_string, parent_name;
  vector <FGFCSComponent*> *Components;
  Element *component_element, *property_element, *sensor_element;
  Element *channel_element;

  Components=0;

  string separator = "/";

// 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()) {
    fname = el->GetAttributeValue("file");
    if (systype == stSystem) {
      file = FindSystemFullPathname(fname);
    } else { 
      file = FDMExec->GetFullAircraftPath() + separator + fname + ".xml";
    }
    if (fname.empty()) {
      cerr << "FCS, Autopilot, or system does not appear to be defined inline nor in a file" << endl;
      return false;
    } else {
      document = LoadXMLDocument(file);
      name = document->GetAttributeValue("name");
    }
  } else {
    document = el;
  }

  if (document->GetName() == "autopilot") {
    Components = &APComponents;
    Name = "Autopilot: " + document->GetAttributeValue("name");
  } 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);

  if (document->GetName() == "flight_control") bindModel();

  // Interface properties from any autopilot, flight control, or other system are
  // all stored in the interface properties array.

  property_element = document->FindElement("property");
  if (property_element) cout << endl << "    Declared properties" << endl << endl;
  while (property_element) {
    double value=0.0;
    if ( ! property_element->GetAttributeValue("value").empty())
      value = property_element->GetAttributeValueAsNumber("value");
    interface_properties.push_back(new double(value));
    interface_property_string = property_element->GetDataLine();
    PropertyManager->Tie(interface_property_string, interface_properties.back());
    cout << "      " << interface_property_string << " (initial value: " << value << ")" << endl;
    property_element = document->FindNextElement("property");
  }

  // After reading interface properties in a file, read properties in the local
  // flight_control, autopiot, 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.

  if (!fname.empty()) {
    property_element = el->FindElement("property");
    if (property_element && debug_lvl > 0) cout << endl << "    Declared 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();
      
      FGPropertyManager* node = PropertyManager->GetNode(interface_property_string);
      if (node) {
        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");
    }
  }

  // Any sensor elements that are outside of a channel (in either the autopilot
  // or the flight_control, or even any possible "system") are placed into the global
  // "sensors" array, and are executed prior to any autopilot, flight control, or
  // system.

  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;
    }
    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 {
        if ((component_element->GetName() == string("lag_filter")) ||
            (component_element->GetName() == string("lead_lag_filter")) ||
            (component_element->GetName() == string("washout_filter")) ||
            (component_element->GetName() == string("second_order_filter")) ||
            (component_element->GetName() == string("integrator")) )
        {
          Components->push_back(new FGFilter(this, component_element));
        } else if ((component_element->GetName() == string("pure_gain")) ||
                   (component_element->GetName() == string("scheduled_gain")) ||
                   (component_element->GetName() == string("aerosurface_scale")))
        {
          Components->push_back(new FGGain(this, component_element));
        } else if (component_element->GetName() == string("summer")) {
          Components->push_back(new FGSummer(this, component_element));
        } else if (component_element->GetName() == string("deadband")) {
          Components->push_back(new FGDeadBand(this, component_element));
        } else if (component_element->GetName() == string("switch")) {
          Components->push_back(new FGSwitch(this, component_element));
        } else if (component_element->GetName() == string("kinematic")) {
          Components->push_back(new FGKinemat(this, component_element));
        } else if (component_element->GetName() == string("fcs_function")) {
          Components->push_back(new FGFCSFunction(this, component_element));
        } else if (component_element->GetName() == string("pid")) {
          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 {
          cerr << "Unknown FCS component: " << component_element->GetName() << endl;
        }
      } catch(string s) {
        cerr << highint << fgred << endl << "  " << s << endl;
        cerr << reset << endl;
        return false;
      }
      component_element = channel_element->GetNextElement();
    }
    channel_element = document->FindNextElement("channel");
  }

  ResetParser();

  return true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double FGFCS::GetBrake(FGLGear::BrakeGroup bg)
{
  switch (bg) {
  case FGLGear::bgLeft: