1 // navradio.cxx -- class to manage a nav radio instance
3 // Written by Curtis Olson, started April 2000.
4 // Rewritten by Torsten Dreyer, August 2011
6 // Copyright (C) 2000 - 2011 Curtis L. Olson - http://www.flightgear.org/~curt
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.
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.
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.
27 #include "newnavradio.hxx"
30 #include <boost/foreach.hpp>
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>
38 #include <Main/fg_props.hxx>
39 #include <Navaids/navlist.hxx>
40 #include <Sound/audioident.hxx>
42 #include "navradio.hxx"
45 namespace Instrumentation {
47 using simgear::PropertyObject;
49 /* --------------The Navigation Indicator ----------------------------- */
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 ))
67 virtual ~NavIndicator() {}
70 * set the normalized CDI deflection
71 * @param norm the cdi deflection normalized [-1..1]
73 void setCDI( double norm )
80 * set the normalized GS deflection
81 * @param norm the gs deflection normalized to [-1..1]
83 void setGS( double norm )
85 _gsDeflectionNorm = norm;
86 _gsDeflectionDeg = norm * 0.7;
87 _gsDeflection = norm * 3.5;
90 void setGS( bool enabled )
98 void showFrom( bool on )
103 void showTo( bool on )
108 void setSelectedCourse( double course )
113 double getSelectedCourse() const
115 return SGMiscd::normalizePeriodic(0.0, 360.0, _course );
118 void setSignalQuality( double signalQuality )
120 _signalQuality = signalQuality;
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;
136 /* ---------------------------------------------------------------- */
138 class NavRadioComponent {
140 NavRadioComponent( const std::string & name, SGPropertyNode_ptr rootNode );
141 virtual ~NavRadioComponent();
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; }
151 virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
152 virtual FGNavList::TypeFilter* getNavaidFilter() = 0;
154 // General-purpose sawtooth function. Graph looks like this:
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)
164 return 4.0 * fabs(xx/4.0 + 0.25 - floor(xx/4.0 + 0.75)) - 1.0;
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;
182 class NavRadioComponentWithIdent : public NavRadioComponent {
184 NavRadioComponentWithIdent( const std::string & name, SGPropertyNode_ptr rootNode, AudioIdent * audioIdent );
185 virtual ~NavRadioComponentWithIdent();
186 void update( double dt, const SGGeod & aircraftPosition );
188 static std::string getIdentString( const std::string & name, int index );
190 AudioIdent * _audioIdent;
191 PropertyObject<double> _identVolume;
192 PropertyObject<bool> _identEnabled;
195 std::string NavRadioComponentWithIdent::getIdentString( const std::string & name, int index )
197 std::ostringstream temp;
198 temp << name << "-ident-" << index;
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) )
211 NavRadioComponentWithIdent::~NavRadioComponentWithIdent()
216 void NavRadioComponentWithIdent::update( double dt, const SGGeod & aircraftPosition )
218 NavRadioComponent::update( dt, aircraftPosition );
219 _audioIdent->update( dt );
221 if( false == ( valid() && _identEnabled && _signalQuality_norm > 0.1 ) ) {
222 _audioIdent->setIdent("", 0.0 );
225 _audioIdent->setIdent( _ident, SGMiscd::clip(_identVolume, 0.0, 1.0) );
228 NavRadioComponent::NavRadioComponent( const std::string & name, SGPropertyNode_ptr rootNode ) :
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) )
243 simgear::props::Type typ = _serviceable.node()->getType();
244 if ((typ == simgear::props::NONE) || (typ == simgear::props::UNSPECIFIED))
248 NavRadioComponent::~NavRadioComponent()
252 double NavRadioComponent::getRange_nm( const SGGeod & aircraftPosition )
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
260 void NavRadioComponent::search( double frequency, const SGGeod & aircraftPosition )
262 _navRecord = FGNavList::findByFreq(frequency, aircraftPosition, getNavaidFilter() );
263 if( NULL == _navRecord ) {
264 SG_LOG(SG_INSTR,SG_ALERT, "No " << _name << " available at " << frequency );
268 SG_LOG(SG_INSTR,SG_ALERT, "Using " << _name << "'" << _navRecord->get_ident() << "' for " << frequency );
269 _ident = _navRecord->ident();
272 double NavRadioComponent::computeSignalQuality_norm( const SGGeod & aircraftPosition )
274 if( false == valid() ) return 0.0;
276 double distance_nm = _slantDistance_m * SG_METER_TO_NM;
277 double range_nm = _range_nm;
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);
285 void NavRadioComponent::update( double dt, const SGGeod & aircraftPosition )
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;
296 _slantDistance_m = dist(_navRecord->cart(), SGVec3d::fromGeod(aircraftPosition));
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());
303 _range_nm = getRange_nm(aircraftPosition);
304 _signalQuality_norm = computeSignalQuality_norm( aircraftPosition );
305 _inRange = _signalQuality_norm > 0.2;
308 /* ---------------------------------------------------------------- */
310 static std::string VORTablePath( const char * name )
312 SGPath path( globals->get_fg_root() );
313 path.append( "Navaids" );
318 class VOR : public NavRadioComponentWithIdent {
320 VOR( SGPropertyNode_ptr rootNode);
322 virtual void update( double dt, const SGGeod & aircraftPosition );
323 virtual void display( NavIndicator & navIndicator );
324 virtual double getRange_nm(const SGGeod & aircraftPosition);
326 virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
327 virtual FGNavList::TypeFilter* getNavaidFilter();
331 class ServiceVolume {
334 term_tbl(VORTablePath("range.term")),
335 low_tbl(VORTablePath("range.low")),
336 high_tbl(VORTablePath("range.high")) {
338 double adjustRange( double height_ft, double nominalRange_nm );
341 SGInterpTable term_tbl;
342 SGInterpTable low_tbl;
343 SGInterpTable high_tbl;
346 PropertyObject<double> _radial;
347 PropertyObject<double> _radialInbound;
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 )
353 if (nominalRange_nm < SGLimitsd::min() )
354 nominalRange_nm = FG_NAV_DEFAULT_RANGE;
356 // extend out actual usable range to be 1.3x the published safe range
357 const double usability_factor = 1.3;
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.
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
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
380 VOR::VOR( SGPropertyNode_ptr rootNode) :
381 NavRadioComponentWithIdent("vor", rootNode,
382 new VORAudioIdent(getIdentString(std::string("vor"),
383 rootNode->getIndex()))),
385 _radial( rootNode->getNode(_name,true)->getNode("radial",true) ),
386 _radialInbound( rootNode->getNode(_name,true)->getNode("radial-inbound",true) )
394 double VOR::getRange_nm( const SGGeod & aircraftPosition )
396 return _serviceVolume.adjustRange( _heightAboveStation_ft, _navRecord->get_range() );
399 FGNavList::TypeFilter* VOR::getNavaidFilter()
401 static FGNavList::TypeFilter filter(FGPositioned::VOR);
405 double VOR::computeSignalQuality_norm( const SGGeod & aircraftPosition )
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);
418 // use default decay function outside the cone of confusion
419 return NavRadioComponentWithIdent::computeSignalQuality_norm( aircraftPosition );
422 void VOR::update( double dt, const SGGeod & aircraftPosition )
425 NavRadioComponentWithIdent::update( dt, aircraftPosition );
427 if( false == valid() ) {
432 // an arbitrary error function
433 double error = 0.5*(sin(_totalTime/11.0) + sin(_totalTime/23.0));
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;
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 );
444 _radialInbound = SGMiscd::normalizePeriodic(0.0,360.0, 180.0 + _radial);
447 void VOR::display( NavIndicator & navIndicator )
449 if( false == valid() ) return;
451 double offset = SGMiscd::normalizePeriodic(-180.0,180.0,_radial - navIndicator.getSelectedCourse());
452 bool to = fabs(offset) >= 90.0;
454 if( to ) offset = -offset + copysign(180.0,offset);
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 );
463 /* ---------------------------------------------------------------- */
464 class LOC : public NavRadioComponentWithIdent {
466 LOC( SGPropertyNode_ptr rootNode );
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);
474 virtual double computeSignalQuality_norm( const SGGeod & aircraftPosition );
475 virtual FGNavList::TypeFilter* getNavaidFilter();
478 class ServiceVolume {
481 double adjustRange( double azimuthAngle_deg, double elevationAngle_deg );
483 SGInterpTable _azimuthTable;
484 SGInterpTable _elevationTable;
486 PropertyObject<double> _localizerOffset_norm;
487 PropertyObject<double> _localizerWidth_deg;
490 LOC::ServiceVolume::ServiceVolume()
492 // maybe this: http://www.tpub.com/content/aviation2/P-1244/P-12440125.htm
493 // ICAO Annex 10 - 3.1.3.2.2: The emission from the localizer
494 // shall be horizontally polarized
495 // very rough abstraction of a 5-element yagi antenna's
496 // E-plane radiation diagram
497 _azimuthTable.addEntry( 0.0, 1.0 );
498 _azimuthTable.addEntry( 10.0, 1.0 );
499 _azimuthTable.addEntry( 30.0, 0.75 );
500 _azimuthTable.addEntry( 40.0, 0.50 );
501 _azimuthTable.addEntry( 50.0, 0.20 );
502 _azimuthTable.addEntry( 60.0, 0.10 );
503 _azimuthTable.addEntry( 70.0, 0.20 );
504 _azimuthTable.addEntry( 80.0, 0.10 );
505 _azimuthTable.addEntry( 90.0, 0.05 );
506 _azimuthTable.addEntry( 105.0, 0.10 );
507 _azimuthTable.addEntry( 130.0, 0.05 );
508 _azimuthTable.addEntry( 150.0, 0.30 );
509 _azimuthTable.addEntry( 160.0, 0.40 );
510 _azimuthTable.addEntry( 170.0, 0.50 );
511 _azimuthTable.addEntry( 180.0, 0.50 );
513 _elevationTable.addEntry( 0.0, 0.1 );
514 _elevationTable.addEntry( 1.05, 1.0 );
515 _elevationTable.addEntry( 7.00, 1.0 );
516 _elevationTable.addEntry( 45.0, 0.3 );
517 _elevationTable.addEntry( 90.0, 0.1 );
518 _elevationTable.addEntry( 180.0, 0.01 );
521 double LOC::ServiceVolume::adjustRange( double azimuthAngle_deg, double elevationAngle_deg )
523 return _azimuthTable.interpolate( fabs(azimuthAngle_deg) ) *
524 _elevationTable.interpolate( fabs(elevationAngle_deg) );
527 LOC::LOC( SGPropertyNode_ptr rootNode) :
528 NavRadioComponentWithIdent("loc", rootNode, new LOCAudioIdent(getIdentString(std::string("loc"),
529 rootNode->getIndex()))),
531 _localizerOffset_norm( rootNode->getNode(_name,true)->getNode("offset-norm",true) ),
532 _localizerWidth_deg( rootNode->getNode(_name,true)->getNode("width-deg",true) )
540 FGNavList::TypeFilter* LOC::getNavaidFilter()
542 return FGNavList::locFilter();
545 void LOC::search( double frequency, const SGGeod & aircraftPosition )
547 NavRadioComponentWithIdent::search( frequency, aircraftPosition );
548 if( false == valid() ) {
549 _localizerWidth_deg = 0.0;
553 // cache slightly expensive value,
554 // sanitized in FGNavRecord::localizerWidth() to never become zero
555 _localizerWidth_deg = _navRecord->localizerWidth();
558 /* Localizer coverage (ICAO Annex 10 Volume I 3.1.3.3
559 25NM within +/-10 deg from the front course line
560 17NM between 10 and 35deg from the front course line
561 10NM outside of +/- 35deg if coverage is provided
562 at and above a height of 2000ft above threshold or
563 1000ft above the highest point within intermediate
564 and final approach areas. Upper limit is a surface
565 extending outward from the localizer and inclined at
566 7 degrees above the horizontal
568 double LOC::getRange_nm(const SGGeod & aircraftPosition)
570 double elevationAngle = ::atan2(_heightAboveStation_ft*SG_FEET_TO_METER, _trackDistance_m)*SG_RADIANS_TO_DEGREES;
571 double azimuthAngle = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
573 // looks like our navrecord declared range is based on 10NM?
574 return _navRecord->get_range() * _serviceVolume.adjustRange( azimuthAngle, elevationAngle );
577 double LOC::computeSignalQuality_norm( const SGGeod & aircraftPosition )
579 return NavRadioComponentWithIdent::computeSignalQuality_norm( aircraftPosition );
582 void LOC::update( double dt, const SGGeod & aircraftPosition )
584 NavRadioComponentWithIdent::update( dt, aircraftPosition );
586 if( false == valid() ) {
587 _localizerOffset_norm = 0.0;
591 double offset = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - _navRecord->get_multiuse() );
593 // The factor of 30.0 gives a period of 120 which gives us 3 cycles and six
594 // zeros i.e. six courses: one front course, one back course, and four
595 // false courses. Three of the six are reverse sensing.
596 offset = 30.0 * sawtooth(offset / 30.0);
598 // normalize offset to the localizer width, scale and clip to [-1..1]
599 offset = SGMiscd::clip( 2.0 * offset / _localizerWidth_deg, -1.0, 1.0 );
601 _localizerOffset_norm = offset;
604 void LOC::display( NavIndicator & navIndicator )
606 if( false == valid() )
609 navIndicator.showTo( true );
610 navIndicator.showFrom( false );
612 navIndicator.setCDI( _localizerOffset_norm * _signalQuality_norm );
613 navIndicator.setSignalQuality( _signalQuality_norm );
616 class GS : public NavRadioComponent {
618 GS( SGPropertyNode_ptr rootNode);
620 virtual void update( double dt, const SGGeod & aircraftPosition );
621 virtual void search( double frequency, const SGGeod & aircraftPosition );
622 virtual void display( NavIndicator & navIndicator );
624 virtual double getRange_nm(const SGGeod & aircraftPosition);
626 virtual FGNavList::TypeFilter* getNavaidFilter();
628 class ServiceVolume {
631 double adjustRange( double azimuthAngle_deg, double elevationAngle_deg );
633 SGInterpTable _azimuthTable;
634 SGInterpTable _elevationTable;
636 static SGVec3d tangentVector(const SGGeod& midpoint, const double heading);
638 PropertyObject<double> _targetGlideslope_deg;
639 PropertyObject<double> _glideslopeOffset_norm;
644 GS::ServiceVolume::ServiceVolume()
646 // maybe this: http://www.tpub.com/content/aviation2/P-1244/P-12440125.htm
647 // ICAO Annex 10 - 3.1.5.2.2: The emission from the glide path equipment
648 // shall be horizontally polarized
649 // very rough abstraction of a 5-element yagi antenna's
650 // E-plane radiation diagram
651 _azimuthTable.addEntry( 0.0, 1.0 );
652 _azimuthTable.addEntry( 10.0, 1.0 );
653 _azimuthTable.addEntry( 30.0, 0.75 );
654 _azimuthTable.addEntry( 40.0, 0.50 );
655 _azimuthTable.addEntry( 50.0, 0.20 );
656 _azimuthTable.addEntry( 60.0, 0.10 );
657 _azimuthTable.addEntry( 70.0, 0.20 );
658 _azimuthTable.addEntry( 80.0, 0.10 );
659 _azimuthTable.addEntry( 90.0, 0.05 );
660 _azimuthTable.addEntry( 105.0, 0.10 );
661 _azimuthTable.addEntry( 130.0, 0.05 );
662 _azimuthTable.addEntry( 150.0, 0.30 );
663 _azimuthTable.addEntry( 160.0, 0.40 );
664 _azimuthTable.addEntry( 170.0, 0.50 );
665 _azimuthTable.addEntry( 180.0, 0.50 );
667 _elevationTable.addEntry( 0.0, 0.1 );
668 _elevationTable.addEntry( 1.05, 1.0 );
669 _elevationTable.addEntry( 7.00, 1.0 );
670 _elevationTable.addEntry( 45.0, 0.3 );
671 _elevationTable.addEntry( 90.0, 0.1 );
672 _elevationTable.addEntry( 180.0, 0.01 );
675 double GS::ServiceVolume::adjustRange( double azimuthAngle_deg, double elevationAngle_deg )
677 return _azimuthTable.interpolate( fabs(azimuthAngle_deg) ) *
678 _elevationTable.interpolate( fabs(elevationAngle_deg) );
681 GS::GS( SGPropertyNode_ptr rootNode) :
682 NavRadioComponent("gs", rootNode ),
683 _targetGlideslope_deg( rootNode->getNode(_name,true)->getNode("slope",true) ),
684 _glideslopeOffset_norm( rootNode->getNode(_name,true)->getNode("offset-norm",true) ),
685 _gsAxis(SGVec3d::zeros()),
686 _gsVertical(SGVec3d::zeros())
694 FGNavList::TypeFilter* GS::getNavaidFilter()
696 static FGNavList::TypeFilter filter(FGPositioned::GS);
700 double GS::getRange_nm(const SGGeod & aircraftPosition)
702 double elevationAngle = ::atan2(_heightAboveStation_ft*SG_FEET_TO_METER, _trackDistance_m)*SG_RADIANS_TO_DEGREES;
703 double azimuthAngle = SGMiscd::normalizePeriodic( -180.0, 180.0, _trueBearingFrom_deg + 180.0 - fmod(_navRecord->get_multiuse(), 1000.0) );
704 return _navRecord->get_range() * _serviceVolume.adjustRange( azimuthAngle, elevationAngle );
707 // Calculate a Cartesian unit vector in the
708 // local horizontal plane, i.e. tangent to the
709 // surface of the earth at the local ground zero.
710 // The tangent vector passes through the given <midpoint>
711 // and points forward along the given <heading>.
712 // The <heading> is given in degrees.
713 SGVec3d GS::tangentVector(const SGGeod& midpoint, const double heading)
715 // move 100m away from the midpoint - arbitrary number
716 const double delta(100.0);
718 double az2; // ignored
719 SGGeodesy::direct(midpoint, heading, delta, head, az2);
720 SGGeodesy::direct(midpoint, 180+heading, delta, tail, az2);
721 head.setElevationM(midpoint.getElevationM());
722 tail.setElevationM(midpoint.getElevationM());
723 SGVec3d head_xyz = SGVec3d::fromGeod(head);
724 SGVec3d tail_xyz = SGVec3d::fromGeod(tail);
725 // Awkward formula here, needed because vector-by-scalar
726 // multiplication is defined, but not vector-by-scalar division.
727 return (head_xyz - tail_xyz) * (0.5/delta);
730 void GS::search( double frequency, const SGGeod & aircraftPosition )
732 NavRadioComponent::search( frequency, aircraftPosition );
733 if( false == valid() ) {
734 _gsAxis = SGVec3d::zeros();
735 _gsVertical = SGVec3d::zeros();
736 _targetGlideslope_deg = 3.0;
740 double gs_radial = SGMiscd::normalizePeriodic(0.0, 360.0, fmod(_navRecord->get_multiuse(), 1000.0) );
742 _gsAxis = tangentVector(_navRecord->geod(), gs_radial);
743 SGVec3d gsBaseline = tangentVector(_navRecord->geod(), gs_radial + 90.0);
744 _gsVertical = cross(gsBaseline, _gsAxis);
746 int tmp = (int)(_navRecord->get_multiuse() / 1000.0);
747 // catch unconfigured glideslopes here, they will cause nan later
748 _targetGlideslope_deg = SGMiscd::max( 1.0, (double)tmp / 100.0 );
751 void GS::update( double dt, const SGGeod & aircraftPosition )
753 NavRadioComponent::update( dt, aircraftPosition );
754 if( false == valid() ) {
755 _glideslopeOffset_norm = 0.0;
759 SGVec3d pos = SGVec3d::fromGeod(aircraftPosition) - _navRecord->cart(); // relative vector from gs antenna to aircraft
760 // The positive GS axis points along the runway in the landing direction,
761 // toward the far end, not toward the approach area, so we need a - sign here:
762 double comp_h = -dot(pos, _gsAxis); // component in horiz direction
763 double comp_v = dot(pos, _gsVertical); // component in vertical direction
764 //double comp_b = dot(pos, _gsBaseline); // component in baseline direction
765 //if (comp_b) {} // ... (useful for debugging)
767 // _gsDirect represents the angle of elevation of the aircraft
768 // as seen by the GS transmitter.
769 double gsDirect = atan2(comp_v, comp_h) * SGD_RADIANS_TO_DEGREES;
770 // At this point, if the aircraft is centered on the glide slope,
771 // _gsDirect will be a small positive number, e.g. 3.0 degrees
773 // Aim the branch cut straight down
774 // into the ground below the GS transmitter:
775 if (gsDirect < -90.0) gsDirect += 360.0;
777 double offset = _targetGlideslope_deg - gsDirect;
779 offset = _targetGlideslope_deg/2 * sawtooth(2.0*offset/_targetGlideslope_deg);
780 assert( false == isnan(offset) );
781 // GS is documented to be 1.4 degrees thick,
782 // i.e. plus or minus 0.7 degrees from the midline:
783 _glideslopeOffset_norm = SGMiscd::clip(offset/0.7, -1.0, 1.0);
786 void GS::display( NavIndicator & navIndicator )
788 if( false == valid() ) {
789 navIndicator.setGS( false );
792 navIndicator.setGS( true );
793 navIndicator.setGS( _glideslopeOffset_norm );
796 /* ------------- A NAV/COMM Frequency formatter ---------------------- */
798 class FrequencyFormatter : public SGPropertyChangeListener {
800 FrequencyFormatter( SGPropertyNode_ptr freqNode, SGPropertyNode_ptr fmtFreqNode, double channelSpacing ) :
801 _freqNode( freqNode ),
802 _fmtFreqNode( fmtFreqNode ),
803 _channelSpacing(channelSpacing)
805 _freqNode->addChangeListener( this );
806 valueChanged(_freqNode);
808 ~FrequencyFormatter()
810 _freqNode->removeChangeListener( this );
813 void valueChanged (SGPropertyNode * prop)
815 // format as fixed decimal "nnn.nn"
816 std::ostringstream buf;
820 << std::setprecision(2)
822 _fmtFreqNode->setStringValue( buf.str() );
825 double getFrequency() const
827 double d = SGMiscd::roundToInt(_freqNode->getDoubleValue() / _channelSpacing) * _channelSpacing;
828 // strip last digit, do not round
829 return ((int)(d*100))/100.0;
833 SGPropertyNode_ptr _freqNode;
834 SGPropertyNode_ptr _fmtFreqNode;
835 double _channelSpacing;
839 /* ------------- The NavRadio implementation ---------------------- */
841 class NavRadioImpl : public NavRadio {
843 NavRadioImpl( SGPropertyNode_ptr node );
844 virtual ~NavRadioImpl();
846 virtual void update( double dt );
853 Legacy( NavRadioImpl * navRadioImpl ) : _navRadioImpl( navRadioImpl ) {}
856 void update( double dt );
858 NavRadioImpl * _navRadioImpl;
859 SGPropertyNode_ptr is_valid_node;
860 SGPropertyNode_ptr nav_serviceable_node;
861 SGPropertyNode_ptr nav_id_node;
862 SGPropertyNode_ptr id_c1_node;
863 SGPropertyNode_ptr id_c2_node;
864 SGPropertyNode_ptr id_c3_node;
865 SGPropertyNode_ptr id_c4_node;
868 const static int VOR_COMPONENT = 0;
869 const static int LOC_COMPONENT = 1;
870 const static int GS_COMPONENT = 2;
874 SGPropertyNode_ptr _rootNode;
875 FrequencyFormatter _useFrequencyFormatter;
876 FrequencyFormatter _stbyFrequencyFormatter;
877 std::vector<NavRadioComponent*> _components;
878 NavIndicator _navIndicator;
881 PropertyObject<bool> _cdiDisconnected;
884 NavRadioImpl::NavRadioImpl( SGPropertyNode_ptr node ) :
886 _name(node->getStringValue("name", "nav")),
887 _num(node->getIntValue("number", 0)),
888 _rootNode(fgGetNode( std::string("/instrumentation/") + _name, _num, true)),
889 _useFrequencyFormatter( _rootNode->getNode("frequencies/selected-mhz",true), _rootNode->getNode("frequencies/selected-mhz-fmt",true), 0.05 ),
890 _stbyFrequencyFormatter( _rootNode->getNode("frequencies/standby-mhz",true), _rootNode->getNode("frequencies/standby-mhz-fmt",true), 0.05 ),
891 _navIndicator(_rootNode),
894 _cdiDisconnected(_rootNode->getNode("cdi-disconnected",true))
898 NavRadioImpl::~NavRadioImpl()
900 BOOST_FOREACH( NavRadioComponent * p, _components ) {
905 void NavRadioImpl::init()
907 if( ! _components.empty() )
910 _components.push_back( new VOR(_rootNode) );
911 _components.push_back( new LOC(_rootNode) );
912 _components.push_back( new GS(_rootNode) );
917 void NavRadioImpl::search()
921 void NavRadioImpl::update( double dt )
923 if( dt < SGLimitsd::min() ) return;
928 position = globals->get_aircraft_position();
930 catch( std::exception & ) {
935 if( _frequency != _useFrequencyFormatter.getFrequency() ) {
936 _frequency = _useFrequencyFormatter.getFrequency();
940 BOOST_FOREACH( NavRadioComponent * p, _components ) {
941 if( _stationTTL <= 0.0 )
942 p->search( _frequency, position );
943 p->update( dt, position );
945 if( false == _cdiDisconnected )
946 p->display( _navIndicator );
949 if( _stationTTL <= 0.0 )
952 _legacy.update( dt );
955 void NavRadioImpl::Legacy::init()
957 is_valid_node = _navRadioImpl->_rootNode->getChild("data-is-valid", 0, true);
958 nav_serviceable_node = _navRadioImpl->_rootNode->getChild("serviceable", 0, true);
960 nav_id_node = _navRadioImpl->_rootNode->getChild("nav-id", 0, true );
961 id_c1_node = _navRadioImpl->_rootNode->getChild("nav-id_asc1", 0, true );
962 id_c2_node = _navRadioImpl->_rootNode->getChild("nav-id_asc2", 0, true );
963 id_c3_node = _navRadioImpl->_rootNode->getChild("nav-id_asc3", 0, true );
964 id_c4_node = _navRadioImpl->_rootNode->getChild("nav-id_asc4", 0, true );
968 void NavRadioImpl::Legacy::update( double dt )
970 is_valid_node->setBoolValue(
971 _navRadioImpl->_components[VOR_COMPONENT]->valid() || _navRadioImpl->_components[LOC_COMPONENT]->valid()
974 std::string ident = _navRadioImpl->_components[VOR_COMPONENT]->getIdent();
976 ident = _navRadioImpl->_components[LOC_COMPONENT]->getIdent();
978 nav_id_node->setStringValue( ident );
980 ident = simgear::strutils::rpad( ident, 4, ' ' );
981 id_c1_node->setIntValue( (int)ident[0] );
982 id_c2_node->setIntValue( (int)ident[1] );
983 id_c3_node->setIntValue( (int)ident[2] );
984 id_c4_node->setIntValue( (int)ident[3] );
988 SGSubsystem * NavRadio::createInstance( SGPropertyNode_ptr rootNode )
990 // use old navradio code by default
991 if( fgGetBool( "/instrumentation/use-new-navradio", false ) )
992 return new NavRadioImpl( rootNode );
994 return new FGNavRadio( rootNode );
997 } // namespace Instrumentation