1 // navlist.cxx -- navaids management class
3 // Written by Curtis Olson, started April 2000.
5 // Copyright (C) 2000 Curtis L. Olson - http://www.flightgear.org/~curt
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
28 #include <boost/foreach.hpp>
31 #include <simgear/debug/logstream.hxx>
32 #include <simgear/math/sg_geodesy.hxx>
33 #include <simgear/sg_inlines.h>
35 #include "navlist.hxx"
37 #include <Airports/runways.hxx>
41 namespace { // anonymous
43 class NavRecordDistanceSortPredicate
46 NavRecordDistanceSortPredicate( const SGGeod & position ) :
47 _position(SGVec3d::fromGeod(position)) {}
49 bool operator()( const nav_rec_ptr & n1, const nav_rec_ptr & n2 )
51 if( n1 == NULL || n2 == NULL ) return false;
52 return distSqr(n1->cart(), _position) < distSqr(n2->cart(), _position);
59 } // of anonymous namespace
61 // FGNavList ------------------------------------------------------------------
64 FGNavList::TypeFilter::TypeFilter(const FGPositioned::Type type)
66 if (type == FGPositioned::INVALID) {
67 _mintype = FGPositioned::VOR;
68 _maxtype = FGPositioned::GS;
70 _mintype = _maxtype = type;
75 FGNavList::FGNavList( void )
80 FGNavList::~FGNavList( void )
82 nav_list_type navlist = navaids.begin()->second;
83 navaids.erase( navaids.begin(), navaids.end() );
87 // load the navaids and build the map
88 bool FGNavList::init()
90 // No need to delete the original navaid structures
91 // since we're using an SGSharedPointer
92 nav_list_type navlist = navaids.begin()->second;
93 navaids.erase( navaids.begin(), navaids.end() );
97 // add an entry to the lists
98 bool FGNavList::add( FGNavRecord *n )
100 navaids[n->get_freq()].push_back(n);
104 FGNavRecord *FGNavList::findByFreq( double freq, const SGGeod& position)
106 const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
107 SG_LOG( SG_INSTR, SG_DEBUG, "findbyFreq " << freq << " size " << stations.size() );
108 return findNavFromList( position, stations );
111 nav_list_type FGNavList::findAllByFreq( double freq, const SGGeod& position, const FGPositioned::Type type)
113 nav_list_type stations;
114 TypeFilter filter(type);
116 BOOST_FOREACH(nav_rec_ptr nav, navaids[(int)(freq*100.0 + 0.5)]) {
117 if (filter.pass(nav.ptr())) {
118 stations.push_back(nav);
122 NavRecordDistanceSortPredicate sortPredicate( position );
123 std::sort( stations.begin(), stations.end(), sortPredicate );
128 // Given an Ident and optional freqency, return the first matching
130 const nav_list_type FGNavList::findByIdentAndFreq(const string& ident, const double freq, const FGPositioned::Type type )
133 TypeFilter filter(type);
136 cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
138 int f = (int)(freq*100.0 + 0.5);
140 FGNavRecord* nav = static_cast<FGNavRecord*>(cur.ptr());
141 if ( f <= 0.0 || nav->get_freq() == f) {
142 reply.push_back( nav );
145 cur = FGPositioned::findNextWithPartialId(cur, ident, &filter);
151 // Given an Ident and optional freqency and type ,
152 // return a list of matching stations sorted by distance to the given position
153 const nav_list_type FGNavList::findByIdentAndFreq( const SGGeod & position,
154 const std::string& ident, const double freq, const FGPositioned::Type type )
156 nav_list_type reply = findByIdentAndFreq( ident, freq, type );
157 NavRecordDistanceSortPredicate sortPredicate( position );
158 std::sort( reply.begin(), reply.end(), sortPredicate );
163 // discount navids if they conflict with another on the same frequency
164 // this only applies to navids associated with opposite ends of a runway,
165 // with matching frequencies.
166 static bool navidUsable(FGNavRecord* aNav, const SGGeod &aircraft)
168 FGRunway* r(aNav->runway());
169 if (!r || !r->reciprocalRunway()) {
173 // check if the runway frequency is paired
174 FGNavRecord* locA = r->ILS();
175 FGNavRecord* locB = r->reciprocalRunway()->ILS();
177 if (!locA || !locB || (locA->get_freq() != locB->get_freq())) {
178 return true; // not paired, ok
181 // okay, both ends have an ILS, and they're paired. We need to select based on
182 // aircraft position. What we're going to use is *runway* (not navid) position,
183 // ie whichever runway end we are closer too. This makes back-course / missed
184 // approach behaviour incorrect, but that's the price we accept.
185 double crs = SGGeodesy::courseDeg(aircraft, r->geod());
186 double hdgDiff = crs - r->headingDeg();
187 SG_NORMALIZE_RANGE(hdgDiff, -180.0, 180.0);
188 return (fabs(hdgDiff) < 90.0);
191 // Given a point and a list of stations, return the closest one to
192 // the specified point.
193 FGNavRecord* FGNavList::findNavFromList( const SGGeod &aircraft,
194 const nav_list_type &stations )
196 FGNavRecord *nav = NULL;
197 double d2; // in meters squared
199 = FG_NAV_MAX_RANGE*SG_NM_TO_METER*FG_NAV_MAX_RANGE*SG_NM_TO_METER;
200 SGVec3d aircraftCart = SGVec3d::fromGeod(aircraft);
202 nav_list_const_iterator it;
203 nav_list_const_iterator end = stations.end();
204 // find the closest station within a sensible range (FG_NAV_MAX_RANGE)
205 for ( it = stations.begin(); it != end; ++it ) {
206 FGNavRecord *station = *it;
207 d2 = distSqr(station->cart(), aircraftCart);
208 if ( d2 > min_dist || !navidUsable(station, aircraft)) {
219 // Given a frequency, return the first matching station.
220 FGNavRecord *FGNavList::findStationByFreq( double freq )
222 const nav_list_type& stations = navaids[(int)(freq*100.0 + 0.5)];
224 SG_LOG( SG_INSTR, SG_DEBUG, "findStationByFreq " << freq << " size " << stations.size() );
226 if (!stations.empty()) {
234 // FGTACANList ----------------------------------------------------------------
236 FGTACANList::FGTACANList( void )
241 FGTACANList::~FGTACANList( void )
246 bool FGTACANList::init()
252 // add an entry to the lists
253 bool FGTACANList::add( FGTACANRecord *c )
255 ident_channels[c->get_channel()].push_back(c);
260 // Given a TACAN Channel return the first matching frequency
261 FGTACANRecord *FGTACANList::findByChannel( const string& channel )
263 const tacan_list_type& stations = ident_channels[channel];
264 SG_LOG( SG_INSTR, SG_DEBUG, "findByChannel " << channel<< " size " << stations.size() );
266 if (!stations.empty()) {