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 <iostream>
   8 #include <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(node->getStringValue("name", "turn-indicator")),
  24     _num(node->getIntValue("number", 0))
  25 {
  26 }
  27
  28 TurnIndicator::~TurnIndicator ()
  29 {
  30 }
  31
  32 void
  33 TurnIndicator::init ()
  34 {
  35     string branch;
  36     branch = "/instrumentation/" + _name;
  37
  38     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  39     _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
  40     _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
  41     _electric_current_node =
  42         fgGetNode("/systems/electrical/outputs/turn-coordinator", true);
  43     _rate_out_node = node->getChild("indicated-turn-rate", 0, true);
  44 }
  45
  46 void
  47 TurnIndicator::bind ()
  48 {
  49     std::ostringstream temp;
  50     string branch;
  51     temp << _num;
  52     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  53
  54     fgTie((branch + "/serviceable").c_str(),
  55           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  56     fgTie((branch + "/spin").c_str(),
  57           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  58 }
  59
  60 void
  61 TurnIndicator::unbind ()
  62 {
  63     std::ostringstream temp;
  64     string branch;
  65     temp << _num;
  66     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  67
  68     fgUntie((branch + "/serviceable").c_str());
  69     fgUntie((branch + "/serviceable").c_str());
  70 }
  71
  72 void
  73 TurnIndicator::update (double dt)
  74 {
  75                                 // Get the spin from the gyro
  76     double power = _electric_current_node->getDoubleValue() / 12.0;
  77     _gyro.set_power_norm(power);
  78     _gyro.update(dt);
  79     double spin = _gyro.get_spin_norm();
  80
  81                                 // Calculate the indicated rate
  82     double factor = 1.0 - ((1.0 - spin) * (1.0 - spin) * (1.0 - spin));
  83     double rate = ((_roll_rate_node->getDoubleValue() / 20.0) +
  84                    (_yaw_rate_node->getDoubleValue() / 3.0));
  85
  86                                 // Clamp the rate
  87     if (rate < -2.5)
  88         rate = -2.5;
  89     else if (rate > 2.5)
  90         rate = 2.5;
  91
  92                                 // Lag left, based on gyro spin
  93     rate = -2.5 + (factor * (rate + 2.5));
  94     rate = fgGetLowPass(_last_rate, rate, dt*RESPONSIVENESS);
  95     _last_rate = rate;
  96
  97                                 // Publish the indicated rate
  98     _rate_out_node->setDoubleValue(rate);
  99 }
 100
 101 // end of turn_indicator.cxx