src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGFCSComponent.cpp
   4  Author:       Jon S. Berndt
   5  Date started: 11/1999
   6
   7  ------------- Copyright (C) 2000 -------------
   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 "FGFCSComponent.h"
  41 #include "input_output/FGPropertyManager.h"
  42 #include "input_output/FGXMLElement.h"
  43 #include "math/FGPropertyValue.h"
  44 #include <iostream>
  45 #include <cstdlib>
  46
  47 using namespace std;
  48
  49 namespace JSBSim {
  50
  51 static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.34 2011/09/11 11:36:04 bcoconni Exp $";
  52 static const char *IdHdr = ID_FCSCOMPONENT;
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55 CLASS IMPLEMENTATION
  56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
  59 {
  60   Element *input_element, *clip_el;
  61   Input = Output = clipmin = clipmax = delay_time = 0.0;
  62   treenode = 0;
  63   delay = index = 0;
  64   ClipMinPropertyNode = ClipMaxPropertyNode = 0;
  65   clipMinSign = clipMaxSign = 1.0;
  66   IsOutput   = clip = false;
  67   string input, clip_string;
  68   dt = fcs->GetDt();
  69
  70   PropertyManager = fcs->GetPropertyManager();
  71   if        (element->GetName() == string("lag_filter")) {
  72     Type = "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")) {
  80     Type = "INTEGRATOR";
  81   } else if (element->GetName() == string("summer")) {
  82     Type = "SUMMER";
  83   } else if (element->GetName() == string("pure_gain")) {
  84     Type = "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")) {
  90     Type = "SWITCH";
  91   } else if (element->GetName() == string("kinematic")) {
  92     Type = "KINEMATIC";
  93   } else if (element->GetName() == string("deadband")) {
  94     Type = "DEADBAND";
  95   } else if (element->GetName() == string("fcs_function")) {
  96     Type = "FCS_FUNCTION";
  97   } else if (element->GetName() == string("pid")) {
  98     Type = "PID";
  99   } else if (element->GetName() == string("sensor")) {
 100     Type = "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")) {
 106     Type = "GYRO";
 107   } else if (element->GetName() == string("actuator")) {
 108     Type = "ACTUATOR";
 109   } else { // illegal component in this channel
 110     Type = "UNKNOWN";
 111   }
 112
 113   Name = element->GetAttributeValue("name");
 114
 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);
 120       input.erase(0,1);
 121     } else {
 122       InputSigns.push_back( 1.0);
 123     }
 124     FGPropertyManager* node = 0L;
 125     if (PropertyManager->HasNode(input)) {
 126       node = PropertyManager->GetNode(input);
 127       InputNodes.push_back(new FGPropertyValue( node ));
 128     } else {
 129       InputNodes.push_back(new FGPropertyValue( input,
 130                                                 PropertyManager ));
 131     }
 132     InputNames.push_back( input );
 133
 134     input_element = element->FindNextElement("input");
 135   }
 136
 137   Element *out_elem = element->FindElement("output");
 138   while (out_elem) {
 139     IsOutput = true;
 140     string output_node_name = out_elem->GetDataLine();
 141     FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
 142     OutputNodes.push_back(OutputNode);
 143     if (!OutputNode) {
 144       cerr << endl << "  Unable to process property: " << output_node_name << endl;
 145       throw(string("Invalid output property name in flight control definition"));
 146     }
 147     out_elem = element->FindNextElement("output");
 148   }
 149
 150   Element* delay_elem = element->FindElement("delay");
 151   if ( delay_elem ) {
 152     delay_time = delay_elem->GetDataAsNumber();
 153     string delayType = delay_elem->GetAttributeValue("type");
 154     if (delayType.length() > 0) {
 155       if (delayType == "time") {
 156         delay = (unsigned int)(delay_time / dt);
 157       } else if (delayType == "frames") {
 158         delay = (unsigned int)delay_time;
 159       } else {
 160         cerr << "Unallowed delay type" << endl;
 161       }
 162     } else {
 163       delay = (unsigned int)(delay_time / dt);
 164     }
 165     output_array.resize(delay);
 166     for (unsigned int i=0; i<delay; i++) output_array[i] = 0.0;
 167   }
 168
 169   clip_el = element->FindElement("clipto");
 170   if (clip_el) {
 171     clip_string = clip_el->FindElementValue("min");
 172     if (!is_number(clip_string)) { // it's a property
 173       if (clip_string[0] == '-') {
 174         clipMinSign = -1.0;
 175         clip_string.erase(0,1);
 176       }
 177       ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
 178     } else {
 179       clipmin = clip_el->FindElementValueAsNumber("min");
 180     }
 181     clip_string = clip_el->FindElementValue("max");
 182     if (!is_number(clip_string)) { // it's a property
 183       if (clip_string[0] == '-') {
 184         clipMaxSign = -1.0;
 185         clip_string.erase(0,1);
 186       }
 187       ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
 188     } else {
 189       clipmax = clip_el->FindElementValueAsNumber("max");
 190     }
 191     clip = true;
 192   }
 193
 194   Debug(0);
 195 }
 196
 197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 198
 199 FGFCSComponent::~FGFCSComponent()
 200 {
 201   Debug(1);
 202   for (unsigned int i=0; i<InputNodes.size(); i++) {
 203     delete InputNodes[i];
 204   }
 205 }
 206
 207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 208
 209 void FGFCSComponent::SetOutput(void)
 210 {
 211   for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
 212 }
 213
 214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 215
 216 bool FGFCSComponent::Run(void)
 217 {
 218   return true;
 219 }
 220
 221 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 222
 223 void FGFCSComponent::Delay(void)
 224 {
 225   output_array[index] = Output;
 226   if ((unsigned int)index == delay-1) index = 0;
 227   else index++;
 228   Output = output_array[index];
 229 }
 230
 231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 232
 233 void FGFCSComponent::Clip(void)
 234 {
 235   if (clip) {
 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;
 240   }
 241 }
 242
 243 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 244 //
 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.
 250
 251 void FGFCSComponent::bind(void)
 252 {
 253   string tmp;
 254   if (Name.find("/") == string::npos) {
 255     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
 256   } else {
 257     tmp = Name;
 258   }
 259   PropertyManager->Tie( tmp, this, &FGFCSComponent::GetOutput);
 260 }
 261
 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
 269 //       whatsoever.
 270 //    1: This value explicity requests the normal JSBSim
 271 //       startup messages
 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
 280
 281 void FGFCSComponent::Debug(int from)
 282 {
 283   string propsign="";
 284
 285   if (debug_lvl <= 0) return;
 286
 287   if (debug_lvl & 1) { // Standard console startup message output
 288     if (from == 0) {
 289       cout << endl << "    Loading Component \"" << Name
 290                    << "\" of type: " << Type << endl;
 291
 292       if (clip) {
 293         if (ClipMinPropertyNode != 0L) {
 294           if (clipMinSign < 0.0) propsign="-";
 295           cout << "      Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
 296           propsign="";
 297         } else {
 298           cout << "      Minimum limit: " << clipmin << endl;
 299         }
 300         if (ClipMaxPropertyNode != 0L) {
 301           if (clipMaxSign < 0.0) propsign="-";
 302           cout << "      Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
 303           propsign="";
 304         } else {
 305           cout << "      Maximum limit: " << clipmax << endl;
 306         }
 307       }
 308       if (delay > 0) cout <<"      Frame delay: " << delay
 309                                    << " frames (" << delay*dt << " sec)" << endl;
 310     }
 311   }
 312   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 313     if (from == 0) cout << "Instantiated: FGFCSComponent" << endl;
 314     if (from == 1) cout << "Destroyed:    FGFCSComponent" << endl;
 315   }
 316   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 317   }
 318   if (debug_lvl & 8 ) { // Runtime state variables
 319   }
 320   if (debug_lvl & 16) { // Sanity checking
 321   }
 322   if (debug_lvl & 64) {
 323     if (from == 0) { // Constructor
 324       cout << IdSrc << endl;
 325       cout << IdHdr << endl;
 326     }
 327   }
 328 }
 329 }