X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlan.cxx;h=f8f7c57636f3ec39f3de1265df69c719021480ef;hb=bcea720db3e29a62626b361f2149a7147e149f3b;hp=2fa5c36092badad57ca64d0d90ecc620fd41ff64;hpb=5228053f7f39eba5ecd0de57ddeafd96097205f8;p=flightgear.git diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index 2fa5c3609..f8f7c5763 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -24,7 +24,6 @@ #include #include -#include #include #include #include @@ -46,56 +45,92 @@ 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; - wpt_iterator = waypoints.begin(); - isValid = true; + 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) { - int i; - sid = 0; - start_time = 0; - leg = 10; - gateId = 0; - taxiRoute = 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; + SGPath path( globals->get_fg_root() ); path.append( ("/AI/FlightPlans/" + filename).c_str() ); SGPropertyNode root; - repeat = false; try { readProperties(path.str(), &root); } catch (const sg_exception &) { - SG_LOG(SG_GENERAL, SG_ALERT, + SG_LOG(SG_AI, SG_ALERT, "Error reading AI flight plan: " << path.str()); // cout << path.str() << endl; return; } SGPropertyNode * node = root.getNode("flightplan"); - for (i = 0; i < node->nChildren(); i++) { + 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->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(); @@ -112,45 +147,51 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) // traffic manager. FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, const std::string& p, - double course, - time_t start, - FGAirport *dep, - FGAirport *arr, - bool firstLeg, - double radius, + double course, + time_t start, + FGAirport *dep, + FGAirport *arr, + bool firstLeg, + double radius, double alt, double lat, double lon, double speed, - const string& fltType, - const string& acType, - const string& airline) + const string& fltType, + const string& acType, + const string& airline) { - sid = 0; - repeat = false; - leg = 10; - gateId=0; - taxiRoute = 0; - start_time = start; - bool useInitialWayPoint = true; - bool useCurrentWayPoint = false; + sid = 0; + repeat = false; + distance_to_go = 0; + lead_distance = 0; + start_time = start; + arrivalTime = 0; + leg = 10; + gateId = 0; + lastNodeVisited = 0; + taxiRoute = 0; + SGPath path( globals->get_fg_root() ); path.append( "/AI/FlightPlans" ); 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 // an AI aircraft. However, if a course value of 999 will be passed // when an update request is received, which will by definition always be // on the ground and should include all waypoints. - if (course == 999) - { - useInitialWayPoint = false; - useCurrentWayPoint = true; - } +// bool useInitialWayPoint = true; +// bool useCurrentWayPoint = false; +// if (course == 999) +// { +// useInitialWayPoint = false; +// useCurrentWayPoint = true; +// } if (path.exists()) { @@ -160,28 +201,27 @@ 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); + 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: " << + SG_LOG(SG_AI, SG_WARN, "Error reading AI flight plan: " << e.getMessage() << " from " << e.getOrigin()); } } else { @@ -192,10 +232,12 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, time_t timeDiff = now-start; leg = 1; - if ((timeDiff > 60) && (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) @@ -204,104 +246,14 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, 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()); + SG_LOG(SG_AI, SG_INFO, "Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << ac->getTrafficRef()->getCallSign()); wpt_iterator = waypoints.begin(); 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++; - // Now that we have dynamically created a flight plan, - // we need to add some code that pops any waypoints already past. - //return; } - /* - waypoint* init_waypoint = new waypoint; - init_waypoint->name = string("initial position"); - init_waypoint->latitude = entity->latitude; - init_waypoint->longitude = entity->longitude; - init_waypoint->altitude = entity->altitude; - init_waypoint->speed = entity->speed; - init_waypoint->crossat = - 10000; - init_waypoint->gear_down = false; - init_waypoint->flaps_down = false; - init_waypoint->finished = false; - - 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; - curr.CourseAndDistance(first, &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 ((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; - } - //if (useInitialWayPoint) - // { - // (*i)->speed = dist; // A hack - // } - //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; + } @@ -314,8 +266,7 @@ FGAIFlightPlan::~FGAIFlightPlan() } -FGAIFlightPlan::waypoint* const -FGAIFlightPlan::getPreviousWaypoint( void ) const +FGAIWaypoint* const FGAIFlightPlan::getPreviousWaypoint( void ) const { if (wpt_iterator == waypoints.begin()) { return 0; @@ -325,14 +276,14 @@ FGAIFlightPlan::getPreviousWaypoint( void ) const } } -FGAIFlightPlan::waypoint* const -FGAIFlightPlan::getCurrentWaypoint( void ) const +FGAIWaypoint* const FGAIFlightPlan::getCurrentWaypoint( void ) const { + if (wpt_iterator == waypoints.end()) + return 0; 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. @@ -379,19 +330,28 @@ void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints ) } 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 @@ -434,14 +394,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() @@ -459,25 +419,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() { @@ -494,7 +464,7 @@ void FGAIFlightPlan::deleteTaxiRoute() int FGAIFlightPlan::getRouteIndex(int i) { if ((i > 0) && (i < (int)waypoints.size())) { - return waypoints[i]->routeIndex; + return waypoints[i]->getRouteIndex(); } else return 0; @@ -507,12 +477,20 @@ double FGAIFlightPlan::checkTrackLength(string wptName) { wpt_vector_iterator wptvec = waypoints.begin(); wptvec++; wptvec++; - while ((wptvec != waypoints.end()) && ((*wptvec)->name != wptName)) { - trackDistance += (*wptvec)->trackLength; + while ((wptvec != waypoints.end()) && (!((*wptvec)->contains(wptName)))) { + trackDistance += (*wptvec)->getTrackLength(); wptvec++; } if (wptvec == waypoints.end()) { trackDistance = 0; // name not found } return trackDistance; -} \ No newline at end of file +} + +void FGAIFlightPlan::shortenToFirst(unsigned int number, string name) +{ + while (waypoints.size() > number + 3) { + eraseLastWaypoint(); + } + (waypoints.back())->setName((waypoints.back())->getName() + name); +}