diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx index 539231da4..365447b45 100644 --- a/src/Instrumentation/mrg.cxx +++ b/src/Instrumentation/mrg.cxx @@ -20,10 +20,11 @@ #include "mrg.hxx" +const double MasterReferenceGyro::gravity = -32.1740485564; MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) : - _name(node->getStringValue("name", "master-reference-gyro")), - _num(node->getIntValue("number", 0)) +_name(node->getStringValue("name", "master-reference-gyro")), +_num(node->getIntValue("number", 0)) { } @@ -34,237 +35,252 @@ MasterReferenceGyro::~MasterReferenceGyro () void MasterReferenceGyro::init () { - _last_hdg = 0; - _last_roll = 0; - _last_pitch = 0; - _indicated_hdg = 0; - _indicated_roll = 0; - _indicated_pitch = 0; - _indicated_hdg_rate = 0; - _indicated_roll_rate = 0; - _indicated_pitch_rate = 0; - _erect_time = 0; - - string branch; - branch = "/instrumentation/" + _name; + _last_hdg = 0; + _last_roll = 0; + _last_pitch = 0; + _indicated_hdg = 0; + _indicated_roll = 0; + _indicated_pitch = 0; + _indicated_hdg_rate = 0; + _indicated_roll_rate = 0; + _indicated_pitch_rate = 0; + _erect_time = 180; + _last_g = 1; + _g_error = 10; - _pitch_in_node = fgGetNode("/orientation/pitch-deg", true); - _roll_in_node = fgGetNode("/orientation/roll-deg", true); - _hdg_in_node = fgGetNode("/orientation/heading-deg", true); - _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true); - _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true); - _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true); - _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true); - _g_in_node = fgGetNode("/accelerations/pilot-g", true); - _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true); - _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true); - - SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true ); - _off_node = node->getChild("off-flag", 0, true); - _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true); - _roll_out_node = node->getChild("indicated-roll-deg", 0, true); - _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true); - _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true); - _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true); - _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true); - _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true); - _responsiveness_node = node->getChild("responsiveness", 0, true); - _error_out_node = node->getChild("heading-bug-error-deg", 0, true); - _hdg_input_source_node = node->getChild("heading-source", 0, true); - _fast_erect_node = node->getChild("fast-erect", 0, true); + string branch; + branch = "/instrumentation/" + _name; - _electrical_node->setDoubleValue(0); - _responsiveness_node->setDoubleValue(0.75); - _off_node->setBoolValue(false); - _hdg_input_source_node->setBoolValue(false); - _fast_erect_node->setBoolValue(false); + _pitch_in_node = fgGetNode("/orientation/pitch-deg", true); + _roll_in_node = fgGetNode("/orientation/roll-deg", true); + _hdg_in_node = fgGetNode("/orientation/heading-deg", true); + _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true); + _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true); + _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true); + _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true); + //_g_in_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true); + _g_in_node = fgGetNode("/accelerations/pilot-g", true); + _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true); + _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true); + + SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true ); + _off_node = node->getChild("off-flag", 0, true); + _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true); + _roll_out_node = node->getChild("indicated-roll-deg", 0, true); + _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true); + _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true); + _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true); + _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true); + _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true); + _responsiveness_node = node->getChild("responsiveness", 0, true); + _error_out_node = node->getChild("heading-bug-error-deg", 0, true); + _hdg_input_source_node = node->getChild("heading-source", 0, true); + _fast_erect_node = node->getChild("fast-erect", 0, true); + + _electrical_node->setDoubleValue(0); + _responsiveness_node->setDoubleValue(0.75); + _off_node->setBoolValue(false); + _hdg_input_source_node->setBoolValue(false); + _fast_erect_node->setBoolValue(false); + _g_in_node->setDoubleValue(1); } void MasterReferenceGyro::bind () { - std::ostringstream temp; - string branch; - temp << _num; - branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; + std::ostringstream temp; + string branch; + temp << _num; + branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; - fgTie((branch + "/serviceable").c_str(), - &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable); - fgTie((branch + "/spin").c_str(), - &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm); + fgTie((branch + "/serviceable").c_str(), + &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable); + fgTie((branch + "/spin").c_str(), + &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm); } void MasterReferenceGyro::unbind () { - std::ostringstream temp; - string branch; - temp << _num; - branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; + std::ostringstream temp; + string branch; + temp << _num; + branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; - fgUntie((branch + "/serviceable").c_str()); - fgUntie((branch + "/spin").c_str()); + fgUntie((branch + "/serviceable").c_str()); + fgUntie((branch + "/spin").c_str()); } void MasterReferenceGyro::update (double dt) { - double indicated_roll = 0; - double indicated_pitch = 0; - double indicated_hdg = 0; - double indicated_roll_rate = 0; - double indicated_pitch_rate = 0; - double indicated_hdg_rate = 0; - - // Get the spin from the gyro - _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 ); - _gyro.update(dt); - double spin = _gyro.get_spin_norm(); - - // set the "off-flag" - if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) { - _off_node->setBoolValue(false); - } else { - _off_node->setBoolValue(true); + //sanity check + if (!fgGetBool("/sim/fdm-initialized", false)) { return; - } + } - // Get the input values - double hdg = _hdg_mag_in_node->getDoubleValue(); + double indicated_roll = 0; + double indicated_pitch = 0; + double indicated_hdg = 0; + double indicated_roll_rate = 0; + double indicated_pitch_rate = 0; + double indicated_hdg_rate = 0; + double hdg = 0; + double erect_time_factor = 1; - if(_hdg_input_source_node->getBoolValue()) - hdg = _hdg_in_node->getDoubleValue(); + const double erect_time = 180; + const double max_g_error = 10.0; - double roll = _roll_in_node->getDoubleValue(); - double pitch = _pitch_in_node->getDoubleValue(); - - double roll_rate = _yaw_rate_node->getDoubleValue(); - double pitch_rate = _yaw_rate_node->getDoubleValue(); - double yaw_rate = _yaw_rate_node->getDoubleValue(); - double g = _g_in_node->getDoubleValue(); + //Get the spin from the gyro + _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 ); + _gyro.update(dt); + double spin = _gyro.get_spin_norm(); - //modulate the input by the spin rate - double responsiveness = spin * spin * spin * spin * spin * spin; - roll = fgGetLowPass( _last_roll, roll, responsiveness ); - pitch = fgGetLowPass( _last_pitch , pitch, responsiveness ); + // set the "off-flag" + if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) { + _off_node->setBoolValue(false); + } else { + _off_node->setBoolValue(true); + return; + } - if ((hdg - _last_hdg) > 180) - _last_hdg += 360; - if ((hdg - _last_hdg) < -180) - _last_hdg -= 360; + // Get the input values + if(_hdg_input_source_node->getBoolValue()){ + hdg = _hdg_in_node->getDoubleValue(); + } else { + hdg = _hdg_mag_in_node->getDoubleValue(); + } - hdg = fgGetLowPass( _last_hdg , hdg, responsiveness ); + double roll = _roll_in_node->getDoubleValue(); + double pitch = _pitch_in_node->getDoubleValue(); + double g = _g_in_node->getDoubleValue()/* / gravity*/; - //but we need to filter the hdg and yaw_rate as well - yuk! - responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin); - yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness ); - g = fgGetLowPass( _last_g , g, 0.015 ); + double roll_rate = _yaw_rate_node->getDoubleValue(); + double pitch_rate = _pitch_rate_node->getDoubleValue(); + double yaw_rate = _yaw_rate_node->getDoubleValue(); + + //modulate the input by the spin rate + double responsiveness = spin * spin * spin * spin * spin * spin; + roll = fgGetLowPass( _last_roll, roll, responsiveness ); + pitch = fgGetLowPass( _last_pitch , pitch, responsiveness ); + + if ((hdg - _last_hdg) > 180) + _last_hdg += 360; + if ((hdg - _last_hdg) < -180) + _last_hdg -= 360; + + hdg = fgGetLowPass( _last_hdg , hdg, responsiveness ); + + //but we need to filter the hdg and yaw_rate as well - yuk! + responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin); + yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness ); + g = fgGetLowPass( _last_g , g, 1.5); - // store the new values - _last_roll = roll; - _last_pitch = pitch; - _last_hdg = hdg; - _last_roll_rate = roll_rate; - _last_pitch_rate = pitch_rate; - _last_yaw_rate = yaw_rate; - _last_g = g; + // store the new values + _last_roll = roll; + _last_pitch = pitch; + _last_hdg = hdg; + _last_roll_rate = roll_rate; + _last_pitch_rate = pitch_rate; + _last_yaw_rate = yaw_rate; + _last_g = g; - if (_erect_time > 0){ + //the gyro only erects inside limits + if ( fabs ( yaw_rate ) <= 5 + && g <= 1.5 && g >= -0.5){ - if ( !_fast_erect_node->getBoolValue() ) - _erect_time -= dt; - else - _erect_time -= 2 * dt; + if ( !_fast_erect_node->getBoolValue() ){ + erect_time_factor = 1; + } else { + erect_time_factor = 2; + } + + _g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor; + } else { + _g_error += (max_g_error /(erect_time * 0.33)) * dt * 2; + + //SG_LOG(SG_GENERAL, SG_ALERT,_num << + // " g input " << _g_in_node->getDoubleValue() * gravity + // <<" _erect_time " << _erect_time + // << " yaw " << yaw_rate + // << " pitch " << _pitch_rate_node->getDoubleValue() + // << " roll " << _roll_rate_node->getDoubleValue()); } - //SG_LOG(SG_GENERAL, SG_ALERT, - // "g input " << g <<" _erect_time " << _erect_time << " spin " << spin); + //cout << "_g_error "<< _g_error << endl; + _g_error = SGMiscd::clip(_g_error, 0, 10); +// cout << "_g_error clip "<< _g_error << endl; + indicated_roll = _last_roll + _g_error; + indicated_pitch = _last_pitch + _g_error; + indicated_hdg = _last_hdg + _g_error; + indicated_roll_rate = _last_roll_rate; + indicated_pitch_rate = _last_pitch_rate; + indicated_hdg_rate = _last_yaw_rate; + // calculate the difference between the indicated heading + // and the selected heading for use with an autopilot + static SGPropertyNode *bnode + = fgGetNode( "/autopilot/settings/heading-bug-deg", false ); - //the gyro only erects inside limits - if ( fabs ( yaw_rate ) <= 5 - && ( g <= 1.5 || g >= -0.5) - && _erect_time <=0 ) { - indicated_roll = _last_roll; - indicated_pitch = _last_pitch; - indicated_hdg = _last_hdg; - indicated_roll_rate = _last_roll_rate; - indicated_pitch_rate = _last_pitch_rate; - indicated_hdg_rate = _last_yaw_rate; - } else { - indicated_roll_rate = 0; - indicated_pitch_rate = 0; - indicated_hdg_rate = 0; - - if (_erect_time <= 0 ) - _erect_time = 34; + if ( bnode ) { + double diff = bnode->getDoubleValue() - indicated_hdg; + if ( diff < -180.0 ) { diff += 360.0; } + if ( diff > 180.0 ) { diff -= 360.0; } + _error_out_node->setDoubleValue( diff ); + //SG_LOG(SG_GENERAL, SG_ALERT, + //"autopilot input " << bnode->getDoubleValue() + //<< " output " << _error_out_node->getDoubleValue()<<); + } - } + //smooth the output + double factor = _responsiveness_node->getDoubleValue() * dt; - // calculate the difference between the indicated heading - // and the selected heading for use with an autopilot - static SGPropertyNode *bnode - = fgGetNode( "/autopilot/settings/heading-bug-deg", false ); + indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor ); + indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor ); + //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor ); - if ( bnode ) { - double diff = bnode->getDoubleValue() - indicated_hdg; - if ( diff < -180.0 ) { diff += 360.0; } - if ( diff > 180.0 ) { diff -= 360.0; } - _error_out_node->setDoubleValue( diff ); - //SG_LOG(SG_GENERAL, SG_ALERT, - //"autopilot input " << bnode->getDoubleValue() - //<< " output " << _error_out_node->getDoubleValue()<<); - } - - //smooth the output - double factor = _responsiveness_node->getDoubleValue() * dt; + indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor ); + indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor ); + indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor ); - indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor ); - indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor ); - //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor ); + // store the new values + _indicated_roll = indicated_roll; + _indicated_pitch = indicated_pitch; + _indicated_hdg = indicated_hdg; - indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor ); - indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor ); - indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor ); + _indicated_roll_rate = indicated_roll_rate; + _indicated_pitch_rate = indicated_pitch_rate; + _indicated_hdg_rate = indicated_hdg_rate; - // store the new values - _indicated_roll = indicated_roll; - _indicated_pitch = indicated_pitch; - _indicated_hdg = indicated_hdg; + // add in a gyro underspin "error" if gyro is spinning too slowly + const double spin_thresh = 0.8; + const double max_roll_error = 40.0; + const double max_pitch_error = 12.0; + const double max_hdg_error = 140.0; + double roll_error; + double pitch_error; + double hdg_error; - _indicated_roll_rate = indicated_roll_rate; - _indicated_pitch_rate = indicated_pitch_rate; - _indicated_hdg_rate = indicated_hdg_rate; + if ( spin <= spin_thresh ) { + double roll_error_factor = ( spin_thresh - spin ) / spin_thresh; + double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh; + double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh; + roll_error = roll_error_factor * roll_error_factor * max_roll_error; + pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error; + hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error; + } else { + roll_error = 0.0; + pitch_error = 0.0; + hdg_error = 0.0; + } - // add in a gyro underspin "error" if gyro is spinning too slowly - const double spin_thresh = 0.8; - const double max_roll_error = 40.0; - const double max_pitch_error = 12.0; - const double max_hdg_error = 140.0; - double roll_error; - double pitch_error; - double hdg_error; - - if ( spin <= spin_thresh ) { - double roll_error_factor = ( spin_thresh - spin ) / spin_thresh; - double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh; - double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh; - roll_error = roll_error_factor * roll_error_factor * max_roll_error; - pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error; - hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error; - } else { - roll_error = 0.0; - pitch_error = 0.0; - hdg_error = 0.0; - } - - _roll_out_node->setDoubleValue( _indicated_roll + roll_error ); - _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error ); - _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error ); - _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate ); - _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate ); - _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate ); + _roll_out_node->setDoubleValue( _indicated_roll + roll_error ); + _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error ); + _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error ); + _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate ); + _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate ); + _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate ); } // end of mrg.cxx diff --git a/src/Instrumentation/mrg.hxx b/src/Instrumentation/mrg.hxx index 4fc8fdee3..b925353a5 100644 --- a/src/Instrumentation/mrg.hxx +++ b/src/Instrumentation/mrg.hxx @@ -49,6 +49,8 @@ public: private: + static const double gravity; //conversion factor + string _name; int _num; @@ -66,6 +68,7 @@ private: double _last_yaw_rate; double _last_g; double _erect_time; + double _g_error; Gyro _gyro;