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