%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGFCSComponent.h"
+#include "input_output/FGPropertyManager.h"
+#include "input_output/FGXMLElement.h"
+#include "math/FGPropertyValue.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.32 2011/06/16 03:39:38 jberndt Exp $";
static const char *IdHdr = ID_FCSCOMPONENT;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
{
Element *input_element, *clip_el;
- Input = Output = clipmin = clipmax = 0.0;
- OutputNode = treenode = 0;
+ Input = Output = clipmin = clipmax = delay_time = 0.0;
+ treenode = 0;
+ delay = index = 0;
ClipMinPropertyNode = ClipMaxPropertyNode = 0;
clipMinSign = clipMaxSign = 1.0;
IsOutput = clip = false;
string input, clip_string;
+ dt = fcs->GetDt();
PropertyManager = fcs->GetPropertyManager();
if (element->GetName() == string("lag_filter")) {
Type = "PID";
} else if (element->GetName() == string("sensor")) {
Type = "SENSOR";
+ } else if (element->GetName() == string("accelerometer")) {
+ Type = "ACCELEROMETER";
+ } else if (element->GetName() == string("magnetometer")) {
+ Type = "MAGNETOMETER";
+ } else if (element->GetName() == string("gyro")) {
+ Type = "GYRO";
+ } else if (element->GetName() == string("actuator")) {
+ Type = "ACTUATOR";
} else { // illegal component in this channel
Type = "UNKNOWN";
}
Name = element->GetAttributeValue("name");
- FGPropertyManager *tmp=0;
-
input_element = element->FindElement("input");
while (input_element) {
input = input_element->GetDataLine();
} else {
InputSigns.push_back( 1.0);
}
- tmp = PropertyManager->GetNode(input);
- if (tmp) {
- InputNodes.push_back( tmp );
+ FGPropertyManager* node = 0L;
+ if (PropertyManager->HasNode(input)) {
+ node = PropertyManager->GetNode(input);
+ InputNodes.push_back(new FGPropertyValue( node ));
} else {
- cerr << fgred << " In component: " << Name << " unknown property "
- << input << " referenced. Aborting" << endl;
- exit(-1);
+ InputNodes.push_back(new FGPropertyValue( input,
+ PropertyManager ));
}
+ InputNames.push_back( input );
+
input_element = element->FindNextElement("input");
}
- if (element->FindElement("output")) {
+ Element *out_elem = element->FindElement("output");
+ while (out_elem) {
IsOutput = true;
- OutputNode = PropertyManager->GetNode( element->FindElementValue("output") );
+ string output_node_name = out_elem->GetDataLine();
+ FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
+ OutputNodes.push_back(OutputNode);
if (!OutputNode) {
- cerr << endl << " Unable to process property: " << element->FindElementValue("output") << endl;
+ cerr << endl << " Unable to process property: " << output_node_name << endl;
throw(string("Invalid output property name in flight control definition"));
}
+ out_elem = element->FindNextElement("output");
+ }
+
+ Element* delay_elem = element->FindElement("delay");
+ if ( delay_elem ) {
+ delay_time = delay_elem->GetDataAsNumber();
+ string delayType = delay_elem->GetAttributeValue("type");
+ if (delayType.length() > 0) {
+ if (delayType == "time") {
+ delay = (unsigned int)(delay_time / dt);
+ } else if (delayType == "frames") {
+ delay = (unsigned int)delay_time;
+ } else {
+ cerr << "Unallowed delay type" << endl;
+ }
+ } else {
+ delay = (unsigned int)(delay_time / dt);
+ }
+ output_array.resize(delay);
+ for (int i=0; i<delay; i++) output_array[i] = 0.0;
}
clip_el = element->FindElement("clipto");
if (clip_el) {
clip_string = clip_el->FindElementValue("min");
- if (clip_string.find_first_not_of("+-.0123456789") != string::npos) { // it's a property
- if (clip_string[0] == '-') clipMinSign = -1.0;
+ if (!is_number(clip_string)) { // it's a property
+ if (clip_string[0] == '-') {
+ clipMinSign = -1.0;
+ clip_string.erase(0,1);
+ }
ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
} else {
clipmin = clip_el->FindElementValueAsNumber("min");
}
clip_string = clip_el->FindElementValue("max");
- if (clip_string.find_first_not_of("+-.0123456789") != string::npos) { // it's a property
- if (clip_string[0] == '-') clipMaxSign = -1.0;
+ if (!is_number(clip_string)) { // it's a property
+ if (clip_string[0] == '-') {
+ clipMaxSign = -1.0;
+ clip_string.erase(0,1);
+ }
ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
} else {
clipmax = clip_el->FindElementValueAsNumber("max");
FGFCSComponent::~FGFCSComponent()
{
Debug(1);
+ for (unsigned int i=0; i<InputNodes.size(); i++) {
+ delete InputNodes[i];
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCSComponent::SetOutput(void)
{
- OutputNode->setDoubleValue(Output);
+ for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGFCSComponent::Delay(void)
+{
+ output_array[index] = Output;
+ if (index == delay-1) index = 0;
+ else index++;
+ Output = output_array[index];
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGFCSComponent::Clip(void)
{
if (clip) {
void FGFCSComponent::Debug(int from)
{
+ string propsign="";
+
if (debug_lvl <= 0) return;
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) {
cout << endl << " Loading Component \"" << Name
<< "\" of type: " << Type << endl;
+
+ if (clip) {
+ if (ClipMinPropertyNode != 0L) {
+ if (clipMinSign < 0.0) propsign="-";
+ cout << " Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
+ propsign="";
+ } else {
+ cout << " Minimum limit: " << clipmin << endl;
+ }
+ if (ClipMaxPropertyNode != 0L) {
+ if (clipMaxSign < 0.0) propsign="-";
+ cout << " Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
+ propsign="";
+ } else {
+ cout << " Maximum limit: " << clipmax << endl;
+ }
+ }
+ if (delay > 0) cout <<" Frame delay: " << delay
+ << " frames (" << delay*dt << " sec)" << endl;
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification