src/Instrumentation/attitude_indicator.cxx

   1 // attitude_indicator.cxx - a vacuum-powered attitude indicator.
   2 // Written by David Megginson, started 2002.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 // TODO:
   7 // - better spin-up
   8
   9 #include "attitude_indicator.hxx"
  10 #include <Main/fg_props.hxx>
  11 #include <Main/util.hxx>
  12
  13
  14 AttitudeIndicator::AttitudeIndicator ()
  15     : _tumble(0)
  16 {
  17 }
  18
  19 AttitudeIndicator::~AttitudeIndicator ()
  20 {
  21 }
  22
  23 void
  24 AttitudeIndicator::init ()
  25 {
  26                                 // TODO: allow index of pump and AI
  27                                 // to be configured.
  28     _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
  29     _roll_in_node = fgGetNode("/orientation/roll-deg", true);
  30     _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
  31     _tumble_flag_node =
  32         fgGetNode("/instrumentation/attitude-indicator/config/tumble-flag",
  33                   true);
  34     _pitch_out_node =
  35         fgGetNode("/instrumentation/attitude-indicator/indicated-pitch-deg",
  36                   true);
  37     _roll_out_node =
  38         fgGetNode("/instrumentation/attitude-indicator/indicated-roll-deg",
  39                   true);
  40 }
  41
  42 void
  43 AttitudeIndicator::bind ()
  44 {
  45     fgTie("/instrumentation/attitude-indicator/serviceable",
  46           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  47     fgTie("/instrumentation/attitude-indicator/spin",
  48           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  49 }
  50
  51 void
  52 AttitudeIndicator::unbind ()
  53 {
  54     fgUntie("/instrumentation/attitude-indicator/serviceable");
  55     fgUntie("/instrumentation/attitude-indicator/spin");
  56 }
  57
  58 void
  59 AttitudeIndicator::update (double dt)
  60 {
  61                                 // Get the spin from the gyro
  62     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  63     _gyro.update(dt);
  64     double spin = _gyro.get_spin_norm();
  65
  66                                 // Calculate the responsiveness
  67     double responsiveness = spin * spin * spin * spin * spin * spin;
  68
  69                                 // Get the indicated roll and pitch
  70     double roll = _roll_in_node->getDoubleValue();
  71     double pitch = _pitch_in_node->getDoubleValue();
  72
  73                                 // Calculate the tumble for the
  74                                 // next pass.
  75     if (_tumble_flag_node->getBoolValue()) {
  76         if (_tumble < 1.0) {
  77             if (fabs(roll) > 45.0) {
  78                 double target = (fabs(roll) - 45.0) / 45.0;
  79                 if (_tumble < target)
  80                     _tumble = target;
  81             }
  82             if (fabs(pitch) > 45.0) {
  83                 double target = (fabs(pitch) - 45.0) / 45.0;
  84                 if (_tumble < target)
  85                     _tumble = target;
  86             }
  87             if (_tumble > 1.0) {
  88                 _tumble = 1.0;
  89             }
  90         }
  91                                 // Reerect in 5 minutes
  92         _tumble -= dt/300.0;
  93         if (_tumble < 0.0)
  94             _tumble = 0.0;
  95
  96         responsiveness *= ((1.0 - _tumble) * (1.0 - _tumble) *
  97                            (1.0 - _tumble) * (1.0 - _tumble));
  98
  99     }
 100
 101     roll = fgGetLowPass(_roll_out_node->getDoubleValue(), roll,
 102                         responsiveness);
 103     pitch = fgGetLowPass(_pitch_out_node->getDoubleValue(), pitch,
 104                          responsiveness);
 105
 106                                 // Assign the new values
 107     _roll_out_node->setDoubleValue(roll);
 108     _pitch_out_node->setDoubleValue(pitch);
 109 }
 110
 111 // end of attitude_indicator.cxx