X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmrg.cxx;h=69b220d7f9204554d58ba6f529d9481be5cc59a4;hb=43b300fe46d8013a90009ec8e1a923ec53a396b5;hp=918160ae54eaf8053c4a4e293cb8afe4a61a0a24;hpb=eb6b28f5074654f25fb9118160a70f5a114f6970;p=flightgear.git diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx index 918160ae5..69b220d7f 100644 --- a/src/Instrumentation/mrg.cxx +++ b/src/Instrumentation/mrg.cxx @@ -1,5 +1,5 @@ -// MRG.cxx - an electrcally powered master reference gyro. -// Written by Vivian Meazza based on wrok by David Megginson, started 2006. +// MRG.cxx - an electrically powered master reference gyro. +// Written by Vivian Meazza based on work by David Megginson, started 2006. // // This file is in the Public Domain and comes with no warranty. @@ -7,9 +7,11 @@ // - better spin-up #include +#include -#include STL_IOSTREAM -#include STL_STRING + +#include +#include #include #include // fabs() @@ -20,28 +22,8 @@ MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) : - name("master-reference-gyro"), - num(0) -{ - int i; - for ( i = 0; i < node->nChildren(); ++i ) { - SGPropertyNode *child = node->getChild(i); - string cname = child->getName(); - string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "number" ) { - num = (int) child->getDoubleValue(); - } else { - SG_LOG( SG_INSTR, SG_WARN, "Error in mrg config logic" ); - if ( name.length() ) { - SG_LOG( SG_INSTR, SG_WARN, "Section = " << name ); - } - } - } -} - -MasterReferenceGyro::MasterReferenceGyro () + _name(node->getStringValue("name", "master-reference-gyro")), + _num(node->getIntValue("number", 0)) { } @@ -63,31 +45,36 @@ MasterReferenceGyro::init () _indicated_pitch_rate = 0; string branch; - branch = "/instrumentation/" + name; + branch = "/instrumentation/" + _name; _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-damped", 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 ); + 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-error-deg", 0, true); + _error_out_node = node->getChild("heading-bug-error-deg", 0, true); + _hdg_input_source_node = node->getChild("heading-source", 0, true); _electrical_node->setDoubleValue(0); _responsiveness_node->setDoubleValue(0.75); _off_node->setBoolValue(false); + _hdg_input_source_node->setBoolValue(false); } void @@ -95,8 +82,8 @@ MasterReferenceGyro::bind () { std::ostringstream temp; string branch; - temp << num; - branch = "/instrumentation/" + name + "[" + temp.str() + "]"; + temp << _num; + branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; fgTie((branch + "/serviceable").c_str(), &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable); @@ -109,8 +96,8 @@ MasterReferenceGyro::unbind () { std::ostringstream temp; string branch; - temp << num; - branch = "/instrumentation/" + name + "[" + temp.str() + "]"; + temp << _num; + branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; fgUntie((branch + "/serviceable").c_str()); fgUntie((branch + "/spin").c_str()); @@ -141,22 +128,35 @@ MasterReferenceGyro::update (double dt) } // Get the input values + double hdg = _hdg_mag_in_node->getDoubleValue(); + + if(_hdg_input_source_node->getBoolValue()) + hdg = _hdg_in_node->getDoubleValue(); + double roll = _roll_in_node->getDoubleValue(); double pitch = _pitch_in_node->getDoubleValue(); - double hdg = _hdg_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(); //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 , yaw_rate, 0.15 ); // store the new values _last_roll = roll; @@ -165,9 +165,10 @@ MasterReferenceGyro::update (double dt) _last_roll_rate = roll_rate; _last_pitch_rate = pitch_rate; _last_yaw_rate = yaw_rate; + _last_g = g; //the gyro only erects inside limits - if ( abs ( yaw_rate ) <= 5 + if ( fabs ( yaw_rate ) <= 5 && (_g_in_node->getDoubleValue() <= 1.5 || _g_in_node->getDoubleValue() >= -0.5) ) { indicated_roll = _last_roll; @@ -185,21 +186,24 @@ MasterReferenceGyro::update (double dt) // calculate the difference between the indicated heading // and the selected heading for use with an autopilot static SGPropertyNode *bnode - = fgGetNode( "autopilot/settings/target-heading-deg", false ); + = fgGetNode( "/autopilot/settings/heading-bug-deg", false ); 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()<<); } - //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<getDoubleValue() * dt; indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor ); indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor ); - indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor ); + //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor ); indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor ); indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );