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_AUTOPILOT, SG_WARN, "Error in turn-indicator config logic" );
  32             if ( name.length() ) {
  33                 SG_LOG( SG_AUTOPILOT, 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     //_serviceable_node->setBoolValue(true);
  62
  63 }
  64
  65 void
  66 TurnIndicator::bind ()
  67 {
  68     string branch;
  69     branch = "/instrumentation/" + name + "/serviceable";
  70     fgTie(branch.c_str(),
  71           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  72     branch = "/instrumentation/" + name + "/spin";
  73     fgTie(branch.c_str(),
  74           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  75 }
  76
  77 void
  78 TurnIndicator::unbind ()
  79 {
  80     string branch;
  81     branch = "/instrumentation/" + name + "/serviceable";
  82     fgUntie(branch.c_str());
  83     branch = "/instrumentation/" + name + "/spin";
  84     fgUntie(branch.c_str());
  85 }
  86
  87 void
  88 TurnIndicator::update (double dt)
  89 {
  90                                 // Get the spin from the gyro
  91     _gyro.set_power_norm(_electric_current_node->getDoubleValue()/60.0);
  92     _gyro.update(dt);
  93     double spin = _gyro.get_spin_norm();
  94
  95                                 // Calculate the indicated rate
  96     double factor = 1.0 - ((1.0 - spin) * (1.0 - spin) * (1.0 - spin));
  97     double rate = ((_roll_rate_node->getDoubleValue() / 20.0) +
  98                    (_yaw_rate_node->getDoubleValue() / 3.0));
  99
 100                                 // Clamp the rate
 101     if (rate < -2.5)
 102         rate = -2.5;
 103     else if (rate > 2.5)
 104         rate = 2.5;
 105
 106                                 // Lag left, based on gyro spin
 107     rate = -2.5 + (factor * (rate + 2.5));
 108     rate = fgGetLowPass(_last_rate, rate, dt*RESPONSIVENESS);
 109     _last_rate = rate;
 110
 111                                 // Publish the indicated rate
 112     _rate_out_node->setDoubleValue(rate);
 113 }
 114
 115 // end of turn_indicator.cxx