X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmrg.cxx;h=69b220d7f9204554d58ba6f529d9481be5cc59a4;hb=c6045147544badd6daefdcab9d4de1ed6936533b;hp=d890ee266b6a28ffd6097f2779a80560978d0e22;hpb=b9e4775a7a4bbeac9e3af2bf617ddacfbc2d47bf;p=flightgear.git

diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx
index d890ee266..69b220d7f 100644
--- a/src/Instrumentation/mrg.cxx
+++ b/src/Instrumentation/mrg.cxx
@@ -7,9 +7,11 @@
 // - better spin-up
 
 #include <simgear/compiler.h>
+#include <simgear/sg_inlines.h>
 
-#include STL_IOSTREAM
-#include STL_STRING
+
+#include <iostream>
+#include <string>
 #include <sstream>
 #include <math.h>	// fabs()
 
@@ -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()<<endl ;
+	
 	//smooth the output
 	double factor = _responsiveness_node->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 );