src/Main/positioninit.cxx

   1 // positioninit.cxx - helpers relating to setting initial aircraft position
   2 //
   3 // Copyright (C) 2012 James Turner  zakalawe@mac.com
   4 //
   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.
   9 //
  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.
  14 //
  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.
  18
  19 #include "positioninit.hxx"
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include "config.h"
  23 #endif
  24
  25 // simgear
  26 #include <simgear/props/props_io.hxx>
  27
  28 #include "globals.hxx"
  29 #include "fg_props.hxx"
  30
  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>
  36
  37 using std::endl;
  38
  39 namespace flightgear
  40 {
  41
  42 // Set current tower position lon/lat given an airport id
  43 static bool fgSetTowerPosFromAirportID( const string& id) {
  44   const FGAirport *a = fgFindAirportID( id);
  45   if (a) {
  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());
  50     return true;
  51   } else {
  52     return false;
  53   }
  54
  55 }
  56
  57 struct FGTowerLocationListener : SGPropertyChangeListener {
  58   void valueChanged(SGPropertyNode* node) {
  59     string id(node->getStringValue());
  60     if (fgGetBool("/sim/tower/auto-position",true))
  61     {
  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))
  65       {
  66         id = closest_airport;
  67         node->setStringValue(id);
  68       }
  69     }
  70     fgSetTowerPosFromAirportID(id);
  71   }
  72 };
  73
  74 struct FGClosestTowerLocationListener : SGPropertyChangeListener
  75 {
  76   void valueChanged(SGPropertyNode* )
  77   {
  78     // closest airport has changed
  79     if (fgGetBool("/sim/tower/auto-position",true))
  80     {
  81       // update tower position
  82       const char* id = fgGetString("/sim/airport/closest-airport-id", "");
  83       if (id && *id!=0)
  84         fgSetString("/sim/tower/airport-id", id);
  85     }
  86   }
  87 };
  88
  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 );
  97 }
  98
  99 static void fgApplyStartOffset(const SGGeod& aStartPos, double aHeading, double aTargetHeading = HUGE_VAL)
 100 {
 101   SGGeod startPos(aStartPos);
 102   if (aTargetHeading == HUGE_VAL) {
 103     aTargetHeading = aHeading;
 104   }
 105
 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;
 113     }
 114
 115     SGGeod offset;
 116     double az2; // dummy
 117     SGGeodesy::direct(startPos, offsetAzimuth + 180, offsetDistance, offset, az2);
 118     startPos = offset;
 119   }
 120
 121   // presets
 122   fgSetDouble("/sim/presets/longitude-deg", startPos.getLongitudeDeg() );
 123   fgSetDouble("/sim/presets/latitude-deg", startPos.getLatitudeDeg() );
 124   fgSetDouble("/sim/presets/heading-deg", aHeading );
 125
 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 );
 130 }
 131
 132 // Set current_options lon/lat given an airport id and heading (degrees)
 133 bool setPosFromAirportIDandHdg( const string& id, double tgt_hdg ) {
 134   if ( id.empty() )
 135     return false;
 136
 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 );
 141
 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());
 146
 147   SGGeod startPos = r->pointOnCenterline(fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
 148   fgApplyStartOffset(startPos, r->headingDeg(), tgt_hdg);
 149   return true;
 150 }
 151
 152 // Set current_options lon/lat given an airport id and parkig position name
 153 static bool fgSetPosFromAirportIDandParkpos( const string& id, const string& parkpos ) {
 154   if ( id.empty() )
 155     return false;
 156
 157   // can't see an easy way around this const_cast at the moment
 158   FGAirport* apt = const_cast<FGAirport*>(fgFindAirportID(id));
 159   if (!apt) {
 160     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport " << id );
 161     return false;
 162   }
 163   FGAirportDynamics* dcs = apt->getDynamics();
 164   if (!dcs) {
 165     SG_LOG( SG_GENERAL, SG_ALERT,
 166            "Airport " << id << "does not appear to have parking information available");
 167     return false;
 168   }
 169
 170   int park_index = dcs->getNrOfParkings() - 1;
 171   bool succes;
 172   double radius = fgGetDouble("/sim/dimensions/radius-m");
 173   if ((parkpos == string("AVAILABLE")) && (radius > 0)) {
 174     double lat, lon, heading;
 175     string fltType;
 176     string acOperator;
 177     SGPath acData;
 178     try {
 179       acData = globals->get_fg_home();
 180       acData.append("aircraft-data");
 181       string acfile = fgGetString("/sim/aircraft") + string(".xml");
 182       acData.append(acfile);
 183       SGPropertyNode root;
 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");
 191
 192       // cout << path.str() << endl;
 193     }
 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"   );
 198     }
 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"   );
 203     }
 204
 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);
 207     if (succes) {
 208       fgGetString("/sim/presets/parkpos");
 209       fgSetString("/sim/presets/parkpos", dcs->getParking(park_index)->getName());
 210     } else {
 211       SG_LOG( SG_GENERAL, SG_ALERT,
 212              "Failed to find a suitable parking at airport " << id );
 213       return false;
 214     }
 215   } else {
 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 );
 222       return false;
 223     }
 224   }
 225   FGParking* parking = dcs->getParking(park_index);
 226   parking->setAvailable(false);
 227   fgApplyStartOffset(
 228                      SGGeod::fromDeg(parking->getLongitude(), parking->getLatitude()),
 229                      parking->getHeading());
 230   return true;
 231 }
 232
 233
 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 ) {
 236   if ( id.empty() )
 237     return false;
 238
 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 );
 243
 244   const FGAirport* apt = fgFindAirportID(id);
 245   if (!apt) {
 246     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport:" << id);
 247     return false;
 248   }
 249
 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." );
 254     return false;
 255   }
 256
 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());
 261   return true;
 262 }
 263
 264
 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");
 272
 273   double apt_elev = 0.0;
 274   if ( ! apt_id.empty() ) {
 275     apt_elev = fgGetAirportElev( apt_id );
 276     if ( apt_elev < -9990.0 ) {
 277       apt_elev = 0.0;
 278     }
 279   } else {
 280     apt_elev = 0.0;
 281   }
 282
 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: "
 290            << alt  << " ft" );
 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: "
 298            << od  << " nm" );
 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);
 305   }
 306 }
 307
 308
 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 )
 311 {
 312   FGNavList::TypeFilter filter(type);
 313   const nav_list_type navlist = FGNavList::findByIdentAndFreq( id.c_str(), freq, &filter );
 314
 315   if (navlist.size() == 0 ) {
 316     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate NAV = "
 317            << id << ":" << freq );
 318     return false;
 319   }
 320
 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()
 331       << endl;
 332     }
 333
 334     SG_LOG( SG_GENERAL, SG_ALERT, buf.str() );
 335     return false;
 336   }
 337
 338   FGNavRecord *nav = navlist[0];
 339   fgApplyStartOffset(nav->geod(), fgGetDouble("/sim/presets/heading-deg"));
 340   return true;
 341 }
 342
 343 // Set current_options lon/lat given an aircraft carrier id
 344 static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
 345
 346   // set initial position from runway and heading
 347   SGGeod geodPos;
 348   double heading;
 349   SGVec3d uvw;
 350   if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
 351     double lon = geodPos.getLongitudeDeg();
 352     double lat = geodPos.getLatitudeDeg();
 353     double alt = geodPos.getElevationFt();
 354
 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);
 358
 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);
 367
 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));
 375
 376     fgSetBool("/sim/presets/onground", true);
 377
 378     return true;
 379   } else {
 380     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carrier = "
 381            << carrier );
 382     return false;
 383   }
 384 }
 385
 386 // Set current_options lon/lat given an airport id and heading (degrees)
 387 static bool fgSetPosFromFix( const string& id )
 388 {
 389   FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
 390   FGPositioned* fix = FGPositioned::findFirstWithIdent(id, &fixFilter);
 391   if (!fix) {
 392     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate fix = " << id );
 393     return false;
 394   }
 395
 396   fgApplyStartOffset(fix->geod(), fgGetDouble("/sim/presets/heading-deg"));
 397   return true;
 398 }
 399
 400 // Set the initial position based on presets (or defaults)
 401 bool initPosition()
 402 {
 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");
 408
 409   bool set_pos = false;
 410
 411   // If glideslope is specified, then calculate offset-distance or
 412   // altitude relative to glide slope if either of those was not
 413   // specified.
 414   if ( fabs( gs ) > 0.01 ) {
 415     fgSetDistOrAltFromGlideSlope();
 416   }
 417
 418
 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 )
 429   {
 430     set_pos = true;
 431   }
 432
 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();
 445
 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);
 452
 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());
 458   }
 459
 460   if (hdg > 9990.0)
 461     hdg = fgGetDouble("/environment/config/boundary/entry/wind-from-heading-deg", 270);
 462
 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());
 469       set_pos = true;
 470     }
 471   }
 472
 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
 477       // runway airports)
 478       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 479       fgSetString("/sim/tower/airport-id",  apt.c_str());
 480       set_pos = true;
 481     }
 482   }
 483
 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
 488       // runway airports)
 489       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 490       fgSetString("/sim/tower/airport-id",  apt.c_str());
 491       set_pos = true;
 492     }
 493   }
 494
 495   if (hdg_preset->getDoubleValue() > 9990.0)
 496     hdg_preset->setDoubleValue(hdg);
 497
 498   if ( !set_pos && !vor.empty() ) {
 499     // a VOR is requested
 500     if ( fgSetPosFromNAV( vor, vor_freq, FGPositioned::VOR ) ) {
 501       set_pos = true;
 502     }
 503   }
 504
 505   if ( !set_pos && !ndb.empty() ) {
 506     // an NDB is requested
 507     if ( fgSetPosFromNAV( ndb, ndb_freq, FGPositioned::NDB ) ) {
 508       set_pos = true;
 509     }
 510   }
 511
 512   if ( !set_pos && !carrier.empty() ) {
 513     // an aircraft carrier is requested
 514     if ( fgSetPosFromCarrier( carrier, parkpos ) ) {
 515       set_pos = true;
 516     }
 517   }
 518
 519   if ( !set_pos && !fix.empty() ) {
 520     // a Fix is requested
 521     if ( fgSetPosFromFix( fix ) ) {
 522       set_pos = true;
 523     }
 524   }
 525
 526   if ( !set_pos ) {
 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);
 531   }
 532
 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());
 538
 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);
 542   } else {
 543     fgSetBool("/sim/presets/onground", true);
 544   }
 545
 546   return true;
 547 }
 548
 549 } // of namespace flightgear