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 #include <simgear/structure/exception.hxx>
  28
  29 #include "globals.hxx"
  30 #include "fg_props.hxx"
  31
  32 #include <Navaids/navlist.hxx>
  33 #include <Airports/runways.hxx>
  34 #include <Airports/simple.hxx>
  35 #include <Airports/dynamics.hxx>
  36 #include <AIModel/AIManager.hxx>
  37
  38 using std::endl;
  39
  40 namespace flightgear
  41 {
  42
  43 // Set current tower position lon/lat given an airport id
  44 static bool fgSetTowerPosFromAirportID( const string& id) {
  45   const FGAirport *a = fgFindAirportID( id);
  46   if (a) {
  47     SGGeod tower = a->getTowerLocation();
  48     fgSetDouble("/sim/tower/longitude-deg",  tower.getLongitudeDeg());
  49     fgSetDouble("/sim/tower/latitude-deg",  tower.getLatitudeDeg());
  50     fgSetDouble("/sim/tower/altitude-ft", tower.getElevationFt());
  51     return true;
  52   } else {
  53     return false;
  54   }
  55
  56 }
  57
  58 struct FGTowerLocationListener : SGPropertyChangeListener {
  59   void valueChanged(SGPropertyNode* node) {
  60     string id(node->getStringValue());
  61     if (fgGetBool("/sim/tower/auto-position",true))
  62     {
  63       // enforce using closest airport when auto-positioning is enabled
  64       const char* closest_airport = fgGetString("/sim/airport/closest-airport-id", "");
  65       if (closest_airport && (id != closest_airport))
  66       {
  67         id = closest_airport;
  68         node->setStringValue(id);
  69       }
  70     }
  71     fgSetTowerPosFromAirportID(id);
  72   }
  73 };
  74
  75 struct FGClosestTowerLocationListener : SGPropertyChangeListener
  76 {
  77   void valueChanged(SGPropertyNode* )
  78   {
  79     // closest airport has changed
  80     if (fgGetBool("/sim/tower/auto-position",true))
  81     {
  82       // update tower position
  83       const char* id = fgGetString("/sim/airport/closest-airport-id", "");
  84       if (id && *id!=0)
  85         fgSetString("/sim/tower/airport-id", id);
  86     }
  87   }
  88 };
  89
  90 void initTowerLocationListener() {
  91   fgGetNode("/sim/tower/airport-id",  true)
  92   ->addChangeListener( new FGTowerLocationListener(), true );
  93   FGClosestTowerLocationListener* ntcl = new FGClosestTowerLocationListener();
  94   fgGetNode("/sim/airport/closest-airport-id", true)
  95   ->addChangeListener(ntcl , true );
  96   fgGetNode("/sim/tower/auto-position", true)
  97   ->addChangeListener(ntcl, true );
  98 }
  99
 100 static void fgApplyStartOffset(const SGGeod& aStartPos, double aHeading, double aTargetHeading = HUGE_VAL)
 101 {
 102   SGGeod startPos(aStartPos);
 103   if (aTargetHeading == HUGE_VAL) {
 104     aTargetHeading = aHeading;
 105   }
 106
 107   if ( fabs( fgGetDouble("/sim/presets/offset-distance-nm") ) > SG_EPSILON ) {
 108     double offsetDistance = fgGetDouble("/sim/presets/offset-distance-nm");
 109     offsetDistance *= SG_NM_TO_METER;
 110     double offsetAzimuth = aHeading;
 111     if ( fabs(fgGetDouble("/sim/presets/offset-azimuth-deg")) > SG_EPSILON ) {
 112       offsetAzimuth = fgGetDouble("/sim/presets/offset-azimuth-deg");
 113       aHeading = aTargetHeading;
 114     }
 115
 116     SGGeod offset;
 117     double az2; // dummy
 118     SGGeodesy::direct(startPos, offsetAzimuth + 180, offsetDistance, offset, az2);
 119     startPos = offset;
 120   }
 121
 122   // presets
 123   fgSetDouble("/sim/presets/longitude-deg", startPos.getLongitudeDeg() );
 124   fgSetDouble("/sim/presets/latitude-deg", startPos.getLatitudeDeg() );
 125   fgSetDouble("/sim/presets/heading-deg", aHeading );
 126
 127   // other code depends on the actual values being set ...
 128   fgSetDouble("/position/longitude-deg",  startPos.getLongitudeDeg() );
 129   fgSetDouble("/position/latitude-deg",  startPos.getLatitudeDeg() );
 130   fgSetDouble("/orientation/heading-deg", aHeading );
 131 }
 132
 133 // Set current_options lon/lat given an airport id and heading (degrees)
 134 bool setPosFromAirportIDandHdg( const string& id, double tgt_hdg ) {
 135   if ( id.empty() )
 136     return false;
 137
 138   // set initial position from runway and heading
 139   SG_LOG( SG_GENERAL, SG_INFO,
 140          "Attempting to set starting position from airport code "
 141          << id << " heading " << tgt_hdg );
 142
 143   const FGAirport* apt = fgFindAirportID(id);
 144   if (!apt) return false;
 145   FGRunway* r = apt->findBestRunwayForHeading(tgt_hdg);
 146   fgSetString("/sim/atc/runway", r->ident().c_str());
 147
 148   SGGeod startPos = r->pointOnCenterline(fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
 149   fgApplyStartOffset(startPos, r->headingDeg(), tgt_hdg);
 150   return true;
 151 }
 152
 153 // Set current_options lon/lat given an airport id and parkig position name
 154 static bool fgSetPosFromAirportIDandParkpos( const string& id, const string& parkpos )
 155 {
 156   if ( id.empty() )
 157     return false;
 158
 159   // can't see an easy way around this const_cast at the moment
 160   FGAirport* apt = const_cast<FGAirport*>(fgFindAirportID(id));
 161   if (!apt) {
 162     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport " << id );
 163     return false;
 164   }
 165   FGAirportDynamics* dcs = apt->getDynamics();
 166   if (!dcs) {
 167     SG_LOG( SG_GENERAL, SG_ALERT,
 168            "Airport " << id << "does not appear to have parking information available");
 169     return false;
 170   }
 171
 172   int gateID;
 173   double radius = fgGetDouble("/sim/dimensions/radius-m");
 174   if ((parkpos == string("AVAILABLE")) && (radius > 0)) {
 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     gateID = dcs->getAvailableParking(radius, fltType, acType, acOperator);
 207     if (gateID >=0 ) {
 208       fgGetString("/sim/presets/parkpos");
 209       fgSetString("/sim/presets/parkpos", dcs->getParking(gateID)->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     gateID = dcs->findParkingByName(parkpos);
 217     if (gateID < 0) {
 218       SG_LOG( SG_GENERAL, SG_ALERT,
 219                "Failed to find a parking at airport " << id << ":" << parkpos);
 220       return false;
 221     }
 222   }
 223
 224   FGParking* parking = dcs->getParking(gateID);
 225   parking->setAvailable(false);
 226   fgApplyStartOffset(parking->getGeod(), parking->getHeading());
 227   return true;
 228 }
 229
 230
 231 // Set current_options lon/lat given an airport id and runway number
 232 static bool fgSetPosFromAirportIDandRwy( const string& id, const string& rwy, bool rwy_req ) {
 233   if ( id.empty() )
 234     return false;
 235
 236   // set initial position from airport and runway number
 237   SG_LOG( SG_GENERAL, SG_INFO,
 238          "Attempting to set starting position for "
 239          << id << ":" << rwy );
 240
 241   const FGAirport* apt = fgFindAirportID(id);
 242   if (!apt) {
 243     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport:" << id);
 244     return false;
 245   }
 246
 247   if (!apt->hasRunwayWithIdent(rwy)) {
 248     SG_LOG( SG_GENERAL, rwy_req ? SG_ALERT : SG_INFO,
 249            "Failed to find runway " << rwy <<
 250            " at airport " << id << ". Using default runway." );
 251     return false;
 252   }
 253
 254   FGRunway* r(apt->getRunwayByIdent(rwy));
 255   fgSetString("/sim/atc/runway", r->ident().c_str());
 256   SGGeod startPos = r->pointOnCenterline( fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
 257   fgApplyStartOffset(startPos, r->headingDeg());
 258   return true;
 259 }
 260
 261
 262 static void fgSetDistOrAltFromGlideSlope() {
 263   // cout << "fgSetDistOrAltFromGlideSlope()" << endl;
 264   string apt_id = fgGetString("/sim/presets/airport-id");
 265   double gs = fgGetDouble("/sim/presets/glideslope-deg")
 266   * SG_DEGREES_TO_RADIANS ;
 267   double od = fgGetDouble("/sim/presets/offset-distance-nm");
 268   double alt = fgGetDouble("/sim/presets/altitude-ft");
 269
 270   double apt_elev = 0.0;
 271   if ( ! apt_id.empty() ) {
 272     apt_elev = fgGetAirportElev( apt_id );
 273     if ( apt_elev < -9990.0 ) {
 274       apt_elev = 0.0;
 275     }
 276   } else {
 277     apt_elev = 0.0;
 278   }
 279
 280   if( fabs(gs) > 0.01 && fabs(od) > 0.1 && alt < -9990 ) {
 281     // set altitude from glideslope and offset-distance
 282     od *= SG_NM_TO_METER * SG_METER_TO_FEET;
 283     alt = fabs(od*tan(gs)) + apt_elev;
 284     fgSetDouble("/sim/presets/altitude-ft", alt);
 285     fgSetBool("/sim/presets/onground", false);
 286     SG_LOG( SG_GENERAL, SG_INFO, "Calculated altitude as: "
 287            << alt  << " ft" );
 288   } else if( fabs(gs) > 0.01 && alt > 0 && fabs(od) < 0.1) {
 289     // set offset-distance from glideslope and altitude
 290     od  = (alt - apt_elev) / tan(gs);
 291     od *= -1*SG_FEET_TO_METER * SG_METER_TO_NM;
 292     fgSetDouble("/sim/presets/offset-distance-nm", od);
 293     fgSetBool("/sim/presets/onground", false);
 294     SG_LOG( SG_GENERAL, SG_INFO, "Calculated offset distance as: "
 295            << od  << " nm" );
 296   } else if( fabs(gs) > 0.01 ) {
 297     SG_LOG( SG_GENERAL, SG_ALERT,
 298            "Glideslope given but not altitude or offset-distance." );
 299     SG_LOG( SG_GENERAL, SG_ALERT, "Resetting glideslope to zero" );
 300     fgSetDouble("/sim/presets/glideslope-deg", 0);
 301     fgSetBool("/sim/presets/onground", true);
 302   }
 303 }
 304
 305
 306 // Set current_options lon/lat given an airport id and heading (degrees)
 307 static bool fgSetPosFromNAV( const string& id, const double& freq, FGPositioned::Type type )
 308 {
 309   FGNavList::TypeFilter filter(type);
 310   const nav_list_type navlist = FGNavList::findByIdentAndFreq( id.c_str(), freq, &filter );
 311
 312   if (navlist.size() == 0 ) {
 313     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate NAV = "
 314            << id << ":" << freq );
 315     return false;
 316   }
 317
 318   if( navlist.size() > 1 ) {
 319     std::ostringstream buf;
 320     buf << "Ambigous NAV-ID: '" << id << "'. Specify id and frequency. Available stations:" << endl;
 321     for( nav_list_type::const_iterator it = navlist.begin(); it != navlist.end(); ++it ) {
 322       // NDB stored in kHz, VOR stored in MHz * 100 :-P
 323       double factor = (*it)->type() == FGPositioned::NDB ? 1.0 : 1/100.0;
 324       string unit = (*it)->type() == FGPositioned::NDB ? "kHz" : "MHz";
 325       buf << (*it)->ident() << " "
 326       << std::setprecision(5) << (double)((*it)->get_freq() * factor) << " "
 327       << (*it)->get_lat() << "/" << (*it)->get_lon()
 328       << endl;
 329     }
 330
 331     SG_LOG( SG_GENERAL, SG_ALERT, buf.str() );
 332     return false;
 333   }
 334
 335   FGNavRecord *nav = navlist[0];
 336   fgApplyStartOffset(nav->geod(), fgGetDouble("/sim/presets/heading-deg"));
 337   return true;
 338 }
 339
 340 // Set current_options lon/lat given an aircraft carrier id
 341 static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
 342
 343   // set initial position from runway and heading
 344   SGGeod geodPos;
 345   double heading;
 346   SGVec3d uvw;
 347   if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
 348     double lon = geodPos.getLongitudeDeg();
 349     double lat = geodPos.getLatitudeDeg();
 350     double alt = geodPos.getElevationFt();
 351
 352     SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for "
 353            << carrier << " at lat = " << lat << ", lon = " << lon
 354            << ", alt = " << alt << ", heading = " << heading);
 355
 356     fgSetDouble("/sim/presets/longitude-deg",  lon);
 357     fgSetDouble("/sim/presets/latitude-deg",  lat);
 358     fgSetDouble("/sim/presets/altitude-ft", alt);
 359     fgSetDouble("/sim/presets/heading-deg", heading);
 360     fgSetDouble("/position/longitude-deg",  lon);
 361     fgSetDouble("/position/latitude-deg",  lat);
 362     fgSetDouble("/position/altitude-ft", alt);
 363     fgSetDouble("/orientation/heading-deg", heading);
 364
 365     fgSetString("/sim/presets/speed-set", "UVW");
 366     fgSetDouble("/velocities/uBody-fps", uvw(0));
 367     fgSetDouble("/velocities/vBody-fps", uvw(1));
 368     fgSetDouble("/velocities/wBody-fps", uvw(2));
 369     fgSetDouble("/sim/presets/uBody-fps", uvw(0));
 370     fgSetDouble("/sim/presets/vBody-fps", uvw(1));
 371     fgSetDouble("/sim/presets/wBody-fps", uvw(2));
 372
 373     fgSetBool("/sim/presets/onground", true);
 374
 375     return true;
 376   } else {
 377     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carrier = "
 378            << carrier );
 379     return false;
 380   }
 381 }
 382
 383 // Set current_options lon/lat given an airport id and heading (degrees)
 384 static bool fgSetPosFromFix( const string& id )
 385 {
 386   FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
 387   FGPositioned* fix = FGPositioned::findFirstWithIdent(id, &fixFilter);
 388   if (!fix) {
 389     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate fix = " << id );
 390     return false;
 391   }
 392
 393   fgApplyStartOffset(fix->geod(), fgGetDouble("/sim/presets/heading-deg"));
 394   return true;
 395 }
 396
 397 // Set the initial position based on presets (or defaults)
 398 bool initPosition()
 399 {
 400   // cout << "initPosition()" << endl;
 401   double gs = fgGetDouble("/sim/presets/glideslope-deg")
 402   * SG_DEGREES_TO_RADIANS ;
 403   double od = fgGetDouble("/sim/presets/offset-distance-nm");
 404   double alt = fgGetDouble("/sim/presets/altitude-ft");
 405
 406   bool set_pos = false;
 407
 408   // If glideslope is specified, then calculate offset-distance or
 409   // altitude relative to glide slope if either of those was not
 410   // specified.
 411   if ( fabs( gs ) > 0.01 ) {
 412     fgSetDistOrAltFromGlideSlope();
 413   }
 414
 415
 416   // If we have an explicit, in-range lon/lat, don't change it, just use it.
 417   // If not, check for an airport-id and use that.
 418   // If not, default to the middle of the KSFO field.
 419   // The default values for lon/lat are deliberately out of range
 420   // so that the airport-id can take effect; valid lon/lat will
 421   // override airport-id, however.
 422   double lon_deg = fgGetDouble("/sim/presets/longitude-deg");
 423   double lat_deg = fgGetDouble("/sim/presets/latitude-deg");
 424   if ( lon_deg >= -180.0 && lon_deg <= 180.0
 425       && lat_deg >= -90.0 && lat_deg <= 90.0 )
 426   {
 427     set_pos = true;
 428   }
 429
 430   string apt = fgGetString("/sim/presets/airport-id");
 431   string rwy_no = fgGetString("/sim/presets/runway");
 432   bool rwy_req = fgGetBool("/sim/presets/runway-requested");
 433   string vor = fgGetString("/sim/presets/vor-id");
 434   double vor_freq = fgGetDouble("/sim/presets/vor-freq");
 435   string ndb = fgGetString("/sim/presets/ndb-id");
 436   double ndb_freq = fgGetDouble("/sim/presets/ndb-freq");
 437   string carrier = fgGetString("/sim/presets/carrier");
 438   string parkpos = fgGetString("/sim/presets/parkpos");
 439   string fix = fgGetString("/sim/presets/fix");
 440   SGPropertyNode *hdg_preset = fgGetNode("/sim/presets/heading-deg", true);
 441   double hdg = hdg_preset->getDoubleValue();
 442
 443   // save some start parameters, so that we can later say what the
 444   // user really requested. TODO generalize that and move it to options.cxx
 445   static bool start_options_saved = false;
 446   if (!start_options_saved) {
 447     start_options_saved = true;
 448     SGPropertyNode *opt = fgGetNode("/sim/startup/options", true);
 449
 450     opt->setDoubleValue("latitude-deg", lat_deg);
 451     opt->setDoubleValue("longitude-deg", lon_deg);
 452     opt->setDoubleValue("heading-deg", hdg);
 453     opt->setStringValue("airport", apt.c_str());
 454     opt->setStringValue("runway", rwy_no.c_str());
 455   }
 456
 457   if (hdg > 9990.0)
 458     hdg = fgGetDouble("/environment/config/boundary/entry/wind-from-heading-deg", 270);
 459
 460   if ( !set_pos && !apt.empty() && !parkpos.empty() ) {
 461     // An airport + parking position is requested
 462     if ( fgSetPosFromAirportIDandParkpos( apt, parkpos ) ) {
 463       // set tower position
 464       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 465       fgSetString("/sim/tower/airport-id",  apt.c_str());
 466       set_pos = true;
 467     }
 468   }
 469
 470   if ( !set_pos && !apt.empty() && !rwy_no.empty() ) {
 471     // An airport + runway is requested
 472     if ( fgSetPosFromAirportIDandRwy( apt, rwy_no, rwy_req ) ) {
 473       // set tower position (a little off the heading for single
 474       // runway airports)
 475       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 476       fgSetString("/sim/tower/airport-id",  apt.c_str());
 477       set_pos = true;
 478     }
 479   }
 480
 481   if ( !set_pos && !apt.empty() ) {
 482     // An airport is requested (find runway closest to hdg)
 483     if ( setPosFromAirportIDandHdg( apt, hdg ) ) {
 484       // set tower position (a little off the heading for single
 485       // runway airports)
 486       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 487       fgSetString("/sim/tower/airport-id",  apt.c_str());
 488       set_pos = true;
 489     }
 490   }
 491
 492   if (hdg_preset->getDoubleValue() > 9990.0)
 493     hdg_preset->setDoubleValue(hdg);
 494
 495   if ( !set_pos && !vor.empty() ) {
 496     // a VOR is requested
 497     if ( fgSetPosFromNAV( vor, vor_freq, FGPositioned::VOR ) ) {
 498       set_pos = true;
 499     }
 500   }
 501
 502   if ( !set_pos && !ndb.empty() ) {
 503     // an NDB is requested
 504     if ( fgSetPosFromNAV( ndb, ndb_freq, FGPositioned::NDB ) ) {
 505       set_pos = true;
 506     }
 507   }
 508
 509   if ( !set_pos && !carrier.empty() ) {
 510     // an aircraft carrier is requested
 511     if ( fgSetPosFromCarrier( carrier, parkpos ) ) {
 512       set_pos = true;
 513     }
 514   }
 515
 516   if ( !set_pos && !fix.empty() ) {
 517     // a Fix is requested
 518     if ( fgSetPosFromFix( fix ) ) {
 519       set_pos = true;
 520     }
 521   }
 522
 523   if ( !set_pos ) {
 524     // No lon/lat specified, no airport specified, default to
 525     // middle of KSFO field.
 526     fgSetDouble("/sim/presets/longitude-deg", -122.374843);
 527     fgSetDouble("/sim/presets/latitude-deg", 37.619002);
 528   }
 529
 530   fgSetDouble( "/position/longitude-deg",
 531               fgGetDouble("/sim/presets/longitude-deg") );
 532   fgSetDouble( "/position/latitude-deg",
 533               fgGetDouble("/sim/presets/latitude-deg") );
 534   fgSetDouble( "/orientation/heading-deg", hdg_preset->getDoubleValue());
 535
 536   // determine if this should be an on-ground or in-air start
 537   if ((fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1) && carrier.empty()) {
 538     fgSetBool("/sim/presets/onground", false);
 539   } else {
 540     fgSetBool("/sim/presets/onground", true);
 541   }
 542
 543   return true;
 544 }
 545
 546 } // of namespace flightgear