From 6a09f01ca9054a778a11d6963e6dcdfea70ccda1 Mon Sep 17 00:00:00 2001
From: Torsten Dreyer <Torsten@t3r.de>
Date: Tue, 5 Nov 2013 22:26:50 +0100
Subject: [PATCH] mag-compass: add deviation table and pitch property

---
 src/Instrumentation/mag_compass.cxx | 27 ++++++++++++++++++++++-----
 src/Instrumentation/mag_compass.hxx |  8 +++++++-
 2 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx
index 74252b1ed..30dc1fb73 100644
--- a/src/Instrumentation/mag_compass.cxx
+++ b/src/Instrumentation/mag_compass.cxx
@@ -18,13 +18,22 @@
 
 #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 ()
@@ -39,6 +48,7 @@ MagCompass::init ()
 
     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);
@@ -55,7 +65,6 @@ MagCompass::init ()
 void
 MagCompass::reinit ()
 {
-    _error_deg = 0.0;
     _rate_degps = 0.0;
 }
 
@@ -110,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;
@@ -160,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)
diff --git a/src/Instrumentation/mag_compass.hxx b/src/Instrumentation/mag_compass.hxx
index 91af5c056..1d05c302c 100644
--- a/src/Instrumentation/mag_compass.hxx
+++ b/src/Instrumentation/mag_compass.hxx
@@ -13,6 +13,7 @@
 
 #include <simgear/props/props.hxx>
 #include <simgear/structure/subsystem_mgr.hxx>
+#include <simgear/math/interpolater.hxx>
 
 
 /**
@@ -21,6 +22,9 @@
  * Input properties:
  *
  * /instrumentation/"name"/serviceable
+ * /instrumentation/"name"/pitch-offset-deg
+ * /instrumentation/"name"/max-pitch-deg
+ * /instrumentation/"name"/max-roll-deg
  * /orientation/roll-deg
  * /orientation/pitch-deg
  * /orientation/heading-magnetic-deg
@@ -49,13 +53,15 @@ public:
 
 private:
 
-    double _error_deg;
     double _rate_degps;
 
     std::string _name;
     int _num;
+    SGSharedPtr<SGInterpTable> _deviation_table;
+    SGPropertyNode_ptr _deviation_node;
 
     SGPropertyNode_ptr _serviceable_node;
+    SGPropertyNode_ptr _pitch_offset_node;
     SGPropertyNode_ptr _roll_node;
     SGPropertyNode_ptr _pitch_node;
     SGPropertyNode_ptr _heading_node;
-- 
2.39.5