From a9475bd0fa2b35840ea73647697a65014fda09c9 Mon Sep 17 00:00:00 2001
From: James Turner <zakalawe@mac.com>
Date: Sun, 2 Dec 2018 13:38:33 +0100
Subject: [PATCH] Basic nav-radio ILS testing

Complex cases still to be added, but this tests the basic reception.
---
 src/Navaids/positioned.cxx                    | 11 ++-
 src/Navaids/positioned.hxx                    | 12 ++-
 .../Instrumentation/test_navRadio.cxx         | 97 +++++++++++++++++++
 3 files changed, 113 insertions(+), 7 deletions(-)

diff --git a/src/Navaids/positioned.cxx b/src/Navaids/positioned.cxx
index 5dd0783d6..da8a21e15 100644
--- a/src/Navaids/positioned.cxx
+++ b/src/Navaids/positioned.cxx
@@ -431,13 +431,18 @@ FGPositionedRef FGPositioned::loadByIdImpl(PositionedID id)
   return flightgear::NavDataCache::instance()->loadById(id);
 }
 
-FGPositioned::TypeFilter::TypeFilter(Type aTy) :
-  mMinType(LAST_TYPE),
-  mMaxType(INVALID)
+FGPositioned::TypeFilter::TypeFilter(Type aTy)
 {
   addType(aTy);
 }
 
+FGPositioned::TypeFilter::TypeFilter(std::initializer_list<Type> types)
+{
+    for (auto t : types) {
+        addType(t);
+    }
+}
+
 void FGPositioned::TypeFilter::addType(Type aTy)
 {
   if (aTy == INVALID) {
diff --git a/src/Navaids/positioned.hxx b/src/Navaids/positioned.hxx
index 9a0f425c1..fe03533c6 100644
--- a/src/Navaids/positioned.hxx
+++ b/src/Navaids/positioned.hxx
@@ -175,12 +175,15 @@ public:
   {
   public:
     TypeFilter(Type aTy = INVALID);
-    virtual bool pass(FGPositioned* aPos) const;
+      
+    TypeFilter(std::initializer_list<Type> types);
+      
+    bool pass(FGPositioned* aPos) const override;
     
-    virtual Type minType() const
+    Type minType() const override
     { return mMinType; }
     
-    virtual Type maxType() const
+    Type maxType() const override
     { return mMaxType; }
     
     void addType(Type aTy);
@@ -189,7 +192,8 @@ public:
   private:
       
     std::vector<Type> types;
-    Type mMinType, mMaxType;
+    Type mMinType = LAST_TYPE,
+      mMaxType = INVALID;
   };
   
   static FGPositionedList findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter);
diff --git a/test_suite/unit_tests/Instrumentation/test_navRadio.cxx b/test_suite/unit_tests/Instrumentation/test_navRadio.cxx
index 7ca2f6b3c..9af246353 100644
--- a/test_suite/unit_tests/Instrumentation/test_navRadio.cxx
+++ b/test_suite/unit_tests/Instrumentation/test_navRadio.cxx
@@ -180,8 +180,105 @@ void NavRadioTests::testCDIDeflection()
 
 void NavRadioTests::testILSBasic()
 {
+    // radio setup
+    SGPropertyNode_ptr configNode(new SGPropertyNode);
+    configNode->setStringValue("name", "navtest");
+    configNode->setIntValue("number", 2);
+    std::unique_ptr<FGNavRadio> r(new FGNavRadio(configNode));
+    r->bind();
+    r->init();
+    
+    SGPropertyNode_ptr node = globals->get_props()->getNode("instrumentation/navtest[2]");
+    node->setBoolValue("serviceable", true);
+    globals->get_props()->setDoubleValue("systems/electrical/outputs/navtest", 6.0);
+    
+    // test basic ILS: KSFO 28L
+    FGPositioned::TypeFilter f{{FGPositioned::VOR, FGPositioned::ILS, FGPositioned::LOC}};
+    FGNavRecordRef ils = fgpositioned_cast<FGNavRecord>(
+        FGPositioned::findClosestWithIdent("ISFO", SGGeod::fromDeg(-112, 37.6), &f));
+    CPPUNIT_ASSERT(ils->type() == FGPositioned::ILS);
+    
+    node->setDoubleValue("frequencies/selected-mhz", 109.55);
+   // node->setDoubleValue("radials/selected-deg", 42.0); // twist is -2.0
+    CPPUNIT_ASSERT(!strcmp("109.55", node->getStringValue("frequencies/selected-mhz-fmt")));
+
+    // note we need full precision here, due to ILS sensitivity
+    SGGeod p = SGGeodesy::direct(ils->geod(), 117.932, 10 * SG_NM_TO_METER);
+    p.setElevationFt(2500);
+    setPositionAndStabilise(r.get(), p);
+    
+    CPPUNIT_ASSERT(!strcmp("ISFO", node->getStringValue("nav-id")));
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(297.9, node->getDoubleValue("radials/target-radial-deg"), 0.1);
+
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, node->getDoubleValue("signal-quality-norm"), 0.01);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(297.9, node->getDoubleValue("heading-deg"), 1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(117.932, node->getDoubleValue("radials/actual-deg"), 0.1);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, node->getDoubleValue("heading-needle-deflection"), 0.1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, node->getDoubleValue("heading-needle-deflection-norm"), 0.01);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, node->getDoubleValue("crosstrack-error-m"), 10.0);
+    CPPUNIT_ASSERT(!node->getBoolValue("from-flag"));
+    CPPUNIT_ASSERT(node->getBoolValue("to-flag"));
+    
+    //  1 degree offset
+    p = SGGeodesy::direct(ils->geod(), 116.932, 6 * SG_NM_TO_METER);
+    p.setElevationFt(1500);
+    setPositionAndStabilise(r.get(), p);
+    
+    const double locWidth = ils->localizerWidth();
+    const double deflectionScale = 20.0 / locWidth; // 20 degrees is full VOR swing (-10 to +10 degrees)
+    
+    CPPUNIT_ASSERT(!strcmp("ISFO", node->getStringValue("nav-id")));
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(297.9, node->getDoubleValue("radials/target-radial-deg"), 0.1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, node->getDoubleValue("signal-quality-norm"), 0.01);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(296.9, node->getDoubleValue("heading-deg"), 1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(116.932, node->getDoubleValue("radials/actual-deg"), 0.1);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0 * deflectionScale, node->getDoubleValue("heading-needle-deflection"), 0.1);
+    
+    double xtkE = sin(-1.0 * SG_DEGREES_TO_RADIANS) * 6.0 * SG_NM_TO_METER;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(xtkE, node->getDoubleValue("crosstrack-error-m"), 1.0);
+    CPPUNIT_ASSERT(!node->getBoolValue("from-flag"));
+    CPPUNIT_ASSERT(node->getBoolValue("to-flag"));
+    
+    
+    // test pegged (4 degrees off course)
+    p = SGGeodesy::direct(ils->geod(), 121.932, 3 * SG_NM_TO_METER);
+    p.setElevationFt(600);
+    setPositionAndStabilise(r.get(), p);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, node->getDoubleValue("signal-quality-norm"), 0.01);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(301.9, node->getDoubleValue("heading-deg"), 1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(121.932, node->getDoubleValue("radials/actual-deg"), 0.1);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(10.0, node->getDoubleValue("heading-needle-deflection"), 0.1);
+    
+    xtkE = sin(4.0 * SG_DEGREES_TO_RADIANS) * 3.0 * SG_NM_TO_METER;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(xtkE, node->getDoubleValue("crosstrack-error-m"), 1.0);
+    CPPUNIT_ASSERT(!node->getBoolValue("from-flag"));
+    CPPUNIT_ASSERT(node->getBoolValue("to-flag"));
+    
     
     // also check ILS back course
+    // 1 degree offset on the BC
+    p = SGGeodesy::direct(ils->geod(), 298.932, 4 * SG_NM_TO_METER);
+    p.setElevationFt(1500);
+    setPositionAndStabilise(r.get(), p);
+
+    CPPUNIT_ASSERT(!strcmp("ISFO", node->getStringValue("nav-id")));
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(297.9, node->getDoubleValue("radials/target-radial-deg"), 0.1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(1.0, node->getDoubleValue("signal-quality-norm"), 0.01);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(118.9, node->getDoubleValue("heading-deg"), 1);
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(298.932, node->getDoubleValue("radials/actual-deg"), 0.1);
+    
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.0 * deflectionScale, node->getDoubleValue("heading-needle-deflection"), 0.1);
+    
+    xtkE = sin(-1.0 * SG_DEGREES_TO_RADIANS) * 4.0 * SG_NM_TO_METER;
+    CPPUNIT_ASSERT_DOUBLES_EQUAL(xtkE, node->getDoubleValue("crosstrack-error-m"), 1.0);
+    
+    // these don't change for an ILS
+    CPPUNIT_ASSERT(!node->getBoolValue("from-flag"));
+    CPPUNIT_ASSERT(node->getBoolValue("to-flag"));
 }
 
 void NavRadioTests::testGS()