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