src/Instrumentation/turn_indicator.cxx

   1 // turn_indicator.cxx - an electric-powered turn indicator.
   2 // Written by David Megginson, started 2003.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #include <simgear/compiler.h>
   7 #include STL_IOSTREAM
   8 #include STL_STRING
   9 #include <sstream>
  10
  11 #include "turn_indicator.hxx"
  12 #include <Main/fg_props.hxx>
  13 #include <Main/util.hxx>
  14
  15
  16 // Use a bigger number to be more responsive, or a smaller number
  17 // to be more sluggish.
  18 #define RESPONSIVENESS 0.5
  19
  20
  21 TurnIndicator::TurnIndicator ( SGPropertyNode *node) :
  22     _last_rate(0),
  23     name("turn-indicator"),
  24     num(0)
  25 {
  26     int i;
  27     for ( i = 0; i < node->nChildren(); ++i ) {
  28         SGPropertyNode *child = node->getChild(i);
  29         string cname = child->getName();
  30         string cval = child->getStringValue();
  31         if ( cname == "name" ) {
  32             name = cval;
  33         } else if ( cname == "number" ) {
  34             num = child->getIntValue();
  35         } else {
  36             SG_LOG( SG_INSTR, SG_WARN, "Error in turn-indicator config logic" );
  37             if ( name.length() ) {
  38                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  39             }
  40         }
  41     }
  42 }
  43
  44 TurnIndicator::TurnIndicator () :
  45     _last_rate(0)
  46 {
  47 }
  48
  49 TurnIndicator::~TurnIndicator ()
  50 {
  51 }
  52
  53 void
  54 TurnIndicator::init ()
  55 {
  56     string branch;
  57     branch = "/instrumentation/" + name;
  58
  59     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  60     _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
  61     _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
  62     _electric_current_node =
  63         fgGetNode("/systems/electrical/outputs/turn-coordinator", true);
  64     _rate_out_node = node->getChild("indicated-turn-rate", 0, true);
  65 }
  66
  67 void
  68 TurnIndicator::bind ()
  69 {
  70     std::ostringstream temp;
  71     string branch;
  72     temp << num;
  73     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  74
  75     fgTie((branch + "/serviceable").c_str(),
  76           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  77     fgTie((branch + "/spin").c_str(),
  78           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  79 }
  80
  81 void
  82 TurnIndicator::unbind ()
  83 {
  84     std::ostringstream temp;
  85     string branch;
  86     temp << num;
  87     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  88
  89     fgUntie((branch + "/serviceable").c_str());
  90     fgUntie((branch + "/serviceable").c_str());
  91 }
  92
  93 void
  94 TurnIndicator::update (double dt)
  95 {
  96                                 // Get the spin from the gyro
  97     double power = _electric_current_node->getDoubleValue() / 12.0;
  98     _gyro.set_power_norm(power);
  99     _gyro.update(dt);
 100     double spin = _gyro.get_spin_norm();
 101
 102                                 // Calculate the indicated rate
 103     double factor = 1.0 - ((1.0 - spin) * (1.0 - spin) * (1.0 - spin));
 104     double rate = ((_roll_rate_node->getDoubleValue() / 20.0) +
 105                    (_yaw_rate_node->getDoubleValue() / 3.0));
 106
 107                                 // Clamp the rate
 108     if (rate < -2.5)
 109         rate = -2.5;
 110     else if (rate > 2.5)
 111         rate = 2.5;
 112
 113                                 // Lag left, based on gyro spin
 114     rate = -2.5 + (factor * (rate + 2.5));
 115     rate = fgGetLowPass(_last_rate, rate, dt*RESPONSIVENESS);
 116     _last_rate = rate;
 117
 118                                 // Publish the indicated rate
 119     _rate_out_node->setDoubleValue(rate);
 120 }
 121
 122 // end of turn_indicator.cxx