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 #ifdef HAVE_CONFIG_H
  10 #  include "config.h"
  11 #endif
  12
  13 #include <simgear/compiler.h>
  14 #include <simgear/sg_inlines.h>
  15 #include <simgear/math/SGMath.hxx>
  16
  17 #include <iostream>
  18 #include <string>
  19 #include <sstream>
  20 #include <math.h>       // fabs()
  21
  22 #include <Main/fg_props.hxx>
  23 #include <Main/util.hxx>
  24
  25 #include "mrg.hxx"
  26
  27 const double MasterReferenceGyro::gravity = -32.1740485564;
  28
  29 using std::string;
  30
  31 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
  32 _name(node->getStringValue("name", "master-reference-gyro")),
  33 _num(node->getIntValue("number", 0))
  34 {
  35 }
  36
  37 MasterReferenceGyro::~MasterReferenceGyro ()
  38 {
  39 }
  40
  41 void
  42 MasterReferenceGyro::init ()
  43 {
  44     string branch;
  45     branch = "/instrumentation/" + _name;
  46
  47     _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
  48     _roll_in_node = fgGetNode("/orientation/roll-deg", true);
  49     _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
  50     _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-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/z-accel-fps_sec", true);
  55     _g_in_node =   fgGetNode("/accelerations/pilot-g", true);
  56     _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
  57     _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
  58
  59     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  60     _off_node = node->getChild("off-flag", 0, true);
  61     _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
  62     _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
  63     _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
  64     _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
  65     _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
  66     _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
  67     _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
  68     _responsiveness_node = node->getChild("responsiveness", 0, true);
  69     _error_out_node = node->getChild("heading-bug-error-deg", 0, true);
  70     _hdg_input_source_node = node->getChild("heading-source", 0, true);
  71     _fast_erect_node = node->getChild("fast-erect", 0, true);
  72
  73     reinit();
  74 }
  75
  76 void
  77 MasterReferenceGyro::reinit ()
  78 {
  79     _last_hdg = 0;
  80     _last_roll = 0;
  81     _last_pitch = 0;
  82     _indicated_hdg = 0;
  83     _indicated_roll = 0;
  84     _indicated_pitch = 0;
  85     _indicated_hdg_rate = 0;
  86     _indicated_roll_rate = 0;
  87     _indicated_pitch_rate = 0;
  88     _erect_time = 180;
  89     _last_g = 1;
  90     _g_error = 10;
  91
  92     _electrical_node->setDoubleValue(0);
  93     _responsiveness_node->setDoubleValue(0.75);
  94     _off_node->setBoolValue(false);
  95     _hdg_input_source_node->setBoolValue(false);
  96     _fast_erect_node->setBoolValue(false);
  97     _g_in_node->setDoubleValue(1);
  98     _gyro.reinit();
  99 }
 100
 101 void
 102 MasterReferenceGyro::bind ()
 103 {
 104     std::ostringstream temp;
 105     string branch;
 106     temp << _num;
 107     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
 108
 109     fgTie((branch + "/serviceable").c_str(),
 110         &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
 111     fgTie((branch + "/spin").c_str(),
 112         &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
 113 }
 114
 115 void
 116 MasterReferenceGyro::unbind ()
 117 {
 118     std::ostringstream temp;
 119     string branch;
 120     temp << _num;
 121     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
 122
 123     fgUntie((branch + "/serviceable").c_str());
 124     fgUntie((branch + "/spin").c_str());
 125 }
 126
 127 void
 128 MasterReferenceGyro::update (double dt)
 129 {
 130     //sanity check
 131     if (!fgGetBool("/sim/fdm-initialized", false)) {
 132         return;
 133     }
 134
 135     double indicated_roll = 0;
 136     double indicated_pitch = 0;
 137     double indicated_hdg = 0;
 138     double indicated_roll_rate = 0;
 139     double indicated_pitch_rate = 0;
 140     double indicated_hdg_rate = 0;
 141     double hdg = 0;
 142     double erect_time_factor = 1;
 143
 144     const double erect_time = 180;
 145     const double max_g_error = 10.0;
 146
 147     //Get the spin from the gyro
 148     _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
 149     _gyro.update(dt);
 150     double spin = _gyro.get_spin_norm();
 151
 152     // set the "off-flag"
 153     if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
 154         _off_node->setBoolValue(false);
 155     } else {
 156         _off_node->setBoolValue(true);
 157         return;
 158     }
 159
 160     // Get the input values
 161     if(_hdg_input_source_node->getBoolValue()){
 162         hdg = _hdg_in_node->getDoubleValue();
 163     } else {
 164         hdg = _hdg_mag_in_node->getDoubleValue();
 165     }
 166
 167     double roll = _roll_in_node->getDoubleValue();
 168     double pitch = _pitch_in_node->getDoubleValue();
 169     double g = _g_in_node->getDoubleValue()/* / gravity*/;
 170
 171     double roll_rate = _yaw_rate_node->getDoubleValue();
 172     double pitch_rate = _pitch_rate_node->getDoubleValue();
 173     double yaw_rate = _yaw_rate_node->getDoubleValue();
 174
 175     //modulate the input by the spin rate
 176     double responsiveness = spin * spin * spin * spin * spin * spin;
 177     roll = fgGetLowPass( _last_roll, roll, responsiveness );
 178     pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
 179
 180     if ((hdg - _last_hdg) > 180)
 181         _last_hdg += 360;
 182     if ((hdg - _last_hdg) < -180)
 183         _last_hdg -= 360;
 184
 185     hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
 186
 187     //but we need to filter the hdg and yaw_rate as well - yuk!
 188     responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
 189     yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
 190     g = fgGetLowPass( _last_g , g, 1.5);
 191
 192
 193     // store the new values
 194     _last_roll = roll;
 195     _last_pitch = pitch;
 196     _last_hdg = hdg;
 197     _last_roll_rate = roll_rate;
 198     _last_pitch_rate = pitch_rate;
 199     _last_yaw_rate = yaw_rate;
 200     _last_g = g;
 201
 202     //the gyro only erects inside limits
 203     if ( fabs ( yaw_rate ) <= 5
 204         && g <= 1.5 && g >= -0.5){
 205
 206             if ( !_fast_erect_node->getBoolValue() ){
 207                 erect_time_factor = 1;
 208             } else {
 209                 erect_time_factor = 2;
 210             }
 211
 212         _g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor;
 213     } else {
 214         _g_error += (max_g_error /(erect_time * 0.33)) * dt * 2;
 215
 216         //SG_LOG(SG_INSTR, SG_ALERT,_num <<
 217         //    " g input " << _g_in_node->getDoubleValue() * gravity
 218         //    <<" _erect_time " << _erect_time
 219         //    << " yaw " <<   yaw_rate
 220         //    << " pitch " << _pitch_rate_node->getDoubleValue()
 221         //    << " roll " << _roll_rate_node->getDoubleValue());
 222
 223     }
 224
 225     //cout << "_g_error "<< _g_error << endl;
 226     _g_error = SGMiscd::clip(_g_error, 0, 10);
 227 //     cout << "_g_error clip "<< _g_error << endl;
 228     indicated_roll = _last_roll + _g_error;
 229     indicated_pitch = _last_pitch + _g_error;
 230     indicated_hdg = _last_hdg + _g_error;
 231     indicated_roll_rate = _last_roll_rate;
 232     indicated_pitch_rate = _last_pitch_rate;
 233     indicated_hdg_rate = _last_yaw_rate;
 234     // calculate the difference between the indicated heading
 235     // and the selected heading for use with an autopilot
 236     SGPropertyNode *bnode
 237         = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
 238
 239     if ( bnode ) {
 240         double diff = bnode->getDoubleValue() - indicated_hdg;
 241         if ( diff < -180.0 ) { diff += 360.0; }
 242         if ( diff > 180.0 ) { diff -= 360.0; }
 243         _error_out_node->setDoubleValue( diff );
 244         //SG_LOG(SG_INSTR, SG_ALERT,
 245         //"autopilot input " << bnode->getDoubleValue()
 246         //<< " output " << _error_out_node->getDoubleValue()<<);
 247     }
 248
 249     //smooth the output
 250     double factor = _responsiveness_node->getDoubleValue() * dt;
 251
 252     indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
 253     indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
 254     //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
 255
 256     indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
 257     indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
 258     indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
 259
 260     // store the new values
 261     _indicated_roll = indicated_roll;
 262     _indicated_pitch = indicated_pitch;
 263     _indicated_hdg = indicated_hdg;
 264
 265     _indicated_roll_rate = indicated_roll_rate;
 266     _indicated_pitch_rate = indicated_pitch_rate;
 267     _indicated_hdg_rate = indicated_hdg_rate;
 268
 269     // add in a gyro underspin "error" if gyro is spinning too slowly
 270     const double spin_thresh = 0.8;
 271     const double max_roll_error = 40.0;
 272     const double max_pitch_error = 12.0;
 273     const double max_hdg_error = 140.0;
 274     double roll_error;
 275     double pitch_error;
 276     double hdg_error;
 277
 278     if ( spin <= spin_thresh ) {
 279         double roll_error_factor        = ( spin_thresh - spin ) / spin_thresh;
 280         double pitch_error_factor       = ( spin_thresh - spin ) / spin_thresh;
 281         double hdg_error_factor         = ( spin_thresh - spin ) / spin_thresh;
 282         roll_error      = roll_error_factor * roll_error_factor * max_roll_error;
 283         pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
 284         hdg_error       = hdg_error_factor * hdg_error_factor * max_hdg_error;
 285     } else {
 286         roll_error = 0.0;
 287         pitch_error = 0.0;
 288         hdg_error = 0.0;
 289     }
 290
 291     _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
 292     _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
 293     _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
 294     _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
 295     _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
 296     _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
 297 }
 298
 299 // end of mrg.cxx