src/Autopilot/inputvalue.cxx

   1 // inputvalue.hxx - provide input to autopilot components
   2 //
   3 // Written by Torsten Dreyer
   4 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19 //
  20
  21 #include "inputvalue.hxx"
  22 #include <Main/fg_props.hxx>
  23 using namespace FGXMLAutopilot;
  24
  25 PeriodicalValue::PeriodicalValue( SGPropertyNode_ptr root )
  26 {
  27   SGPropertyNode_ptr minNode = root->getChild( "min" );
  28   SGPropertyNode_ptr maxNode = root->getChild( "max" );
  29   if( minNode == NULL || maxNode == NULL ) {
  30     SG_LOG(SG_AUTOPILOT, SG_ALERT, "periodical defined, but no <min> and/or <max> tag. Period ignored." );
  31   } else {
  32     minPeriod = new InputValue( minNode );
  33     maxPeriod = new InputValue( maxNode );
  34   }
  35 }
  36
  37 double PeriodicalValue::normalize( double value )
  38 {
  39   if( !(minPeriod && maxPeriod )) return value;
  40
  41   double p1 = minPeriod->get_value();
  42   double p2 = maxPeriod->get_value();
  43
  44   double min = std::min<double>(p1,p2);
  45   double max = std::max<double>(p1,p2);
  46   double phase = fabs(max - min);
  47
  48   if( phase > SGLimitsd::min() ) {
  49     while( value < min )  value += phase;
  50     while( value >= max ) value -= phase;
  51   } else {
  52     value = min; // phase is zero
  53   }
  54
  55   return value;
  56 }
  57
  58 InputValue::InputValue( SGPropertyNode_ptr node, double value, double offset, double scale) :
  59   _value(0.0),
  60   _abs(false)
  61 {
  62   parse( node, value, offset, scale );
  63 }
  64
  65
  66 void InputValue::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
  67 {
  68     _value = aValue;
  69     _property = NULL;
  70     _offset = NULL;
  71     _scale = NULL;
  72     _min = NULL;
  73     _max = NULL;
  74     _periodical = NULL;
  75
  76     if( node == NULL )
  77         return;
  78
  79     SGPropertyNode * n;
  80
  81     if( (n = node->getChild("condition")) != NULL ) {
  82         _condition = sgReadCondition(fgGetNode("/"), n);
  83     }
  84
  85     if( (n = node->getChild( "scale" )) != NULL ) {
  86         _scale = new InputValue( n, aScale );
  87     }
  88
  89     if( (n = node->getChild( "offset" )) != NULL ) {
  90         _offset = new InputValue( n, aOffset );
  91     }
  92
  93     if( (n = node->getChild( "max" )) != NULL ) {
  94         _max = new InputValue( n );
  95     }
  96
  97     if( (n = node->getChild( "min" )) != NULL ) {
  98         _min = new InputValue( n );
  99     }
 100
 101     if( (n = node->getChild( "abs" )) != NULL ) {
 102       _abs = n->getBoolValue();
 103     }
 104
 105     if( (n = node->getChild( "period" )) != NULL ) {
 106       _periodical = new PeriodicalValue( n );
 107     }
 108
 109     SGPropertyNode *valueNode = node->getChild( "value" );
 110     if ( valueNode != NULL ) {
 111         _value = valueNode->getDoubleValue();
 112     }
 113
 114     if ((n = node->getChild("expression")) != NULL) {
 115       _expression = SGReadDoubleExpression(fgGetNode("/"), n->getChild(0));
 116       return;
 117     }
 118
 119     n = node->getChild( "property" );
 120     // if no <property> element, check for <prop> element for backwards
 121     // compatibility
 122     if(  n == NULL )
 123         n = node->getChild( "prop" );
 124
 125     if (  n != NULL ) {
 126         _property = fgGetNode(  n->getStringValue(), true );
 127         if ( valueNode != NULL ) {
 128             // initialize property with given value
 129             // if both <prop> and <value> exist
 130             double s = get_scale();
 131             if( s != 0 )
 132               _property->setDoubleValue( (_value - get_offset())/s );
 133             else
 134               _property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
 135         }
 136
 137         return;
 138     } // of have a <property> or <prop>
 139
 140
 141     if (valueNode == NULL) {
 142         // no <value>, <prop> or <expression> element, use text node
 143         const char * textnode = node->getStringValue();
 144         char * endp = NULL;
 145         // try to convert to a double value. If the textnode does not start with a number
 146         // endp will point to the beginning of the string. We assume this should be
 147         // a property name
 148         _value = strtod( textnode, &endp );
 149         if( endp == textnode ) {
 150           _property = fgGetNode( textnode, true );
 151         }
 152     }
 153 }
 154
 155 void InputValue::set_value( double aValue )
 156 {
 157     if (!_property)
 158       return;
 159
 160     double s = get_scale();
 161     if( s != 0 )
 162         _property->setDoubleValue( (aValue - get_offset())/s );
 163     else
 164         _property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
 165 }
 166
 167 double InputValue::get_value() const
 168 {
 169     double value = _value;
 170
 171     if (_expression) {
 172         // compute the expression value
 173         value = _expression->getValue(NULL);
 174     } else if( _property != NULL ) {
 175         value = _property->getDoubleValue();
 176     }
 177
 178     if( _scale )
 179         value *= _scale->get_value();
 180
 181     if( _offset )
 182         value += _offset->get_value();
 183
 184     if( _min ) {
 185         double m = _min->get_value();
 186         if( value < m )
 187             value = m;
 188     }
 189
 190     if( _max ) {
 191         double m = _max->get_value();
 192         if( value > m )
 193             value = m;
 194     }
 195
 196     if( _periodical ) {
 197       value = _periodical->normalize( value );
 198     }
 199
 200     return _abs ? fabs(value) : value;
 201 }
 202