src/Instrumentation/mrg.cxx

   1 // MRG.cxx - an electrically powered master reference gyro.
   2 // Written by Vivian Meazza based on work by David Megginson, started 2006.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 // TODO:
   7 // - better spin-up
   8
   9 #include <simgear/compiler.h>
  10
  11 #include STL_IOSTREAM
  12 #include STL_STRING
  13 #include <sstream>
  14 #include <math.h>       // fabs()
  15
  16 #include <Main/fg_props.hxx>
  17 #include <Main/util.hxx>
  18
  19 #include "mrg.hxx"
  20
  21
  22 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
  23         _name(node->getStringValue("name", "master-reference-gyro")),
  24         _num(node->getIntValue("number", 0))
  25 {
  26 }
  27
  28 MasterReferenceGyro::~MasterReferenceGyro ()
  29 {
  30 }
  31
  32 void
  33 MasterReferenceGyro::init ()
  34 {
  35         _last_hdg = 0;
  36         _last_roll = 0;
  37         _last_pitch = 0;
  38         _indicated_hdg = 0;
  39         _indicated_roll = 0;
  40         _indicated_pitch = 0;
  41         _indicated_hdg_rate = 0;
  42         _indicated_roll_rate = 0;
  43         _indicated_pitch_rate = 0;
  44
  45         string branch;
  46         branch = "/instrumentation/" + _name;
  47
  48         _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
  49         _roll_in_node = fgGetNode("/orientation/roll-deg", true);
  50         _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
  51         _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
  52         _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
  53         _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
  54         _g_in_node =   fgGetNode("/accelerations/pilot-g-damped", true);
  55         _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
  56
  57         SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  58         _off_node = node->getChild("off-flag", 0, true);
  59         _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
  60         _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
  61         _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
  62         _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
  63         _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
  64         _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
  65         _responsiveness_node = node->getChild("responsiveness", 0, true);
  66         _error_out_node = node->getChild("heading-error-deg", 0, true);
  67
  68         _electrical_node->setDoubleValue(0);
  69         _responsiveness_node->setDoubleValue(0.75);
  70         _off_node->setBoolValue(false);
  71 }
  72
  73 void
  74 MasterReferenceGyro::bind ()
  75 {
  76         std::ostringstream temp;
  77         string branch;
  78         temp << _num;
  79         branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  80
  81         fgTie((branch + "/serviceable").c_str(),
  82                 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  83         fgTie((branch + "/spin").c_str(),
  84                 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  85 }
  86
  87 void
  88 MasterReferenceGyro::unbind ()
  89 {
  90         std::ostringstream temp;
  91         string branch;
  92         temp << _num;
  93         branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  94
  95         fgUntie((branch + "/serviceable").c_str());
  96         fgUntie((branch + "/spin").c_str());
  97 }
  98
  99 void
 100 MasterReferenceGyro::update (double dt)
 101 {
 102         double indicated_roll = 0;
 103         double indicated_pitch = 0;
 104         double indicated_hdg = 0;
 105         double indicated_roll_rate = 0;
 106         double indicated_pitch_rate = 0;
 107         double indicated_hdg_rate = 0;
 108
 109         // Get the spin from the gyro
 110         _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
 111         _gyro.update(dt);
 112         double spin = _gyro.get_spin_norm();
 113
 114         // set the "off-flag"
 115         if ( _electrical_node->getDoubleValue() == 0 ) {
 116                 _off_node->setBoolValue(true);
 117         } else if ( spin == 1 ) {
 118                 _off_node->setBoolValue(false);
 119         } else {
 120                 _off_node->setBoolValue(true);
 121         }
 122
 123         // Get the input values
 124         double roll = _roll_in_node->getDoubleValue();
 125         double pitch = _pitch_in_node->getDoubleValue();
 126         double hdg = _hdg_in_node->getDoubleValue();
 127         double roll_rate = _yaw_rate_node->getDoubleValue();
 128         double pitch_rate = _yaw_rate_node->getDoubleValue();
 129         double yaw_rate = _yaw_rate_node->getDoubleValue();
 130
 131         //modulate the input by the spin rate
 132         double responsiveness = spin * spin * spin * spin * spin * spin;
 133         roll = fgGetLowPass( _last_roll, roll, responsiveness );
 134         pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
 135         hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
 136
 137         //but we need to filter the hdg and yaw_rate as well - yuk!
 138         responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
 139         yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
 140
 141         // store the new values
 142         _last_roll = roll;
 143         _last_pitch = pitch;
 144         _last_hdg = hdg;
 145         _last_roll_rate = roll_rate;
 146         _last_pitch_rate = pitch_rate;
 147         _last_yaw_rate = yaw_rate;
 148
 149         //the gyro only erects inside limits
 150         if ( fabs ( yaw_rate ) <= 5
 151                         && (_g_in_node->getDoubleValue() <= 1.5
 152                         || _g_in_node->getDoubleValue() >= -0.5) ) {
 153                 indicated_roll = _last_roll;
 154                 indicated_pitch = _last_pitch;
 155                 indicated_hdg = _last_hdg;
 156                 indicated_roll_rate = _last_roll_rate;
 157                 indicated_pitch_rate = _last_pitch_rate;
 158                 indicated_hdg_rate = _last_yaw_rate;
 159         } else {
 160                 indicated_roll_rate = 0;
 161                 indicated_pitch_rate = 0;
 162                 indicated_hdg_rate = 0;
 163         }
 164
 165         // calculate the difference between the indicated heading
 166         // and the selected heading for use with an autopilot
 167         static SGPropertyNode *bnode
 168                 = fgGetNode( "autopilot/settings/target-heading-deg", false );
 169
 170         if ( bnode ) {
 171                 double diff = bnode->getDoubleValue() - indicated_hdg;
 172                 if ( diff < -180.0 ) { diff += 360.0; }
 173                 if ( diff > 180.0 ) { diff -= 360.0; }
 174                 _error_out_node->setDoubleValue( diff );
 175         }
 176         //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
 177         //smooth the output
 178         double factor = _responsiveness_node->getDoubleValue() * dt;
 179
 180         indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
 181         indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
 182         indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
 183
 184         indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
 185         indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
 186         indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
 187
 188         // store the new values
 189         _indicated_roll = indicated_roll;
 190         _indicated_pitch = indicated_pitch;
 191         _indicated_hdg = indicated_hdg;
 192
 193         _indicated_roll_rate = indicated_roll_rate;
 194         _indicated_pitch_rate = indicated_pitch_rate;
 195         _indicated_hdg_rate = indicated_hdg_rate;
 196
 197         // add in a gyro underspin "error" if gyro is spinning too slowly
 198         const double spin_thresh = 0.8;
 199         const double max_roll_error = 40.0;
 200         const double max_pitch_error = 12.0;
 201         const double max_hdg_error = 140.0;
 202         double roll_error;
 203         double pitch_error;
 204         double hdg_error;
 205
 206         if ( spin <= spin_thresh ) {
 207                 double roll_error_factor        = ( spin_thresh - spin ) / spin_thresh;
 208                 double pitch_error_factor       = ( spin_thresh - spin ) / spin_thresh;
 209                 double hdg_error_factor         = ( spin_thresh - spin ) / spin_thresh;
 210                 roll_error      = roll_error_factor * roll_error_factor * max_roll_error;
 211                 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
 212                 hdg_error       = hdg_error_factor * hdg_error_factor * max_hdg_error;
 213         } else {
 214                 roll_error = 0.0;
 215                 pitch_error = 0.0;
 216                 hdg_error = 0.0;
 217         }
 218
 219         _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
 220         _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
 221         _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
 222         _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
 223         _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
 224         _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
 225 }
 226
 227 // end of mrg.cxx