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 #ifdef HAVE_CONFIG_H
  10 #  include "config.h"
  11 #endif
  12
  13 #include <simgear/compiler.h>
  14
  15 #include <iostream>
  16 #include <string>
  17 #include <sstream>
  18
  19 #include <math.h>       // fabs()
  20
  21 #include "attitude_indicator.hxx"
  22 #include <Main/fg_props.hxx>
  23 #include <Main/util.hxx>
  24
  25 using std::string;
  26
  27 AttitudeIndicator::AttitudeIndicator ( SGPropertyNode *node )
  28     :
  29     _name(node->getStringValue("name", "attitude-indicator")),
  30     _num(node->getIntValue("number", 0)),
  31     _suction(node->getStringValue("suction", "/systems/vacuum/suction-inhg")),
  32     spin_thresh(0.8),
  33     max_roll_error(40.0),
  34     max_pitch_error(12.0)
  35 {
  36 }
  37
  38 AttitudeIndicator::~AttitudeIndicator ()
  39 {
  40 }
  41
  42 void
  43 AttitudeIndicator::init ()
  44 {
  45     string branch;
  46     branch = "/instrumentation/" + _name;
  47
  48     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  49     SGPropertyNode *n;
  50
  51     _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
  52     _roll_in_node = fgGetNode("/orientation/roll-deg", true);
  53     _suction_node = fgGetNode(_suction.c_str(), true);
  54     SGPropertyNode *cnode = node->getChild("config", 0, true);
  55     _tumble_flag_node = cnode->getChild("tumble-flag", 0, true);
  56     _caged_node = node->getChild("caged-flag", 0, true);
  57     _tumble_node = node->getChild("tumble-norm", 0, true);
  58     if( ( n = cnode->getChild("spin-thresh", 0, false ) ) != NULL )
  59       spin_thresh = n->getDoubleValue();
  60     if( ( n = cnode->getChild("max-roll-error-deg", 0, false ) ) != NULL )
  61       max_roll_error = n->getDoubleValue();
  62     if( ( n = cnode->getChild("max-pitch-error-deg", 0, false ) ) != NULL )
  63       max_pitch_error = n->getDoubleValue();
  64     _pitch_int_node = node->getChild("internal-pitch-deg", 0, true);
  65     _roll_int_node = node->getChild("internal-roll-deg", 0, true);
  66     _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
  67     _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
  68
  69     reinit();
  70 }
  71
  72 void
  73 AttitudeIndicator::reinit ()
  74 {
  75     _roll_int_node->setDoubleValue(0.0);
  76     _pitch_int_node->setDoubleValue(0.0);
  77     _gyro.reinit();
  78 }
  79
  80 void
  81 AttitudeIndicator::bind ()
  82 {
  83     std::ostringstream temp;
  84     string branch;
  85     temp << _num;
  86     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  87
  88     fgTie((branch + "/serviceable").c_str(),
  89           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  90     fgTie((branch + "/spin").c_str(),
  91           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  92 }
  93
  94 void
  95 AttitudeIndicator::unbind ()
  96 {
  97     std::ostringstream temp;
  98     string branch;
  99     temp << _num;
 100     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
 101
 102     fgUntie((branch + "/serviceable").c_str());
 103     fgUntie((branch + "/spin").c_str());
 104 }
 105
 106 void
 107 AttitudeIndicator::update (double dt)
 108 {
 109                                 // If it's caged, it doesn't indicate
 110     if (_caged_node->getBoolValue()) {
 111         _roll_int_node->setDoubleValue(0.0);
 112         _pitch_int_node->setDoubleValue(0.0);
 113         return;
 114     }
 115
 116                                 // Get the spin from the gyro
 117     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
 118     _gyro.update(dt);
 119     double spin = _gyro.get_spin_norm();
 120
 121                                 // Calculate the responsiveness
 122     double responsiveness = spin * spin * spin * spin * spin * spin;
 123
 124                                 // Get the indicated roll and pitch
 125     double roll = _roll_in_node->getDoubleValue();
 126     double pitch = _pitch_in_node->getDoubleValue();
 127
 128                                 // Calculate the tumble for the
 129                                 // next pass.
 130     if (_tumble_flag_node->getBoolValue()) {
 131         double tumble = _tumble_node->getDoubleValue();
 132         if (fabs(roll) > 45.0) {
 133             double target = (fabs(roll) - 45.0) / 45.0;
 134             target *= target;   // exponential past +-45 degrees
 135             if (roll < 0)
 136                 target = -target;
 137
 138             if (fabs(target) > fabs(tumble))
 139                 tumble = target;
 140
 141             if (tumble > 1.0)
 142                 tumble = 1.0;
 143             else if (tumble < -1.0)
 144                 tumble = -1.0;
 145         }
 146                                     // Reerect in 5 minutes
 147         double step = dt/300.0;
 148         if (tumble < -step)
 149             tumble += step;
 150         else if (tumble > step)
 151             tumble -= step;
 152
 153         roll += tumble * 45;
 154         _tumble_node->setDoubleValue(tumble);
 155     }
 156
 157     roll = fgGetLowPass(_roll_int_node->getDoubleValue(), roll,
 158                         responsiveness);
 159     pitch = fgGetLowPass(_pitch_int_node->getDoubleValue(), pitch,
 160                          responsiveness);
 161
 162                                 // Assign the new values
 163     _roll_int_node->setDoubleValue(roll);
 164     _pitch_int_node->setDoubleValue(pitch);
 165
 166     // add in a gyro underspin "error" if gyro is spinning too slowly
 167     double roll_error;
 168     double pitch_error;
 169     if ( spin <= spin_thresh ) {
 170         double roll_error_factor = (spin_thresh - spin) / spin_thresh;
 171         double pitch_error_factor = (spin_thresh - spin) / spin_thresh;
 172         roll_error = roll_error_factor * roll_error_factor * max_roll_error;
 173         pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
 174     } else {
 175         roll_error = 0.0;
 176         pitch_error = 0.0;
 177     }
 178
 179     _roll_out_node->setDoubleValue(roll + roll_error);
 180     _pitch_out_node->setDoubleValue(pitch + pitch_error);
 181 }
 182
 183 // end of attitude_indicator.cxx