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