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>
28 #include "globals.hxx"
29 #include "fg_props.hxx"
31 #include <Navaids/navlist.hxx>
32 #include <Airports/runways.hxx>
33 #include <Airports/simple.hxx>
34 #include <Airports/dynamics.hxx>
35 #include <AIModel/AIManager.hxx>
42 // Set current tower position lon/lat given an airport id
43 static bool fgSetTowerPosFromAirportID( const string& id) {
44 const FGAirport *a = fgFindAirportID( id);
46 SGGeod tower = a->getTowerLocation();
47 fgSetDouble("/sim/tower/longitude-deg", tower.getLongitudeDeg());
48 fgSetDouble("/sim/tower/latitude-deg", tower.getLatitudeDeg());
49 fgSetDouble("/sim/tower/altitude-ft", tower.getElevationFt());
57 struct FGTowerLocationListener : SGPropertyChangeListener {
58 void valueChanged(SGPropertyNode* node) {
59 string id(node->getStringValue());
60 if (fgGetBool("/sim/tower/auto-position",true))
62 // enforce using closest airport when auto-positioning is enabled
63 const char* closest_airport = fgGetString("/sim/airport/closest-airport-id", "");
64 if (closest_airport && (id != closest_airport))
67 node->setStringValue(id);
70 fgSetTowerPosFromAirportID(id);
74 struct FGClosestTowerLocationListener : SGPropertyChangeListener
76 void valueChanged(SGPropertyNode* )
78 // closest airport has changed
79 if (fgGetBool("/sim/tower/auto-position",true))
81 // update tower position
82 const char* id = fgGetString("/sim/airport/closest-airport-id", "");
84 fgSetString("/sim/tower/airport-id", id);
89 void initTowerLocationListener() {
90 fgGetNode("/sim/tower/airport-id", true)
91 ->addChangeListener( new FGTowerLocationListener(), true );
92 FGClosestTowerLocationListener* ntcl = new FGClosestTowerLocationListener();
93 fgGetNode("/sim/airport/closest-airport-id", true)
94 ->addChangeListener(ntcl , true );
95 fgGetNode("/sim/tower/auto-position", true)
96 ->addChangeListener(ntcl, true );
99 static void fgApplyStartOffset(const SGGeod& aStartPos, double aHeading, double aTargetHeading = HUGE_VAL)
101 SGGeod startPos(aStartPos);
102 if (aTargetHeading == HUGE_VAL) {
103 aTargetHeading = aHeading;
106 if ( fabs( fgGetDouble("/sim/presets/offset-distance-nm") ) > SG_EPSILON ) {
107 double offsetDistance = fgGetDouble("/sim/presets/offset-distance-nm");
108 offsetDistance *= SG_NM_TO_METER;
109 double offsetAzimuth = aHeading;
110 if ( fabs(fgGetDouble("/sim/presets/offset-azimuth-deg")) > SG_EPSILON ) {
111 offsetAzimuth = fgGetDouble("/sim/presets/offset-azimuth-deg");
112 aHeading = aTargetHeading;
117 SGGeodesy::direct(startPos, offsetAzimuth + 180, offsetDistance, offset, az2);
122 fgSetDouble("/sim/presets/longitude-deg", startPos.getLongitudeDeg() );
123 fgSetDouble("/sim/presets/latitude-deg", startPos.getLatitudeDeg() );
124 fgSetDouble("/sim/presets/heading-deg", aHeading );
126 // other code depends on the actual values being set ...
127 fgSetDouble("/position/longitude-deg", startPos.getLongitudeDeg() );
128 fgSetDouble("/position/latitude-deg", startPos.getLatitudeDeg() );
129 fgSetDouble("/orientation/heading-deg", aHeading );
132 // Set current_options lon/lat given an airport id and heading (degrees)
133 bool setPosFromAirportIDandHdg( const string& id, double tgt_hdg ) {
137 // set initial position from runway and heading
138 SG_LOG( SG_GENERAL, SG_INFO,
139 "Attempting to set starting position from airport code "
140 << id << " heading " << tgt_hdg );
142 const FGAirport* apt = fgFindAirportID(id);
143 if (!apt) return false;
144 FGRunway* r = apt->findBestRunwayForHeading(tgt_hdg);
145 fgSetString("/sim/atc/runway", r->ident().c_str());
147 SGGeod startPos = r->pointOnCenterline(fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
148 fgApplyStartOffset(startPos, r->headingDeg(), tgt_hdg);
152 // Set current_options lon/lat given an airport id and parkig position name
153 static bool fgSetPosFromAirportIDandParkpos( const string& id, const string& parkpos ) {
157 // can't see an easy way around this const_cast at the moment
158 FGAirport* apt = const_cast<FGAirport*>(fgFindAirportID(id));
160 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport " << id );
163 FGAirportDynamics* dcs = apt->getDynamics();
165 SG_LOG( SG_GENERAL, SG_ALERT,
166 "Airport " << id << "does not appear to have parking information available");
170 int park_index = dcs->getNrOfParkings() - 1;
172 double radius = fgGetDouble("/sim/dimensions/radius-m");
173 if ((parkpos == string("AVAILABLE")) && (radius > 0)) {
174 double lat, lon, heading;
179 acData = globals->get_fg_home();
180 acData.append("aircraft-data");
181 string acfile = fgGetString("/sim/aircraft") + string(".xml");
182 acData.append(acfile);
184 readProperties(acData.str(), &root);
185 SGPropertyNode * node = root.getNode("sim");
186 fltType = node->getStringValue("aircraft-class", "NONE" );
187 acOperator = node->getStringValue("aircraft-operator", "NONE" );
188 } catch (const sg_exception &) {
189 SG_LOG(SG_GENERAL, SG_INFO,
190 "Could not load aircraft aircrat type and operator information from: " << acData.str() << ". Using defaults");
192 // cout << path.str() << endl;
194 if (fltType.empty() || fltType == "NONE") {
195 SG_LOG(SG_GENERAL, SG_INFO,
196 "Aircraft type information not found in: " << acData.str() << ". Using default value");
197 fltType = fgGetString("/sim/aircraft-class" );
199 if (acOperator.empty() || fltType == "NONE") {
200 SG_LOG(SG_GENERAL, SG_INFO,
201 "Aircraft operator information not found in: " << acData.str() << ". Using default value");
202 acOperator = fgGetString("/sim/aircraft-operator" );
205 string acType; // Currently not used by findAvailable parking, so safe to leave empty.
206 succes = dcs->getAvailableParking(&lat, &lon, &heading, &park_index, radius, fltType, acType, acOperator);
208 fgGetString("/sim/presets/parkpos");
209 fgSetString("/sim/presets/parkpos", dcs->getParking(park_index)->getName());
211 SG_LOG( SG_GENERAL, SG_ALERT,
212 "Failed to find a suitable parking at airport " << id );
216 //cerr << "We shouldn't get here when AVAILABLE" << endl;
217 while (park_index >= 0 && dcs->getParkingName(park_index) != parkpos) park_index--;
218 if (park_index < 0) {
219 SG_LOG( SG_GENERAL, SG_ALERT,
220 "Failed to find parking position " << parkpos <<
221 " at airport " << id );
225 FGParking* parking = dcs->getParking(park_index);
226 parking->setAvailable(false);
228 SGGeod::fromDeg(parking->getLongitude(), parking->getLatitude()),
229 parking->getHeading());
234 // Set current_options lon/lat given an airport id and runway number
235 static bool fgSetPosFromAirportIDandRwy( const string& id, const string& rwy, bool rwy_req ) {
239 // set initial position from airport and runway number
240 SG_LOG( SG_GENERAL, SG_INFO,
241 "Attempting to set starting position for "
242 << id << ":" << rwy );
244 const FGAirport* apt = fgFindAirportID(id);
246 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport:" << id);
250 if (!apt->hasRunwayWithIdent(rwy)) {
251 SG_LOG( SG_GENERAL, rwy_req ? SG_ALERT : SG_INFO,
252 "Failed to find runway " << rwy <<
253 " at airport " << id << ". Using default runway." );
257 FGRunway* r(apt->getRunwayByIdent(rwy));
258 fgSetString("/sim/atc/runway", r->ident().c_str());
259 SGGeod startPos = r->pointOnCenterline( fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
260 fgApplyStartOffset(startPos, r->headingDeg());
265 static void fgSetDistOrAltFromGlideSlope() {
266 // cout << "fgSetDistOrAltFromGlideSlope()" << endl;
267 string apt_id = fgGetString("/sim/presets/airport-id");
268 double gs = fgGetDouble("/sim/presets/glideslope-deg")
269 * SG_DEGREES_TO_RADIANS ;
270 double od = fgGetDouble("/sim/presets/offset-distance-nm");
271 double alt = fgGetDouble("/sim/presets/altitude-ft");
273 double apt_elev = 0.0;
274 if ( ! apt_id.empty() ) {
275 apt_elev = fgGetAirportElev( apt_id );
276 if ( apt_elev < -9990.0 ) {
283 if( fabs(gs) > 0.01 && fabs(od) > 0.1 && alt < -9990 ) {
284 // set altitude from glideslope and offset-distance
285 od *= SG_NM_TO_METER * SG_METER_TO_FEET;
286 alt = fabs(od*tan(gs)) + apt_elev;
287 fgSetDouble("/sim/presets/altitude-ft", alt);
288 fgSetBool("/sim/presets/onground", false);
289 SG_LOG( SG_GENERAL, SG_INFO, "Calculated altitude as: "
291 } else if( fabs(gs) > 0.01 && alt > 0 && fabs(od) < 0.1) {
292 // set offset-distance from glideslope and altitude
293 od = (alt - apt_elev) / tan(gs);
294 od *= -1*SG_FEET_TO_METER * SG_METER_TO_NM;
295 fgSetDouble("/sim/presets/offset-distance-nm", od);
296 fgSetBool("/sim/presets/onground", false);
297 SG_LOG( SG_GENERAL, SG_INFO, "Calculated offset distance as: "
299 } else if( fabs(gs) > 0.01 ) {
300 SG_LOG( SG_GENERAL, SG_ALERT,
301 "Glideslope given but not altitude or offset-distance." );
302 SG_LOG( SG_GENERAL, SG_ALERT, "Resetting glideslope to zero" );
303 fgSetDouble("/sim/presets/glideslope-deg", 0);
304 fgSetBool("/sim/presets/onground", true);
309 // Set current_options lon/lat given an airport id and heading (degrees)
310 static bool fgSetPosFromNAV( const string& id, const double& freq, FGPositioned::Type type )
312 FGNavList::TypeFilter filter(type);
313 const nav_list_type navlist = FGNavList::findByIdentAndFreq( id.c_str(), freq, &filter );
315 if (navlist.size() == 0 ) {
316 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate NAV = "
317 << id << ":" << freq );
321 if( navlist.size() > 1 ) {
322 std::ostringstream buf;
323 buf << "Ambigous NAV-ID: '" << id << "'. Specify id and frequency. Available stations:" << endl;
324 for( nav_list_type::const_iterator it = navlist.begin(); it != navlist.end(); ++it ) {
325 // NDB stored in kHz, VOR stored in MHz * 100 :-P
326 double factor = (*it)->type() == FGPositioned::NDB ? 1.0 : 1/100.0;
327 string unit = (*it)->type() == FGPositioned::NDB ? "kHz" : "MHz";
328 buf << (*it)->ident() << " "
329 << std::setprecision(5) << (double)((*it)->get_freq() * factor) << " "
330 << (*it)->get_lat() << "/" << (*it)->get_lon()
334 SG_LOG( SG_GENERAL, SG_ALERT, buf.str() );
338 FGNavRecord *nav = navlist[0];
339 fgApplyStartOffset(nav->geod(), fgGetDouble("/sim/presets/heading-deg"));
343 // Set current_options lon/lat given an aircraft carrier id
344 static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
346 // set initial position from runway and heading
350 if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
351 double lon = geodPos.getLongitudeDeg();
352 double lat = geodPos.getLatitudeDeg();
353 double alt = geodPos.getElevationFt();
355 SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for "
356 << carrier << " at lat = " << lat << ", lon = " << lon
357 << ", alt = " << alt << ", heading = " << heading);
359 fgSetDouble("/sim/presets/longitude-deg", lon);
360 fgSetDouble("/sim/presets/latitude-deg", lat);
361 fgSetDouble("/sim/presets/altitude-ft", alt);
362 fgSetDouble("/sim/presets/heading-deg", heading);
363 fgSetDouble("/position/longitude-deg", lon);
364 fgSetDouble("/position/latitude-deg", lat);
365 fgSetDouble("/position/altitude-ft", alt);
366 fgSetDouble("/orientation/heading-deg", heading);
368 fgSetString("/sim/presets/speed-set", "UVW");
369 fgSetDouble("/velocities/uBody-fps", uvw(0));
370 fgSetDouble("/velocities/vBody-fps", uvw(1));
371 fgSetDouble("/velocities/wBody-fps", uvw(2));
372 fgSetDouble("/sim/presets/uBody-fps", uvw(0));
373 fgSetDouble("/sim/presets/vBody-fps", uvw(1));
374 fgSetDouble("/sim/presets/wBody-fps", uvw(2));
376 fgSetBool("/sim/presets/onground", true);
380 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carrier = "
386 // Set current_options lon/lat given an airport id and heading (degrees)
387 static bool fgSetPosFromFix( const string& id )
389 FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
390 FGPositioned* fix = FGPositioned::findFirstWithIdent(id, &fixFilter);
392 SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate fix = " << id );
396 fgApplyStartOffset(fix->geod(), fgGetDouble("/sim/presets/heading-deg"));
400 // Set the initial position based on presets (or defaults)
403 // cout << "initPosition()" << endl;
404 double gs = fgGetDouble("/sim/presets/glideslope-deg")
405 * SG_DEGREES_TO_RADIANS ;
406 double od = fgGetDouble("/sim/presets/offset-distance-nm");
407 double alt = fgGetDouble("/sim/presets/altitude-ft");
409 bool set_pos = false;
411 // If glideslope is specified, then calculate offset-distance or
412 // altitude relative to glide slope if either of those was not
414 if ( fabs( gs ) > 0.01 ) {
415 fgSetDistOrAltFromGlideSlope();
419 // If we have an explicit, in-range lon/lat, don't change it, just use it.
420 // If not, check for an airport-id and use that.
421 // If not, default to the middle of the KSFO field.
422 // The default values for lon/lat are deliberately out of range
423 // so that the airport-id can take effect; valid lon/lat will
424 // override airport-id, however.
425 double lon_deg = fgGetDouble("/sim/presets/longitude-deg");
426 double lat_deg = fgGetDouble("/sim/presets/latitude-deg");
427 if ( lon_deg >= -180.0 && lon_deg <= 180.0
428 && lat_deg >= -90.0 && lat_deg <= 90.0 )
433 string apt = fgGetString("/sim/presets/airport-id");
434 string rwy_no = fgGetString("/sim/presets/runway");
435 bool rwy_req = fgGetBool("/sim/presets/runway-requested");
436 string vor = fgGetString("/sim/presets/vor-id");
437 double vor_freq = fgGetDouble("/sim/presets/vor-freq");
438 string ndb = fgGetString("/sim/presets/ndb-id");
439 double ndb_freq = fgGetDouble("/sim/presets/ndb-freq");
440 string carrier = fgGetString("/sim/presets/carrier");
441 string parkpos = fgGetString("/sim/presets/parkpos");
442 string fix = fgGetString("/sim/presets/fix");
443 SGPropertyNode *hdg_preset = fgGetNode("/sim/presets/heading-deg", true);
444 double hdg = hdg_preset->getDoubleValue();
446 // save some start parameters, so that we can later say what the
447 // user really requested. TODO generalize that and move it to options.cxx
448 static bool start_options_saved = false;
449 if (!start_options_saved) {
450 start_options_saved = true;
451 SGPropertyNode *opt = fgGetNode("/sim/startup/options", true);
453 opt->setDoubleValue("latitude-deg", lat_deg);
454 opt->setDoubleValue("longitude-deg", lon_deg);
455 opt->setDoubleValue("heading-deg", hdg);
456 opt->setStringValue("airport", apt.c_str());
457 opt->setStringValue("runway", rwy_no.c_str());
461 hdg = fgGetDouble("/environment/config/boundary/entry/wind-from-heading-deg", 270);
463 if ( !set_pos && !apt.empty() && !parkpos.empty() ) {
464 // An airport + parking position is requested
465 if ( fgSetPosFromAirportIDandParkpos( apt, parkpos ) ) {
466 // set tower position
467 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
468 fgSetString("/sim/tower/airport-id", apt.c_str());
473 if ( !set_pos && !apt.empty() && !rwy_no.empty() ) {
474 // An airport + runway is requested
475 if ( fgSetPosFromAirportIDandRwy( apt, rwy_no, rwy_req ) ) {
476 // set tower position (a little off the heading for single
478 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
479 fgSetString("/sim/tower/airport-id", apt.c_str());
484 if ( !set_pos && !apt.empty() ) {
485 // An airport is requested (find runway closest to hdg)
486 if ( setPosFromAirportIDandHdg( apt, hdg ) ) {
487 // set tower position (a little off the heading for single
489 fgSetString("/sim/airport/closest-airport-id", apt.c_str());
490 fgSetString("/sim/tower/airport-id", apt.c_str());
495 if (hdg_preset->getDoubleValue() > 9990.0)
496 hdg_preset->setDoubleValue(hdg);
498 if ( !set_pos && !vor.empty() ) {
499 // a VOR is requested
500 if ( fgSetPosFromNAV( vor, vor_freq, FGPositioned::VOR ) ) {
505 if ( !set_pos && !ndb.empty() ) {
506 // an NDB is requested
507 if ( fgSetPosFromNAV( ndb, ndb_freq, FGPositioned::NDB ) ) {
512 if ( !set_pos && !carrier.empty() ) {
513 // an aircraft carrier is requested
514 if ( fgSetPosFromCarrier( carrier, parkpos ) ) {
519 if ( !set_pos && !fix.empty() ) {
520 // a Fix is requested
521 if ( fgSetPosFromFix( fix ) ) {
527 // No lon/lat specified, no airport specified, default to
528 // middle of KSFO field.
529 fgSetDouble("/sim/presets/longitude-deg", -122.374843);
530 fgSetDouble("/sim/presets/latitude-deg", 37.619002);
533 fgSetDouble( "/position/longitude-deg",
534 fgGetDouble("/sim/presets/longitude-deg") );
535 fgSetDouble( "/position/latitude-deg",
536 fgGetDouble("/sim/presets/latitude-deg") );
537 fgSetDouble( "/orientation/heading-deg", hdg_preset->getDoubleValue());
539 // determine if this should be an on-ground or in-air start
540 if ((fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1) && carrier.empty()) {
541 fgSetBool("/sim/presets/onground", false);
543 fgSetBool("/sim/presets/onground", true);
549 } // of namespace flightgear