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 (the base time is 1.0).
  13 #define RESPONSIVENESS 0.25
  14
  15
  16 TurnIndicator::TurnIndicator () :
  17     _last_rate(0)
  18 {
  19 }
  20
  21 TurnIndicator::~TurnIndicator ()
  22 {
  23 }
  24
  25 void
  26 TurnIndicator::init ()
  27 {
  28     _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
  29     _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
  30     _electric_current_node =
  31         fgGetNode("/systems/electrical/outputs/turn-coordinator", true);
  32     _rate_out_node =
  33         fgGetNode("/instrumentation/turn-indicator/indicated-turn-rate", true);
  34 }
  35
  36 void
  37 TurnIndicator::bind ()
  38 {
  39     fgTie("/instrumentation/turn-indicator/serviceable",
  40           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  41     fgTie("/instrumentation/turn-indicator/spin",
  42           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  43 }
  44
  45 void
  46 TurnIndicator::unbind ()
  47 {
  48     fgUntie("/instrumentation/turn-indicator/serviceable");
  49     fgUntie("/instrumentation/turn-indicator/spin");
  50 }
  51
  52 void
  53 TurnIndicator::update (double dt)
  54 {
  55                                 // Get the spin from the gyro
  56     _gyro.set_power_norm(_electric_current_node->getDoubleValue()/60.0);
  57     _gyro.update(dt);
  58     double spin = _gyro.get_spin_norm();
  59
  60                                 // Calculate the indicated rate
  61     double factor = 1.0 - ((1.0 - spin) * (1.0 - spin) * (1.0 - spin));
  62     double rate = ((_roll_rate_node->getDoubleValue() / 20.0) +
  63                    (_yaw_rate_node->getDoubleValue() / 3.0));
  64
  65                                 // Clamp the rate
  66     if (rate < -2.5)
  67         rate = -2.5;
  68     else if (rate > 2.5)
  69         rate = 2.5;
  70
  71                                 // Lag left, based on gyro spin
  72     rate = -2.5 + (factor * (rate + 2.5));
  73     rate = fgGetLowPass(_last_rate, rate, dt*RESPONSIVENESS);
  74     _last_rate = rate;
  75
  76                                 // Publish the indicated rate
  77     _rate_out_node->setDoubleValue(rate);
  78 }
  79
  80 // end of turn_indicator.cxx