From 025a1e249125872ae3e1318744a309e72eab5307 Mon Sep 17 00:00:00 2001 From: ehofman Date: Thu, 22 Jul 2004 18:50:29 +0000 Subject: [PATCH] Durk Talsma: I've included the latest fixes to the Traffic Manager/AI flightplan generation code. Most of the code changes are in AIFllightplan.cxx. This is the code that runs without depending on predefined FlightPlans in #FG_ROOT/Data/AI/Flightplans.i As suggested by Dave, I've also added a new property in preferences.xml: /sim/traffic-manager/enabled, which is used to control whether or not the traffic manager is active. I'm still working on a few more 737 traffic patterns, those are going to take a little longer, so I didn't want to wait sending in this code. Finally, I haven't put much effort into ensuring "aeronautical correctness" in this version yet. The code works on my system, but what the AI plane do may actaully be quite rediculous. But I'd like to leave that for the next version. --- src/AIModel/AIFlightPlan.cxx | 486 +++++++++++++++++++++++++++++------ src/AIModel/AIFlightPlan.hxx | 11 +- src/Traffic/Schedule.cxx | 31 ++- 3 files changed, 428 insertions(+), 100 deletions(-) diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index b8fdc884b..2c838f824 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -17,10 +17,11 @@ // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. -#include "AIFlightPlan.hxx" + #include #include #include +#include #include #include #ifdef __BORLANDC__ @@ -29,6 +30,15 @@ #include #include
#include
+#include
+#include +#include + + +#include +#include + +#include "AIFlightPlan.hxx" FGAIFlightPlan::FGAIFlightPlan(string filename) @@ -84,26 +94,52 @@ FGAIFlightPlan::FGAIFlightPlan(string filename, double lon, double alt, double speed, - double course) + double course, + FGAirport *dep, + FGAirport *arr) { - int i; bool useInitialWayPoint = true; + bool useCurrentWayPoint = false; SGPath path( globals->get_fg_root() ); path.append( ("/Data/AI/FlightPlans/" + filename).c_str() ); SGPropertyNode root; try { - readProperties(path.str(), &root); - } catch (const sg_exception &e) { - SG_LOG(SG_GENERAL, SG_ALERT, - "Error reading AI flight plan: "); - cout << path.str() << endl; - return; + readProperties(path.str(), &root); + + SGPropertyNode * node = root.getNode("flightplan"); + + //waypoints.push_back( init_waypoint ); + for (int i = 0; i < node->nChildren(); i++) { + //cout << "Reading waypoint " << i << endl; + waypoint* wpt = new waypoint; + SGPropertyNode * wpt_node = node->getChild(i); + wpt->name = wpt_node->getStringValue("name", "END"); + wpt->latitude = wpt_node->getDoubleValue("lat", 0); + wpt->longitude = wpt_node->getDoubleValue("lon", 0); + wpt->altitude = wpt_node->getDoubleValue("alt", 0); + wpt->speed = wpt_node->getDoubleValue("ktas", 0); + //wpt->speed = speed; + wpt->crossat = wpt_node->getDoubleValue("crossat", -10000); + wpt->gear_down = wpt_node->getBoolValue("gear-down", false); + wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false); + + if (wpt->name == "END") wpt->finished = true; + else wpt->finished = false; + waypoints.push_back(wpt); + } + } + catch (const sg_exception &e) { + //SG_LOG(SG_GENERAL, SG_ALERT, + // "Error reading AI flight plan: "); + // cout << path.str() << endl; + // cout << "Trying to create this plan dynamically" << endl; + // cout << "Route from " << dep->id << " to " << arr->id << endl; + create(dep,arr, alt, speed); + // Now that we have dynamically created a flight plan, + // we need to add some code that pops any waypoints already past. + //return; } - - SGPropertyNode * node = root.getNode("flightplan"); - // First waypoint is current position of the aircraft as - // dictated by the traffic manager. waypoint* init_waypoint = new waypoint; init_waypoint->name = string("initial position"); init_waypoint->latitude = lat; @@ -113,77 +149,74 @@ FGAIFlightPlan::FGAIFlightPlan(string filename, init_waypoint->crossat = - 10000; init_waypoint->gear_down = false; init_waypoint->flaps_down = false; - waypoints.push_back( init_waypoint ); - for (i = 0; i < node->nChildren(); i++) { - //cout << "Reading waypoint " << i << endl; - waypoint* wpt = new waypoint; - SGPropertyNode * wpt_node = node->getChild(i); - wpt->name = wpt_node->getStringValue("name", "END"); - wpt->latitude = wpt_node->getDoubleValue("lat", 0); - wpt->longitude = wpt_node->getDoubleValue("lon", 0); - wpt->altitude = wpt_node->getDoubleValue("alt", 0); - wpt->speed = wpt_node->getDoubleValue("ktas", 0); - //wpt->speed = speed; - wpt->crossat = wpt_node->getDoubleValue("crossat", -10000); - wpt->gear_down = wpt_node->getBoolValue("gear-down", false); - wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false); + init_waypoint->finished = false; - if (wpt->name == "END") wpt->finished = true; - else wpt->finished = false; - // discard this waypoint if it's bearing differs more than - // 90 degrees from the course we should fly according to the - // Traffic manager. Those are considered "behind" us. - SGWayPoint first(init_waypoint->longitude, - init_waypoint->latitude, - init_waypoint->altitude); - SGWayPoint curr (wpt->longitude, - wpt->latitude, - wpt->altitude); - double crse, crsDiff; - double dist; - first.CourseAndDistance(curr, &crse, &dist); - - dist *= SG_METER_TO_NM; - - // We're only interested in the absolute value of crsDiff - // wich should fall in the 0-180 deg range. - crsDiff = fabs(crse-course); - if (crsDiff > 180) - crsDiff -= 180; - // These are the threee conditions that we consder including - // in our flight plan: - // 1) current waypoint is less then 100 miles away OR - // 2) curren waypoint is ahead of us, at any distance - bool useWpt = false; - if ((dist > 100.0) && (crsDiff > 90.0) && (wpt->name != string ("EOF"))) - { - //useWpt = false; - // Once we start including waypoints, we have to continue, even though - // one of the following way point would suffice. - // so once is the useWpt flag is set to true, we cannot reset it to false. - // cerr << "Discarding waypoint: " << wpt->name - // << ": Course difference = " << crsDiff << endl; - } - else - useWpt = true; + wpt_vector_iterator i = waypoints.begin(); + while (i != waypoints.end()) + { + //cerr << "Checking status of each waypoint: " << (*i)->name << endl; + SGWayPoint first(init_waypoint->longitude, + init_waypoint->latitude, + init_waypoint->altitude); + SGWayPoint curr ((*i)->longitude, + (*i)->latitude, + (*i)->altitude); + double crse, crsDiff; + double dist; + first.CourseAndDistance(curr, &crse, &dist); + + dist *= SG_METER_TO_NM; + + // We're only interested in the absolute value of crsDiff + // wich should fall in the 0-180 deg range. + crsDiff = fabs(crse-course); + if (crsDiff > 180) + crsDiff = 360-crsDiff; + // These are the three conditions that we consider including + // in our flight plan: + // 1) current waypoint is less then 100 miles away OR + // 2) curren waypoint is ahead of us, at any distance - if (useWpt) - { - if ((dist > 100.0) && (useInitialWayPoint)) - { - waypoints.push_back(init_waypoint); - //cerr << "Using waypoint : " << init_waypoint->name << endl; - } - waypoints.push_back( wpt ); - //cerr << "Using waypoint : " << wpt->name - // << ": course diff : " << crsDiff - // << "distance : " << dist << endl; - useInitialWayPoint = false; - } - else - delete wpt; - } - + if ((dist > 20.0) && (crsDiff > 90.0) && ((*i)->name != string ("EOF"))) + { + //useWpt = false; + // Once we start including waypoints, we have to continue, even though + // one of the following way point would suffice. + // so once is the useWpt flag is set to true, we cannot reset it to false. + //cerr << "Discarding waypoint: " << (*i)->name + // << ": Course difference = " << crsDiff + // << "Course = " << course + // << "crse = " << crse << endl; + } + else + useCurrentWayPoint = true; + + if (useCurrentWayPoint) + { + if ((dist > 100.0) && (useInitialWayPoint)) + { + //waypoints.push_back(init_waypoint); + waypoints.insert(i, init_waypoint); + //cerr << "Using waypoint : " << init_waypoint->name << endl; + } + //waypoints.push_back( wpt ); + //cerr << "Using waypoint : " << (*i)->name + // << ": course diff : " << crsDiff + // << "Course = " << course + // << "crse = " << crse << endl + // << "distance : " << dist << endl; + useInitialWayPoint = false; + i++; + } + else + { + //delete wpt; + delete *(i); + i = waypoints.erase(i); + } + } + //for (i = waypoints.begin(); i != waypoints.end(); i++) + // cerr << "Using waypoint : " << (*i)->name << endl; wpt_iterator = waypoints.begin(); //cout << waypoints.size() << " waypoints read." << endl; } @@ -301,3 +334,292 @@ double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){ } +/* FGAIFlightPlan::create() + * dynamically create a flight plan for AI traffic, based on data provided by the + * Traffic Manager, when reading a filed flightplan failes. (DT, 2004/07/10) + * + * Probably need to split this into separate functions for different parts of the flight + + * once the code matures a bit more. + * + */ +void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, double alt, double speed) +{ + double wind_speed; + double wind_heading; + FGRunway rwy; + + //waypoints.push_back(wpt); + // Create the outbound taxi leg, for now simplified as a + // Direct route from the airport center point to the start + // of the runway. + /////////////////////////////////////////////////////////// + //cerr << "Cruise Alt << " << alt << endl; + waypoint *wpt = new waypoint; + wpt->name = dep->id; //wpt_node->getStringValue("name", "END"); + wpt->latitude = dep->latitude; + wpt->longitude = dep->longitude; + wpt->altitude = dep->elevation + 19; // probably need to add some model height to it + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + waypoints.push_back(wpt); + + // Get the current active runway, based on code from David Luff + FGEnvironment + stationweather = ((FGEnvironmentMgr *) globals->get_subsystem("environment")) + ->getEnvironment(dep->latitude, dep->longitude, dep->elevation); + + wind_speed = stationweather.get_wind_speed_kt(); + wind_heading = stationweather.get_wind_from_heading_deg(); + if (wind_speed == 0) { + wind_heading = 270; // This forces West-facing rwys to be used in no-wind situations + // which is consistent with Flightgear's initial setup. + } + + string rwy_no = globals->get_runways()->search(dep->id, int(wind_heading)); + if (!(globals->get_runways()->search(dep->id, (int) wind_heading, &rwy ))) + { + cout << "Failed to find runway for " << dep->id << endl; + // Hmm, how do we handle a potential error like this? + exit(1); + } + + double lat, lon, az; + double lat2, lon2, az2; + double heading = rwy.heading; + double azimuth = heading + 180.0; + while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + rwy.length * SG_FEET_TO_METER * 0.5 - 5.0, + &lat2, &lon2, &az2 ); + + //Add the runway startpoint; + wpt = new waypoint; + wpt->name = rwy.id; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->elevation + 19; + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + waypoints.push_back(wpt); + + //Next: The point on the runway where we begin to accelerate to take-off speed + //100 meters down the runway seems to work. Shorter distances cause problems with + // the turn with larger aircraft + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + rwy.length * SG_FEET_TO_METER * 0.5 - 105.0, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "accel"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = dep->elevation + 19; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + waypoints.push_back(wpt); + + lat = lat2; + lon = lon2; + az = az2; + + //Next: the Start of Climb + geo_direct_wgs_84 ( 0, lat, lon, heading, + 2560 * SG_FEET_TO_METER, + &lat2, &lon2, &az2 ); + + wpt = new waypoint; + wpt->name = "SOC"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = alt + 19; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + +//Next: the Top of Climb + geo_direct_wgs_84 ( 0, lat, lon, heading, + 20*SG_NM_TO_METER, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "10000ft climb"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = 10000; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + + + //Beginning of Decent + stationweather = ((FGEnvironmentMgr *)globals->get_subsystem("environment")) + ->getEnvironment(arr->latitude, arr->longitude, arr->elevation); + + wind_speed = stationweather.get_wind_speed_kt(); + wind_heading = stationweather.get_wind_from_heading_deg(); + + if (wind_speed == 0) { + wind_heading = 270; // This forces West-facing rwys to be used in no-wind situations + // which is consistent with Flightgear's initial setup. + } + + rwy_no = globals->get_runways()->search(arr->id, int(wind_heading)); + //cout << "Using runway # " << rwy_no << " for departure at " << dep->id << endl; + + if (!(globals->get_runways()->search(arr->id, (int) wind_heading, &rwy ))) + { + cout << "Failed to find runway for " << arr->id << endl; + // Hmm, how do we handle a potential error like this? + exit(1); + } + //cerr << "Done" << endl; + heading = rwy.heading; + azimuth = heading + 180.0; + while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + + + + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + 100000, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "BOD"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = 10000; + wpt->speed = speed; + wpt->crossat = alt +19; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + // Ten thousand ft. Slowing down to 240 kts + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + 20*SG_NM_TO_METER, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "Dec 10000ft"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = arr->elevation + 19; + wpt->speed = 240; + wpt->crossat = 10000; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + // Three thousand ft. Slowing down to 160 kts + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + 8*SG_NM_TO_METER, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "DEC 3000ft"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = arr->elevation + 19; + wpt->speed = 160; + wpt->crossat = 3000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + //Runway Threshold + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + rwy.length*0.45, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "Threshold"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = arr->elevation + 19; + wpt->speed = 15; + wpt->crossat = arr->elevation + 19; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + waypoints.push_back(wpt); + + //Full stop at the runway centerpoint + geo_direct_wgs_84 ( 0, rwy.lat, rwy.lon, azimuth, + rwy.length*0.45, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "Center"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = rwy.lat; + wpt->longitude = rwy.lon; + wpt->altitude = arr->elevation + 19; + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + // Add the final destination waypoint + wpt = new waypoint; + wpt->name = arr->id; //wpt_node->getStringValue("name", "END"); + wpt->latitude = arr->latitude; + wpt->longitude = arr->longitude; + wpt->altitude = arr->elevation+19; + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + // And finally one more named "END" + wpt = new waypoint; + wpt->name = "END"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = arr->latitude; + wpt->longitude = arr->longitude; + wpt->altitude = 19; + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = true; + wpt->on_ground = true; + waypoints.push_back(wpt); + + // And finally one more named "EOF" + wpt = new waypoint; + wpt->name = "EOF"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = arr->latitude; + wpt->longitude = arr->longitude; + wpt->altitude = 19; + wpt->speed = 15; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = true; + wpt->finished = true; + waypoints.push_back(wpt); +} diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 50040e554..4aebc90b0 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -22,6 +22,9 @@ #include #include #include + +#include + SG_USING_STD(vector); SG_USING_STD(string); @@ -46,10 +49,12 @@ public: FGAIFlightPlan(string filename); FGAIFlightPlan(string filename, double lat, - double lon, + double lon, double alt, double speed, - double course); + double course, + FGAirport *dep, + FGAirport *arr); ~FGAIFlightPlan(); waypoint* getPreviousWaypoint( void ); @@ -64,6 +69,8 @@ public: double getBearing(waypoint* previous, waypoint* next); double getBearing(double lat, double lon, waypoint* next); + void create(FGAirport *dep, FGAirport *arr, double alt, double speed); + private: typedef vector wpt_vector_type; diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx index cc9a90e45..89613021b 100644 --- a/src/Traffic/Schedule.cxx +++ b/src/Traffic/Schedule.cxx @@ -122,7 +122,7 @@ void FGAISchedule::update(time_t now) userLatitude, userLongitude; - if (fgGetBool("/sim/ai/enabled") == false) + if (fgGetBool("/sim/traffic-manager/enabled") == false) return; aimgr = (FGAIManager *) globals-> get_subsystem("ai_model"); @@ -251,25 +251,23 @@ void FGAISchedule::update(time_t now) // one hour flight time, so that would be a good approximate point // to start a more detailed simulation of this aircraft. //cerr << registration << " is currently enroute from " - // << dep->id << " to " << arr->id << "distance : " << distanceToUser*SG_METER_TO_NM << endl; + // << dep->id << " to " << arr->id << "distance : " + // << distanceToUser*SG_METER_TO_NM << endl; if ((distanceToUser*SG_METER_TO_NM) < 500.0) { - string flightPlanName = dep->id + string("-") + arr->id + string(".xml"); + string flightPlanName = dep->id + string("-") + arr->id + + string(".xml"); FGAIFlightPlan* f; - // If we're less then 10 minutes behind schedule, do a normal - // full flight plan initialization, otherwise, do a modified - // in-air initializition, at the location where this flight is - // according to the Traffic Manager - if (elapsedTimeEnroute < 600) - f = new FGAIFlightPlan(flightPlanName); - else - f = new FGAIFlightPlan(flightPlanName, - lat, - lon, - i->getCruiseAlt() * 1000, // convert from FL to feet - speed, - courseToDest); + f = new FGAIFlightPlan(flightPlanName, + lat, + lon, + i->getCruiseAlt() * 100, // convert from FL to feet + //speed, + 450, + courseToDest, + dep, + arr); // Fixme: A non-existent model path results in an // abort, due to an unhandled exeption, in fg main loop. AIManagerRef = aimgr->createAircraft("jet_transport", @@ -278,6 +276,7 @@ void FGAISchedule::update(time_t now) } return; } + // Both departure and arrival time are in the future, so this // the aircraft is parked at the departure airport. // Currently this status is mostly ignored, but in furture