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
  42 namespace JSBSim {
  43
  44 static const char *IdSrc = "$Id$";
  45 static const char *IdHdr = ID_FCSCOMPONENT;
  46
  47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  48 CLASS IMPLEMENTATION
  49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  50
  51 FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
  52 {
  53   Element *input_element, *clip_el;
  54   Input = Output = clipmin = clipmax = 0.0;
  55   treenode = 0;
  56   delay = index = 0;
  57   ClipMinPropertyNode = ClipMaxPropertyNode = 0;
  58   clipMinSign = clipMaxSign = 1.0;
  59   IsOutput   = clip = false;
  60   string input, clip_string;
  61   dt = fcs->GetDt();
  62
  63   PropertyManager = fcs->GetPropertyManager();
  64   if        (element->GetName() == string("lag_filter")) {
  65     Type = "LAG_FILTER";
  66   } else if (element->GetName() == string("lead_lag_filter")) {
  67     Type = "LEAD_LAG_FILTER";
  68   } else if (element->GetName() == string("washout_filter")) {
  69     Type = "WASHOUT_FILTER";
  70   } else if (element->GetName() == string("second_order_filter")) {
  71     Type = "SECOND_ORDER_FILTER";
  72   } else if (element->GetName() == string("integrator")) {
  73     Type = "INTEGRATOR";
  74   } else if (element->GetName() == string("summer")) {
  75     Type = "SUMMER";
  76   } else if (element->GetName() == string("pure_gain")) {
  77     Type = "PURE_GAIN";
  78   } else if (element->GetName() == string("scheduled_gain")) {
  79     Type = "SCHEDULED_GAIN";
  80   } else if (element->GetName() == string("aerosurface_scale")) {
  81     Type = "AEROSURFACE_SCALE";
  82   } else if (element->GetName() == string("switch")) {
  83     Type = "SWITCH";
  84   } else if (element->GetName() == string("kinematic")) {
  85     Type = "KINEMATIC";
  86   } else if (element->GetName() == string("deadband")) {
  87     Type = "DEADBAND";
  88   } else if (element->GetName() == string("fcs_function")) {
  89     Type = "FCS_FUNCTION";
  90   } else if (element->GetName() == string("pid")) {
  91     Type = "PID";
  92   } else if (element->GetName() == string("sensor")) {
  93     Type = "SENSOR";
  94   } else if (element->GetName() == string("accelerometer")) {
  95     Type = "ACCELEROMETER";
  96   } else if (element->GetName() == string("magnetometer")) {
  97     Type = "MAGNETOMETER";
  98   } else if (element->GetName() == string("gyro")) {
  99     Type = "GYRO";
 100   } else if (element->GetName() == string("actuator")) {
 101     Type = "ACTUATOR";
 102   } else { // illegal component in this channel
 103     Type = "UNKNOWN";
 104   }
 105
 106   Name = element->GetAttributeValue("name");
 107
 108   FGPropertyManager *tmp=0;
 109
 110   input_element = element->FindElement("input");
 111   while (input_element) {
 112     input = input_element->GetDataLine();
 113     if (input[0] == '-') {
 114       InputSigns.push_back(-1.0);
 115       input.erase(0,1);
 116     } else {
 117       InputSigns.push_back( 1.0);
 118     }
 119     tmp = PropertyManager->GetNode(input);
 120     if (tmp) {
 121       InputNodes.push_back( tmp );
 122     } else {
 123       cerr << fgred << "  In component: " << Name << " unknown property "
 124            << input << " referenced. Aborting" << reset << endl;
 125       exit(-1);
 126     }
 127     input_element = element->FindNextElement("input");
 128   }
 129
 130   Element *out_elem = element->FindElement("output");
 131   while (out_elem) {
 132     IsOutput = true;
 133     string output_node_name = out_elem->GetDataLine();
 134     FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
 135     OutputNodes.push_back(OutputNode);
 136     if (!OutputNode) {
 137       cerr << endl << "  Unable to process property: " << output_node_name << endl;
 138       throw(string("Invalid output property name in flight control definition"));
 139     }
 140     out_elem = element->FindNextElement("output");
 141   }
 142
 143   Element* delay_elem = element->FindElement("delay");
 144   if ( delay_elem ) {
 145     delay = (unsigned int)delay_elem->GetDataAsNumber();
 146     string delayType = delay_elem->GetAttributeValue("type");
 147     if (delayType.length() > 0) {
 148       if (delayType == "time") {
 149         delay = (int)(delay / dt);
 150       } else {
 151         cerr << "Unallowed delay type" << endl;
 152       }
 153     } else {
 154       delay = (int)(delay / dt);
 155     }
 156     output_array.resize(delay);
 157     for (int i=0; i<delay; i++) output_array[i] = 0.0;
 158   }
 159
 160   clip_el = element->FindElement("clipto");
 161   if (clip_el) {
 162     clip_string = clip_el->FindElementValue("min");
 163     if (!is_number(clip_string)) { // it's a property
 164       if (clip_string[0] == '-') {
 165         clipMinSign = -1.0;
 166         clip_string.erase(0,1);
 167       }
 168       ClipMinPropertyNode = PropertyManager->GetNode( clip_string );
 169     } else {
 170       clipmin = clip_el->FindElementValueAsNumber("min");
 171     }
 172     clip_string = clip_el->FindElementValue("max");
 173     if (!is_number(clip_string)) { // it's a property
 174       if (clip_string[0] == '-') {
 175         clipMaxSign = -1.0;
 176         clip_string.erase(0,1);
 177       }
 178       ClipMaxPropertyNode = PropertyManager->GetNode( clip_string );
 179     } else {
 180       clipmax = clip_el->FindElementValueAsNumber("max");
 181     }
 182     clip = true;
 183   }
 184
 185   Debug(0);
 186 }
 187
 188 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 189
 190 FGFCSComponent::~FGFCSComponent()
 191 {
 192   Debug(1);
 193 }
 194
 195 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 196
 197 void FGFCSComponent::SetOutput(void)
 198 {
 199   for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
 200 }
 201
 202 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 203
 204 bool FGFCSComponent::Run(void)
 205 {
 206   return true;
 207 }
 208
 209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 210
 211 void FGFCSComponent::Delay(void)
 212 {
 213   output_array[index] = Output;
 214   if (index == delay-1) index = 0;
 215   else index++;
 216   Output = output_array[index];
 217 }
 218
 219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 220
 221 void FGFCSComponent::Clip(void)
 222 {
 223   if (clip) {
 224     if (ClipMinPropertyNode != 0) clipmin = clipMinSign*ClipMinPropertyNode->getDoubleValue();
 225     if (ClipMaxPropertyNode != 0) clipmax = clipMaxSign*ClipMaxPropertyNode->getDoubleValue();
 226     if (Output > clipmax)      Output = clipmax;
 227     else if (Output < clipmin) Output = clipmin;
 228   }
 229 }
 230
 231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 232 //
 233 // The old way of naming FCS components allowed upper or lower case, spaces, etc.
 234 // but then the names were modified to fit into a property name heirarchy. This
 235 // was confusing (it wasn't done intentionally - it was a carryover from the early
 236 // design). We now support the direct naming of properties in the FCS component
 237 // name attribute. The old way is supported in code at this time, but deprecated.
 238
 239 void FGFCSComponent::bind(void)
 240 {
 241   string tmp;
 242   if (Name.find("/") == string::npos) {
 243     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
 244   } else {
 245     tmp = Name;
 246   }
 247   PropertyManager->Tie( tmp, this, &FGFCSComponent::GetOutput);
 248 }
 249
 250 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 251 //    The bitmasked value choices are as follows:
 252 //    unset: In this case (the default) JSBSim would only print
 253 //       out the normally expected messages, essentially echoing
 254 //       the config files as they are read. If the environment
 255 //       variable is not set, debug_lvl is set to 1 internally
 256 //    0: This requests JSBSim not to output any messages
 257 //       whatsoever.
 258 //    1: This value explicity requests the normal JSBSim
 259 //       startup messages
 260 //    2: This value asks for a message to be printed out when
 261 //       a class is instantiated
 262 //    4: When this value is set, a message is displayed when a
 263 //       FGModel object executes its Run() method
 264 //    8: When this value is set, various runtime state variables
 265 //       are printed out periodically
 266 //    16: When set various parameters are sanity checked and
 267 //       a message is printed out when they go out of bounds
 268
 269 void FGFCSComponent::Debug(int from)
 270 {
 271   string propsign="";
 272
 273   if (debug_lvl <= 0) return;
 274
 275   if (debug_lvl & 1) { // Standard console startup message output
 276     if (from == 0) {
 277       cout << endl << "    Loading Component \"" << Name
 278                    << "\" of type: " << Type << endl;
 279
 280       if (clip) {
 281         if (ClipMinPropertyNode != 0L) {
 282           if (clipMinSign < 0.0) propsign="-";
 283           cout << "      Minimum limit: " << propsign << ClipMinPropertyNode->GetName() << endl;
 284           propsign="";
 285         } else {
 286           cout << "      Minimum limit: " << clipmin << endl;
 287         }
 288         if (ClipMaxPropertyNode != 0L) {
 289           if (clipMaxSign < 0.0) propsign="-";
 290           cout << "      Maximum limit: " << propsign << ClipMaxPropertyNode->GetName() << endl;
 291           propsign="";
 292         } else {
 293           cout << "      Maximum limit: " << clipmax << endl;
 294         }
 295       }
 296       if (delay > 0) cout <<"      Frame delay: " << delay
 297                                    << " frames (" << delay*dt << " sec)" << endl;
 298     }
 299   }
 300   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 301     if (from == 0) cout << "Instantiated: FGFCSComponent" << endl;
 302     if (from == 1) cout << "Destroyed:    FGFCSComponent" << endl;
 303   }
 304   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 305   }
 306   if (debug_lvl & 8 ) { // Runtime state variables
 307   }
 308   if (debug_lvl & 16) { // Sanity checking
 309   }
 310   if (debug_lvl & 64) {
 311     if (from == 0) { // Constructor
 312       cout << IdSrc << endl;
 313       cout << IdHdr << endl;
 314     }
 315   }
 316 }
 317 }