1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGFCSComponent.cpp
7 ------------- Copyright (C) 2000 -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 You should have received a copy of the GNU Lesser General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU Lesser General Public License can also be found on
24 the world wide web at http://www.gnu.org.
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
30 --------------------------------------------------------------------------------
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES, and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
40 #include "FGFCSComponent.h"
41 #include "input_output/FGPropertyManager.h"
42 #include "input_output/FGXMLElement.h"
43 #include "math/FGPropertyValue.h"
51 static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.30 2011/04/05 20:20:21 andgi Exp $";
52 static const char *IdHdr = ID_FCSCOMPONENT;
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
58 FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
60 Element *input_element, *clip_el;
61 Input = Output = clipmin = clipmax = 0.0;
64 ClipMinPropertyNode = ClipMaxPropertyNode = 0;
65 clipMinSign = clipMaxSign = 1.0;
66 IsOutput = clip = false;
67 string input, clip_string;
70 PropertyManager = fcs->GetPropertyManager();
71 if (element->GetName() == string("lag_filter")) {
73 } else if (element->GetName() == string("lead_lag_filter")) {
74 Type = "LEAD_LAG_FILTER";
75 } else if (element->GetName() == string("washout_filter")) {
76 Type = "WASHOUT_FILTER";
77 } else if (element->GetName() == string("second_order_filter")) {
78 Type = "SECOND_ORDER_FILTER";
79 } else if (element->GetName() == string("integrator")) {
81 } else if (element->GetName() == string("summer")) {
83 } else if (element->GetName() == string("pure_gain")) {
85 } else if (element->GetName() == string("scheduled_gain")) {
86 Type = "SCHEDULED_GAIN";
87 } else if (element->GetName() == string("aerosurface_scale")) {
88 Type = "AEROSURFACE_SCALE";
89 } else if (element->GetName() == string("switch")) {
91 } else if (element->GetName() == string("kinematic")) {
93 } else if (element->GetName() == string("deadband")) {
95 } else if (element->GetName() == string("fcs_function")) {
96 Type = "FCS_FUNCTION";
97 } else if (element->GetName() == string("pid")) {
99 } else if (element->GetName() == string("sensor")) {
101 } else if (element->GetName() == string("accelerometer")) {
102 Type = "ACCELEROMETER";
103 } else if (element->GetName() == string("magnetometer")) {
104 Type = "MAGNETOMETER";
105 } else if (element->GetName() == string("gyro")) {
107 } else if (element->GetName() == string("actuator")) {
109 } else { // illegal component in this channel
113 Name = element->GetAttributeValue("name");
115 input_element = element->FindElement("input");
116 while (input_element) {
117 input = input_element->GetDataLine();
118 if (input[0] == '-') {
119 InputSigns.push_back(-1.0);
122 InputSigns.push_back( 1.0);
124 FGPropertyManager* node = 0L;
125 if (PropertyManager->HasNode(input)) {
126 node = PropertyManager->GetNode(input);
127 InputNodes.push_back(new FGPropertyValue( node ));
129 InputNodes.push_back(new FGPropertyValue( input,
132 InputNames.push_back( input );
134 input_element = element->FindNextElement("input");
137 Element *out_elem = element->FindElement("output");
140 string output_node_name = out_elem->GetDataLine();
141 FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
142 OutputNodes.push_back(OutputNode);
144 cerr << endl << " Unable to process property: " << output_node_name << endl;
145 throw(string("Invalid output property name in flight control definition"));
147 out_elem = element->FindNextElement("output");
150 Element* delay_elem = element->FindElement("delay");
152 delay = (unsigned int)delay_elem->GetDataAsNumber();
153 string delayType = delay_elem->GetAttributeValue("type");
154 if (delayType.length() > 0) {
155 if (delayType == "time") {
156 delay = (int)(delay / dt);
157 } else if (delayType == "frames") {
158 // no op. the delay type of "frames" is assumed and is the default.
160 cerr << "Unallowed delay type" << endl;
163 delay = (int)(delay / dt);
165 output_array.resize(delay);
166 for (int i=0; i<delay; i++) output_array[i] = 0.0;
169 clip_el = element->FindElement("clipto");
171 clip_string = clip_el->FindElementValue("min");
172 if (!is_number(clip_string)) { // it's a property
173 if (clip_string[0] == '-') {
175 clip_string.erase(0,1);
177 ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
179 clipmin = clip_el->FindElementValueAsNumber("min");
181 clip_string = clip_el->FindElementValue("max");
182 if (!is_number(clip_string)) { // it's a property
183 if (clip_string[0] == '-') {
185 clip_string.erase(0,1);
187 ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
189 clipmax = clip_el->FindElementValueAsNumber("max");
197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
199 FGFCSComponent::~FGFCSComponent()
202 for (unsigned int i=0; i<InputNodes.size(); i++) {
203 delete InputNodes[i];
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 void FGFCSComponent::SetOutput(void)
211 for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
216 bool FGFCSComponent::Run(void)
221 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
223 void FGFCSComponent::Delay(void)
225 output_array[index] = Output;
226 if (index == delay-1) index = 0;
228 Output = output_array[index];
231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
233 void FGFCSComponent::Clip(void)
236 if (ClipMinPropertyNode != 0) clipmin = clipMinSign*ClipMinPropertyNode->getDoubleValue();
237 if (ClipMaxPropertyNode != 0) clipmax = clipMaxSign*ClipMaxPropertyNode->getDoubleValue();
238 if (Output > clipmax) Output = clipmax;
239 else if (Output < clipmin) Output = clipmin;
243 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
245 // The old way of naming FCS components allowed upper or lower case, spaces, etc.
246 // but then the names were modified to fit into a property name heirarchy. This
247 // was confusing (it wasn't done intentionally - it was a carryover from the early
248 // design). We now support the direct naming of properties in the FCS component
249 // name attribute. The old way is supported in code at this time, but deprecated.
251 void FGFCSComponent::bind(void)
254 if (Name.find("/") == string::npos) {
255 tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
259 PropertyManager->Tie( tmp, this, &FGFCSComponent::GetOutput);
262 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
263 // The bitmasked value choices are as follows:
264 // unset: In this case (the default) JSBSim would only print
265 // out the normally expected messages, essentially echoing
266 // the config files as they are read. If the environment
267 // variable is not set, debug_lvl is set to 1 internally
268 // 0: This requests JSBSim not to output any messages
270 // 1: This value explicity requests the normal JSBSim
272 // 2: This value asks for a message to be printed out when
273 // a class is instantiated
274 // 4: When this value is set, a message is displayed when a
275 // FGModel object executes its Run() method
276 // 8: When this value is set, various runtime state variables
277 // are printed out periodically
278 // 16: When set various parameters are sanity checked and
279 // a message is printed out when they go out of bounds
281 void FGFCSComponent::Debug(int from)
285 if (debug_lvl <= 0) return;
287 if (debug_lvl & 1) { // Standard console startup message output
289 cout << endl << " Loading Component \"" << Name
290 << "\" of type: " << Type << endl;
293 if (ClipMinPropertyNode != 0L) {
294 if (clipMinSign < 0.0) propsign="-";
295 cout << " Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
298 cout << " Minimum limit: " << clipmin << endl;
300 if (ClipMaxPropertyNode != 0L) {
301 if (clipMaxSign < 0.0) propsign="-";
302 cout << " Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
305 cout << " Maximum limit: " << clipmax << endl;
308 if (delay > 0) cout <<" Frame delay: " << delay
309 << " frames (" << delay*dt << " sec)" << endl;
312 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
313 if (from == 0) cout << "Instantiated: FGFCSComponent" << endl;
314 if (from == 1) cout << "Destroyed: FGFCSComponent" << endl;
316 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
318 if (debug_lvl & 8 ) { // Runtime state variables
320 if (debug_lvl & 16) { // Sanity checking
322 if (debug_lvl & 64) {
323 if (from == 0) { // Constructor
324 cout << IdSrc << endl;
325 cout << IdHdr << endl;