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