X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmag_compass.cxx;h=30dc1fb7372d422afedace374ca4cb670f1151e4;hb=34433c1fa1d10e389ffd84ec115d6c8036c5b885;hp=1f57f0791923154a44fd9e98301f3ec08225a268;hpb=59895306a57eb44d1777e08b96c867fb3b8a046d;p=flightgear.git

diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx
index 1f57f0791..30dc1fb73 100644
--- a/src/Instrumentation/mag_compass.cxx
+++ b/src/Instrumentation/mag_compass.cxx
@@ -11,18 +11,29 @@
 #endif
 
 #include <simgear/sg_inlines.h>
+#include <simgear/math/SGMath.hxx>
 
-#include "mag_compass.hxx"
 #include <Main/fg_props.hxx>
 #include <Main/util.hxx>
 
+#include "mag_compass.hxx"
 
 MagCompass::MagCompass ( SGPropertyNode *node )
-    : _error_deg(0.0),
-      _rate_degps(0.0),
+    : _rate_degps(0.0),
       _name(node->getStringValue("name", "magnetic-compass")),
       _num(node->getIntValue("number", 0))
 {
+    SGPropertyNode_ptr n = node->getNode( "deviation", false );
+    if( n ) {
+      SGPropertyNode_ptr deviation_table_node = n->getNode( "table", false );
+      if( NULL != deviation_table_node ) {
+        _deviation_table = new SGInterpTable( deviation_table_node );
+      } else {
+        std::string deviation_node_name = n->getStringValue();
+        if( false == deviation_node_name.empty() )
+          _deviation_node = fgGetNode( deviation_node_name, true );
+      }
+    }
 }
 
 MagCompass::~MagCompass ()
@@ -32,11 +43,12 @@ MagCompass::~MagCompass ()
 void
 MagCompass::init ()
 {
-    string branch;
+    std::string branch;
     branch = "/instrumentation/" + _name;
 
     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
     _serviceable_node = node->getChild("serviceable", 0, true);
+    _pitch_offset_node = node->getChild("pitch-offset-deg", 0, true);
     _roll_node = fgGetNode("/orientation/roll-deg", true);
     _pitch_node = fgGetNode("/orientation/pitch-deg", true);
     _heading_node = fgGetNode("/orientation/heading-magnetic-deg", true);
@@ -46,8 +58,15 @@ MagCompass::init ()
     _y_accel_node = fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
     _z_accel_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
     _out_node = node->getChild("indicated-heading-deg", 0, true);
+
+    reinit();
 }
 
+void
+MagCompass::reinit ()
+{
+    _rate_degps = 0.0;
+}
 
 void
 MagCompass::update (double delta_time_sec)
@@ -100,7 +119,8 @@ MagCompass::update (double delta_time_sec)
     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 
                                 // pitch angle (radians)
-    double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+    double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS
+                   + _pitch_offset_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 
                                 // magnetic heading (radians)
     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
@@ -150,6 +170,13 @@ MagCompass::update (double delta_time_sec)
                                 // This is the value that the compass
                                 // is *trying* to display.
     double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
+
+    if( _deviation_node ) {
+      target_deg -= _deviation_node->getDoubleValue();
+    } else if( _deviation_table ) { 
+       target_deg -= _deviation_table->interpolate( SGMiscd::normalizePeriodic( 0.0, 360.0, target_deg ) );  
+    }
+
     double old_deg = _out_node->getDoubleValue();
 
     while ((target_deg - old_deg) > 180.0)