1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5 Date started: 21 February 2006
7 ------------- Copyright (C) 2007 Jon S. Berndt (jon@jsbsim.org) -------------
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 "FGActuator.h"
46 static const char *IdSrc = "$Id: FGActuator.cpp,v 1.23 2012/04/08 15:04:41 jberndt Exp $";
47 static const char *IdHdr = ID_ACTUATOR;
49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
54 FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
58 // inputs are read from the base class constructor
61 PreviousHystOutput = 0.0;
62 PreviousRateLimOutput = 0.0;
63 PreviousLagInput = PreviousLagOutput = 0.0;
64 bias = lag = hysteresis_width = deadband_width = 0.0;
65 rate_limit = 0.0; // no limit
66 fail_zero = fail_hardover = fail_stuck = false;
71 if ( element->FindElement("deadband_width") ) {
72 deadband_width = element->FindElementValueAsNumber("deadband_width");
74 if ( element->FindElement("hysteresis_width") ) {
75 hysteresis_width = element->FindElementValueAsNumber("hysteresis_width");
77 if ( element->FindElement("rate_limit") ) {
78 rate_limit = fabs(element->FindElementValueAsNumber("rate_limit"));
80 if ( element->FindElement("bias") ) {
81 bias = element->FindElementValueAsNumber("bias");
83 if ( element->FindElement("lag") ) {
84 lag = element->FindElementValueAsNumber("lag");
85 denom = 2.00 + dt*lag;
87 cb = (2.00 - dt*lag) / denom;
90 FGFCSComponent::bind();
96 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
98 FGActuator::~FGActuator()
103 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
105 bool FGActuator::Run(void )
107 Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
109 if( fcs->GetTrimStatus() ) initialized = 0;
111 if (fail_zero) Input = 0;
112 if (fail_hardover) Input = clipmax*sign(Input);
114 Output = Input; // Perfect actuator. At this point, if no failures are present
115 // and no subsequent lag, limiting, etc. is done, the output
116 // is simply the input. If any further processing is done
117 // (below) such as lag, rate limiting, hysteresis, etc., then
118 // the Input will be further processed and the eventual Output
119 // will be overwritten from this perfect value.
122 Output = PreviousOutput;
124 if (lag != 0.0) Lag(); // models actuator lag
125 if (rate_limit != 0) RateLimit(); // limit the actuator rate
126 if (deadband_width != 0.0) Deadband();
127 if (hysteresis_width != 0.0) Hysteresis();
128 if (bias != 0.0) Bias(); // models a finite bias
131 PreviousOutput = Output; // previous value needed for "stuck" malfunction
139 if (Output >= clipmax) saturated = true;
140 else if (Output <= clipmin) saturated = true;
143 if (IsOutput) SetOutput();
148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
150 void FGActuator::Bias(void)
155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
157 void FGActuator::Lag(void)
159 // "Output" on the right side of the "=" is the current frame input
160 // for this Lag filter
161 double input = Output;
164 Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
166 PreviousLagInput = input;
167 PreviousLagOutput = Output;
170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
172 void FGActuator::Hysteresis(void)
174 // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
175 // is - for the purposes of this Hysteresis method - really the input to the
177 double input = Output;
180 if (input > PreviousHystOutput)
181 Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
182 else if (input < PreviousHystOutput)
183 Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
186 PreviousHystOutput = Output;
189 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
191 void FGActuator::RateLimit(void)
193 // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
194 // is - for the purposes of this RateLimit method - really the input to the
196 double input = Output;
198 double delta = input - PreviousRateLimOutput;
199 if (fabs(delta) > dt * rate_limit) {
200 double signed_rate_limit = delta > 0.0 ? rate_limit : -rate_limit;
201 Output = PreviousRateLimOutput + signed_rate_limit * dt;
204 PreviousRateLimOutput = Output;
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 void FGActuator::Deadband(void)
211 // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
212 // is - for the purposes of this Deadband method - really the input to the
214 double input = Output;
216 if (input < -deadband_width/2.0) {
217 Output = (input + deadband_width/2.0);
218 } else if (input > deadband_width/2.0) {
219 Output = (input - deadband_width/2.0);
225 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
227 void FGActuator::bind(void)
230 if (Name.find("/") == string::npos) {
231 tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
233 const string tmp_zero = tmp + "/malfunction/fail_zero";
234 const string tmp_hardover = tmp + "/malfunction/fail_hardover";
235 const string tmp_stuck = tmp + "/malfunction/fail_stuck";
236 const string tmp_sat = tmp + "/saturated";
238 PropertyManager->Tie( tmp_zero, this, &FGActuator::GetFailZero, &FGActuator::SetFailZero);
239 PropertyManager->Tie( tmp_hardover, this, &FGActuator::GetFailHardover, &FGActuator::SetFailHardover);
240 PropertyManager->Tie( tmp_stuck, this, &FGActuator::GetFailStuck, &FGActuator::SetFailStuck);
241 PropertyManager->Tie( tmp_sat, this, &FGActuator::IsSaturated);
244 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
245 // The bitmasked value choices are as follows:
246 // unset: In this case (the default) JSBSim would only print
247 // out the normally expected messages, essentially echoing
248 // the config files as they are read. If the environment
249 // variable is not set, debug_lvl is set to 1 internally
250 // 0: This requests JSBSim not to output any messages
252 // 1: This value explicity requests the normal JSBSim
254 // 2: This value asks for a message to be printed out when
255 // a class is instantiated
256 // 4: When this value is set, a message is displayed when a
257 // FGModel object executes its Run() method
258 // 8: When this value is set, various runtime state variables
259 // are printed out periodically
260 // 16: When set various parameters are sanity checked and
261 // a message is printed out when they go out of bounds
263 void FGActuator::Debug(int from)
265 if (debug_lvl <= 0) return;
267 if (debug_lvl & 1) { // Standard console startup message output
268 if (from == 0) { // Constructor
269 if (InputSigns[0] < 0)
270 cout << " INPUT: -" << InputNames[0] << endl;
272 cout << " INPUT: " << InputNames[0] << endl;
275 for (unsigned int i=0; i<OutputNodes.size(); i++)
276 cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
278 if (bias != 0.0) cout << " Bias: " << bias << endl;
279 if (rate_limit != 0) cout << " Rate limit: " << rate_limit << endl;
280 if (lag != 0) cout << " Actuator lag: " << lag << endl;
281 if (hysteresis_width != 0) cout << " Hysteresis width: " << hysteresis_width << endl;
282 if (deadband_width != 0) cout << " Deadband width: " << deadband_width << endl;
285 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
286 if (from == 0) cout << "Instantiated: FGActuator" << endl;
287 if (from == 1) cout << "Destroyed: FGActuator" << endl;
289 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
291 if (debug_lvl & 8 ) { // Runtime state variables
293 if (debug_lvl & 16) { // Sanity checking
295 if (debug_lvl & 64) {
296 if (from == 0) { // Constructor
297 cout << IdSrc << endl;
298 cout << IdHdr << endl;