From b3e969726249e772da4c9d1aa6eb212e6472aed3 Mon Sep 17 00:00:00 2001 From: ehofman Date: Thu, 3 Jun 2004 17:59:14 +0000 Subject: [PATCH] Add the AIModel based air traffic subsystem from Durk Talsma. --- Thanks | 2 +- configure.ac | 3 +- src/AIModel/AIBase.hxx | 1 + src/AIModel/AIFlightPlan.cxx | 174 +++++++++++++++++--- src/AIModel/AIFlightPlan.hxx | 6 + src/AIModel/AIManager.cxx | 2 +- src/AIModel/AIScenario.cxx | 4 +- src/FDM/JSBSim/FGTrimAxis.cpp | 33 ++-- src/Main/Makefile.am | 1 + src/Main/fg_init.cxx | 15 +- src/Main/globals.hxx | 4 + src/Makefile.am | 1 + src/Traffic/Makefile.am | 9 ++ src/Traffic/SchedFlight.cxx | 257 ++++++++++++++++++++++++++++++ src/Traffic/SchedFlight.hxx | 104 ++++++++++++ src/Traffic/Schedule.cxx | 290 ++++++++++++++++++++++++++++++++++ src/Traffic/Schedule.hxx | 62 ++++++++ src/Traffic/TrafficMgr.cxx | 207 ++++++++++++++++++++++++ src/Traffic/TrafficMgr.hxx | 69 ++++++++ 19 files changed, 1203 insertions(+), 41 deletions(-) create mode 100644 src/Traffic/Makefile.am create mode 100644 src/Traffic/SchedFlight.cxx create mode 100644 src/Traffic/SchedFlight.hxx create mode 100644 src/Traffic/Schedule.cxx create mode 100644 src/Traffic/Schedule.hxx create mode 100644 src/Traffic/TrafficMgr.cxx create mode 100644 src/Traffic/TrafficMgr.hxx diff --git a/Thanks b/Thanks index e8cd8c8e9..e1e9ba2f7 100644 --- a/Thanks +++ b/Thanks @@ -145,7 +145,7 @@ Bruce Finney Melchior Franz Made that joystick high/low support for joystick hats. Nasal-ified the Saitek Cyborg Gold 3D (USB) joystick which now forms - the basis of a lot of porgrammable joysticks. + the basis of a lot of programmable joysticks. Created the led.txf font. Ruthlessly hunted down memory leaks in FlightGear, SimGear, and JSBSim. Maintains the only fully working helicopter model in FlightGear (Bolkow 105). diff --git a/configure.ac b/configure.ac index 73b49cb5a..797e353fa 100644 --- a/configure.ac +++ b/configure.ac @@ -120,7 +120,7 @@ case "${host}" in if test -d /opt/X11R6 ; then EXTRA_DIR2="/opt/X11R6" fi - EXTRA_DIRS="${EXTRA_DIRS} $EXTRA_DIR1 $EXTRA_DIR2" + EXTRA_DIRS="${EXTRA_DIRS} $EXTRA_DIR1 $EXTRA_DIR2 /usr/local/" ;; esac @@ -564,6 +564,7 @@ AC_CONFIG_FILES([ \ src/Sound/Makefile \ src/Systems/Makefile \ src/Time/Makefile \ + src/Traffic/Makefile \ tests/Makefile \ utils/Makefile \ utils/TerraSync/Makefile \ diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index b9db98c6c..b98cb5d26 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -163,6 +163,7 @@ inline void FGAIBase::setAltitude( double altitude_ft ) { inline void FGAIBase::setBank( double bank ) { roll = tgt_roll = bank; + no_roll = false; } inline void FGAIBase::setLongitude( double longitude ) { diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 289d850dd..b8fdc884b 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -20,6 +20,7 @@ #include "AIFlightPlan.hxx" #include #include +#include #include #include #ifdef __BORLANDC__ @@ -72,6 +73,124 @@ FGAIFlightPlan::FGAIFlightPlan(string filename) } +// This is a modified version of the constructor, +// Which not only reads the waypoints from a +// Flight plan file, but also adds the current +// Position computed by the traffic manager, as well +// as setting speeds and altitude computed by the +// traffic manager. +FGAIFlightPlan::FGAIFlightPlan(string filename, + double lat, + double lon, + double alt, + double speed, + double course) +{ + int i; + bool useInitialWayPoint = true; + 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; + } + + 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; + init_waypoint->longitude = lon; + init_waypoint->altitude = alt; + init_waypoint->speed = speed; + 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); + + 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; + + 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; + } + + wpt_iterator = waypoints.begin(); + //cout << waypoints.size() << " waypoints read." << endl; +} + + + + FGAIFlightPlan::~FGAIFlightPlan() { waypoints.clear(); @@ -144,32 +263,41 @@ double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second){ double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){ - double latd = lat; - double lond = lon; - double latt = wp->latitude; - double lont = wp->longitude; - double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES); - double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES); + double course, distance; + // double latd = lat; +// double lond = lon; +// double latt = wp->latitude; +// double lont = wp->longitude; +// double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES); +// double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES); - if (lond < 0.0) lond+=360.0; - if (lont < 0.0) lont+=360.0; - latd+=90.0; - latt+=90.0; +// if (lond < 0.0) { +// lond+=360.0; +// lont+=360; +// } +// if (lont < 0.0) { +// lond+=360.0; +// lont+=360.0; +// } +// latd+=90.0; +// latt+=90.0; - double lat_diff = (latt - latd) * ft_per_deg_lat; - double lon_diff = (lont - lond) * ft_per_deg_lon; - double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES; - - bool southerly = true; - if (latt > latd) southerly = false; - bool easterly = false; - if (lont > lond) easterly = true; - if (southerly && easterly) return 90.0 + angle; - if (!southerly && easterly) return 90.0 - angle; - if (southerly && !easterly) return 270.0 - angle; - if (!southerly && !easterly) return 270.0 + angle; +// double lat_diff = (latt - latd) * ft_per_deg_lat; +// double lon_diff = (lont - lond) * ft_per_deg_lon; +// double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES; +// bool southerly = true; +// if (latt > latd) southerly = false; +// bool easterly = false; +// if (lont > lond) easterly = true; +// if (southerly && easterly) return 90.0 + angle; +// if (!southerly && easterly) return 90.0 - angle; +// if (southerly && !easterly) return 270.0 - angle; +// if (!southerly && !easterly) return 270.0 + angle; + SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp")); + sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance); + return course; // Omit a compiler warning. - return 0; + } diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index bc75b9ab3..50040e554 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -44,6 +44,12 @@ public: } waypoint; FGAIFlightPlan(string filename); + FGAIFlightPlan(string filename, + double lat, + double lon, + double alt, + double speed, + double course); ~FGAIFlightPlan(); waypoint* getPreviousWaypoint( void ); diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index 1aed8f732..3667d1556 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -123,7 +123,7 @@ int FGAIManager::assignID() { int maxint = 30000; int x; bool used; - for (x=0; xazimuth = entry_node->getDoubleValue("azimuth", 0.0); en->elevation = entry_node->getDoubleValue("elevation", 0.0); en->rudder = entry_node->getDoubleValue("rudder", 0.0); - en->strength = entry_node->getDoubleValue("strength", 0.0); - en->diameter = entry_node->getDoubleValue("diameter", 0.0); + en->strength = entry_node->getDoubleValue("strength-fps", 0.0); + en->diameter = entry_node->getDoubleValue("diameter-ft", 0.0); } entry_iterator = entries.begin(); diff --git a/src/FDM/JSBSim/FGTrimAxis.cpp b/src/FDM/JSBSim/FGTrimAxis.cpp index e95d1c54b..9181bae7c 100644 --- a/src/FDM/JSBSim/FGTrimAxis.cpp +++ b/src/FDM/JSBSim/FGTrimAxis.cpp @@ -290,7 +290,8 @@ void FGTrimAxis::SetThetaOnGround(double ff) { bool FGTrimAxis::initTheta(void) { int i,N,iAft, iForward; - double zAft,zForward,zDiff,theta; + double zAft,zForward,zDiff,theta; + double xAft,xForward,xDiff; bool level; double saveAlt; @@ -317,17 +318,25 @@ bool FGTrimAxis::initTheta(void) { } // now adjust theta till the wheels are the same distance from the ground - zAft=fdmex->GetGroundReactions()->GetGearUnit(1)->GetLocalGear(3); - zForward=fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear(3); - zDiff = zForward - zAft; - level=false; - theta=fgic->GetPitchAngleDegIC(); - while(!level && (i < 100)) { - theta+=2.0*zDiff; - fgic->SetPitchAngleDegIC(theta); - fdmex->RunIC(); - zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3); - zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3); + xAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(1); + xForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(1); + xDiff = xForward - xAft; + zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3); + zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3); + zDiff = zForward - zAft; + level=false; + theta=fgic->GetPitchAngleDegIC(); + while(!level && (i < 100)) { + theta+=180.0/M_PI*zDiff/fabs(xDiff); + fgic->SetPitchAngleDegIC(theta); + fdmex->RunIC(); + xAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(1); + xForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(1); + xDiff = xForward - xAft; + zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3); + zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3); + zDiff = zForward - zAft; + zDiff = zForward - zAft; //cout << endl << theta << " " << zDiff << endl; //cout << "0: " << fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear() << endl; diff --git a/src/Main/Makefile.am b/src/Main/Makefile.am index f3f801463..d1373a27a 100644 --- a/src/Main/Makefile.am +++ b/src/Main/Makefile.am @@ -83,6 +83,7 @@ fgfs_LDADD = \ $(top_builddir)/src/Replay/libReplay.a \ $(top_builddir)/src/Systems/libSystems.a \ $(top_builddir)/src/Time/libTime.a \ + $(top_builddir)/src/Traffic/libTraffic.a \ $(top_builddir)/src/Environment/libEnvironment.a \ $(CLOUD3D_LIBS) \ -lsgroute -lsgsky -lsgsound -lsgephem -lsgmaterial -lsgtgdb -lsgmodel \ diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 43f699fd6..15681791f 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -111,6 +111,7 @@ #include