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