From e324c891716bf18bb56e9ea2dcdb9b884688ebdd Mon Sep 17 00:00:00 2001
From: Torsten Dreyer <torsten@t3r.de>
Date: Thu, 8 Oct 2015 10:17:27 +0200
Subject: [PATCH] newnavradio: expose localizer crosstrack error

---
 src/Instrumentation/newnavradio.cxx | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/Instrumentation/newnavradio.cxx b/src/Instrumentation/newnavradio.cxx
index fc779db8c..8c2dabbb6 100644
--- a/src/Instrumentation/newnavradio.cxx
+++ b/src/Instrumentation/newnavradio.cxx
@@ -484,6 +484,7 @@ private:
       SGInterpTable _elevationTable;
   } _serviceVolume;
   PropertyObject<double> _localizerOffset_norm;
+  PropertyObject<double> _localizerOffset_m;
   PropertyObject<double> _localizerWidth_deg;
 };
 
@@ -529,6 +530,7 @@ LOC::LOC( SGPropertyNode_ptr rootNode) :
                                                                                  rootNode->getIndex()))),
   _serviceVolume(),
   _localizerOffset_norm( rootNode->getNode(_name,true)->getNode("offset-norm",true) ),
+  _localizerOffset_m( rootNode->getNode(_name,true)->getNode("offset-m",true) ),
   _localizerWidth_deg( rootNode->getNode(_name,true)->getNode("width-deg",true) )
 {
 }
@@ -585,20 +587,24 @@ void LOC::update( double dt, const SGGeod & aircraftPosition )
 
   if( false == valid() ) {
     _localizerOffset_norm = 0.0;
+    _localizerOffset_m = 0.0;
     return;
   }
 
-  double offset = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
+  double offsetDeg = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
+
+  // cross-track error (in meters)
+  _localizerOffset_m = _trackDistance_m * sin(offsetDeg * SGD_DEGREES_TO_RADIANS);
 
   // The factor of 30.0 gives a period of 120 which gives us 3 cycles and six 
   // zeros i.e. six courses: one front course, one back course, and four 
   // false courses. Three of the six are reverse sensing.
-  offset = 30.0 * sawtooth(offset / 30.0);
+  offsetDeg = 30.0 * sawtooth(offsetDeg / 30.0);
 
-  // normalize offset to the localizer width, scale and clip to [-1..1]
-  offset = SGMiscd::clip( 2.0 * offset / _localizerWidth_deg, -1.0, 1.0 );
+  // normalize offsetDeg to the localizer width, scale and clip to [-1..1]
+  offsetDeg = SGMiscd::clip( 2.0 * offsetDeg / _localizerWidth_deg, -1.0, 1.0 );
   
-  _localizerOffset_norm = offset;
+  _localizerOffset_norm = offsetDeg;
 }
 
 void LOC::display( NavIndicator & navIndicator )
-- 
2.39.5