JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
FGActuator.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 
3  Module: FGActuator.cpp
4  Author: Jon Berndt
5  Date started: 21 February 2006
6 
7  ------------- Copyright (C) 2007 Jon S. Berndt (jon@jsbsim.org) -------------
8 
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
12  version.
13 
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
17  details.
18 
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.
22 
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.
25 
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
28 
29 HISTORY
30 --------------------------------------------------------------------------------
31 
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES, and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 INCLUDES
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39 
40 #include <stdlib.h>
41 
42 #include "FGActuator.h"
43 #include "input_output/FGXMLElement.h"
44 #include "math/FGRealValue.h"
45 #include "models/FGFCS.h"
46 
47 using namespace std;
48 
49 namespace JSBSim {
50 
51 IDENT(IdSrc,"$Id: FGActuator.cpp,v 1.38 2015/07/12 19:34:08 bcoconni Exp $");
52 IDENT(IdHdr,ID_ACTUATOR);
53 
54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
55 CLASS IMPLEMENTATION
56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
57 
58 
59 FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
60 {
61  // inputs are read from the base class constructor
62 
63  PreviousOutput = 0.0;
64  PreviousHystOutput = 0.0;
65  PreviousRateLimOutput = 0.0;
66  PreviousLagInput = PreviousLagOutput = 0.0;
67  bias = lag = hysteresis_width = deadband_width = 0.0;
68  rate_limit_incr = rate_limit_decr = 0; // no limit
69  fail_zero = fail_hardover = fail_stuck = false;
70  ca = cb = 0.0;
71  initialized = 0;
72  saturated = false;
73 
74  if ( element->FindElement("deadband_width") ) {
75  deadband_width = element->FindElementValueAsNumber("deadband_width");
76  }
77  if ( element->FindElement("hysteresis_width") ) {
78  hysteresis_width = element->FindElementValueAsNumber("hysteresis_width");
79  }
80 
81  // There can be a single rate limit specified, or increasing and
82  // decreasing rate limits specified, and rate limits can be numeric, or
83  // a property.
84  Element* ratelim_el = element->FindElement("rate_limit");
85  while ( ratelim_el ) {
86  FGParameter* rate_limit = 0;
87  string rate_limit_str = ratelim_el->GetDataLine();
88 
89  trim(rate_limit_str);
90  if (is_number(rate_limit_str))
91  rate_limit = new FGRealValue(fabs(atof(rate_limit_str.c_str())));
92  else {
93  if (rate_limit_str[0] == '-') rate_limit_str.erase(0,1);
94  FGPropertyNode* rate_limit_prop = PropertyManager->GetNode(rate_limit_str, true);
95  if (!rate_limit_prop) {
96  std::cerr << "No such property, " << rate_limit_str << " for rate limiting" << std::endl;
97  ratelim_el = element->FindNextElement("rate_limit");
98  continue;
99  }
100  rate_limit = new FGPropertyValue(rate_limit_prop);
101  }
102 
103  if (ratelim_el->HasAttribute("sense")) {
104  string sense = ratelim_el->GetAttributeValue("sense");
105  if (sense.substr(0,4) == "incr")
106  rate_limit_incr = rate_limit;
107  else if (sense.substr(0,4) == "decr")
108  rate_limit_decr = rate_limit;
109  } else {
110  rate_limit_incr = rate_limit;
111  rate_limit_decr = rate_limit;
112  }
113  ratelim_el = element->FindNextElement("rate_limit");
114  }
115 
116  if ( element->FindElement("bias") ) {
117  bias = element->FindElementValueAsNumber("bias");
118  }
119  if ( element->FindElement("lag") ) {
120  lag = element->FindElementValueAsNumber("lag");
121  double denom = 2.00 + dt*lag;
122  ca = dt*lag / denom;
123  cb = (2.00 - dt*lag) / denom;
124  }
125 
126  FGFCSComponent::bind();
127  bind();
128 
129  Debug(0);
130 }
131 
132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
133 
135 {
136  delete rate_limit_incr;
137  if (rate_limit_decr != rate_limit_incr)
138  delete rate_limit_decr;
139 
140  Debug(1);
141 }
142 
143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
144 
145 void FGActuator::ResetPastStates(void)
146 {
147  FGFCSComponent::ResetPastStates();
148 
149  PreviousOutput = PreviousHystOutput = PreviousRateLimOutput
150  = PreviousLagInput = PreviousLagOutput = Output = 0.0;
151 }
152 
153 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
154 
155 bool FGActuator::Run(void )
156 {
157  Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
158 
159  if( fcs->GetTrimStatus() ) initialized = 0;
160 
161  if (fail_zero) Input = 0;
162  if (fail_hardover) Input = clipmax*sign(Input);
163 
164  Output = Input; // Perfect actuator. At this point, if no failures are present
165  // and no subsequent lag, limiting, etc. is done, the output
166  // is simply the input. If any further processing is done
167  // (below) such as lag, rate limiting, hysteresis, etc., then
168  // the Input will be further processed and the eventual Output
169  // will be overwritten from this perfect value.
170 
171  if (fail_stuck) {
172  Output = PreviousOutput;
173  } else {
174  if (lag != 0.0) Lag(); // models actuator lag
175  if (rate_limit_incr != 0 || rate_limit_decr != 0) RateLimit(); // limit the actuator rate
176  if (deadband_width != 0.0) Deadband();
177  if (hysteresis_width != 0.0) Hysteresis();
178  if (bias != 0.0) Bias(); // models a finite bias
179  if (delay != 0) Delay(); // Model transport latency
180  }
181 
182  PreviousOutput = Output; // previous value needed for "stuck" malfunction
183 
184  initialized = 1;
185 
186  Clip();
187 
188  if (clip) {
189  saturated = false;
190  if (Output >= clipmax && clipmax != 0) saturated = true;
191  else if (Output <= clipmin && clipmin != 0) saturated = true;
192  }
193 
194  if (IsOutput) SetOutput();
195 
196  return true;
197 }
198 
199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
200 
201 void FGActuator::Bias(void)
202 {
203  Output += bias;
204 }
205 
206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
207 
208 void FGActuator::Lag(void)
209 {
210  // "Output" on the right side of the "=" is the current frame input
211  // for this Lag filter
212  double input = Output;
213 
214  if ( initialized )
215  Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
216 
217  PreviousLagInput = input;
218  PreviousLagOutput = Output;
219 }
220 
221 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
222 
223 void FGActuator::Hysteresis(void)
224 {
225  // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
226  // is - for the purposes of this Hysteresis method - really the input to the
227  // method.
228  double input = Output;
229 
230  if ( initialized ) {
231  if (input > PreviousHystOutput)
232  Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
233  else if (input < PreviousHystOutput)
234  Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
235  }
236 
237  PreviousHystOutput = Output;
238 }
239 
240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241 
242 void FGActuator::RateLimit(void)
243 {
244  // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
245  // is - for the purposes of this RateLimit method - really the input to the
246  // method.
247  double input = Output;
248  if ( initialized ) {
249  double delta = input - PreviousRateLimOutput;
250  if (rate_limit_incr) {
251  double rate_limit = rate_limit_incr->GetValue();
252  if (delta > dt * rate_limit)
253  Output = PreviousRateLimOutput + rate_limit * dt;
254  }
255  if (rate_limit_decr) {
256  double rate_limit = -rate_limit_decr->GetValue();
257  if (delta < dt * rate_limit)
258  Output = PreviousRateLimOutput + rate_limit * dt;
259  }
260  }
261  PreviousRateLimOutput = Output;
262 }
263 
264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
265 
266 void FGActuator::Deadband(void)
267 {
268  // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
269  // is - for the purposes of this Deadband method - really the input to the
270  // method.
271  double input = Output;
272 
273  if (input < -deadband_width/2.0) {
274  Output = (input + deadband_width/2.0);
275  } else if (input > deadband_width/2.0) {
276  Output = (input - deadband_width/2.0);
277  } else {
278  Output = 0.0;
279  }
280 }
281 
282 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
283 
284 void FGActuator::bind(void)
285 {
286  string tmp = Name;
287  if (Name.find("/") == string::npos) {
288  tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
289  }
290  const string tmp_zero = tmp + "/malfunction/fail_zero";
291  const string tmp_hardover = tmp + "/malfunction/fail_hardover";
292  const string tmp_stuck = tmp + "/malfunction/fail_stuck";
293  const string tmp_sat = tmp + "/saturated";
294 
295  PropertyManager->Tie( tmp_zero, this, &FGActuator::GetFailZero, &FGActuator::SetFailZero);
296  PropertyManager->Tie( tmp_hardover, this, &FGActuator::GetFailHardover, &FGActuator::SetFailHardover);
297  PropertyManager->Tie( tmp_stuck, this, &FGActuator::GetFailStuck, &FGActuator::SetFailStuck);
298  PropertyManager->Tie( tmp_sat, this, &FGActuator::IsSaturated);
299 }
300 
301 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
302 // The bitmasked value choices are as follows:
303 // unset: In this case (the default) JSBSim would only print
304 // out the normally expected messages, essentially echoing
305 // the config files as they are read. If the environment
306 // variable is not set, debug_lvl is set to 1 internally
307 // 0: This requests JSBSim not to output any messages
308 // whatsoever.
309 // 1: This value explicity requests the normal JSBSim
310 // startup messages
311 // 2: This value asks for a message to be printed out when
312 // a class is instantiated
313 // 4: When this value is set, a message is displayed when a
314 // FGModel object executes its Run() method
315 // 8: When this value is set, various runtime state variables
316 // are printed out periodically
317 // 16: When set various parameters are sanity checked and
318 // a message is printed out when they go out of bounds
319 
320 void FGActuator::Debug(int from)
321 {
322  if (debug_lvl <= 0) return;
323 
324  if (debug_lvl & 1) { // Standard console startup message output
325  if (from == 0) { // Constructor
326  if (InputSigns[0] < 0)
327  cout << " INPUT: -" << InputNames[0] << endl;
328  else
329  cout << " INPUT: " << InputNames[0] << endl;
330 
331  if (IsOutput) {
332  for (unsigned int i=0; i<OutputNodes.size(); i++)
333  cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
334  }
335  if (bias != 0.0) cout << " Bias: " << bias << endl;
336  if (rate_limit_incr != 0) {
337  cout << " Increasing rate limit: " << rate_limit_incr->GetName() << endl;
338  }
339  if (rate_limit_decr != 0) {
340  cout << " Decreasing rate limit: " << rate_limit_decr->GetName() << endl;
341  }
342  if (lag != 0) cout << " Actuator lag: " << lag << endl;
343  if (hysteresis_width != 0) cout << " Hysteresis width: " << hysteresis_width << endl;
344  if (deadband_width != 0) cout << " Deadband width: " << deadband_width << endl;
345  }
346  }
347  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
348  if (from == 0) cout << "Instantiated: FGActuator" << endl;
349  if (from == 1) cout << "Destroyed: FGActuator" << endl;
350  }
351  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
352  }
353  if (debug_lvl & 8 ) { // Runtime state variables
354  }
355  if (debug_lvl & 16) { // Sanity checking
356  }
357  if (debug_lvl & 64) {
358  if (from == 0) { // Constructor
359  cout << IdSrc << endl;
360  cout << IdHdr << endl;
361  }
362  }
363 }
364 }
~FGActuator()
Destructor.
Definition: FGActuator.cpp:134
void SetFailZero(bool set)
This function fails the actuator to zero.
Definition: FGActuator.h:147
std::string GetAttributeValue(const std::string &key)
Retrieves an attribute.
std::string mkPropertyName(std::string name, bool lowercase)
Property-ify a name replaces spaces with &#39;-&#39; and, optionally, makes name all lower case...
bool HasAttribute(const std::string &key)
Determines if an element has the supplied attribute.
Definition: FGXMLElement.h:162
Represents a property value which can use late binding.
Class wrapper for property handling.
STL namespace.
Represents a real value.
Definition: FGRealValue.h:63
Element * FindElement(const std::string &el="")
Searches for a specified element.
double FindElementValueAsNumber(const std::string &el="")
Searches for the named element and returns the data belonging to it as a number.
Represents various types of parameters.
Definition: FGParameter.h:63
void Tie(const std::string &name, bool *pointer, bool useDefault=true)
Tie a property to an external bool variable.
std::string GetDataLine(unsigned int i=0)
Gets a line of data belonging to an element.
bool Run(void)
This function processes the input.
Definition: FGActuator.cpp:155
Encapsulates the Flight Control System (FCS) functionality.
Definition: FGFCS.h:193
Element * FindNextElement(const std::string &el="")
Searches for the next element as specified.
Base class for JSBSim Flight Control System Components.