X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlan.cxx;h=0f99e7a9ef5b6287c966d4ea1f26e19232fcef69;hb=b796df3bd7d8fa6998a30ed033198a41a63a32b7;hp=41c7966be01d95ed37bc52eaac9009a27a442724;hpb=9cc92035b19c3407344398503a1ea570769aed85;p=flightgear.git diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 41c7966be..0f99e7a9e 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -1,4 +1,4 @@ -// FGAIFlightPlan - class for loading and storing AI flight plans +// // FGAIFlightPlan - class for loading and storing AI flight plans // Written by David Culp, started May 2004 // - davidculp2@comcast.net // @@ -46,9 +46,43 @@ using std::cerr; +FGAIWaypoint::FGAIWaypoint() { + latitude = 0; + longitude = 0; + altitude = 0; + speed = 0; + crossat = 0; + finished = 0; + gear_down = 0; + flaps_down = 0; + on_ground = 0; + routeIndex = 0; + time_sec = 0; + trackLength = 0; +} + +bool FGAIWaypoint::contains(string target) { + size_t found = name.find(target); + if (found == string::npos) + return false; + else + return true; +} + FGAIFlightPlan::FGAIFlightPlan() { - sid = 0; + sid = 0; + repeat = false; + distance_to_go = 0; + lead_distance = 0; + start_time = 0; + arrivalTime = 0; + leg = 10; + gateId = 0; + lastNodeVisited = 0; + taxiRoute = 0; + wpt_iterator = waypoints.begin(); + isValid = true; } FGAIFlightPlan::FGAIFlightPlan(const string& filename) @@ -66,7 +100,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; @@ -76,27 +110,28 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) SGPropertyNode * node = root.getNode("flightplan"); for (i = 0; i < node->nChildren(); i++) { //cout << "Reading waypoint " << i << endl; - waypoint* wpt = new waypoint; + FGAIWaypoint* wpt = new FGAIWaypoint; 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->crossat = wpt_node->getDoubleValue("crossat", -10000); - wpt->gear_down = wpt_node->getBoolValue("gear-down", false); - wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false); - wpt->on_ground = wpt_node->getBoolValue("on-ground", false); - wpt->time_sec = wpt_node->getDoubleValue("time-sec", 0); - wpt->time = wpt_node->getStringValue("time", ""); - - if (wpt->name == "END") wpt->finished = true; - else wpt->finished = false; - - waypoints.push_back( wpt ); + wpt->setName (wpt_node->getStringValue("name", "END" )); + wpt->setLatitude (wpt_node->getDoubleValue("lat", 0 )); + wpt->setLongitude (wpt_node->getDoubleValue("lon", 0 )); + wpt->setAltitude (wpt_node->getDoubleValue("alt", 0 )); + wpt->setSpeed (wpt_node->getDoubleValue("ktas", 0 )); + wpt->setCrossat (wpt_node->getDoubleValue("crossat", -10000 )); + wpt->setGear_down (wpt_node->getBoolValue("gear-down", false )); + wpt->setFlaps_down (wpt_node->getBoolValue("flaps-down", false )); + wpt->setOn_ground (wpt_node->getBoolValue("on-ground", false )); + wpt->setTime_sec (wpt_node->getDoubleValue("time-sec", 0 )); + wpt->setTime (wpt_node->getStringValue("time", "" )); + + if (wpt->getName() == "END") wpt->setFinished(true); + else wpt->setFinished(false); + + pushBackWaypoint( wpt ); } wpt_iterator = waypoints.begin(); + isValid = true; //cout << waypoints.size() << " waypoints read." << endl; } @@ -136,7 +171,7 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, path.append( p ); SGPropertyNode root; - + isValid = true; // This is a bit of a hack: // Normally the value of course will be used to evaluate whether // or not a waypoint will be used for midair initialization of @@ -157,61 +192,56 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, SGPropertyNode * node = root.getNode("flightplan"); - //waypoints.push_back( init_waypoint ); + //pushBackWaypoint( init_waypoint ); for (int i = 0; i < node->nChildren(); i++) { //cout << "Reading waypoint " << i << endl; - waypoint* wpt = new waypoint; + FGAIWaypoint* wpt = new FGAIWaypoint; 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); + wpt->setName (wpt_node->getStringValue("name", "END" )); + wpt->setLatitude (wpt_node->getDoubleValue("lat", 0 )); + wpt->setLongitude (wpt_node->getDoubleValue("lon", 0 )); + wpt->setAltitude (wpt_node->getDoubleValue("alt", 0 )); + wpt->setSpeed (wpt_node->getDoubleValue("ktas", 0 )); + wpt->setCrossat (wpt_node->getDoubleValue("crossat", -10000 )); + wpt->setGear_down (wpt_node->getBoolValue("gear-down", false )); + wpt->setFlaps_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_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"); - } - } + if (wpt->getName() == "END") wpt->setFinished(true); + else wpt->setFinished(false); + pushBackWaypoint(wpt); + } // of node loop + wpt_iterator = waypoints.begin(); + } 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; time_t now = time(NULL) + fgGetLong("/sim/time/warp"); time_t timeDiff = now-start; leg = 1; - /* - if ((timeDiff > 300) && (timeDiff < 1200)) + + if ((timeDiff > 60) && (timeDiff < 1500)) leg = 2; - else if ((timeDiff >= 1200) && (timeDiff < 1500)) - leg = 3; + //else if ((timeDiff >= 1200) && (timeDiff < 1500)) { + //leg = 3; + //ac->setTakeOffStatus(2); + //} else if ((timeDiff >= 1500) && (timeDiff < 2000)) leg = 4; else if (timeDiff >= 2000) leg = 5; - */ + /* if (timeDiff >= 2000) leg = 5; - + */ 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(ac, dep,arr, leg, alt, speed, lat, lon, - firstLeg, radius, fltType, acType, airline); + bool dist = 0; + isValid = create(ac, dep,arr, leg, alt, speed, lat, lon, + firstLeg, radius, fltType, acType, airline, dist); wpt_iterator = waypoints.begin(); //cerr << "after create: " << (*wpt_iterator)->name << endl; //leg++; @@ -275,7 +305,7 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, { if ((dist > 100.0) && (useInitialWayPoint)) { - //waypoints.push_back(init_waypoint);; + //pushBackWaypoint(init_waypoint);; waypoints.insert(i, init_waypoint); //cerr << "Using waypoint : " << init_waypoint->name << endl; } @@ -283,7 +313,7 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, // { // (*i)->speed = dist; // A hack // } - //waypoints.push_back( wpt ); + //pushBackWaypoint( wpt ); //cerr << "Using waypoint : " << (*i)->name // << ": course diff : " << crsDiff // << "Course = " << course @@ -317,8 +347,7 @@ FGAIFlightPlan::~FGAIFlightPlan() } -FGAIFlightPlan::waypoint* const -FGAIFlightPlan::getPreviousWaypoint( void ) const +FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const { if (wpt_iterator == waypoints.begin()) { return 0; @@ -328,14 +357,12 @@ FGAIFlightPlan::getPreviousWaypoint( void ) const } } -FGAIFlightPlan::waypoint* const -FGAIFlightPlan::getCurrentWaypoint( void ) const +FGAIWaypoint* const FGAIFlightPlan::getCurrentWaypoint( void ) const { return *wpt_iterator; } -FGAIFlightPlan::waypoint* const -FGAIFlightPlan::getNextWaypoint( void ) const +FGAIWaypoint* const FGAIFlightPlan::getNextWaypoint( void ) const { wpt_vector_iterator i = waypoints.end(); i--; // end() points to one element after the last one. @@ -366,15 +393,44 @@ 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--; +} + +void FGAIFlightPlan::eraseLastWaypoint() +{ + delete (waypoints.back()); + waypoints.pop_back();; + wpt_iterator = waypoints.begin(); + wpt_iterator++; +} + + + + // gives distance in feet from a position to a waypoint -double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{ +double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{ return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), - SGGeod::fromDeg(wp->longitude, wp->latitude)); + SGGeod::fromDeg(wp->getLongitude(), wp->getLatitude())); } // sets distance in feet from a lead point to the current waypoint void FGAIFlightPlan::setLeadDistance(double speed, double bearing, - waypoint* current, waypoint* next){ + FGAIWaypoint* current, FGAIWaypoint* next){ double turn_radius; // Handle Ground steering // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn @@ -385,7 +441,7 @@ void FGAIFlightPlan::setLeadDistance(double speed, double bearing, 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 @@ -399,9 +455,17 @@ 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){ @@ -409,14 +473,14 @@ void FGAIFlightPlan::setLeadDistance(double distance_ft){ } -double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second) const{ - return getBearing(first->latitude, first->longitude, second); +double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const{ + return getBearing(first->getLatitude(), first->getLongitude(), second); } -double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp) const{ +double FGAIFlightPlan::getBearing(double lat, double lon, FGAIWaypoint* wp) const{ return SGGeodesy::courseDeg(SGGeod::fromDeg(lon, lat), - SGGeod::fromDeg(wp->longitude, wp->latitude)); + SGGeod::fromDeg(wp->getLongitude(), wp->getLatitude())); } void FGAIFlightPlan::deleteWaypoints() @@ -434,25 +498,35 @@ void FGAIFlightPlan::resetWaypoints() return; else { - waypoint *wpt = new waypoint; + FGAIWaypoint *wpt = new FGAIWaypoint; wpt_vector_iterator i = waypoints.end(); i--; - wpt->name = (*i)->name; - wpt->latitude = (*i)->latitude; - wpt->longitude = (*i)->longitude; - wpt->altitude = (*i)->altitude; - wpt->speed = (*i)->speed; - wpt->crossat = (*i)->crossat; - wpt->gear_down = (*i)->gear_down; - wpt->flaps_down= (*i)->flaps_down; - wpt->finished = false; - wpt->on_ground = (*i)->on_ground; + wpt->setName ( (*i)->getName() ); + wpt->setLatitude ( (*i)->getLatitude() ); + wpt->setLongitude ( (*i)->getLongitude() ); + wpt->setAltitude ( (*i)->getAltitude() ); + wpt->setSpeed ( (*i)->getSpeed() ); + wpt->setCrossat ( (*i)->getCrossat() ); + wpt->setGear_down ( (*i)->getGear_down() ); + wpt->setFlaps_down ( (*i)->getFlaps_down() ); + wpt->setFinished ( false ); + wpt->setOn_ground ( (*i)->getOn_ground() ); //cerr << "Recycling waypoint " << wpt->name << endl; deleteWaypoints(); - waypoints.push_back(wpt); + pushBackWaypoint(wpt); } } +void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt) +{ + // std::vector::push_back invalidates waypoints + // so we should restore wpt_iterator after push_back + // (or it could be an index in the vector) + size_t pos = wpt_iterator - waypoints.begin(); + waypoints.push_back(wpt); + wpt_iterator = waypoints.begin() + pos; +} + // Start flightplan over from the beginning void FGAIFlightPlan::restart() { @@ -468,9 +542,34 @@ void FGAIFlightPlan::deleteTaxiRoute() int FGAIFlightPlan::getRouteIndex(int i) { - if ((i > 0) && (i < waypoints.size())) { - return waypoints[i]->routeIndex; + if ((i > 0) && (i < (int)waypoints.size())) { + return waypoints[i]->getRouteIndex(); } else return 0; } + + +double FGAIFlightPlan::checkTrackLength(string wptName) { + // skip the first two waypoints: first one is behind, second one is partially done; + double trackDistance = 0; + wpt_vector_iterator wptvec = waypoints.begin(); + wptvec++; + wptvec++; + while ((wptvec != waypoints.end()) && (!((*wptvec)->contains(wptName)))) { + trackDistance += (*wptvec)->getTrackLength(); + wptvec++; + } + if (wptvec == waypoints.end()) { + trackDistance = 0; // name not found + } + return trackDistance; +} + +void FGAIFlightPlan::shortenToFirst(unsigned int number, string name) +{ + while (waypoints.size() > number + 3) { + eraseLastWaypoint(); + } + (waypoints.back())->setName((waypoints.back())->getName() + name); +}