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