From: timoore Date: Tue, 11 Nov 2008 21:20:02 +0000 (+0000) Subject: Master reference gyro changes from Vivian Meazza. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=aa716b350fb8732308c0a0dded0d936f96445f42;p=flightgear.git Master reference gyro changes from Vivian Meazza. --- diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx index 3e7c07162..69b220d7f 100644 --- a/src/Instrumentation/mrg.cxx +++ b/src/Instrumentation/mrg.cxx @@ -7,6 +7,8 @@ // - better spin-up #include +#include + #include #include @@ -48,26 +50,31 @@ MasterReferenceGyro::init () _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 ); _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 @@ -121,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; @@ -145,6 +165,7 @@ 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 ( fabs ( yaw_rate ) <= 5 @@ -165,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 ); diff --git a/src/Instrumentation/mrg.hxx b/src/Instrumentation/mrg.hxx index a12df8aaf..9bfaf741f 100644 --- a/src/Instrumentation/mrg.hxx +++ b/src/Instrumentation/mrg.hxx @@ -15,7 +15,7 @@ /** - * Model a vacuum-powered attitude indicator. + * Model an electrically-powered master reference gyro. * * Input properties: * @@ -64,6 +64,7 @@ private: double _last_roll_rate; double _last_pitch_rate; double _last_yaw_rate; + double _last_g; Gyro _gyro; @@ -74,6 +75,7 @@ private: SGPropertyNode_ptr _pitch_in_node; SGPropertyNode_ptr _roll_in_node; SGPropertyNode_ptr _hdg_in_node; + SGPropertyNode_ptr _hdg_mag_in_node; SGPropertyNode_ptr _g_in_node; SGPropertyNode_ptr _electrical_node; SGPropertyNode_ptr _pitch_int_node; @@ -82,6 +84,7 @@ private: SGPropertyNode_ptr _pitch_out_node; SGPropertyNode_ptr _roll_out_node; SGPropertyNode_ptr _hdg_out_node; + SGPropertyNode_ptr _hdg_mag_out_node; SGPropertyNode_ptr _pitch_rate_out_node; SGPropertyNode_ptr _roll_rate_out_node; SGPropertyNode_ptr _hdg_rate_out_node; @@ -90,6 +93,7 @@ private: SGPropertyNode_ptr _roll_rate_node; SGPropertyNode_ptr _pitch_rate_node; SGPropertyNode_ptr _responsiveness_node; + SGPropertyNode_ptr _hdg_input_source_node; }; #endif // __INSTRUMENTS_MRG_HXX