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