Branch data Line data Source code
1 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 : :
3 : : Module: FGPID.cpp
4 : : Author: Jon S. Berndt
5 : : Date started: 6/17/2006
6 : :
7 : : ------------- Copyright (C) 2006 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 : : HISTORY
27 : : --------------------------------------------------------------------------------
28 : : Initial code 6/17/2006 JSB
29 : :
30 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
31 : : COMMENTS, REFERENCES, and NOTES
32 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 : :
34 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35 : : INCLUDES
36 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
37 : :
38 : : #include "FGPID.h"
39 : : #include "input_output/FGXMLElement.h"
40 : : #include <string>
41 : : #include <iostream>
42 : :
43 : : using namespace std;
44 : :
45 : : namespace JSBSim {
46 : :
47 : : static const char *IdSrc = "$Id: FGPID.cpp,v 1.17 2010/08/21 22:56:11 jberndt Exp $";
48 : : static const char *IdHdr = ID_PID;
49 : :
50 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
51 : : CLASS IMPLEMENTATION
52 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
53 : :
54 : 7 : FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
55 : : {
56 : 7 : string kp_string, ki_string, kd_string;
57 : :
58 : 7 : Kp = Ki = Kd = 0.0;
59 : 7 : KpPropertyNode = 0;
60 : 7 : KiPropertyNode = 0;
61 : 7 : KdPropertyNode = 0;
62 : 7 : KpPropertySign = 1.0;
63 : 7 : KiPropertySign = 1.0;
64 : 7 : KdPropertySign = 1.0;
65 : 7 : I_out_total = 0.0;
66 : 7 : Input_prev = Input_prev2 = 0.0;
67 : 7 : Trigger = 0;
68 : :
69 [ + - ][ # # ]: 7 : if ( element->FindElement("kp") ) {
70 : 14 : kp_string = element->FindElementValue("kp");
71 [ - + # # ]: 7 : if (!is_number(kp_string)) { // property
72 [ # # # # ]: 0 : if (kp_string[0] == '-') {
73 : 0 : KpPropertySign = -1.0;
74 : 0 : kp_string.erase(0,1);
75 : : }
76 : 0 : KpPropertyNode = PropertyManager->GetNode(kp_string);
77 : : } else {
78 : 7 : Kp = element->FindElementValueAsNumber("kp");
79 : : }
80 : : }
81 : :
82 [ + - ][ # # ]: 7 : if ( element->FindElement("ki") ) {
83 : 14 : ki_string = element->FindElementValue("ki");
84 [ - + # # ]: 7 : if (!is_number(ki_string)) { // property
85 [ # # # # ]: 0 : if (ki_string[0] == '-') {
86 : 0 : KiPropertySign = -1.0;
87 : 0 : ki_string.erase(0,1);
88 : : }
89 : 0 : KiPropertyNode = PropertyManager->GetNode(ki_string);
90 : : } else {
91 : 7 : Ki = element->FindElementValueAsNumber("ki");
92 : : }
93 : : }
94 : :
95 [ + - ][ # # ]: 7 : if ( element->FindElement("kd") ) {
96 : 14 : kd_string = element->FindElementValue("kd");
97 [ - + # # ]: 7 : if (!is_number(kd_string)) { // property
98 [ # # # # ]: 0 : if (kd_string[0] == '-') {
99 : 0 : KdPropertySign = -1.0;
100 : 0 : kd_string.erase(0,1);
101 : : }
102 : 0 : KdPropertyNode = PropertyManager->GetNode(kd_string);
103 : : } else {
104 : 7 : Kd = element->FindElementValueAsNumber("kd");
105 : : }
106 : : }
107 : :
108 [ + - ][ # # ]: 7 : if (element->FindElement("trigger")) {
109 : 7 : Trigger = PropertyManager->GetNode(element->FindElementValue("trigger"));
110 : : }
111 : :
112 : 7 : FGFCSComponent::bind();
113 : :
114 : 7 : Debug(0);
115 : 7 : }
116 : :
117 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
118 : :
119 : 7 : FGPID::~FGPID()
120 : : {
121 : 7 : Debug(1);
122 [ + - ][ # # ]: 7 : }
[ # # ]
123 : :
124 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
125 : :
126 : 378035 : bool FGPID::Run(void )
127 : : {
128 : 378035 : double I_out_delta = 0.0;
129 : : double P_out, D_out;
130 : :
131 : 378035 : Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
132 : :
133 [ - + ]: 378035 : if (KpPropertyNode != 0) Kp = KpPropertyNode->getDoubleValue() * KpPropertySign;
134 [ - + ]: 378035 : if (KiPropertyNode != 0) Ki = KiPropertyNode->getDoubleValue() * KiPropertySign;
135 [ - + ]: 378035 : if (KdPropertyNode != 0) Kd = KdPropertyNode->getDoubleValue() * KdPropertySign;
136 : :
137 : 378035 : P_out = Kp * Input;
138 : 378035 : D_out = (Kd / dt) * (Input - Input_prev);
139 : :
140 : : // Do not continue to integrate the input to the integrator if a wind-up
141 : : // condition is sensed - that is, if the property pointed to by the trigger
142 : : // element is non-zero. Reset the integrator to 0.0 if the Trigger value
143 : : // is negative.
144 : :
145 [ + - ]: 378035 : if (Trigger != 0) {
146 : 378035 : double test = Trigger->getDoubleValue();
147 [ + + ]: 378035 : if (fabs(test) < 0.000001) I_out_delta = Ki * dt * Input; // Normal
148 [ - + ]: 378035 : if (test < 0.0) I_out_total = 0.0; // Reset integrator to 0.0
149 : : } else { // no anti-wind-up trigger defined
150 : 0 : I_out_delta = Ki * dt * Input;
151 : : }
152 : :
153 : 378035 : I_out_total += I_out_delta;
154 : :
155 : 378035 : Output = P_out + I_out_total + D_out;
156 : :
157 : 378035 : Input_prev = Input;
158 : 378035 : Input_prev2 = Input_prev;
159 : :
160 : 378035 : Clip();
161 [ - + ]: 378035 : if (IsOutput) SetOutput();
162 : :
163 : 378035 : return true;
164 : : }
165 : :
166 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
167 : : // The bitmasked value choices are as follows:
168 : : // unset: In this case (the default) JSBSim would only print
169 : : // out the normally expected messages, essentially echoing
170 : : // the config files as they are read. If the environment
171 : : // variable is not set, debug_lvl is set to 1 internally
172 : : // 0: This requests JSBSim not to output any messages
173 : : // whatsoever.
174 : : // 1: This value explicity requests the normal JSBSim
175 : : // startup messages
176 : : // 2: This value asks for a message to be printed out when
177 : : // a class is instantiated
178 : : // 4: When this value is set, a message is displayed when a
179 : : // FGModel object executes its Run() method
180 : : // 8: When this value is set, various runtime state variables
181 : : // are printed out periodically
182 : : // 16: When set various parameters are sanity checked and
183 : : // a message is printed out when they go out of bounds
184 : :
185 : 14 : void FGPID::Debug(int from)
186 : : {
187 [ + - ]: 14 : if (debug_lvl <= 0) return;
188 : :
189 [ + - ]: 14 : if (debug_lvl & 1) { // Standard console startup message output
190 [ + + ]: 14 : if (from == 0) { // Constructor
191 [ - + ]: 7 : if (InputSigns[0] < 0)
192 : 0 : cout << " INPUT: -" << InputNames[0] << endl;
193 : : else
194 : 7 : cout << " INPUT: " << InputNames[0] << endl;
195 : :
196 [ - + ]: 7 : if (IsOutput) {
197 [ # # ]: 0 : for (unsigned int i=0; i<OutputNodes.size(); i++)
198 : 0 : cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
199 : : }
200 : : }
201 : : }
202 [ - + ]: 14 : if (debug_lvl & 2 ) { // Instantiation/Destruction notification
203 [ # # ]: 0 : if (from == 0) cout << "Instantiated: FGPID" << endl;
204 [ # # ]: 0 : if (from == 1) cout << "Destroyed: FGPID" << endl;
205 : : }
206 : 14 : if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
207 : : }
208 : 14 : if (debug_lvl & 8 ) { // Runtime state variables
209 : : }
210 : 14 : if (debug_lvl & 16) { // Sanity checking
211 : : }
212 [ - + ]: 14 : if (debug_lvl & 64) {
213 [ # # ]: 0 : if (from == 0) { // Constructor
214 : 0 : cout << IdSrc << endl;
215 : 0 : cout << IdHdr << endl;
216 : : }
217 : : }
218 : : }
219 [ + + ][ + - ]: 12 : }
|
