src/Instrumentation/newnavradio.cxx

   1 // navradio.cxx -- class to manage a nav radio instance
   2 //
   3 // Written by Curtis Olson, started April 2000.
   4 // Rewritten by Torsten Dreyer, August 2011
   5 //
   6 // Copyright (C) 2000 - 2011  Curtis L. Olson - http://www.flightgear.org/~curt
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21 //
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include "newnavradio.hxx"
  28
  29 #include <assert.h>
  30 #include <boost/foreach.hpp>
  31
  32 #include <simgear/math/interpolater.hxx>
  33 #include <simgear/sg_inlines.h>
  34 #include <simgear/props/propertyObject.hxx>
  35 #include <simgear/misc/strutils.hxx>
  36 #include <simgear/sound/sample_group.hxx>
  37
  38 #include <Main/fg_props.hxx>
  39 #include <Navaids/navlist.hxx>
  40 #include <Sound/audioident.hxx>
  41
  42 #include "navradio.hxx"
  43 #include "frequencyformatter.hxx"
  44
  45 namespace Instrumentation {
  46
  47 using simgear::PropertyObject;
  48
  49 /* --------------The Navigation Indicator ----------------------------- */
  50
  51 class NavIndicator {
  52 public:
  53     NavIndicator( SGPropertyNode * rootNode ) :
  54       _cdi( rootNode->getNode("heading-needle-deflection", true ) ),
  55       _cdiNorm( rootNode->getNode("heading-needle-deflection-norm", true ) ),
  56       _course( rootNode->getNode("radials/selected-deg", true ) ),
  57       _toFlag( rootNode->getNode("to-flag", true ) ),
  58       _fromFlag( rootNode->getNode("from-flag", true ) ),
  59       _signalQuality( rootNode->getNode("signal-quality-norm", true ) ),
  60       _hasGS( rootNode->getNode("has-gs", true ) ),
  61       _gsDeflection(rootNode->getNode("gs-needle-deflection", true )),
  62       _gsDeflectionDeg(rootNode->getNode("gs-needle-deflection-deg", true )),
  63       _gsDeflectionNorm(rootNode->getNode("gs-needle-deflection-norm", true ))
  64   {
  65   }
  66
  67   virtual ~NavIndicator() {}
  68
  69   /**
  70    * set the normalized CDI deflection
  71    * @param norm the cdi deflection normalized [-1..1]
  72    */
  73   void setCDI( double norm )
  74   {
  75       _cdi = norm * 10.0;
  76       _cdiNorm = norm;
  77   }
  78
  79   /**
  80    * set the normalized GS deflection
  81    * @param norm the gs deflection normalized to [-1..1]
  82    */
  83   void setGS( double norm )
  84   {
  85       _gsDeflectionNorm = norm;
  86       _gsDeflectionDeg = norm * 0.7;
  87       _gsDeflection = norm * 3.5;
  88   }
  89
  90   void setGS( bool enabled )
  91   {
  92       _hasGS = enabled;
  93       if( !enabled ) {
  94         setGS( 0.0 );
  95       }
  96   }
  97
  98   void showFrom( bool on )
  99   {
 100       _fromFlag = on;
 101   }
 102
 103   void showTo( bool on )
 104   {
 105       _toFlag = on;
 106   }
 107
 108   void setSelectedCourse( double course )
 109   {
 110       _course = course;
 111   }
 112
 113   double getSelectedCourse() const
 114   {
 115       return SGMiscd::normalizePeriodic(0.0, 360.0, _course );
 116   }
 117
 118   void setSignalQuality( double signalQuality )
 119   {
 120       _signalQuality = signalQuality;
 121   }
 122
 123 private:
 124   PropertyObject<double> _cdi;
 125   PropertyObject<double> _cdiNorm;
 126   PropertyObject<double> _course;
 127   PropertyObject<double> _toFlag;
 128   PropertyObject<double> _fromFlag;
 129   PropertyObject<double> _signalQuality;
 130   PropertyObject<double> _hasGS;
 131   PropertyObject<double> _gsDeflection;
 132   PropertyObject<double> _gsDeflectionDeg;
 133   PropertyObject<double> _gsDeflectionNorm;
 134 };
 135
 136 /* ---------------------------------------------------------------- */
 137
 138 class NavRadioComponent {
 139 public:
 140   NavRadioComponent( const std::string & name, SGPropertyNode_ptr rootNode );
 141   virtual ~NavRadioComponent();
 142
 143   virtual void   update( double dt, const SGGeod & aircraftPosition );
 144   virtual void   search( double frequency, const SGGeod & aircraftPosition );
 145   virtual double getRange_nm( const SGGeod & aircraftPosition );
 146   virtual void   display( NavIndicator & navIndicator ) = 0;
 147   virtual bool   valid() const { return NULL != _navRecord && true == _serviceable; }
 148   virtual const std::string getIdent() const { return _ident; }
 149
 150 protected:
 151   virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
 152   virtual FGNavList::TypeFilter* getNavaidFilter() = 0;
 153
 154   // General-purpose sawtooth function.  Graph looks like this:
 155   //         /\                                    .
 156   //       \/
 157   // Odd symmetry, inversion symmetry about the origin.
 158   // Unit slope at the origin.
 159   // Max 1, min -1, period 4.
 160   // Two zero-crossings per period, one with + slope, one with - slope.
 161   // Useful for false localizer courses.
 162   static double sawtooth(double xx)
 163   {
 164     return 4.0 * fabs(xx/4.0 + 0.25 - floor(xx/4.0 + 0.75)) - 1.0;
 165   }
 166
 167   SGPropertyNode_ptr _rootNode;
 168   const std::string _name;
 169   FGNavRecord * _navRecord;
 170   PropertyObject<bool>   _serviceable;
 171   PropertyObject<double> _signalQuality_norm;
 172   PropertyObject<double> _trueBearingTo_deg;
 173   PropertyObject<double> _trueBearingFrom_deg;
 174   PropertyObject<double> _trackDistance_m;
 175   PropertyObject<double> _slantDistance_m;
 176   PropertyObject<double> _heightAboveStation_ft;
 177   PropertyObject<std::string> _ident;
 178   PropertyObject<bool>   _inRange;
 179   PropertyObject<double> _range_nm;
 180 };
 181
 182 class NavRadioComponentWithIdent : public NavRadioComponent {
 183 public:
 184   NavRadioComponentWithIdent( const std::string & name, SGPropertyNode_ptr rootNode, AudioIdent * audioIdent );
 185   virtual ~NavRadioComponentWithIdent();
 186   void update( double dt, const SGGeod & aircraftPosition );
 187 protected:
 188   static std::string getIdentString( const std::string & name, int index );
 189 private:
 190   AudioIdent * _audioIdent;
 191   PropertyObject<double> _identVolume;
 192   PropertyObject<bool>   _identEnabled;
 193 };
 194
 195 std::string NavRadioComponentWithIdent::getIdentString( const std::string & name, int index )
 196 {
 197   std::ostringstream temp;
 198   temp << name << "-ident-" << index;
 199   return temp.str();
 200 }
 201
 202 NavRadioComponentWithIdent::NavRadioComponentWithIdent( const std::string & name, SGPropertyNode_ptr rootNode, AudioIdent * audioIdent ) :
 203   NavRadioComponent( name, rootNode ),
 204   _audioIdent( audioIdent ),
 205   _identVolume( rootNode->getNode(name,true)->getNode("ident-volume",true) ),
 206   _identEnabled( rootNode->getNode(name,true)->getNode("ident-enabled",true) )
 207 {
 208   _audioIdent->init();
 209
 210 }
 211 NavRadioComponentWithIdent::~NavRadioComponentWithIdent()
 212 {
 213   delete _audioIdent;
 214 }
 215
 216 void NavRadioComponentWithIdent::update( double dt, const SGGeod & aircraftPosition )
 217 {
 218   NavRadioComponent::update( dt, aircraftPosition );
 219   _audioIdent->update( dt );
 220
 221   if( false == ( valid() && _identEnabled && _signalQuality_norm > 0.1 ) ) {
 222       _audioIdent->setIdent("", 0.0 );
 223       return;
 224   }
 225   _audioIdent->setIdent( _ident, SGMiscd::clip(_identVolume, 0.0, 1.0) );
 226 }
 227
 228 NavRadioComponent::NavRadioComponent( const std::string & name, SGPropertyNode_ptr rootNode ) :
 229   _rootNode(rootNode),
 230   _name(name),
 231   _navRecord(NULL),
 232   _serviceable( rootNode->getNode(name,true)->getNode("serviceable",true) ),
 233   _signalQuality_norm( rootNode->getNode(name,true)->getNode("signal-quality-norm",true) ),
 234   _trueBearingTo_deg( rootNode->getNode(name,true)->getNode("true-bearing-to-deg",true) ),
 235   _trueBearingFrom_deg( rootNode->getNode(name,true)->getNode("true-bearing-from-deg",true) ),
 236   _trackDistance_m( rootNode->getNode(name,true)->getNode("track-distance-m",true) ),
 237   _slantDistance_m( rootNode->getNode(name,true)->getNode("slant-distance-m",true) ),
 238   _heightAboveStation_ft( rootNode->getNode(name,true)->getNode("height-above-station-ft",true) ),
 239   _ident( rootNode->getNode(name,true)->getNode("ident",true) ),
 240   _inRange( rootNode->getNode(name,true)->getNode("in-range",true) ),
 241   _range_nm( rootNode->getNode(_name,true)->getNode("range-nm",true) )
 242 {
 243   simgear::props::Type typ = _serviceable.node()->getType();
 244   if ((typ == simgear::props::NONE) || (typ == simgear::props::UNSPECIFIED))
 245     _serviceable = true;
 246 }
 247
 248 NavRadioComponent::~NavRadioComponent()
 249 {
 250 }
 251
 252 double NavRadioComponent::getRange_nm( const SGGeod & aircraftPosition )
 253 {
 254   if( _navRecord == NULL ) return 0.0; // no station: no range
 255   double d = _navRecord->get_range();
 256   if( d <= SGLimitsd::min() ) return 25.0; // no configured range: arbitrary number
 257   return d; // configured range
 258 }
 259
 260 void NavRadioComponent::search( double frequency, const SGGeod & aircraftPosition )
 261 {
 262   _navRecord = FGNavList::findByFreq(frequency, aircraftPosition, getNavaidFilter() );
 263   if( NULL == _navRecord ) {
 264     SG_LOG(SG_INSTR,SG_DEBUG, "No " << _name << " available at " << frequency );
 265     _ident = "";
 266     return;
 267   }
 268   SG_LOG(SG_INSTR,SG_INFO, "Using " << _name << "'" << _navRecord->get_ident() << "' for " << frequency );
 269   _ident = _navRecord->ident();
 270 }
 271
 272 double NavRadioComponent::computeSignalQuality_norm( const SGGeod & aircraftPosition )
 273 {
 274   if( false == valid() ) return 0.0;
 275
 276   double distance_nm = _slantDistance_m * SG_METER_TO_NM;
 277   double range_nm = _range_nm;
 278
 279   // assume signal quality is 100% up to the published range and
 280   // decay with the distance squared further out
 281   if ( distance_nm <= range_nm ) return 1.0;
 282   return range_nm*range_nm/(distance_nm*distance_nm);
 283 }
 284
 285 void NavRadioComponent::update( double dt, const SGGeod & aircraftPosition )
 286 {
 287     if( false == valid() ) {
 288       _signalQuality_norm = 0.0;
 289       _trueBearingTo_deg = 0.0;
 290       _trueBearingFrom_deg = 0.0;
 291       _trackDistance_m = 0.0;
 292       _slantDistance_m = 0.0;
 293       return;
 294     }
 295
 296     _slantDistance_m = dist(_navRecord->cart(), SGVec3d::fromGeod(aircraftPosition));
 297
 298     double az1 = 0.0, az2 = 0.0, dist = 0.0;
 299     SGGeodesy::inverse(aircraftPosition, _navRecord->geod(), az1, az2, dist );
 300     _trueBearingTo_deg = az1; _trueBearingFrom_deg = az2; _trackDistance_m = dist;
 301     _heightAboveStation_ft = SGMiscd::max(0.0, aircraftPosition.getElevationFt() - _navRecord->get_elev_ft());
 302
 303     _range_nm = getRange_nm(aircraftPosition);
 304     _signalQuality_norm = computeSignalQuality_norm( aircraftPosition );
 305     _inRange = _signalQuality_norm > 0.2;
 306 }
 307
 308 /* ---------------------------------------------------------------- */
 309
 310 static std::string VORTablePath( const char * name )
 311 {
 312     SGPath path( globals->get_fg_root() );
 313     path.append( "Navaids" );
 314     path.append(name);
 315     return path.str();
 316 }
 317
 318 class VOR : public NavRadioComponentWithIdent {
 319 public:
 320   VOR( SGPropertyNode_ptr rootNode);
 321   virtual ~VOR();
 322   virtual void update( double dt, const SGGeod & aircraftPosition );
 323   virtual void display( NavIndicator & navIndicator );
 324   virtual double getRange_nm(const SGGeod & aircraftPosition);
 325 protected:
 326   virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
 327   virtual FGNavList::TypeFilter* getNavaidFilter();
 328
 329 private:
 330   double _totalTime;
 331   class ServiceVolume {
 332   public:
 333     ServiceVolume() :
 334       term_tbl(VORTablePath("range.term")),
 335       low_tbl(VORTablePath("range.low")),
 336       high_tbl(VORTablePath("range.high")) {
 337     }
 338     double adjustRange( double height_ft, double nominalRange_nm );
 339
 340   private:
 341     SGInterpTable term_tbl;
 342     SGInterpTable low_tbl;
 343     SGInterpTable high_tbl;
 344   } _serviceVolume;
 345
 346   PropertyObject<double> _radial;
 347   PropertyObject<double> _radialInbound;
 348 };
 349
 350 // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8
 351 double VOR::ServiceVolume::adjustRange( double height_ft, double nominalRange_nm )
 352 {
 353     if (nominalRange_nm < SGLimitsd::min() )
 354       nominalRange_nm = FG_NAV_DEFAULT_RANGE;
 355
 356     // extend out actual usable range to be 1.3x the published safe range
 357     const double usability_factor = 1.3;
 358
 359     // assumptions we model the standard service volume, plus
 360     // ... rather than specifying a cylinder, we model a cone that
 361     // contains the cylinder.  Then we put an upside down cone on top
 362     // to model diminishing returns at too-high altitudes.
 363
 364     if ( nominalRange_nm < 25.0 + SG_EPSILON ) {
 365         // Standard Terminal Service Volume
 366         return term_tbl.interpolate( height_ft ) * usability_factor;
 367     } else if ( nominalRange_nm < 50.0 + SG_EPSILON ) {
 368         // Standard Low Altitude Service Volume
 369         // table is based on range of 40, scale to actual range
 370         return low_tbl.interpolate( height_ft ) * nominalRange_nm / 40.0
 371             * usability_factor;
 372     } else {
 373         // Standard High Altitude Service Volume
 374         // table is based on range of 130, scale to actual range
 375         return high_tbl.interpolate( height_ft ) * nominalRange_nm / 130.0
 376             * usability_factor;
 377     }
 378 }
 379
 380 VOR::VOR( SGPropertyNode_ptr rootNode) :
 381     NavRadioComponentWithIdent("vor", rootNode,
 382                                new VORAudioIdent(getIdentString(std::string("vor"),
 383                                                                 rootNode->getIndex()))),
 384   _totalTime(0.0),
 385   _radial( rootNode->getNode(_name,true)->getNode("radial",true) ),
 386   _radialInbound( rootNode->getNode(_name,true)->getNode("radial-inbound",true) )
 387 {
 388 }
 389
 390 VOR::~VOR()
 391 {
 392 }
 393
 394 double VOR::getRange_nm( const SGGeod & aircraftPosition )
 395 {
 396   return _serviceVolume.adjustRange( _heightAboveStation_ft, _navRecord->get_range() );
 397 }
 398
 399 FGNavList::TypeFilter* VOR::getNavaidFilter()
 400 {
 401   static FGNavList::TypeFilter filter(FGPositioned::VOR);
 402   return &filter;
 403 }
 404
 405 double VOR::computeSignalQuality_norm( const SGGeod & aircraftPosition )
 406 {
 407   // apply cone of confusion. Some sources say it's opening angle is 53deg, others estimate
 408   // a diameter of 1NM per 6000ft (approx. 45deg). ICAO Annex 10 says minimum 40deg.
 409   // We use 1NM@6000ft and a distance-squared
 410   // function to make signal-quality=100% 0.5NM@6000ft from the center and zero overhead
 411   double cone_of_confusion_width = 0.5 * _heightAboveStation_ft / 6000.0 * SG_NM_TO_METER;
 412   if( _trackDistance_m < cone_of_confusion_width ) {
 413     double d = cone_of_confusion_width <= SGLimitsd::min() ? 1 :
 414               (1 - _trackDistance_m/cone_of_confusion_width);
 415     return 1-d*d;
 416   }
 417
 418   // use default decay function outside the cone of confusion
 419   return NavRadioComponentWithIdent::computeSignalQuality_norm( aircraftPosition );
 420 }
 421
 422 void VOR::update( double dt, const SGGeod & aircraftPosition )
 423 {
 424   _totalTime += dt;
 425   NavRadioComponentWithIdent::update( dt, aircraftPosition );
 426
 427   if( false == valid() ) {
 428       _radial = 0.0;
 429       return;
 430   }
 431
 432   // an arbitrary error function
 433   double error = 0.5*(sin(_totalTime/11.0) + sin(_totalTime/23.0));
 434
 435   // add 1% error at 100% signal-quality
 436   // add 50% error at  0% signal-quality
 437   // of full deflection (+/-10deg)
 438   double e = 10.0 * ( 0.01 + (1-_signalQuality_norm) * 0.49 ) * error;
 439
 440   // compute magnetic bearing from the station (aka current radial)
 441   double r = SGMiscd::normalizePeriodic(0.0, 360.0, _trueBearingFrom_deg - _navRecord->get_multiuse() + e );
 442
 443   _radial = r;
 444   _radialInbound = SGMiscd::normalizePeriodic(0.0,360.0, 180.0 + _radial);
 445 }
 446
 447 void VOR::display( NavIndicator & navIndicator )
 448 {
 449   if( false == valid() ) return;
 450
 451   double offset = SGMiscd::normalizePeriodic(-180.0,180.0,_radial - navIndicator.getSelectedCourse());
 452   bool to = fabs(offset) >= 90.0;
 453
 454   if( to ) offset = -offset + copysign(180.0,offset);
 455
 456   navIndicator.showTo( to );
 457   navIndicator.showFrom( !to );
 458   // normalize to +/- 1.0 for +/- 10deg, decrease deflection with decreasing signal
 459   navIndicator.setCDI( SGMiscd::clip( -offset/10.0, -1.0, 1.0 ) * _signalQuality_norm );
 460   navIndicator.setSignalQuality( _signalQuality_norm );
 461 }
 462
 463 /* ---------------------------------------------------------------- */
 464 class LOC : public NavRadioComponentWithIdent {
 465 public:
 466   LOC( SGPropertyNode_ptr rootNode );
 467   virtual ~LOC();
 468   virtual void update( double dt, const SGGeod & aircraftPosition );
 469   virtual void search( double frequency, const SGGeod & aircraftPosition );
 470   virtual void display( NavIndicator & navIndicator );
 471   virtual double getRange_nm(const SGGeod & aircraftPosition);
 472
 473 protected:
 474   virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
 475   virtual FGNavList::TypeFilter* getNavaidFilter();
 476
 477 private:
 478   class ServiceVolume {
 479   public:
 480       ServiceVolume();
 481       double adjustRange( double azimuthAngle_deg, double elevationAngle_deg );
 482   private:
 483       SGInterpTable _azimuthTable;
 484       SGInterpTable _elevationTable;
 485   } _serviceVolume;
 486   PropertyObject<double> _localizerOffset_norm;
 487   PropertyObject<double> _localizerOffset_m;
 488   PropertyObject<double> _localizerWidth_deg;
 489 };
 490
 491 LOC::ServiceVolume::ServiceVolume()
 492 {
 493 // maybe this: http://www.tpub.com/content/aviation2/P-1244/P-12440125.htm
 494   // ICAO Annex 10 - 3.1.3.2.2: The emission from the localizer
 495   // shall be horizontally polarized
 496   // very rough abstraction of a 5-element yagi antenna's
 497   // E-plane radiation diagram
 498   _azimuthTable.addEntry(   0.0, 1.0 );
 499   _azimuthTable.addEntry(  10.0, 1.0 );
 500   _azimuthTable.addEntry(  30.0, 0.75 );
 501   _azimuthTable.addEntry(  40.0, 0.50 );
 502   _azimuthTable.addEntry(  50.0, 0.20 );
 503   _azimuthTable.addEntry(  60.0, 0.10 );
 504   _azimuthTable.addEntry(  70.0, 0.20 );
 505   _azimuthTable.addEntry(  80.0, 0.10 );
 506   _azimuthTable.addEntry(  90.0, 0.05 );
 507   _azimuthTable.addEntry( 105.0, 0.10 );
 508   _azimuthTable.addEntry( 130.0, 0.05 );
 509   _azimuthTable.addEntry( 150.0, 0.30 );
 510   _azimuthTable.addEntry( 160.0, 0.40 );
 511   _azimuthTable.addEntry( 170.0, 0.50 );
 512   _azimuthTable.addEntry( 180.0, 0.50 );
 513
 514   _elevationTable.addEntry(   0.0, 0.1 );
 515   _elevationTable.addEntry(  1.05, 1.0 );
 516   _elevationTable.addEntry(  7.00, 1.0 );
 517   _elevationTable.addEntry(  45.0, 0.3 );
 518   _elevationTable.addEntry(  90.0, 0.1 );
 519   _elevationTable.addEntry( 180.0, 0.01 );
 520 }
 521
 522 double LOC::ServiceVolume::adjustRange( double azimuthAngle_deg, double elevationAngle_deg )
 523 {
 524     return _azimuthTable.interpolate( fabs(azimuthAngle_deg) ) *
 525         _elevationTable.interpolate( fabs(elevationAngle_deg) );
 526 }
 527
 528 LOC::LOC( SGPropertyNode_ptr rootNode) :
 529     NavRadioComponentWithIdent("loc", rootNode, new LOCAudioIdent(getIdentString(std::string("loc"),
 530                                                                                  rootNode->getIndex()))),
 531   _serviceVolume(),
 532   _localizerOffset_norm( rootNode->getNode(_name,true)->getNode("offset-norm",true) ),
 533   _localizerOffset_m( rootNode->getNode(_name,true)->getNode("offset-m",true) ),
 534   _localizerWidth_deg( rootNode->getNode(_name,true)->getNode("width-deg",true) )
 535 {
 536 }
 537
 538 LOC::~LOC()
 539 {
 540 }
 541
 542 FGNavList::TypeFilter* LOC::getNavaidFilter()
 543 {
 544   return FGNavList::locFilter();
 545 }
 546
 547 void LOC::search( double frequency, const SGGeod & aircraftPosition )
 548 {
 549   NavRadioComponentWithIdent::search( frequency, aircraftPosition );
 550   if( false == valid() ) {
 551       _localizerWidth_deg = 0.0;
 552       return;
 553   }
 554
 555   // cache slightly expensive value,
 556   // sanitized in FGNavRecord::localizerWidth() to  never become zero
 557   _localizerWidth_deg = _navRecord->localizerWidth();
 558 }
 559
 560 /* Localizer coverage (ICAO Annex 10 Volume I 3.1.3.3
 561   25NM within +/-10 deg from the front course line
 562   17NM between 10 and 35deg from the front course line
 563   10NM outside of +/- 35deg  if coverage is provided
 564   at and above a height of 2000ft above threshold or
 565   1000ft above the highest point within intermediate
 566   and final approach areas. Upper limit is a surface
 567   extending outward from the localizer and inclined at
 568   7 degrees above the horizontal
 569  */
 570 double LOC::getRange_nm(const SGGeod & aircraftPosition)
 571 {
 572   double elevationAngle = ::atan2(_heightAboveStation_ft*SG_FEET_TO_METER, _trackDistance_m)*SG_RADIANS_TO_DEGREES;
 573   double azimuthAngle = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
 574
 575   // looks like our navrecord declared range is based on 10NM?
 576   return  _navRecord->get_range() * _serviceVolume.adjustRange( azimuthAngle, elevationAngle );
 577 }
 578
 579 double LOC::computeSignalQuality_norm( const SGGeod & aircraftPosition )
 580 {
 581   return NavRadioComponentWithIdent::computeSignalQuality_norm( aircraftPosition );
 582 }
 583
 584 void LOC::update( double dt, const SGGeod & aircraftPosition )
 585 {
 586   NavRadioComponentWithIdent::update( dt, aircraftPosition );
 587
 588   if( false == valid() ) {
 589     _localizerOffset_norm = 0.0;
 590     _localizerOffset_m = 0.0;
 591     return;
 592   }
 593
 594   double offsetDeg = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
 595
 596   // cross-track error (in meters)
 597   _localizerOffset_m = _trackDistance_m * sin(offsetDeg * SGD_DEGREES_TO_RADIANS);
 598
 599   // The factor of 30.0 gives a period of 120 which gives us 3 cycles and six
 600   // zeros i.e. six courses: one front course, one back course, and four
 601   // false courses. Three of the six are reverse sensing.
 602   offsetDeg = 30.0 * sawtooth(offsetDeg / 30.0);
 603
 604   // normalize offsetDeg to the localizer width, scale and clip to [-1..1]
 605   offsetDeg = SGMiscd::clip( 2.0 * offsetDeg / _localizerWidth_deg, -1.0, 1.0 );
 606
 607   _localizerOffset_norm = offsetDeg;
 608 }
 609
 610 void LOC::display( NavIndicator & navIndicator )
 611 {
 612   if( false == valid() )
 613     return;
 614
 615   navIndicator.showTo( true );
 616   navIndicator.showFrom( false );
 617
 618   navIndicator.setCDI( _localizerOffset_norm * _signalQuality_norm );
 619   navIndicator.setSignalQuality( _signalQuality_norm );
 620 }
 621
 622 class GS : public NavRadioComponent {
 623 public:
 624   GS( SGPropertyNode_ptr rootNode);
 625   virtual ~GS();
 626   virtual void update( double dt, const SGGeod & aircraftPosition );
 627   virtual void search( double frequency, const SGGeod & aircraftPosition );
 628   virtual void display( NavIndicator & navIndicator );
 629
 630   virtual double getRange_nm(const SGGeod & aircraftPosition);
 631 protected:
 632   virtual FGNavList::TypeFilter* getNavaidFilter();
 633 private:
 634   class ServiceVolume {
 635   public:
 636       ServiceVolume();
 637       double adjustRange( double azimuthAngle_deg, double elevationAngle_deg );
 638   private:
 639       SGInterpTable _azimuthTable;
 640       SGInterpTable _elevationTable;
 641   } _serviceVolume;
 642   static SGVec3d tangentVector(const SGGeod& midpoint, const double heading);
 643
 644   PropertyObject<double>  _targetGlideslope_deg;
 645   PropertyObject<double>  _glideslopeOffset_norm;
 646   SGVec3d _gsAxis;
 647   SGVec3d _gsVertical;
 648 };
 649
 650 GS::ServiceVolume::ServiceVolume()
 651 {
 652 // maybe this: http://www.tpub.com/content/aviation2/P-1244/P-12440125.htm
 653   // ICAO Annex 10 - 3.1.5.2.2: The emission from the glide path equipment
 654   // shall be horizontally polarized
 655   // very rough abstraction of a 5-element yagi antenna's
 656   // E-plane radiation diagram
 657   _azimuthTable.addEntry(   0.0, 1.0 );
 658   _azimuthTable.addEntry(  10.0, 1.0 );
 659   _azimuthTable.addEntry(  30.0, 0.75 );
 660   _azimuthTable.addEntry(  40.0, 0.50 );
 661   _azimuthTable.addEntry(  50.0, 0.20 );
 662   _azimuthTable.addEntry(  60.0, 0.10 );
 663   _azimuthTable.addEntry(  70.0, 0.20 );
 664   _azimuthTable.addEntry(  80.0, 0.10 );
 665   _azimuthTable.addEntry(  90.0, 0.05 );
 666   _azimuthTable.addEntry( 105.0, 0.10 );
 667   _azimuthTable.addEntry( 130.0, 0.05 );
 668   _azimuthTable.addEntry( 150.0, 0.30 );
 669   _azimuthTable.addEntry( 160.0, 0.40 );
 670   _azimuthTable.addEntry( 170.0, 0.50 );
 671   _azimuthTable.addEntry( 180.0, 0.50 );
 672
 673   _elevationTable.addEntry(   0.0, 0.1 );
 674   _elevationTable.addEntry(  1.05, 1.0 );
 675   _elevationTable.addEntry(  7.00, 1.0 );
 676   _elevationTable.addEntry(  45.0, 0.3 );
 677   _elevationTable.addEntry(  90.0, 0.1 );
 678   _elevationTable.addEntry( 180.0, 0.01 );
 679 }
 680
 681 double GS::ServiceVolume::adjustRange( double azimuthAngle_deg, double elevationAngle_deg )
 682 {
 683     return _azimuthTable.interpolate( fabs(azimuthAngle_deg) ) *
 684         _elevationTable.interpolate( fabs(elevationAngle_deg) );
 685 }
 686
 687 GS::GS( SGPropertyNode_ptr rootNode) :
 688   NavRadioComponent("gs", rootNode ),
 689   _targetGlideslope_deg( rootNode->getNode(_name,true)->getNode("slope",true) ),
 690   _glideslopeOffset_norm( rootNode->getNode(_name,true)->getNode("offset-norm",true) ),
 691   _gsAxis(SGVec3d::zeros()),
 692   _gsVertical(SGVec3d::zeros())
 693 {
 694 }
 695
 696 GS::~GS()
 697 {
 698 }
 699
 700 FGNavList::TypeFilter* GS::getNavaidFilter()
 701 {
 702   static FGNavList::TypeFilter filter(FGPositioned::GS);
 703   return &filter;
 704 }
 705
 706 double GS::getRange_nm(const SGGeod & aircraftPosition)
 707 {
 708   double elevationAngle = ::atan2(_heightAboveStation_ft*SG_FEET_TO_METER, _trackDistance_m)*SG_RADIANS_TO_DEGREES;
 709   double azimuthAngle = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - fmod(_navRecord->get_multiuse(), 1000.0) );
 710   return  _navRecord->get_range() * _serviceVolume.adjustRange( azimuthAngle, elevationAngle );
 711 }
 712
 713 // Calculate a Cartesian unit vector in the
 714 // local horizontal plane, i.e. tangent to the
 715 // surface of the earth at the local ground zero.
 716 // The tangent vector passes through the given  <midpoint>
 717 // and points forward along the given <heading>.
 718 // The <heading> is given in degrees.
 719 SGVec3d GS::tangentVector(const SGGeod& midpoint, const double heading)
 720 {
 721   // move 100m away from the midpoint - arbitrary number
 722   const double delta(100.0);
 723   SGGeod head, tail;
 724   double az2;                   // ignored
 725   SGGeodesy::direct(midpoint, heading,     delta, head, az2);
 726   SGGeodesy::direct(midpoint, 180+heading, delta, tail, az2);
 727   head.setElevationM(midpoint.getElevationM());
 728   tail.setElevationM(midpoint.getElevationM());
 729   SGVec3d head_xyz = SGVec3d::fromGeod(head);
 730   SGVec3d tail_xyz = SGVec3d::fromGeod(tail);
 731 // Awkward formula here, needed because vector-by-scalar
 732 // multiplication is defined, but not vector-by-scalar division.
 733   return (head_xyz - tail_xyz) * (0.5/delta);
 734 }
 735
 736 void GS::search( double frequency, const SGGeod & aircraftPosition )
 737 {
 738   NavRadioComponent::search( frequency, aircraftPosition );
 739   if( false == valid() ) {
 740       _gsAxis = SGVec3d::zeros();
 741       _gsVertical = SGVec3d::zeros();
 742       _targetGlideslope_deg = 3.0;
 743       return;
 744   }
 745
 746   double gs_radial = SGMiscd::normalizePeriodic(0.0, 360.0, fmod(_navRecord->get_multiuse(), 1000.0) );
 747
 748   _gsAxis = tangentVector(_navRecord->geod(), gs_radial);
 749   SGVec3d gsBaseline = tangentVector(_navRecord->geod(), gs_radial + 90.0);
 750   _gsVertical = cross(gsBaseline, _gsAxis);
 751
 752   int tmp = (int)(_navRecord->get_multiuse() / 1000.0);
 753   // catch unconfigured glideslopes here, they will cause nan later
 754   _targetGlideslope_deg = SGMiscd::max( 1.0, (double)tmp / 100.0 );
 755 }
 756
 757 void GS::update( double dt, const SGGeod & aircraftPosition )
 758 {
 759   NavRadioComponent::update( dt, aircraftPosition );
 760   if( false == valid() ) {
 761       _glideslopeOffset_norm = 0.0;
 762       return;
 763   }
 764
 765   SGVec3d pos = SGVec3d::fromGeod(aircraftPosition) - _navRecord->cart(); // relative vector from gs antenna to aircraft
 766   // The positive GS axis points along the runway in the landing direction,
 767   // toward the far end, not toward the approach area, so we need a - sign here:
 768   double comp_h = -dot(pos, _gsAxis);      // component in horiz direction
 769   double comp_v = dot(pos, _gsVertical);   // component in vertical direction
 770   //double comp_b = dot(pos, _gsBaseline);   // component in baseline direction
 771   //if (comp_b) {}                           // ... (useful for debugging)
 772
 773 // _gsDirect represents the angle of elevation of the aircraft
 774 // as seen by the GS transmitter.
 775   double gsDirect = atan2(comp_v, comp_h) * SGD_RADIANS_TO_DEGREES;
 776 // At this point, if the aircraft is centered on the glide slope,
 777 // _gsDirect will be a small positive number, e.g. 3.0 degrees
 778
 779 // Aim the branch cut straight down
 780 // into the ground below the GS transmitter:
 781   if (gsDirect < -90.0) gsDirect += 360.0;
 782
 783   double offset = _targetGlideslope_deg - gsDirect;
 784   if( offset < 0.0 )
 785     offset = _targetGlideslope_deg/2 * sawtooth(2.0*offset/_targetGlideslope_deg);
 786   assert( false == isnan(offset) );
 787 // GS is documented to be 1.4 degrees thick,
 788 // i.e. plus or minus 0.7 degrees from the midline:
 789   _glideslopeOffset_norm = SGMiscd::clip(offset/0.7, -1.0, 1.0);
 790 }
 791
 792 void GS::display( NavIndicator & navIndicator )
 793 {
 794   if( false == valid() ) {
 795     navIndicator.setGS( false );
 796     return;
 797   }
 798   navIndicator.setGS( true );
 799   navIndicator.setGS( _glideslopeOffset_norm );
 800 }
 801
 802 /* ------------- The NavRadio implementation ---------------------- */
 803
 804 class NavRadioImpl : public NavRadio {
 805 public:
 806   NavRadioImpl( SGPropertyNode_ptr node );
 807   virtual ~NavRadioImpl();
 808
 809   virtual void update( double dt );
 810   virtual void init();
 811 private:
 812   void search();
 813
 814   class Legacy {
 815   public:
 816       Legacy( NavRadioImpl * navRadioImpl ) : _navRadioImpl( navRadioImpl ) {}
 817
 818       void init();
 819       void update( double dt );
 820   private:
 821       NavRadioImpl * _navRadioImpl;
 822       SGPropertyNode_ptr is_valid_node;
 823       SGPropertyNode_ptr nav_serviceable_node;
 824       SGPropertyNode_ptr nav_id_node;
 825       SGPropertyNode_ptr id_c1_node;
 826       SGPropertyNode_ptr id_c2_node;
 827       SGPropertyNode_ptr id_c3_node;
 828       SGPropertyNode_ptr id_c4_node;
 829   } _legacy;
 830
 831   const static int VOR_COMPONENT = 0;
 832   const static int LOC_COMPONENT = 1;
 833   const static int GS_COMPONENT  = 2;
 834
 835   std::string _name;
 836   int         _num;
 837   SGPropertyNode_ptr _rootNode;
 838   FrequencyFormatter _useFrequencyFormatter;
 839   FrequencyFormatter _stbyFrequencyFormatter;
 840   std::vector<NavRadioComponent*> _components;
 841   NavIndicator _navIndicator;
 842   double _stationTTL;
 843   double _frequency;
 844   PropertyObject<bool> _cdiDisconnected;
 845 };
 846
 847 NavRadioImpl::NavRadioImpl( SGPropertyNode_ptr node ) :
 848   _legacy( this ),
 849   _name(node->getStringValue("name", "nav")),
 850   _num(node->getIntValue("number", 0)),
 851   _rootNode(fgGetNode( std::string("/instrumentation/") + _name, _num, true)),
 852   _useFrequencyFormatter( _rootNode->getNode("frequencies/selected-mhz",true), _rootNode->getNode("frequencies/selected-mhz-fmt",true), 0.05, 108.0, 118.0 ),
 853   _stbyFrequencyFormatter( _rootNode->getNode("frequencies/standby-mhz",true), _rootNode->getNode("frequencies/standby-mhz-fmt",true), 0.05, 108.0, 118.0 ),
 854   _navIndicator(_rootNode),
 855   _stationTTL(0.0),
 856   _frequency(-1.0),
 857   _cdiDisconnected(_rootNode->getNode("cdi-disconnected",true))
 858 {
 859 }
 860
 861 NavRadioImpl::~NavRadioImpl()
 862 {
 863   BOOST_FOREACH( NavRadioComponent * p, _components ) {
 864     delete p;
 865   }
 866 }
 867
 868 void NavRadioImpl::init()
 869 {
 870   if( ! _components.empty() )
 871     return;
 872
 873   _components.push_back( new VOR(_rootNode) );
 874   _components.push_back( new LOC(_rootNode) );
 875   _components.push_back( new GS(_rootNode) );
 876
 877   _legacy.init();
 878 }
 879
 880 void NavRadioImpl::search()
 881 {
 882 }
 883
 884 void NavRadioImpl::update( double dt )
 885 {
 886   if( dt < SGLimitsd::min() ) return;
 887
 888   SGGeod position;
 889
 890   try {
 891     position = globals->get_aircraft_position();
 892   }
 893   catch( std::exception & ) {
 894     return;
 895   }
 896
 897   _stationTTL -= dt;
 898   if( _frequency != _useFrequencyFormatter.getFrequency() ) {
 899       _frequency = _useFrequencyFormatter.getFrequency();
 900       _stationTTL = 0.0;
 901   }
 902
 903   BOOST_FOREACH( NavRadioComponent * p, _components ) {
 904       if( _stationTTL <= 0.0 )
 905           p->search( _frequency, position );
 906       p->update( dt, position );
 907
 908       if( false == _cdiDisconnected )
 909           p->display( _navIndicator );
 910   }
 911
 912   if( _stationTTL <= 0.0 )
 913       _stationTTL = 30.0;
 914
 915   _legacy.update( dt );
 916 }
 917
 918 void NavRadioImpl::Legacy::init()
 919 {
 920     is_valid_node = _navRadioImpl->_rootNode->getChild("data-is-valid", 0, true);
 921     nav_serviceable_node = _navRadioImpl->_rootNode->getChild("serviceable", 0, true);
 922
 923     nav_id_node = _navRadioImpl->_rootNode->getChild("nav-id", 0, true );
 924     id_c1_node = _navRadioImpl->_rootNode->getChild("nav-id_asc1", 0, true );
 925     id_c2_node = _navRadioImpl->_rootNode->getChild("nav-id_asc2", 0, true );
 926     id_c3_node = _navRadioImpl->_rootNode->getChild("nav-id_asc3", 0, true );
 927     id_c4_node = _navRadioImpl->_rootNode->getChild("nav-id_asc4", 0, true );
 928
 929 }
 930
 931 void NavRadioImpl::Legacy::update( double dt )
 932 {
 933     is_valid_node->setBoolValue(
 934         _navRadioImpl->_components[VOR_COMPONENT]->valid() || _navRadioImpl->_components[LOC_COMPONENT]->valid()
 935         );
 936
 937     std::string ident = _navRadioImpl->_components[VOR_COMPONENT]->getIdent();
 938     if( ident.empty() )
 939         ident = _navRadioImpl->_components[LOC_COMPONENT]->getIdent();
 940
 941     nav_id_node->setStringValue( ident );
 942
 943     ident = simgear::strutils::rpad( ident, 4, ' ' );
 944     id_c1_node->setIntValue( (int)ident[0] );
 945     id_c2_node->setIntValue( (int)ident[1] );
 946     id_c3_node->setIntValue( (int)ident[2] );
 947     id_c4_node->setIntValue( (int)ident[3] );
 948 }
 949
 950
 951 SGSubsystem * NavRadio::createInstance( SGPropertyNode_ptr rootNode )
 952 {
 953     // use old navradio code by default
 954     if( fgGetBool( "/instrumentation/use-new-navradio", false ) )
 955         return new NavRadioImpl( rootNode );
 956
 957     return new FGNavRadio( rootNode );
 958 }
 959
 960 } // namespace Instrumentation
 961