diff --git a/src/AIModel/AIAircraft.cxx b/src/AIModel/AIAircraft.cxx index 9e5a5675b..c312e5104 100644 --- a/src/AIModel/AIAircraft.cxx +++ b/src/AIModel/AIAircraft.cxx @@ -193,7 +193,6 @@ void FGAIAircraft::Run(double dt) { // from control properties. These default to the initial // settings in the config file, but can be changed "on the // fly". - string lat_mode = props->getStringValue("controls/flight/lateral-mode"); if ( lat_mode == "roll" ) { double angle @@ -218,7 +217,7 @@ void FGAIAircraft::Run(double dt) { AccelTo( props->getDoubleValue("controls/flight/target-spd" ) ); } - + double turn_radius_ft; double turn_circum_ft; double speed_north_deg_sec; @@ -233,19 +232,30 @@ void FGAIAircraft::Run(double dt) { } else { speed_diff = groundTargetSpeed - speed; } + if (speed_diff > 0.0) { speed += performance->accel * dt; - if ( speed > tgt_speed ) speed = tgt_speed; // don't overshoot + if (no_roll) { // apply overshoot correction + if ( speed > groundTargetSpeed ) speed = groundTargetSpeed; + }else { + if ( speed > tgt_speed ) speed = tgt_speed; + } } else if (speed_diff < 0.0) { if (no_roll) { // on ground (aircraft can't roll) + // deceleration performance is better due to wheel brakes. speed -= performance->decel * dt * 3; } else { speed -= performance->decel * dt; } - if ( speed < tgt_speed ) speed = tgt_speed; // don't overshoot + if (no_roll) { // apply overshoot correction + if (speed < groundTargetSpeed ) speed = groundTargetSpeed; + } else { + if ( speed < tgt_speed ) speed = tgt_speed; + } } - + + // convert speed to degrees per second speed_north_deg_sec = cos( hdg * SGD_DEGREES_TO_RADIANS ) * speed * 1.686 / ft_per_deg_lat; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 8f728a8af..318d734ca 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -110,6 +110,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, const string& acType, const string& airline) { + repeat = false; leg = 10; gateId=0; start_time = start; @@ -118,6 +119,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, SGPath path( globals->get_fg_root() ); path.append( "/AI/FlightPlans" ); path.append( p ); + SGPropertyNode root; // This is a bit of a hack: diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index eee1ac531..755b672ec 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -14,7 +14,7 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. #ifndef _FG_AIFLIGHTPLAN_HXX #define _FG_AIFLIGHTPLAN_HXX @@ -25,6 +25,7 @@ #include #include +#include #include "AIBase.hxx" @@ -92,6 +93,8 @@ public: void setRepeat(bool r) { repeat = r; }; bool getRepeat(void) const { return repeat; }; void restart(void); + + int getNrOfWayPoints() { return waypoints.end() - waypoints.begin(); }; private: FGRunway rwy; @@ -109,6 +112,16 @@ private: int leg; int gateId; string activeRunway; + FGAirRoute route; + + + Point3D temp; + sgdVec3 a, b, cross; + sgdVec3 newPos; + sgdMat4 matrix; + double angle; + double midlat, midlon; + double course, distance; void createPushBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); void createTaxi(bool, int, FGAirport *, double, double, double, const string&, const string&, const string&); @@ -120,6 +133,9 @@ private: void createParking(FGAirport *, double radius); void deleteWaypoints(); void resetWaypoints(); + + //void createCruiseFallback(bool, FGAirport*, FGAirport*, double, double, double, double); + void evaluateRoutePart(double deplat, double deplon, double arrlat, double arrlon); }; diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 9a2b81561..90403432d 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -636,6 +636,7 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, heading = rwy._heading; double azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + cerr << "Creating climb at : " << rwy._id << " " << rwy._rwy_no << endl; geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading, 10*SG_NM_TO_METER, &lat2, &lon2, &az2 ); @@ -671,69 +672,69 @@ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, } -/******************************************************************* - * CreateCruise - * initialize the Aircraft at the parking location - ******************************************************************/ -void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, - FGAirport *arr, double latitude, - double longitude, double speed, - double alt) -{ - double wind_speed; - double wind_heading; - double heading; - double lat, lon, az; - double lat2, lon2, az2; - double azimuth; - waypoint *wpt; +// /******************************************************************* +// * CreateCruise +// * initialize the Aircraft at the parking location +// ******************************************************************/ +// void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, +// FGAirport *arr, double latitude, +// double longitude, double speed, +// double alt) +// { +// double wind_speed; +// double wind_heading; +// double heading; +// double lat, lon, az; +// double lat2, lon2, az2; +// double azimuth; +// waypoint *wpt; - wpt = new waypoint; - wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = latitude; - wpt->longitude = longitude; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - waypoints.push_back(wpt); +// wpt = new waypoint; +// wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); +// wpt->latitude = latitude; +// wpt->longitude = longitude; +// wpt->altitude = alt; +// wpt->speed = speed; +// wpt->crossat = -10000; +// wpt->gear_down = false; +// wpt->flaps_down= false; +// wpt->finished = false; +// wpt->on_ground = false; +// waypoints.push_back(wpt); - // should be changed dynamically to allow "gen" and "mil" - arr->getDynamics()->getActiveRunway("com", 2, activeRunway); - if (!(globals->get_runways()->search(arr->getId(), - activeRunway, - &rwy))) - { - SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << - activeRunway << - " at airport " << arr->getId()); - exit(1); - } - heading = rwy._heading; - azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } +// // should be changed dynamically to allow "gen" and "mil" +// arr->getDynamics()->getActiveRunway("com", 2, activeRunway); +// if (!(globals->get_runways()->search(arr->getId(), +// activeRunway, +// &rwy))) +// { +// SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << +// activeRunway << +// " at airport " << arr->getId()); +// exit(1); +// } +// heading = rwy._heading; +// azimuth = heading + 180.0; +// while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, - 110000, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "BOD"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = alt; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - waypoints.push_back(wpt); -} +// geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, +// 110000, +// &lat2, &lon2, &az2 ); +// wpt = new waypoint; +// wpt->name = "BOD"; +// wpt->latitude = lat2; +// wpt->longitude = lon2; +// wpt->altitude = alt; +// wpt->speed = speed; +// wpt->crossat = alt; +// wpt->gear_down = false; +// wpt->flaps_down= false; +// wpt->finished = false; +// wpt->on_ground = false; +// waypoints.push_back(wpt); +// } /******************************************************************* * CreateDecent diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx new file mode 100755 index 000000000..03bfce80f --- /dev/null +++ b/src/AIModel/AIFlightPlanCreateCruise.cxx @@ -0,0 +1,377 @@ +/****************************************************************************** + * AIFlightPlanCreateCruise.cxx + * Written by Durk Talsma, started February, 2006. + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation; either version 2 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + * + * + **************************************************************************/ +#include +#include +#include "AIFlightPlan.hxx" +#include +#include +#include + +#include +#include + +#include +#include + +SG_USING_STD(iostream); + +void FGAIFlightPlan::evaluateRoutePart(double deplat, + double deplon, + double arrlat, + double arrlon) +{ + // First do a prescan of all the waypoints that are within a reasonable distance of the + // ideal route. + intVec nodes; + int tmpNode, prevNode; + + + SGWayPoint first (deplon, + deplat, + 100); + SGWayPoint sec (arrlon, + arrlat, + 100); + first.CourseAndDistance(sec, &course, &distance); + distance *= SG_METER_TO_NM; + temp = sgPolarToCart3d(Point3D(deplon * + SG_DEGREES_TO_RADIANS, + deplat * + SG_DEGREES_TO_RADIANS, + 1.0)); + a[0] = temp.x(); + a[1] = temp.y(); + a[2] = temp.z(); + + temp = sgPolarToCart3d(Point3D(arrlon * + SG_DEGREES_TO_RADIANS, + arrlat * + SG_DEGREES_TO_RADIANS, + 1.0)); + b[0] = temp.x(); + b[1] = temp.y(); + b[2] = temp.z(); + sgdNormaliseVec3(a); + sgdNormaliseVec3(b); + sgdVectorProductVec3(cross,b,a); + + angle = sgACos(sgdScalarProductVec3(a,b)); + tmpNode = 0; + for (double ang = 0.0; ang < angle; ang += 0.05) + { + //cerr << "Angle = " << ang << endl; + sgdMakeRotMat4(matrix, ang, cross); + for(int j = 0; j < 3; j++) + { + newPos[j] =0.0; + for (int k = 0; k<3; k++) + { + newPos[j] += matrix[j][k]*a[k]; + } + } + //cerr << "1"<< endl; + temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2])); + midlat = temp.lat() * SG_RADIANS_TO_DEGREES; + midlon = temp.lon() * SG_RADIANS_TO_DEGREES; + + prevNode = tmpNode; + tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon); + + double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude (); + double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude (); + SGWayPoint curr(midlat, + midlon, + 100); + SGWayPoint node(nodelat, + nodelon, + 100); + curr.CourseAndDistance(node, &course, &distance); + if ((distance < 25000) && (tmpNode != prevNode)) + nodes.push_back(tmpNode); + } + + intVecIterator i = nodes.begin(); + intVecIterator j = nodes.end(); + while (i != nodes.end()) + { + j = nodes.end(); + while (j != i) + { + j--; + FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j); + if (!(routePart.empty())) + { + route.add(routePart); + i = j; + break; + } + } + i++; + } +} + + +/* +void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, + FGAirport *arr, double latitude, + double longitude, double speed, double alt) +{ + bool useInitialWayPoint = true; + bool useCurrentWayPoint = false; + double heading; + double lat, lon, az; + double lat2, lon2, az2; + double azimuth; + waypoint *wpt; + waypoint *init_waypoint; + intVec ids; + char buffer[32]; + SGPath routefile = globals->get_fg_root(); + init_waypoint = new waypoint; + init_waypoint->name = "Initial waypoint"; + init_waypoint->latitude = latitude; + init_waypoint->longitude = longitude;; + //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too + init_waypoint->altitude = alt; + init_waypoint->speed = 450; //speed; + init_waypoint->crossat = -10000; + init_waypoint->gear_down = false; + init_waypoint->flaps_down= false; + init_waypoint->finished = false; + init_waypoint->on_ground = false; + waypoints.push_back(init_waypoint); + routefile.append("Data/AI/FlightPlans"); + snprintf(buffer, 32, "%s-%s.txt", + dep->getId().c_str(), + arr->getId().c_str()); + routefile.append(buffer); + cerr << "trying to read " << routefile.c_str()<< endl; + //exit(1); + if (routefile.exists()) + { + sg_gzifstream in( routefile.str() ); + do { + in >> route; + } while (!(in.eof())); + } + else { + //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2); + //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude()); + //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude()); + //FGAirRoute route; + evaluateRoutePart(dep->getLatitude(), dep->getLongitude(), + arr->getLatitude(), arr->getLongitude()); + //exit(1); + } + route.first(); + int node; + if (route.empty()) { + // if no route could be found, create a direct gps route... + cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <get_airwaynet()->findNode(node); + //cerr << "Checking status of each waypoint: " << fn->getIdent(); + + SGWayPoint first(init_waypoint->longitude, + init_waypoint->latitude, + alt); + SGWayPoint curr (fn->getLongitude(), + fn->getLatitude(), + alt); + SGWayPoint arr (arr->getLongitude(), + arr->getLatitude(), + alt); + + double crse, crsDiff; + double dist; + first.CourseAndDistance(arr, &course, &distance); + 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 + //cerr << " Distance : " << dist << " : Course diff " << crsDiff + // << " crs to dest : " << course + // << " crs to wpt : " << crse; + if ((dist > 20.0) && (crsDiff > 90.0)) + { + //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 " << endl; + // << ": Course difference = " << crsDiff + // << "Course = " << course + // << "crse = " << crse << endl; + } + else { + //i = ids.end()-1; + //cerr << " accepting " << endl; + + //ids.pop_back(); + wpt = new waypoint; + wpt->name = "Airway"; // fixme: should be the name of the waypoint + wpt->latitude = fn->getLatitude(); + wpt->longitude = fn->getLongitude(); + //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too + wpt->altitude = alt; + wpt->speed = 450; //speed; + wpt->crossat = -10000; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + } + + if (!(routefile.exists())) + { + route.first(); + fstream outf( routefile.c_str(), fstream::out ); + while (route.next(&node)) + outf << node << endl; + } + } + } + arr->getDynamics()->getActiveRunway("com", 2, activeRunway); + if (!(globals->get_runways()->search(arr->getId(), + activeRunway, + &rwy))) + { + cout << "Failed to find runway for " << arr->getId() << endl; + // Hmm, how do we handle a potential error like this? + exit(1); + } + //string test; + //arr->getActiveRunway(string("com"), 1, test); + //exit(1); + + //cerr << "Altitude = " << alt << endl; + //cerr << "Done" << endl; + //if (arr->getId() == "EHAM") + // { + // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl; + // } + heading = rwy._heading; + azimuth = heading + 180.0; + while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + + + // Note: This places us at the location of the active + // runway during initial cruise. This needs to be + // fixed later. + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, + 110000, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "BOD"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = alt; + wpt->speed = speed; + wpt->crossat = alt; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); +} +*/ + + +/******************************************************************* + * CreateCruise + * initialize the Aircraft at the parking location + * + * Note that this is the original version that does not + * do any dynamic route computation. + ******************************************************************/ +void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, + FGAirport *arr, double latitude, + double longitude, double speed, + double alt) +{ + double wind_speed; + double wind_heading; + double heading; + double lat, lon, az; + double lat2, lon2, az2; + double azimuth; + waypoint *wpt; + + wpt = new waypoint; + wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); + wpt->latitude = latitude; + wpt->longitude = longitude; + wpt->altitude = alt; + wpt->speed = speed; + wpt->crossat = -10000; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); + + + // should be changed dynamically to allow "gen" and "mil" + arr->getDynamics()->getActiveRunway("com", 2, activeRunway); + if (!(globals->get_runways()->search(arr->getId(), + activeRunway, + &rwy))) + { + SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << + activeRunway << + " at airport " << arr->getId()); + exit(1); + } + heading = rwy._heading; + azimuth = heading + 180.0; + while ( azimuth >= 360.0 ) { azimuth -= 360.0; } + + + geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, + 110000, + &lat2, &lon2, &az2 ); + wpt = new waypoint; + wpt->name = "BOD"; + wpt->latitude = lat2; + wpt->longitude = lon2; + wpt->altitude = alt; + wpt->speed = speed; + wpt->crossat = alt; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + waypoints.push_back(wpt); +} diff --git a/src/AIModel/Makefile.am b/src/AIModel/Makefile.am index e0bb7c1a1..e5b376584 100644 --- a/src/AIModel/Makefile.am +++ b/src/AIModel/Makefile.am @@ -10,7 +10,8 @@ libAIModel_a_SOURCES = \ AIBallistic.hxx AIBallistic.cxx \ AIStorm.hxx AIStorm.cxx \ AIThermal.hxx AIThermal.cxx \ - AIFlightPlan.hxx AIFlightPlan.cxx AIFlightPlanCreate.cxx \ + AIFlightPlan.hxx AIFlightPlan.cxx \ + AIFlightPlanCreate.cxx AIFlightPlanCreateCruise.cxx \ AICarrier.hxx AICarrier.cxx \ AIStatic.hxx AIStatic.cxx