1 // positioninit.cxx - helpers relating to setting initial aircraft position
3 // Copyright (C) 2012 James Turner zakalawe@mac.com
5 // This program is free software; you can redistribute it and/or
6 // modify it under the terms of the GNU General Public License as
7 // published by the Free Software Foundation; either version 2 of the
8 // License, or (at your option) any later version.
10 // This program is distributed in the hope that it will be useful, but
11 // WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
15 // You should have received a copy of the GNU General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 #include "positioninit.hxx"
26 #include <simgear/props/props_io.hxx>
27 #include <simgear/structure/exception.hxx>
28 #include <simgear/structure/event_mgr.hxx>
30 #include "globals.hxx"
31 #include "fg_props.hxx"
33 #include <Navaids/navlist.hxx>
34 #include <Airports/runways.hxx>
35 #include <Airports/airport.hxx>
36 #include <Airports/dynamics.hxx>
37 #include <AIModel/AIManager.hxx>
45 /// to avoid blocking when metar-fetch is enabled, but the network is
46 /// unresponsive, we need a timeout value. This value is reset on initPosition,
47 /// and tracked through each call to finalizePosition.
48 static SGTimeStamp global_finalizeTime;
49 static bool global_callbackRegistered = false;
51 static void finalizePosition();
53 // Set current tower position lon/lat given an airport id
54 static bool fgSetTowerPosFromAirportID( const string& id) {
55 const FGAirport *a = fgFindAirportID( id);
57 SGGeod tower = a->getTowerLocation();
58 fgSetDouble("/sim/tower/longitude-deg", tower.getLongitudeDeg());
59 fgSetDouble("/sim/tower/latitude-deg", tower.getLatitudeDeg());
60 fgSetDouble("/sim/tower/altitude-ft", tower.getElevationFt());
68 class FGTowerLocationListener : public SGPropertyChangeListener {
70 void valueChanged(SGPropertyNode* node) {
71 string id(node->getStringValue());
72 if (fgGetBool("/sim/tower/auto-position",true))
74 // enforce using closest airport when auto-positioning is enabled
75 const char* closest_airport = fgGetString("/sim/airport/closest-airport-id", "");
76 if (closest_airport && (id != closest_airport))
79 node->setStringValue(id);
82 fgSetTowerPosFromAirportID(id);
86 class FGClosestTowerLocationListener : public SGPropertyChangeListener
88 void valueChanged(SGPropertyNode* )
90 // closest airport has changed
91 if (fgGetBool("/sim/tower/auto-position",true))
93 // update tower position
94 const char* id = fgGetString("/sim/airport/closest-airport-id", "");
96 fgSetString("/sim/tower/airport-id", id);
101 void initTowerLocationListener() {
103 SGPropertyChangeListener* tll = new FGTowerLocationListener();
104 globals->addListenerToCleanup(tll);
105 fgGetNode("/sim/tower/airport-id", true)
106 ->addChangeListener( tll, true );
108 FGClosestTowerLocationListener* ntcl = new FGClosestTowerLocationListener();
109 globals->addListenerToCleanup(ntcl);
110 fgGetNode("/sim/airport/closest-airport-id", true)
111 ->addChangeListener(ntcl , true );
112 fgGetNode("/sim/tower/auto-position", true)
113 ->addChangeListener(ntcl, true );
116 static void fgApplyStartOffset(const SGGeod& aStartPos, double aHeading, double aTargetHeading = HUGE_VAL)
118 SGGeod startPos(aStartPos);
119 if (aTargetHeading == HUGE_VAL) {
120 aTargetHeading = aHeading;
123 if ( fabs( fgGetDouble("/sim/presets/offset-distance-nm") ) > SG_EPSILON ) {
124 double offsetDistance = fgGetDouble("/sim/presets/offset-distance-nm");
125 offsetDistance *= SG_NM_TO_METER;
126 double offsetAzimuth = aHeading;
127 if ( fabs(fgGetDouble("/sim/presets/offset-azimuth-deg")) > SG_EPSILON ) {
128 offsetAzimuth = fgGetDouble("/sim/presets/offset-azimuth-deg");
129 aHeading = aTargetHeading;
134 SGGeodesy::direct(startPos, offsetAzimuth + 180, offsetDistance, offset, az2);
139 fgSetDouble("/sim/presets/longitude-deg", startPos.getLongitudeDeg() );
140 fgSetDouble("/sim/presets/latitude-deg", startPos.getLatitudeDeg() );
141 fgSetDouble("/sim/presets/heading-deg", aHeading );
143 // other code depends on the actual values being set ...
144 fgSetDouble("/position/longitude-deg", startPos.getLongitudeDeg() );
145 fgSetDouble("/position/latitude-deg", startPos.getLatitudeDeg() );
146 fgSetDouble("/orientation/heading-deg", aHeading );
149 // Set current_options lon/lat given an airport id and heading (degrees)
150 static bool setPosFromAirportIDandHdg( const string& id, double tgt_hdg ) {
154 // set initial position from runway and heading
155 SG_LOG( SG_GENERAL, SG_INFO,
156 "Attempting to set starting position from airport code "
157 << id << " heading " << tgt_hdg );
159 const FGAirport* apt = fgFindAirportID(id);
160 if (!apt) return false;
163 double heading = tgt_hdg;
164 if (apt->type() == FGPositioned::HELIPORT) {
165 if (apt->numHelipads() > 0) {
166 startPos = apt->getHelipadByIndex(0)->geod();
168 startPos = apt->geod();
171 FGRunway* r = apt->findBestRunwayForHeading(tgt_hdg);
172 fgSetString("/sim/atc/runway", r->ident().c_str());
173 startPos = r->pointOnCenterline(fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
174 heading = r->headingDeg();
177 fgApplyStartOffset(startPos, heading, tgt_hdg);
181 // Set current_options lon/lat given an airport id and parkig position name
182 static bool fgSetPosFromAirportIDandParkpos( const string& id, const string& parkpos )
187 // can't see an easy way around this const_cast at the moment
188 FGAirport* apt = const_cast<FGAirport*>(fgFindAirportID(id));
190 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport " << id );
193 FGAirportDynamics* dcs = apt->getDynamics();
195 SG_LOG( SG_GENERAL, SG_ALERT,
196 "Airport " << id << "does not appear to have parking information available");
200 ParkingAssignment pka;
201 double radius = fgGetDouble("/sim/dimensions/radius-m");
202 if ((parkpos == string("AVAILABLE")) && (radius > 0)) {
207 acData = globals->get_fg_home();
208 acData.append("aircraft-data");
209 string acfile = fgGetString("/sim/aircraft") + string(".xml");
210 acData.append(acfile);
212 readProperties(acData.str(), &root);
213 SGPropertyNode * node = root.getNode("sim");
214 fltType = node->getStringValue("aircraft-class", "NONE" );
215 acOperator = node->getStringValue("aircraft-operator", "NONE" );
216 } catch (const sg_exception &) {
217 SG_LOG(SG_GENERAL, SG_INFO,
218 "Could not load aircraft aircrat type and operator information from: " << acData.str() << ". Using defaults");
220 // cout << path.str() << endl;
222 if (fltType.empty() || fltType == "NONE") {
223 SG_LOG(SG_GENERAL, SG_INFO,
224 "Aircraft type information not found in: " << acData.str() << ". Using default value");
225 fltType = fgGetString("/sim/aircraft-class" );
227 if (acOperator.empty() || fltType == "NONE") {
228 SG_LOG(SG_GENERAL, SG_INFO,
229 "Aircraft operator information not found in: " << acData.str() << ". Using default value");
230 acOperator = fgGetString("/sim/aircraft-operator" );
233 string acType; // Currently not used by findAvailable parking, so safe to leave empty.
234 pka = dcs->getAvailableParking(radius, fltType, acType, acOperator);
236 fgGetString("/sim/presets/parkpos");
237 fgSetString("/sim/presets/parkpos", pka.parking()->getName());
239 SG_LOG( SG_GENERAL, SG_ALERT,
240 "Failed to find a suitable parking at airport " << id );
244 pka = dcs->getParkingByName(parkpos);
245 if (!pka.isValid()) {
246 SG_LOG( SG_GENERAL, SG_ALERT,
247 "Failed to find a parking at airport " << id << ":" << parkpos);
252 fgApplyStartOffset(pka.parking()->geod(), pka.parking()->getHeading());
257 // Set current_options lon/lat given an airport id and runway number
258 static bool fgSetPosFromAirportIDandRwy( const string& id, const string& rwy, bool rwy_req ) {
262 // set initial position from airport and runway number
263 SG_LOG( SG_GENERAL, SG_INFO,
264 "Attempting to set starting position for "
265 << id << ":" << rwy );
267 const FGAirport* apt = fgFindAirportID(id);
269 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport:" << id);
273 if (!apt->hasRunwayWithIdent(rwy)) {
274 SG_LOG( SG_GENERAL, rwy_req ? SG_ALERT : SG_INFO,
275 "Failed to find runway " << rwy <<
276 " at airport " << id << ". Using default runway." );
280 FGRunway* r(apt->getRunwayByIdent(rwy));
281 fgSetString("/sim/atc/runway", r->ident().c_str());
282 SGGeod startPos = r->pointOnCenterline( fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
283 fgApplyStartOffset(startPos, r->headingDeg());
288 static void fgSetDistOrAltFromGlideSlope() {
289 // cout << "fgSetDistOrAltFromGlideSlope()" << endl;
290 string apt_id = fgGetString("/sim/presets/airport-id");
291 double gs = fgGetDouble("/sim/presets/glideslope-deg")
292 * SG_DEGREES_TO_RADIANS ;
293 double od = fgGetDouble("/sim/presets/offset-distance-nm");
294 double alt = fgGetDouble("/sim/presets/altitude-ft");
296 double apt_elev = 0.0;
297 if ( ! apt_id.empty() ) {
298 apt_elev = fgGetAirportElev( apt_id );
299 if ( apt_elev < -9990.0 ) {
306 if( fabs(gs) > 0.01 && fabs(od) > 0.1 && alt < -9990 ) {
307 // set altitude from glideslope and offset-distance
308 od *= SG_NM_TO_METER * SG_METER_TO_FEET;
309 alt = fabs(od*tan(gs)) + apt_elev;
310 fgSetDouble("/sim/presets/altitude-ft", alt);
311 fgSetBool("/sim/presets/onground", false);
312 SG_LOG( SG_GENERAL, SG_INFO, "Calculated altitude as: "
314 } else if( fabs(gs) > 0.01 && alt > 0 && fabs(od) < 0.1) {
315 // set offset-distance from glideslope and altitude
316 od = (alt - apt_elev) / tan(gs);
317 od *= -1*SG_FEET_TO_METER * SG_METER_TO_NM;
318 fgSetDouble("/sim/presets/offset-distance-nm", od);
319 fgSetBool("/sim/presets/onground", false);
320 SG_LOG( SG_GENERAL, SG_INFO, "Calculated offset distance as: "
322 } else if( fabs(gs) > 0.01 ) {
323 SG_LOG( SG_GENERAL, SG_ALERT,
324 "Glideslope given but not altitude or offset-distance." );
325 SG_LOG( SG_GENERAL, SG_ALERT, "Resetting glideslope to zero" );
326 fgSetDouble("/sim/presets/glideslope-deg", 0);
327 fgSetBool("/sim/presets/onground", true);
332 // Set current_options lon/lat given an airport id and heading (degrees)
333 static bool fgSetPosFromNAV( const string& id, const double& freq, FGPositioned::Type type )
335 FGNavList::TypeFilter filter(type);
336 const nav_list_type navlist = FGNavList::findByIdentAndFreq( id.c_str(), freq, &filter );
338 if (navlist.empty()) {
339 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate NAV = "
340 << id << ":" << freq );
344 if( navlist.size() > 1 ) {
345 std::ostringstream buf;
346 buf << "Ambigous NAV-ID: '" << id << "'. Specify id and frequency. Available stations:" << endl;
347 for( nav_list_type::const_iterator it = navlist.begin(); it != navlist.end(); ++it ) {
348 // NDB stored in kHz, VOR stored in MHz * 100 :-P
349 double factor = (*it)->type() == FGPositioned::NDB ? 1.0 : 1/100.0;
350 string unit = (*it)->type() == FGPositioned::NDB ? "kHz" : "MHz";
351 buf << (*it)->ident() << " "
352 << std::setprecision(5) << (double)((*it)->get_freq() * factor) << " "
353 << (*it)->get_lat() << "/" << (*it)->get_lon()
357 SG_LOG( SG_GENERAL, SG_ALERT, buf.str() );
361 FGNavRecord *nav = navlist[0];
362 fgApplyStartOffset(nav->geod(), fgGetDouble("/sim/presets/heading-deg"));
366 // Set current_options lon/lat given an aircraft carrier id
367 static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
369 // set initial position from runway and heading
373 if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
374 double lon = geodPos.getLongitudeDeg();
375 double lat = geodPos.getLatitudeDeg();
376 double alt = geodPos.getElevationFt();
378 SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for "
379 << carrier << " at lat = " << lat << ", lon = " << lon
380 << ", alt = " << alt << ", heading = " << heading);
382 fgSetDouble("/sim/presets/longitude-deg", lon);
383 fgSetDouble("/sim/presets/latitude-deg", lat);
384 fgSetDouble("/sim/presets/altitude-ft", alt);
385 fgSetDouble("/sim/presets/heading-deg", heading);
386 fgSetDouble("/position/longitude-deg", lon);
387 fgSetDouble("/position/latitude-deg", lat);
388 fgSetDouble("/position/altitude-ft", alt);
389 fgSetDouble("/orientation/heading-deg", heading);
391 fgSetString("/sim/presets/speed-set", "UVW");
392 fgSetDouble("/velocities/uBody-fps", uvw(0));
393 fgSetDouble("/velocities/vBody-fps", uvw(1));
394 fgSetDouble("/velocities/wBody-fps", uvw(2));
395 fgSetDouble("/sim/presets/uBody-fps", uvw(0));
396 fgSetDouble("/sim/presets/vBody-fps", uvw(1));
397 fgSetDouble("/sim/presets/wBody-fps", uvw(2));
399 fgSetBool("/sim/presets/onground", true);
403 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carrier = "
409 // Set current_options lon/lat given an airport id and heading (degrees)
410 static bool fgSetPosFromFix( const string& id )
412 FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
413 FGPositioned* fix = FGPositioned::findFirstWithIdent(id, &fixFilter);
415 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate fix = " << id );
419 fgApplyStartOffset(fix->geod(), fgGetDouble("/sim/presets/heading-deg"));
423 // Set the initial position based on presets (or defaults)
426 global_finalizeTime = SGTimeStamp(); // reset to invalid
427 if (!global_callbackRegistered) {
428 globals->get_event_mgr()->addTask("finalizePosition", &finalizePosition, 0.1);
429 global_callbackRegistered = true;
432 double gs = fgGetDouble("/sim/presets/glideslope-deg")
433 * SG_DEGREES_TO_RADIANS ;
434 double od = fgGetDouble("/sim/presets/offset-distance-nm");
435 double alt = fgGetDouble("/sim/presets/altitude-ft");
437 bool set_pos = false;
439 // If glideslope is specified, then calculate offset-distance or
440 // altitude relative to glide slope if either of those was not
442 if ( fabs( gs ) > 0.01 ) {
443 fgSetDistOrAltFromGlideSlope();
447 // If we have an explicit, in-range lon/lat, don't change it, just use it.
448 // If not, check for an airport-id and use that.
449 // If not, default to the middle of the KSFO field.
450 // The default values for lon/lat are deliberately out of range
451 // so that the airport-id can take effect; valid lon/lat will
452 // override airport-id, however.
453 double lon_deg = fgGetDouble("/sim/presets/longitude-deg");
454 double lat_deg = fgGetDouble("/sim/presets/latitude-deg");
455 if ( lon_deg >= -180.0 && lon_deg <= 180.0
456 && lat_deg >= -90.0 && lat_deg <= 90.0 )
461 string apt = fgGetString("/sim/presets/airport-id");
462 string rwy_no = fgGetString("/sim/presets/runway");
463 bool rwy_req = fgGetBool("/sim/presets/runway-requested");
464 string vor = fgGetString("/sim/presets/vor-id");
465 double vor_freq = fgGetDouble("/sim/presets/vor-freq");
466 string ndb = fgGetString("/sim/presets/ndb-id");
467 double ndb_freq = fgGetDouble("/sim/presets/ndb-freq");
468 string carrier = fgGetString("/sim/presets/carrier");
469 string parkpos = fgGetString("/sim/presets/parkpos");
470 string fix = fgGetString("/sim/presets/fix");
471 SGPropertyNode *hdg_preset = fgGetNode("/sim/presets/heading-deg", true);
472 double hdg = hdg_preset->getDoubleValue();
474 // save some start parameters, so that we can later say what the
475 // user really requested. TODO generalize that and move it to options.cxx
476 static bool start_options_saved = false;
477 if (!start_options_saved) {
478 start_options_saved = true;
479 SGPropertyNode *opt = fgGetNode("/sim/startup/options", true);
481 opt->setDoubleValue("latitude-deg", lat_deg);
482 opt->setDoubleValue("longitude-deg", lon_deg);
483 opt->setDoubleValue("heading-deg", hdg);
484 opt->setStringValue("airport", apt.c_str());
485 opt->setStringValue("runway", rwy_no.c_str());
489 hdg = fgGetDouble("/environment/config/boundary/entry/wind-from-heading-deg", 270);
491 if ( !set_pos && !apt.empty() && !parkpos.empty() ) {
492 // An airport + parking position is requested
493 if ( fgSetPosFromAirportIDandParkpos( apt, parkpos ) ) {
494 // set tower position
495 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
496 fgSetString("/sim/tower/airport-id", apt.c_str());
501 if ( !set_pos && !apt.empty() && !rwy_no.empty() ) {
502 // An airport + runway is requested
503 if ( fgSetPosFromAirportIDandRwy( apt, rwy_no, rwy_req ) ) {
504 // set tower position (a little off the heading for single
506 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
507 fgSetString("/sim/tower/airport-id", apt.c_str());
512 if ( !set_pos && !apt.empty() ) {
513 // An airport is requested (find runway closest to hdg)
514 if ( setPosFromAirportIDandHdg( apt, hdg ) ) {
515 // set tower position (a little off the heading for single
517 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
518 fgSetString("/sim/tower/airport-id", apt.c_str());
523 if (hdg_preset->getDoubleValue() > 9990.0)
524 hdg_preset->setDoubleValue(hdg);
526 if ( !set_pos && !vor.empty() ) {
527 // a VOR is requested
528 if ( fgSetPosFromNAV( vor, vor_freq, FGPositioned::VOR ) ) {
533 if ( !set_pos && !ndb.empty() ) {
534 // an NDB is requested
535 if ( fgSetPosFromNAV( ndb, ndb_freq, FGPositioned::NDB ) ) {
540 if ( !set_pos && !carrier.empty() ) {
541 // an aircraft carrier is requested
542 if ( fgSetPosFromCarrier( carrier, parkpos ) ) {
547 if ( !set_pos && !fix.empty() ) {
548 // a Fix is requested
549 if ( fgSetPosFromFix( fix ) ) {
555 // No lon/lat specified, no airport specified, default to
556 // middle of KSFO field.
557 fgSetDouble("/sim/presets/longitude-deg", -122.374843);
558 fgSetDouble("/sim/presets/latitude-deg", 37.619002);
561 fgSetDouble( "/position/longitude-deg",
562 fgGetDouble("/sim/presets/longitude-deg") );
563 fgSetDouble( "/position/latitude-deg",
564 fgGetDouble("/sim/presets/latitude-deg") );
565 fgSetDouble( "/orientation/heading-deg", hdg_preset->getDoubleValue());
567 // determine if this should be an on-ground or in-air start
568 if ((fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1) && carrier.empty()) {
569 fgSetBool("/sim/presets/onground", false);
571 fgSetBool("/sim/presets/onground", true);
574 fgSetBool("/sim/position-finalized", false);
576 // Initialize the longitude, latitude and altitude to the initial position
577 fgSetDouble("/position/altitude-ft", fgGetDouble("/sim/presets/altitude-ft"));
578 fgSetDouble("/position/longitude-deg", fgGetDouble("/sim/presets/longitude-deg"));
579 fgSetDouble("/position/latitude-deg", fgGetDouble("/sim/presets/latitude-deg"));
586 double hdg = fgGetDouble( "/environment/metar/base-wind-dir-deg", 9999.0 );
587 string apt = fgGetString("/sim/presets/airport-id");
588 string rwy = fgGetString("/sim/presets/runway");
589 double strthdg = fgGetDouble( "/sim/startup/options/heading-deg", 9999.0 );
590 string parkpos = fgGetString( "/sim/presets/parkpos" );
591 bool onground = fgGetBool( "/sim/presets/onground", false );
592 // this logic is taken from former startup.nas
593 bool needMetar = (hdg < 360.0) && !apt.empty() && (strthdg > 360.0) &&
594 rwy.empty() && onground && parkpos.empty();
597 // timeout so we don't spin forever if the network is down
598 if (global_finalizeTime.elapsedMSec() > fgGetInt("/sim/startup/metar-fetch-timeout-msec", 10000)) {
599 SG_LOG(SG_GENERAL, SG_WARN, "finalizePosition: timed out waiting for METAR fetch");
603 if (!fgGetBool( "/environment/metar/valid" )) {
607 SG_LOG(SG_ENVIRONMENT, SG_INFO,
608 "Using METAR for runway selection: '" << fgGetString("/environment/metar/data") << "'" );
609 setPosFromAirportIDandHdg( apt, hdg );
610 // fall through to return true
611 } // of need-metar case
616 void finalizePosition()
618 // first call to finalize after an initPosition call
619 if (global_finalizeTime.get_usec() == 0) {
620 global_finalizeTime = SGTimeStamp::now();
625 /* Scenarios require Nasal, so FGAIManager loads the scenarios,
626 * including its models such as a/c carriers, in its 'postinit',
627 * which is the very last thing we do.
628 * flightgear::initPosition is called very early in main.cxx/fgIdleFunction,
629 * one of the first things we do, long before scenarios/carriers are
630 * loaded. => When requested "initial preset position" relates to a
631 * carrier, recalculate the 'initial' position here
633 std::string carrier = fgGetString("/sim/presets/carrier","");
634 if (!carrier.empty())
636 SG_LOG(SG_GENERAL, SG_INFO, "finalizePositioned: re-init-ing position on carrier");
637 // clear preset location and re-trigger position setup
638 fgSetDouble("/sim/presets/longitude-deg", 9999);
639 fgSetDouble("/sim/presets/latitude-deg", 9999);
642 done = finalizeMetar();
645 fgSetBool("/sim/position-finalized", done);
647 globals->get_event_mgr()->removeTask("finalizePosition");
648 global_callbackRegistered = false;
652 } // of namespace flightgear