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   SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << cfg_name << ")");
  42
  43   if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
  44     return true;
  45
  46   if( cfg_name == "config" ) {
  47     Component::configure(prop_root, cfg_node);
  48     return true;
  49   }
  50
  51   if (cfg_name == "seconds") {
  52     _seconds.push_back( new InputValue(prop_root, cfg_node, 0) );
  53     return true;
  54   }
  55
  56   if (cfg_name == "filter-gain") {
  57     _filter_gain.push_back( new InputValue(prop_root, cfg_node, 0) );
  58     return true;
  59   }
  60
  61   SG_LOG
  62   (
  63     SG_AUTOPILOT,
  64     SG_BULK,
  65     "Predictor::configure(" << cfg_name << ") [unhandled]"
  66   );
  67   return false;
  68 }
  69
  70 //------------------------------------------------------------------------------
  71 void Predictor::update( bool firstTime, double dt )
  72 {
  73     double ivalue = _valueInput.get_value();
  74
  75     if ( firstTime ) {
  76         _last_value = ivalue;
  77     }
  78
  79     double current = (ivalue - _last_value)/dt; // calculate current error change (per second)
  80     _average = dt < 1.0 ? ((1.0 - dt) * _average + current * dt) : current;
  81
  82     // calculate output with filter gain adjustment
  83     double output = ivalue +
  84            (1.0 - _filter_gain.get_value()) * (_average * _seconds.get_value()) +
  85            _filter_gain.get_value() * (current * _seconds.get_value());
  86     output = clamp( output );
  87     set_output_value( output );
  88
  89     _last_value = ivalue;
  90 }