src/Autopilot/predictor.cxx

   1 // predictor.cxx - predict future values
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23
  24 #include "predictor.hxx"
  25
  26 using namespace FGXMLAutopilot;
  27
  28 //------------------------------------------------------------------------------
  29 Predictor::Predictor () :
  30   AnalogComponent(),
  31   _last_value(0),
  32   _average(0)
  33 {
  34 }
  35
  36 //------------------------------------------------------------------------------
  37 bool Predictor::configure( SGPropertyNode& cfg_node,
  38                            const std::string& cfg_name,
  39                            SGPropertyNode& prop_root )
  40 {
  41   if( cfg_name == "config" ) {
  42     Component::configure(prop_root, cfg_node);
  43     return true;
  44   }
  45
  46   if (cfg_name == "seconds") {
  47     _seconds.push_back( new InputValue(prop_root, cfg_node, 0) );
  48     return true;
  49   }
  50
  51   if (cfg_name == "filter-gain") {
  52     _filter_gain.push_back( new InputValue(prop_root, cfg_node, 0) );
  53     return true;
  54   }
  55
  56   return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
  57 }
  58
  59 //------------------------------------------------------------------------------
  60 void Predictor::update( bool firstTime, double dt )
  61 {
  62     double ivalue = _valueInput.get_value();
  63
  64     if ( firstTime ) {
  65         _last_value = ivalue;
  66     }
  67
  68     double current = (ivalue - _last_value)/dt; // calculate current error change (per second)
  69     _average = dt < 1.0 ? ((1.0 - dt) * _average + current * dt) : current;
  70
  71     // calculate output with filter gain adjustment
  72     double output = ivalue +
  73            (1.0 - _filter_gain.get_value()) * (_average * _seconds.get_value()) +
  74            _filter_gain.get_value() * (current * _seconds.get_value());
  75     output = clamp( output );
  76     set_output_value( output );
  77
  78     _last_value = ivalue;
  79 }