X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlan.cxx;h=60952d799621889b7ced9a87f5578853c5b9ec5b;hb=388a0a5b6af2b91f2ed36488177813d693ed374e;hp=6f3f3dc41d0f0378515c4f3454873bab31e712a0;hpb=14885374de79779e3f642f6ecb39d4723836166a;p=flightgear.git diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 6f3f3dc41..60952d799 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -20,6 +20,8 @@ # include #endif +#include + #include #include #include @@ -34,17 +36,25 @@ #include
#include #include - +#include #include #include #include "AIFlightPlan.hxx" +#include "AIAircraft.hxx" +using std::cerr; + +FGAIFlightPlan::FGAIFlightPlan() +{ + sid = 0; +} FGAIFlightPlan::FGAIFlightPlan(const string& filename) { int i; + sid = 0; start_time = 0; leg = 10; gateId = 0; @@ -56,7 +66,7 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) try { readProperties(path.str(), &root); - } catch (const sg_exception &e) { + } catch (const sg_exception &) { SG_LOG(SG_GENERAL, SG_ALERT, "Error reading AI flight plan: " << path.str()); // cout << path.str() << endl; @@ -97,7 +107,8 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) // Position computed by the traffic manager, as well // as setting speeds and altitude computed by the // traffic manager. -FGAIFlightPlan::FGAIFlightPlan(const std::string& p, +FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, + const std::string& p, double course, time_t start, FGAirport *dep, @@ -112,6 +123,7 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, const string& acType, const string& airline) { + sid = 0; repeat = false; leg = 10; gateId=0; @@ -163,20 +175,12 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, 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_WARN, - "Error reading AI flight plan: "); - cerr << "Errno = " << errno << endl; - if (errno == ENOENT) - { - SG_LOG(SG_GENERAL, SG_WARN, "Reason: No such file or directory"); - } - } + } // of node loop + } catch (const sg_exception &e) { + SG_LOG(SG_GENERAL, SG_WARN, "Error reading AI flight plan: " << + e.getMessage() << " from " << e.getOrigin()); } - else - { + } else { // cout << path.str() << endl; // cout << "Trying to create this plan dynamically" << endl; // cout << "Route from " << dep->id << " to " << arr->id << endl; @@ -196,9 +200,9 @@ FGAIFlightPlan::FGAIFlightPlan(const std::string& p, if (timeDiff >= 2000) leg = 5; - SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg); + SG_LOG(SG_GENERAL, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign()); wpt_iterator = waypoints.begin(); - create(dep,arr, leg, alt, speed, lat, lon, + create(ac, dep,arr, leg, alt, speed, lat, lon, firstLeg, radius, fltType, acType, airline); wpt_iterator = waypoints.begin(); //cerr << "after create: " << (*wpt_iterator)->name << endl; @@ -354,19 +358,30 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) } +void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints ) +{ + if (eraseWaypoints) + { + if (wpt_iterator == waypoints.end()) + wpt_iterator--; + else + { + delete *(waypoints.end()); + waypoints.erase(waypoints.end()); + wpt_iterator = waypoints.end(); + wpt_iterator--; + } + } + else + wpt_iterator--; + +} + + // gives distance in feet from a position to a waypoint double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{ - double course, distance; - // get size of a degree2 at the present latitude - // this won't work over large distances - //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 lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat; - //double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon; - //return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft)); - SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp")); - sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance); - return distance; + return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), + SGGeod::fromDeg(wp->longitude, wp->latitude)); } // sets distance in feet from a lead point to the current waypoint @@ -377,8 +392,12 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn // So, to get an estimate of the turn radius, calculate the cicumference of the circle // we travel on. Get the turn radius by dividing by PI (*2). + if (speed < 0.5) { + lead_distance = 0.5; + return; + } if (speed < 25) { - turn_radius = ((360/30)*15) / (2*M_PI); + turn_radius = ((360/30)*fabs(speed)) / (2*M_PI); } else turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank @@ -392,9 +411,16 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, //lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS); lead_distance = turn_radius * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); - // if ((errno == EDOM) || (errno == ERANGE) || lead_distance < 1.0) - // { - // } + if ((lead_distance > (3*turn_radius)) && (current->on_ground == false)) { + // cerr << "Warning: Lead-in distance is large. Inbound = " << inbound + // << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle << ". Turn radius = " << turn_radius << endl; + lead_distance = 3 * turn_radius; + return; + } + if ((leadInAngle > 90) && (current->on_ground == true)) { + lead_distance = turn_radius * tan((90 * SG_DEGREES_TO_RADIANS)/2); + return; + } } void FGAIFlightPlan::setLeadDistance(double distance_ft){ @@ -408,54 +434,10 @@ double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second) const{ double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp) const{ - 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; -// 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; - 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. - //if ((errno == EDOM) || (errno == ERANGE)) - // { - // cerr << "Lon: " << wp->longitude - // << "Lat = " << wp->latitude - // << "Tgt Lon = " << - // << "TgT Lat = " << speed << endl; - // } - + return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat), + SGGeod::fromDeg(wp->longitude, wp->latitude)); } - - void FGAIFlightPlan::deleteWaypoints() { for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++) @@ -505,7 +487,7 @@ void FGAIFlightPlan::deleteTaxiRoute() int FGAIFlightPlan::getRouteIndex(int i) { - if ((i > 0) && (i < waypoints.size())) { + if ((i > 0) && (i < (int)waypoints.size())) { return waypoints[i]->routeIndex; } else