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 {
  18 }
  19
  20 AttitudeIndicator::~AttitudeIndicator ()
  21 {
  22 }
  23
  24 void
  25 AttitudeIndicator::init ()
  26 {
  27                                 // TODO: allow index of pump and AI
  28                                 // to be configured.
  29     _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
  30     _roll_in_node = fgGetNode("/orientation/roll-deg", true);
  31     _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
  32     _tumble_flag_node =
  33         fgGetNode("/instrumentation/attitude-indicator/config/tumble-flag",
  34                   true);
  35     _caged_node =
  36         fgGetNode("/instrumentation/attitude-indicator/caged-flag", true);
  37     _tumble_node =
  38         fgGetNode("/instrumentation/attitude-indicator/tumble-norm", true);
  39     _pitch_int_node =
  40         fgGetNode("/instrumentation/attitude-indicator/internal-pitch-deg",
  41                   true);
  42     _roll_int_node =
  43         fgGetNode("/instrumentation/attitude-indicator/internal-roll-deg",
  44                   true);
  45     _pitch_out_node =
  46         fgGetNode("/instrumentation/attitude-indicator/indicated-pitch-deg",
  47                   true);
  48     _roll_out_node =
  49         fgGetNode("/instrumentation/attitude-indicator/indicated-roll-deg",
  50                   true);
  51 }
  52
  53 void
  54 AttitudeIndicator::bind ()
  55 {
  56     fgTie("/instrumentation/attitude-indicator/serviceable",
  57           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  58     fgTie("/instrumentation/attitude-indicator/spin",
  59           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  60 }
  61
  62 void
  63 AttitudeIndicator::unbind ()
  64 {
  65     fgUntie("/instrumentation/attitude-indicator/serviceable");
  66     fgUntie("/instrumentation/attitude-indicator/spin");
  67 }
  68
  69 void
  70 AttitudeIndicator::update (double dt)
  71 {
  72                                 // If it's caged, it doesn't indicate
  73     if (_caged_node->getBoolValue()) {
  74         _roll_int_node->setDoubleValue(0.0);
  75         _pitch_int_node->setDoubleValue(0.0);
  76         return;
  77     }
  78
  79                                 // Get the spin from the gyro
  80     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  81     _gyro.update(dt);
  82     double spin = _gyro.get_spin_norm();
  83
  84                                 // Calculate the responsiveness
  85     double responsiveness = spin * spin * spin * spin * spin * spin;
  86
  87                                 // Get the indicated roll and pitch
  88     double roll = _roll_in_node->getDoubleValue();
  89     double pitch = _pitch_in_node->getDoubleValue();
  90
  91                                 // Calculate the tumble for the
  92                                 // next pass.
  93     if (_tumble_flag_node->getBoolValue()) {
  94         double tumble = _tumble_node->getDoubleValue();
  95         if (fabs(roll) > 45.0) {
  96             double target = (fabs(roll) - 45.0) / 45.0;
  97             target *= target;   // exponential past +-45 degrees
  98             if (roll < 0)
  99                 target = -target;
 100
 101             if (fabs(target) > fabs(tumble))
 102                 tumble = target;
 103
 104             if (tumble > 1.0)
 105                 tumble = 1.0;
 106             else if (tumble < -1.0)
 107                 tumble = -1.0;
 108         }
 109                                     // Reerect in 5 minutes
 110         double step = dt/300.0;
 111         if (tumble < -step)
 112             tumble += step;
 113         else if (tumble > step)
 114             tumble -= step;
 115
 116         roll += tumble * 45;
 117         _tumble_node->setDoubleValue(tumble);
 118     }
 119
 120     roll = fgGetLowPass(_roll_int_node->getDoubleValue(), roll,
 121                         responsiveness);
 122     pitch = fgGetLowPass(_pitch_int_node->getDoubleValue(), pitch,
 123                          responsiveness);
 124
 125                                 // Assign the new values
 126     _roll_int_node->setDoubleValue(roll);
 127     _pitch_int_node->setDoubleValue(pitch);
 128
 129     // add in a gyro underspin "error" if gyro is spinning too slowly
 130     const double spin_thresh = 0.4;
 131     const double max_roll_error = 40.0;
 132     const double max_pitch_error = 15.0;
 133     double roll_error;
 134     double pitch_error;
 135     if ( spin <= spin_thresh ) {
 136         double roll_error_factor = (spin_thresh - spin) / spin_thresh;
 137         double pitch_error_factor = (spin_thresh - spin) / spin_thresh;
 138         roll_error = roll_error_factor * roll_error_factor * max_roll_error;
 139         pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
 140     } else {
 141         roll_error = 0.0;
 142         pitch_error = 0.0;
 143     }
 144
 145     _roll_out_node->setDoubleValue(roll + roll_error);
 146     _pitch_out_node->setDoubleValue(pitch + pitch_error);
 147
 148 }
 149
 150 // end of attitude_indicator.cxx