]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGActuator.cpp
latest changes for JSBSim (1.0 prerelease)
[flightgear.git] / src / FDM / JSBSim / models / flight_control / 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 (jsb@hal-pc.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 "FGActuator.h"
41
42 namespace JSBSim {
43
44 static const char *IdSrc = "$Id$";
45 static const char *IdHdr = ID_ACTUATOR;
46
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
48 CLASS IMPLEMENTATION
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
50
51
52 FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
53 {
54   double denom;
55   dt = fcs->GetDt();
56
57   // inputs are read from the base class constructor
58
59   PreviousOutput = 0.0;
60   PreviousHystOutput = 0.0;
61   PreviousRateLimOutput = 0.0;
62   PreviousLagInput = PreviousLagOutput = 0.0;
63   bias = lag = hysteresis_width = deadband_width = 0.0;
64   rate_limit = 0.0; // no limit
65   fail_zero = fail_hardover = fail_stuck = false;
66   ca = cb = 0.0;
67
68   if ( element->FindElement("deadband_width") ) {
69     deadband_width = element->FindElementValueAsNumber("deadband_width");
70   }
71   if ( element->FindElement("hysteresis_width") ) {
72     hysteresis_width = element->FindElementValueAsNumber("hysteresis_width");
73   }
74   if ( element->FindElement("rate_limit") ) {
75     rate_limit = element->FindElementValueAsNumber("rate_limit");
76   }
77   if ( element->FindElement("bias") ) {
78     bias = element->FindElementValueAsNumber("bias");
79   }
80   if ( element->FindElement("lag") ) {
81     lag = element->FindElementValueAsNumber("lag");
82     denom = 2.00 + dt*lag;
83     ca = dt*lag / denom;
84     cb = (2.00 - dt*lag) / denom;
85   }
86
87   FGFCSComponent::bind();
88   bind();
89
90   Debug(0);
91 }
92
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94
95 FGActuator::~FGActuator()
96 {
97   Debug(1);
98 }
99
100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
101
102 bool FGActuator::Run(void )
103 {
104   dt = fcs->GetDt();
105
106   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
107
108   if (fail_zero) Input = 0;
109   if (fail_hardover) Input =  clipmax*sign(Input);
110
111   Output = Input; // Perfect actuator. At this point, if no failures are present
112                   // and no subsequent lag, limiting, etc. is done, the output
113                   // is simply the input. If any further processing is done
114                   // (below) such as lag, rate limiting, hysteresis, etc., then
115                   // the Input will be further processed and the eventual Output
116                   // will be overwritten from this perfect value.
117
118   if (lag != 0.0)              Lag();        // models actuator lag
119   if (rate_limit != 0)         RateLimit();  // limit the actuator rate
120   if (deadband_width != 0.0)   Deadband();
121   if (hysteresis_width != 0.0) Hysteresis();
122   if (bias != 0.0)             Bias();       // models a finite bias
123
124   if (fail_stuck) Output = PreviousOutput;
125   PreviousOutput = Output; // previous value needed for "stuck" malfunction
126
127   Clip();
128   if (IsOutput) SetOutput();
129
130   return true;
131 }
132
133 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
134
135 void FGActuator::Bias(void)
136 {
137   Output += bias;
138 }
139
140 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
141
142 void FGActuator::Lag(void)
143 {
144   // "Output" on the right side of the "=" is the current frame input
145   // for this Lag filter
146   double input = Output;
147   Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
148   PreviousLagInput = input;
149   PreviousLagOutput = Output;
150 }
151
152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153
154 void FGActuator::Hysteresis(void)
155 {
156   // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
157   // is - for the purposes of this Hysteresis method - really the input to the
158   // method.
159   double input = Output;
160   
161   if (input > PreviousHystOutput) {
162     Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
163   } else if (input < PreviousHystOutput) {
164     Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
165   }
166
167   PreviousHystOutput = Output;
168 }
169
170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
171
172 void FGActuator::RateLimit(void)
173 {
174   // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
175   // is - for the purposes of this RateLimit method - really the input to the
176   // method.
177   double input = Output;
178   if (dt > 0.0) {
179     double rate = (input - PreviousRateLimOutput)/dt;
180     if (fabs(rate) > rate_limit) {
181       Output = PreviousRateLimOutput + (rate_limit*fabs(rate)/rate)*dt;
182     }
183   }
184   PreviousRateLimOutput = Output;
185 }
186
187 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
188
189 void FGActuator::Deadband(void)
190 {
191 }
192
193 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
194
195 void FGActuator::bind(void)
196 {
197   string tmp = Name;
198   if (Name.find("/") == string::npos) {
199     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
200   }
201   const string tmp_zero = tmp + "/malfunction/fail_zero";
202   const string tmp_hardover = tmp + "/malfunction/fail_hardover";
203   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
204
205   PropertyManager->Tie( tmp_zero, this, &FGActuator::GetFailZero, &FGActuator::SetFailZero);
206   PropertyManager->Tie( tmp_hardover, this, &FGActuator::GetFailHardover, &FGActuator::SetFailHardover);
207   PropertyManager->Tie( tmp_stuck, this, &FGActuator::GetFailStuck, &FGActuator::SetFailStuck);
208 }
209
210 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
211 //    The bitmasked value choices are as follows:
212 //    unset: In this case (the default) JSBSim would only print
213 //       out the normally expected messages, essentially echoing
214 //       the config files as they are read. If the environment
215 //       variable is not set, debug_lvl is set to 1 internally
216 //    0: This requests JSBSim not to output any messages
217 //       whatsoever.
218 //    1: This value explicity requests the normal JSBSim
219 //       startup messages
220 //    2: This value asks for a message to be printed out when
221 //       a class is instantiated
222 //    4: When this value is set, a message is displayed when a
223 //       FGModel object executes its Run() method
224 //    8: When this value is set, various runtime state variables
225 //       are printed out periodically
226 //    16: When set various parameters are sanity checked and
227 //       a message is printed out when they go out of bounds
228
229 void FGActuator::Debug(int from)
230 {
231   if (debug_lvl <= 0) return;
232
233   if (debug_lvl & 1) { // Standard console startup message output
234     if (from == 0) { // Constructor
235       if (InputSigns[0] < 0)
236         cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
237       else
238         cout << "      INPUT: " << InputNodes[0]->getName() << endl;
239
240       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
241       if (bias != 0.0) cout << "      Bias: " << bias << endl;
242       if (rate_limit != 0) cout << "      Rate limit: " << rate_limit << endl;
243       if (lag != 0) cout << "      Actuator lag: " << lag << endl;
244       if (hysteresis_width != 0) cout << "      Hysteresis width: " << hysteresis_width << endl;
245       if (deadband_width != 0) cout << "      Deadband width: " << deadband_width << endl;
246     }
247   }
248   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
249     if (from == 0) cout << "Instantiated: FGActuator" << endl;
250     if (from == 1) cout << "Destroyed:    FGActuator" << endl;
251   }
252   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
253   }
254   if (debug_lvl & 8 ) { // Runtime state variables
255   }
256   if (debug_lvl & 16) { // Sanity checking
257   }
258   if (debug_lvl & 64) {
259     if (from == 0) { // Constructor
260       cout << IdSrc << endl;
261       cout << IdHdr << endl;
262     }
263   }
264 }
265 }