Added code to put aircraft at the end of the runway closest to the desired

[flightgear.git] / src / Main / fg_init.cxx

diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx
index ef9ee26a8d8315f9e470e8c8fd88b38ae2f743b0..bd0818d9a81c0f0a32998b5e463e46be9a2804a0 100644 (file)
--- a/src/Main/fg_init.cxx
+++ b/src/Main/fg_init.cxx
@@ -56,6 +56,7 @@
 #include <simgear/timing/sg_time.hxx>
 
 #include <Aircraft/aircraft.hxx>
+#include <Airports/runways.hxx>
 #include <Airports/simple.hxx>
 #include <Autopilot/auto_gui.hxx>
 #include <Autopilot/newauto.hxx>
@@ -183,6 +184,110 @@ bool fgSetPosFromAirportID( const string& id ) {
     return true;
 }
 
+
+// Set current_options lon/lat given an airport id and heading (degrees)
+bool fgSetPosFromAirportIDandHdg( const string& id, double tgt_hdg ) {
+    FGRunway r;
+    FGRunway found_r;
+    double found_dir;
+
+    if ( id.length() ) {
+       // set initial position from runway and heading
+
+       FGPath path( current_options.get_fg_root() );
+       path.append( "Airports" );
+       path.append( "runways.mk4" );
+       FGRunways runways( path.c_str() );
+
+       FG_LOG( FG_GENERAL, FG_INFO,
+               "Attempting to set starting position from runway code "
+               << id << " heading " << tgt_hdg );
+
+       // FGPath inpath( current_options.get_fg_root() );
+       // inpath.append( "Airports" );
+       // inpath.append( "apt_simple" );
+       // airports.load( inpath.c_str() );
+
+       // FGPath outpath( current_options.get_fg_root() );
+       // outpath.append( "Airports" );
+       // outpath.append( "simple.gdbm" );
+       // airports.dump_gdbm( outpath.c_str() );
+
+       if ( ! runways.search( id, &r ) ) {
+           FG_LOG( FG_GENERAL, FG_ALERT,
+                   "Failed to find " << id << " in database." );
+           return false;
+       }
+
+       double diff;
+       double min_diff = 360.0;
+
+       while ( r.id == id ) {
+           // forward direction
+           diff = tgt_hdg - r.heading;
+           while ( diff < -180.0 ) { diff += 360.0; }
+           diff = fabs(diff);
+           FG_LOG( FG_GENERAL, FG_INFO,
+                   "Runway " << r.rwy_no << " heading = " << r.heading <<
+                   " diff = " << diff );
+           if ( diff < min_diff ) {
+               min_diff = diff;
+               found_r = r;
+               found_dir = 0;
+           }
+
+           // reverse direction
+           diff = tgt_hdg - r.heading - 180.0;
+           while ( diff < -180.0 ) { diff += 360.0; }
+           diff = fabs(diff);
+           FG_LOG( FG_GENERAL, FG_INFO,
+                   "Runway -" << r.rwy_no << " heading = " <<
+                   r.heading + 180.0 <<
+                   " diff = " << diff );
+           if ( diff < min_diff ) {
+               min_diff = diff;
+               found_r = r;
+               found_dir = 180.0;
+           }
+
+           runways.next( &r );
+       }
+
+       FG_LOG( FG_GENERAL, FG_INFO, "closest runway = " << found_r.rwy_no
+               << " + " << found_dir );
+
+    } else {
+       return false;
+    }
+
+    double heading = found_r.heading + found_dir;
+    while ( heading >= 360.0 ) { heading -= 360.0; }
+
+    double lat2, lon2, az2;
+    double azimuth = found_r.heading + found_dir + 180.0;
+    while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+
+    FG_LOG( FG_GENERAL, FG_INFO,
+           "runway =  " << found_r.lon << ", " << found_r.lat
+           << " length = " << found_r.length * FEET_TO_METER * 0.5 
+           << " heading = " << azimuth );
+    geo_direct_wgs_84 ( 0, found_r.lat, found_r.lon, 
+                       azimuth, found_r.length * FEET_TO_METER * 0.5 - 5.0,
+                       &lat2, &lon2, &az2 );
+    current_options.set_lon( lon2 );
+    current_options.set_lat( lat2 );
+    current_options.set_heading( heading );
+
+    FG_LOG( FG_GENERAL, FG_INFO,
+           "Position for " << id << " is ("
+           << lon2 << ", "
+           << lat2 << ") new heading is "
+           << heading );
+
+    return true;
+}
+
+
 // Set initial position and orientation
 bool fgInitPosition( void ) {
     FGInterface *f = current_aircraft.fdm_state;
@@ -192,8 +297,8 @@ bool fgInitPosition( void ) {
     f->set_Longitude( current_options.get_lon() * DEG_TO_RAD );
     f->set_Latitude( current_options.get_lat() * DEG_TO_RAD );
 
-    if ( scenery.cur_elev > current_options.get_altitude() - 2 ) {
-       current_options.set_altitude( scenery.cur_elev + 2 );
+    if ( scenery.cur_elev > current_options.get_altitude() - 1) {
+       current_options.set_altitude( scenery.cur_elev + 1 );
     }
 
     FG_LOG( FG_GENERAL, FG_INFO,
@@ -201,7 +306,7 @@ bool fgInitPosition( void ) {
 
     f->set_Altitude( current_options.get_altitude() * METER_TO_FEET );
     fgFDMSetGroundElevation( current_options.get_flight_model(),
-                            (f->get_Altitude() - 3.758099) * FEET_TO_METER );
+                            f->get_Altitude() * FEET_TO_METER );
 
     FG_LOG( FG_GENERAL, FG_INFO,
            "Initial position is: ("