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   ParkingAssignment pka;
 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     pka = dcs->getAvailableParking(radius, fltType, acType, acOperator);
 207     if (pka.isValid()) {
 208       fgGetString("/sim/presets/parkpos");
 209       fgSetString("/sim/presets/parkpos", pka.parking()->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     pka = dcs->getParkingByName(parkpos);
 217     if (!pka.isValid()) {
 218       SG_LOG( SG_GENERAL, SG_ALERT,
 219                "Failed to find a parking at airport " << id << ":" << parkpos);
 220       return false;
 221     }
 222   }
 223
 224   fgApplyStartOffset(pka.parking()->geod(), pka.parking()->getHeading());
 225   return true;
 226 }
 227
 228
 229 // Set current_options lon/lat given an airport id and runway number
 230 static bool fgSetPosFromAirportIDandRwy( const string& id, const string& rwy, bool rwy_req ) {
 231   if ( id.empty() )
 232     return false;
 233
 234   // set initial position from airport and runway number
 235   SG_LOG( SG_GENERAL, SG_INFO,
 236          "Attempting to set starting position for "
 237          << id << ":" << rwy );
 238
 239   const FGAirport* apt = fgFindAirportID(id);
 240   if (!apt) {
 241     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to find airport:" << id);
 242     return false;
 243   }
 244
 245   if (!apt->hasRunwayWithIdent(rwy)) {
 246     SG_LOG( SG_GENERAL, rwy_req ? SG_ALERT : SG_INFO,
 247            "Failed to find runway " << rwy <<
 248            " at airport " << id << ". Using default runway." );
 249     return false;
 250   }
 251
 252   FGRunway* r(apt->getRunwayByIdent(rwy));
 253   fgSetString("/sim/atc/runway", r->ident().c_str());
 254   SGGeod startPos = r->pointOnCenterline( fgGetDouble("/sim/airport/runways/start-offset-m", 5.0));
 255   fgApplyStartOffset(startPos, r->headingDeg());
 256   return true;
 257 }
 258
 259
 260 static void fgSetDistOrAltFromGlideSlope() {
 261   // cout << "fgSetDistOrAltFromGlideSlope()" << endl;
 262   string apt_id = fgGetString("/sim/presets/airport-id");
 263   double gs = fgGetDouble("/sim/presets/glideslope-deg")
 264   * SG_DEGREES_TO_RADIANS ;
 265   double od = fgGetDouble("/sim/presets/offset-distance-nm");
 266   double alt = fgGetDouble("/sim/presets/altitude-ft");
 267
 268   double apt_elev = 0.0;
 269   if ( ! apt_id.empty() ) {
 270     apt_elev = fgGetAirportElev( apt_id );
 271     if ( apt_elev < -9990.0 ) {
 272       apt_elev = 0.0;
 273     }
 274   } else {
 275     apt_elev = 0.0;
 276   }
 277
 278   if( fabs(gs) > 0.01 && fabs(od) > 0.1 && alt < -9990 ) {
 279     // set altitude from glideslope and offset-distance
 280     od *= SG_NM_TO_METER * SG_METER_TO_FEET;
 281     alt = fabs(od*tan(gs)) + apt_elev;
 282     fgSetDouble("/sim/presets/altitude-ft", alt);
 283     fgSetBool("/sim/presets/onground", false);
 284     SG_LOG( SG_GENERAL, SG_INFO, "Calculated altitude as: "
 285            << alt  << " ft" );
 286   } else if( fabs(gs) > 0.01 && alt > 0 && fabs(od) < 0.1) {
 287     // set offset-distance from glideslope and altitude
 288     od  = (alt - apt_elev) / tan(gs);
 289     od *= -1*SG_FEET_TO_METER * SG_METER_TO_NM;
 290     fgSetDouble("/sim/presets/offset-distance-nm", od);
 291     fgSetBool("/sim/presets/onground", false);
 292     SG_LOG( SG_GENERAL, SG_INFO, "Calculated offset distance as: "
 293            << od  << " nm" );
 294   } else if( fabs(gs) > 0.01 ) {
 295     SG_LOG( SG_GENERAL, SG_ALERT,
 296            "Glideslope given but not altitude or offset-distance." );
 297     SG_LOG( SG_GENERAL, SG_ALERT, "Resetting glideslope to zero" );
 298     fgSetDouble("/sim/presets/glideslope-deg", 0);
 299     fgSetBool("/sim/presets/onground", true);
 300   }
 301 }
 302
 303
 304 // Set current_options lon/lat given an airport id and heading (degrees)
 305 static bool fgSetPosFromNAV( const string& id, const double& freq, FGPositioned::Type type )
 306 {
 307   FGNavList::TypeFilter filter(type);
 308   const nav_list_type navlist = FGNavList::findByIdentAndFreq( id.c_str(), freq, &filter );
 309
 310   if (navlist.size() == 0 ) {
 311     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate NAV = "
 312            << id << ":" << freq );
 313     return false;
 314   }
 315
 316   if( navlist.size() > 1 ) {
 317     std::ostringstream buf;
 318     buf << "Ambigous NAV-ID: '" << id << "'. Specify id and frequency. Available stations:" << endl;
 319     for( nav_list_type::const_iterator it = navlist.begin(); it != navlist.end(); ++it ) {
 320       // NDB stored in kHz, VOR stored in MHz * 100 :-P
 321       double factor = (*it)->type() == FGPositioned::NDB ? 1.0 : 1/100.0;
 322       string unit = (*it)->type() == FGPositioned::NDB ? "kHz" : "MHz";
 323       buf << (*it)->ident() << " "
 324       << std::setprecision(5) << (double)((*it)->get_freq() * factor) << " "
 325       << (*it)->get_lat() << "/" << (*it)->get_lon()
 326       << endl;
 327     }
 328
 329     SG_LOG( SG_GENERAL, SG_ALERT, buf.str() );
 330     return false;
 331   }
 332
 333   FGNavRecord *nav = navlist[0];
 334   fgApplyStartOffset(nav->geod(), fgGetDouble("/sim/presets/heading-deg"));
 335   return true;
 336 }
 337
 338 // Set current_options lon/lat given an aircraft carrier id
 339 static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
 340
 341   // set initial position from runway and heading
 342   SGGeod geodPos;
 343   double heading;
 344   SGVec3d uvw;
 345   if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
 346     double lon = geodPos.getLongitudeDeg();
 347     double lat = geodPos.getLatitudeDeg();
 348     double alt = geodPos.getElevationFt();
 349
 350     SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for "
 351            << carrier << " at lat = " << lat << ", lon = " << lon
 352            << ", alt = " << alt << ", heading = " << heading);
 353
 354     fgSetDouble("/sim/presets/longitude-deg",  lon);
 355     fgSetDouble("/sim/presets/latitude-deg",  lat);
 356     fgSetDouble("/sim/presets/altitude-ft", alt);
 357     fgSetDouble("/sim/presets/heading-deg", heading);
 358     fgSetDouble("/position/longitude-deg",  lon);
 359     fgSetDouble("/position/latitude-deg",  lat);
 360     fgSetDouble("/position/altitude-ft", alt);
 361     fgSetDouble("/orientation/heading-deg", heading);
 362
 363     fgSetString("/sim/presets/speed-set", "UVW");
 364     fgSetDouble("/velocities/uBody-fps", uvw(0));
 365     fgSetDouble("/velocities/vBody-fps", uvw(1));
 366     fgSetDouble("/velocities/wBody-fps", uvw(2));
 367     fgSetDouble("/sim/presets/uBody-fps", uvw(0));
 368     fgSetDouble("/sim/presets/vBody-fps", uvw(1));
 369     fgSetDouble("/sim/presets/wBody-fps", uvw(2));
 370
 371     fgSetBool("/sim/presets/onground", true);
 372
 373     return true;
 374   } else {
 375     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate aircraft carrier = "
 376            << carrier );
 377     return false;
 378   }
 379 }
 380
 381 // Set current_options lon/lat given an airport id and heading (degrees)
 382 static bool fgSetPosFromFix( const string& id )
 383 {
 384   FGPositioned::TypeFilter fixFilter(FGPositioned::FIX);
 385   FGPositioned* fix = FGPositioned::findFirstWithIdent(id, &fixFilter);
 386   if (!fix) {
 387     SG_LOG( SG_GENERAL, SG_ALERT, "Failed to locate fix = " << id );
 388     return false;
 389   }
 390
 391   fgApplyStartOffset(fix->geod(), fgGetDouble("/sim/presets/heading-deg"));
 392   return true;
 393 }
 394
 395 // Set the initial position based on presets (or defaults)
 396 bool initPosition()
 397 {
 398   // cout << "initPosition()" << endl;
 399   double gs = fgGetDouble("/sim/presets/glideslope-deg")
 400   * SG_DEGREES_TO_RADIANS ;
 401   double od = fgGetDouble("/sim/presets/offset-distance-nm");
 402   double alt = fgGetDouble("/sim/presets/altitude-ft");
 403
 404   bool set_pos = false;
 405
 406   // If glideslope is specified, then calculate offset-distance or
 407   // altitude relative to glide slope if either of those was not
 408   // specified.
 409   if ( fabs( gs ) > 0.01 ) {
 410     fgSetDistOrAltFromGlideSlope();
 411   }
 412
 413
 414   // If we have an explicit, in-range lon/lat, don't change it, just use it.
 415   // If not, check for an airport-id and use that.
 416   // If not, default to the middle of the KSFO field.
 417   // The default values for lon/lat are deliberately out of range
 418   // so that the airport-id can take effect; valid lon/lat will
 419   // override airport-id, however.
 420   double lon_deg = fgGetDouble("/sim/presets/longitude-deg");
 421   double lat_deg = fgGetDouble("/sim/presets/latitude-deg");
 422   if ( lon_deg >= -180.0 && lon_deg <= 180.0
 423       && lat_deg >= -90.0 && lat_deg <= 90.0 )
 424   {
 425     set_pos = true;
 426   }
 427
 428   string apt = fgGetString("/sim/presets/airport-id");
 429   string rwy_no = fgGetString("/sim/presets/runway");
 430   bool rwy_req = fgGetBool("/sim/presets/runway-requested");
 431   string vor = fgGetString("/sim/presets/vor-id");
 432   double vor_freq = fgGetDouble("/sim/presets/vor-freq");
 433   string ndb = fgGetString("/sim/presets/ndb-id");
 434   double ndb_freq = fgGetDouble("/sim/presets/ndb-freq");
 435   string carrier = fgGetString("/sim/presets/carrier");
 436   string parkpos = fgGetString("/sim/presets/parkpos");
 437   string fix = fgGetString("/sim/presets/fix");
 438   SGPropertyNode *hdg_preset = fgGetNode("/sim/presets/heading-deg", true);
 439   double hdg = hdg_preset->getDoubleValue();
 440
 441   // save some start parameters, so that we can later say what the
 442   // user really requested. TODO generalize that and move it to options.cxx
 443   static bool start_options_saved = false;
 444   if (!start_options_saved) {
 445     start_options_saved = true;
 446     SGPropertyNode *opt = fgGetNode("/sim/startup/options", true);
 447
 448     opt->setDoubleValue("latitude-deg", lat_deg);
 449     opt->setDoubleValue("longitude-deg", lon_deg);
 450     opt->setDoubleValue("heading-deg", hdg);
 451     opt->setStringValue("airport", apt.c_str());
 452     opt->setStringValue("runway", rwy_no.c_str());
 453   }
 454
 455   if (hdg > 9990.0)
 456     hdg = fgGetDouble("/environment/config/boundary/entry/wind-from-heading-deg", 270);
 457
 458   if ( !set_pos && !apt.empty() && !parkpos.empty() ) {
 459     // An airport + parking position is requested
 460     if ( fgSetPosFromAirportIDandParkpos( apt, parkpos ) ) {
 461       // set tower position
 462       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 463       fgSetString("/sim/tower/airport-id",  apt.c_str());
 464       set_pos = true;
 465     }
 466   }
 467
 468   if ( !set_pos && !apt.empty() && !rwy_no.empty() ) {
 469     // An airport + runway is requested
 470     if ( fgSetPosFromAirportIDandRwy( apt, rwy_no, rwy_req ) ) {
 471       // set tower position (a little off the heading for single
 472       // runway airports)
 473       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 474       fgSetString("/sim/tower/airport-id",  apt.c_str());
 475       set_pos = true;
 476     }
 477   }
 478
 479   if ( !set_pos && !apt.empty() ) {
 480     // An airport is requested (find runway closest to hdg)
 481     if ( setPosFromAirportIDandHdg( apt, hdg ) ) {
 482       // set tower position (a little off the heading for single
 483       // runway airports)
 484       fgSetString("/sim/airport/closest-airport-id",  apt.c_str());
 485       fgSetString("/sim/tower/airport-id",  apt.c_str());
 486       set_pos = true;
 487     }
 488   }
 489
 490   if (hdg_preset->getDoubleValue() > 9990.0)
 491     hdg_preset->setDoubleValue(hdg);
 492
 493   if ( !set_pos && !vor.empty() ) {
 494     // a VOR is requested
 495     if ( fgSetPosFromNAV( vor, vor_freq, FGPositioned::VOR ) ) {
 496       set_pos = true;
 497     }
 498   }
 499
 500   if ( !set_pos && !ndb.empty() ) {
 501     // an NDB is requested
 502     if ( fgSetPosFromNAV( ndb, ndb_freq, FGPositioned::NDB ) ) {
 503       set_pos = true;
 504     }
 505   }
 506
 507   if ( !set_pos && !carrier.empty() ) {
 508     // an aircraft carrier is requested
 509     if ( fgSetPosFromCarrier( carrier, parkpos ) ) {
 510       set_pos = true;
 511     }
 512   }
 513
 514   if ( !set_pos && !fix.empty() ) {
 515     // a Fix is requested
 516     if ( fgSetPosFromFix( fix ) ) {
 517       set_pos = true;
 518     }
 519   }
 520
 521   if ( !set_pos ) {
 522     // No lon/lat specified, no airport specified, default to
 523     // middle of KSFO field.
 524     fgSetDouble("/sim/presets/longitude-deg", -122.374843);
 525     fgSetDouble("/sim/presets/latitude-deg", 37.619002);
 526   }
 527
 528   fgSetDouble( "/position/longitude-deg",
 529               fgGetDouble("/sim/presets/longitude-deg") );
 530   fgSetDouble( "/position/latitude-deg",
 531               fgGetDouble("/sim/presets/latitude-deg") );
 532   fgSetDouble( "/orientation/heading-deg", hdg_preset->getDoubleValue());
 533
 534   // determine if this should be an on-ground or in-air start
 535   if ((fabs(gs) > 0.01 || fabs(od) > 0.1 || alt > 0.1) && carrier.empty()) {
 536     fgSetBool("/sim/presets/onground", false);
 537   } else {
 538     fgSetBool("/sim/presets/onground", true);
 539   }
 540
 541   return true;
 542 }
 543
 544 } // of namespace flightgear