fly/flightgear
1
0
Fork 0
Code Activity

Maintenance: AIFlightPlan

Browse source 
shortenToFirst() 'name' parameter to const&.
use const functions when appropriate.
This commit is contained in:
scttgs0 2023-05-21 20:31:52 -05:00
parent 6284ab6268
commit 4413bcc7ed
2 changed files with 243 additions and 245 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

474
src/AIModel/AIFlightPlan.cxx
View file

@ -6,11 +6,11 @@
*/ */
#ifdef HAVE_CONFIG_H #ifdef HAVE_CONFIG_H
# include <config.h> #include <config.h>
#endif #endif
#include <iterator>
#include <algorithm> #include <algorithm>
#include <iterator>
#include <simgear/constants.h> #include <simgear/constants.h>
#include <simgear/debug/logstream.hxx> #include <simgear/debug/logstream.hxx>
@ -22,37 +22,39 @@
#include <simgear/structure/exception.hxx> #include <simgear/structure/exception.hxx>
#include <simgear/timing/sg_time.hxx> #include <simgear/timing/sg_time.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
#include <Main/fg_init.hxx>
#include <Airports/airport.hxx> #include <Airports/airport.hxx>
#include <Airports/dynamics.hxx> #include <Airports/dynamics.hxx>
#include <Airports/runways.hxx>
#include <Airports/groundnetwork.hxx> #include <Airports/groundnetwork.hxx>
#include <Airports/runways.hxx>
#include <Main/fg_init.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <Environment/environment_mgr.hxx>
#include <Environment/environment.hxx> #include <Environment/environment.hxx>
#include <Environment/environment_mgr.hxx>
#include <Traffic/Schedule.hxx> #include <Traffic/Schedule.hxx>
#include "AIFlightPlan.hxx"
#include "AIAircraft.hxx" #include "AIAircraft.hxx"
#include "AIFlightPlan.hxx"
using std::string; using std::string;
FGAIWaypoint::FGAIWaypoint() { FGAIWaypoint::FGAIWaypoint()
speed = 0; {
crossat = 0; speed = 0;
finished = 0; crossat = 0;
gear_down = 0; finished = 0;
flaps = 0; gear_down = 0;
on_ground = 0; flaps = 0;
routeIndex = 0; on_ground = 0;
time_sec = 0; routeIndex = 0;
trackLength = 0; time_sec = 0;
trackLength = 0;
} }
bool FGAIWaypoint::contains(const string& target) { bool FGAIWaypoint::contains(const string& target)
{
size_t found = name.find(target); size_t found = name.find(target);
if (found == string::npos) if (found == string::npos)
return false; return false;
@ -60,62 +62,60 @@ bool FGAIWaypoint::contains(const string& target) {
return true; return true;
} }
double FGAIWaypoint::getLatitude() double FGAIWaypoint::getLatitude() const
{ {
return pos.getLatitudeDeg(); return pos.getLatitudeDeg();
} }
double FGAIWaypoint::getLongitude() double FGAIWaypoint::getLongitude() const
{ {
return pos.getLongitudeDeg(); return pos.getLongitudeDeg();
} }
double FGAIWaypoint::getAltitude() double FGAIWaypoint::getAltitude() const
{ {
return pos.getElevationFt(); return pos.getElevationFt();
} }
void FGAIWaypoint::setLatitude(double lat) void FGAIWaypoint::setLatitude(double lat)
{ {
pos.setLatitudeDeg(lat); pos.setLatitudeDeg(lat);
} }
void FGAIWaypoint::setLongitude(double lon) void FGAIWaypoint::setLongitude(double lon)
{ {
pos.setLongitudeDeg(lon); pos.setLongitudeDeg(lon);
} }
void FGAIWaypoint::setAltitude(double alt) void FGAIWaypoint::setAltitude(double alt)
{ {
pos.setElevationFt(alt); pos.setElevationFt(alt);
} }
FGAIFlightPlan::FGAIFlightPlan() : FGAIFlightPlan::FGAIFlightPlan() : sid(NULL),
sid(NULL), repeat(false),
repeat(false), distance_to_go(0),
distance_to_go(0), lead_distance_ft(0),
lead_distance_ft(0), leadInAngle(0),
leadInAngle(0), start_time(0),
start_time(0), arrivalTime(0),
arrivalTime(0), leg(0),
leg(0), lastNodeVisited(0),
lastNodeVisited(0), isValid(true)
isValid(true)
{ {
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
} }
FGAIFlightPlan::FGAIFlightPlan(const string& filename) : FGAIFlightPlan::FGAIFlightPlan(const string& filename) : sid(NULL),
sid(NULL), repeat(false),
repeat(false), distance_to_go(0),
distance_to_go(0), lead_distance_ft(0),
lead_distance_ft(0), leadInAngle(0),
leadInAngle(0), start_time(0),
start_time(0), arrivalTime(0),
arrivalTime(0), leg(10),
leg(10), lastNodeVisited(0),
lastNodeVisited(0), isValid(parseProperties(filename))
isValid(parseProperties(filename))
{ {
} }
@ -128,13 +128,13 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename) :
* as setting speeds and altitude computed by the * as setting speeds and altitude computed by the
* traffic manager. * traffic manager.
*/ */
FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac, FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft* ac,
const std::string& p, const std::string& p,
double course, double course,
time_t start, time_t start,
time_t remainingTime, time_t remainingTime,
FGAirport *dep, FGAirport* dep,
FGAirport *arr, FGAirport* arr,
bool firstLeg, bool firstLeg,
double radius, double radius,
double alt, double alt,
@ -143,40 +143,39 @@ FGAIFlightPlan::FGAIFlightPlan(FGAIAircraft *ac,
double speed, double speed,
const string& fltType, const string& fltType,
const string& acType, const string& acType,
const string& airline) : const string& airline) : sid(NULL),
sid(NULL), repeat(false),
repeat(false), distance_to_go(0),
distance_to_go(0), lead_distance_ft(0),
lead_distance_ft(0), leadInAngle(0),
leadInAngle(0), start_time(start),
start_time(start), arrivalTime(0),
arrivalTime(0), leg(10),
leg(10), lastNodeVisited(0),
lastNodeVisited(0), isValid(false),
isValid(false), departure(dep),
departure(dep), arrival(arr)
arrival(arr)
{ {
if (parseProperties(p)) { if (parseProperties(p)) {
isValid = true; isValid = true;
} else { } else {
createWaypoints(ac, course, start, remainingTime, dep, arr, firstLeg, radius, createWaypoints(ac, course, start, remainingTime, dep, arr, firstLeg, radius,
alt, lat, lon, speed, fltType, acType, airline); alt, lat, lon, speed, fltType, acType, airline);
} }
} }
FGAIFlightPlan::~FGAIFlightPlan() FGAIFlightPlan::~FGAIFlightPlan()
{ {
deleteWaypoints(); deleteWaypoints();
//delete taxiRoute; //delete taxiRoute;
} }
void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac, void FGAIFlightPlan::createWaypoints(FGAIAircraft* ac,
double course, double course,
time_t start, time_t start,
time_t remainingTime, time_t remainingTime,
FGAirport *dep, FGAirport* dep,
FGAirport *arr, FGAirport* arr,
bool firstLeg, bool firstLeg,
double radius, double radius,
double alt, double alt,
@ -187,30 +186,29 @@ void FGAIFlightPlan::createWaypoints(FGAIAircraft *ac,
const string& acType, const string& acType,
const string& airline) const string& airline)
{ {
time_t now = globals->get_time_params()->get_cur_time(); time_t now = globals->get_time_params()->get_cur_time();
time_t timeDiff = now-start; time_t timeDiff = now - start;
leg = AILeg::STARTUP_PUSHBACK; leg = AILeg::STARTUP_PUSHBACK;
if ((timeDiff > 60) && (timeDiff < 1500)) if ((timeDiff > 60) && (timeDiff < 1500))
leg = AILeg::TAXI; leg = AILeg::TAXI;
else if ((timeDiff >= 1500) && (timeDiff < 2000)) else if ((timeDiff >= 1500) && (timeDiff < 2000))
leg = AILeg::TAKEOFF; leg = AILeg::TAKEOFF;
else if (timeDiff >= 2000) { else if (timeDiff >= 2000) {
if (remainingTime > 2000) { if (remainingTime > 2000) {
leg = AILeg::CRUISE; leg = AILeg::CRUISE;
} else { } else {
leg = AILeg::APPROACH; leg = AILeg::APPROACH;
}
} }
}
SG_LOG(SG_AI, SG_DEBUG, ac->getTrafficRef()->getCallSign() << "|Route from " << dep->getId() << " to " << arr->getId() << SG_LOG(SG_AI, SG_DEBUG, ac->getTrafficRef()->getCallSign() << "|Route from " << dep->getId() << " to " << arr->getId() << ". Set leg to : " << leg << " " << remainingTime);
". Set leg to : " << leg << " " << remainingTime);
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
bool dist = 0; bool dist = 0;
isValid = create(ac, dep, arr, leg, alt, speed, lat, lon, isValid = create(ac, dep, arr, leg, alt, speed, lat, lon,
firstLeg, radius, fltType, acType, airline, dist); firstLeg, radius, fltType, acType, airline, dist);
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
} }
bool FGAIFlightPlan::parseProperties(const std::string& filename) bool FGAIFlightPlan::parseProperties(const std::string& filename)
@ -307,7 +305,7 @@ bool FGAIFlightPlan::readFlightplan(std::istream& stream, const sg_location& loc
return true; return true;
} }
FGAIWaypoint* FGAIFlightPlan::getLastWaypoint() FGAIWaypoint* FGAIFlightPlan::getLastWaypoint() const
{ {
if (waypoints.empty()) if (waypoints.empty())
return nullptr; return nullptr;
@ -315,7 +313,7 @@ FGAIWaypoint* FGAIFlightPlan::getLastWaypoint()
return waypoints.back(); return waypoints.back();
}; };
FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint( void ) const FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint() const
{ {
if (empty()) if (empty())
return nullptr; return nullptr;
@ -328,14 +326,14 @@ FGAIWaypoint* FGAIFlightPlan::getPreviousWaypoint( void ) const
} }
} }
FGAIWaypoint* FGAIFlightPlan::getCurrentWaypoint( void ) const FGAIWaypoint* FGAIFlightPlan::getCurrentWaypoint() const
{ {
if (wpt_iterator == waypoints.end()) if (wpt_iterator == waypoints.end())
return nullptr; return nullptr;
return *wpt_iterator; return *wpt_iterator;
} }
FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const FGAIWaypoint* FGAIFlightPlan::getNextWaypoint() const
{ {
if (wpt_iterator == waypoints.end()) if (wpt_iterator == waypoints.end())
return nullptr; return nullptr;
@ -348,43 +346,39 @@ FGAIWaypoint* FGAIFlightPlan::getNextWaypoint( void ) const
} }
} }
int FGAIFlightPlan::getNextTurnAngle( void ) const int FGAIFlightPlan::getNextTurnAngle() const
{ {
return nextTurnAngle; return nextTurnAngle;
} }
void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints)
{ {
if (empty()) if (empty())
return; return;
if (eraseWaypoints) if (eraseWaypoints) {
{
if (wpt_iterator == waypoints.begin()) if (wpt_iterator == waypoints.begin())
++wpt_iterator; ++wpt_iterator;
else else if (!waypoints.empty()) {
if (!waypoints.empty())
{
delete *(waypoints.begin()); delete *(waypoints.begin());
waypoints.erase(waypoints.begin()); waypoints.erase(waypoints.begin());
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
++wpt_iterator; ++wpt_iterator;
} }
} } else {
else { ++wpt_iterator;
++wpt_iterator;
} }
// Calculate the angle of the next turn. // Calculate the angle of the next turn.
if (wpt_iterator == waypoints.end()) if (wpt_iterator == waypoints.end())
return; return;
if (wpt_iterator == waypoints.begin()) if (wpt_iterator == waypoints.begin())
return; return;
if (wpt_iterator+1 == waypoints.end()) if (wpt_iterator + 1 == waypoints.end())
return; return;
if (waypoints.size()<3) if (waypoints.size() < 3)
return; return;
FGAIWaypoint* previousWP = *(wpt_iterator -1); FGAIWaypoint* previousWP = *(wpt_iterator - 1);
FGAIWaypoint* currentWP = *(wpt_iterator); FGAIWaypoint* currentWP = *(wpt_iterator);
FGAIWaypoint* nextWP = *(wpt_iterator + 1); FGAIWaypoint* nextWP = *(wpt_iterator + 1);
int currentBearing = this->getBearing(previousWP, currentWP); int currentBearing = this->getBearing(previousWP, currentWP);
@ -393,8 +387,8 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
nextTurnAngle = SGMiscd::normalizePeriodic(-180, 180, nextBearing - currentBearing); nextTurnAngle = SGMiscd::normalizePeriodic(-180, 180, nextBearing - currentBearing);
if ((previousWP->getSpeed() > 0 && nextWP->getSpeed() < 0) || if ((previousWP->getSpeed() > 0 && nextWP->getSpeed() < 0) ||
(previousWP->getSpeed() < 0 && nextWP->getSpeed() > 0)) { (previousWP->getSpeed() < 0 && nextWP->getSpeed() > 0)) {
nextTurnAngle += 180; nextTurnAngle += 180;
SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end"); SG_LOG(SG_AI, SG_BULK, "Add 180 to turn angle pushback end");
} }
SG_LOG(SG_AI, SG_BULK, "Calculated next turn angle " << nextTurnAngle << " " << previousWP->getName() << " " << currentWP->getName() << " Previous Speed " << previousWP->getSpeed() << " Next Speed " << nextWP->getSpeed()); SG_LOG(SG_AI, SG_BULK, "Calculated next turn angle " << nextTurnAngle << " " << previousWP->getName() << " " << currentWP->getName() << " Previous Speed " << previousWP->getSpeed() << " Next Speed " << nextWP->getSpeed());
} }
@ -413,14 +407,15 @@ void FGAIFlightPlan::eraseLastWaypoint()
return; return;
delete (waypoints.back()); delete (waypoints.back());
waypoints.pop_back();; waypoints.pop_back();
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
++wpt_iterator; ++wpt_iterator;
} }
// gives distance in meters from a position to a waypoint // gives distance in meters from a position to a waypoint
double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const{ double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp) const
return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos()); {
return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), wp->getPos());
} }
/** /**
@ -430,55 +425,56 @@ double FGAIFlightPlan::getDistanceToGo(double lat, double lon, FGAIWaypoint* wp)
void FGAIFlightPlan::setLeadDistance(double speed, void FGAIFlightPlan::setLeadDistance(double speed,
double bearing, double bearing,
FGAIWaypoint* current, FGAIWaypoint* current,
FGAIWaypoint* next){ FGAIWaypoint* next)
double turn_radius_m; {
// Handle Ground steering double turn_radius_m;
// At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn // Handle Ground steering
// So, to get an estimate of the turn radius, calculate the circumference of the circle // At a turn rate of 30 degrees per second, it takes 12 seconds to do a full 360 degree turn
// we travel on. Get the turn radius by dividing by PI (*2). // So, to get an estimate of the turn radius, calculate the circumference of the circle
// FIXME Why when going backwards? No fabs // we travel on. Get the turn radius by dividing by PI (*2).
if (speed < 0.5) { // FIXME Why when going backwards? No fabs
setLeadDistance(0.5); if (speed < 0.5) {
return; setLeadDistance(0.5);
} return;
if (speed > 0 && speed < 0.5) {
setLeadDistance(5 * SG_METER_TO_FEET);
SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft*SG_FEET_TO_METER));
return;
}
double speed_mps = speed * SG_KT_TO_MPS;
if (speed < 25) {
turn_radius_m = ((360/30)*fabs(speed_mps)) / (2*M_PI);
} else {
turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank
}
double inbound = bearing;
double outbound = getBearing(current, next);
leadInAngle = fabs(inbound - outbound);
if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle;
//if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
// leadInAngle = 30.0;
//lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
if ((int)leadInAngle==0) {
double lead_distance_m = fabs(2*speed) * SG_FEET_TO_METER;
setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance leadin 0 " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
} }
} else { if (speed > 0 && speed < 0.5) {
double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS)/2); setLeadDistance(5 * SG_METER_TO_FEET);
setLeadDistance(lead_distance_m * SG_METER_TO_FEET); SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance fixed " << (lead_distance_ft * SG_FEET_TO_METER));
SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft*SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle)/2); return;
if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound << " at " << current->getName());
} }
}
/* double speed_mps = speed * SG_KT_TO_MPS;
if (speed < 25) {
turn_radius_m = ((360 / 30) * fabs(speed_mps)) / (2 * M_PI);
} else {
turn_radius_m = 0.1911 * speed * speed; // an estimate for 25 degrees bank
}
double inbound = bearing;
double outbound = getBearing(current, next);
leadInAngle = fabs(inbound - outbound);
if (leadInAngle > 180.0) leadInAngle = 360.0 - leadInAngle;
//if (leadInAngle < 30.0) // To prevent lead_dist from getting so small it is skipped
// leadInAngle = 30.0;
//lead_distance_ft = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
if ((int)leadInAngle == 0) {
double lead_distance_m = fabs(2 * speed) * SG_FEET_TO_METER;
setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance leadin 0 " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound);
}
} else {
double lead_distance_m = turn_radius_m * tan((leadInAngle * SG_DEGREES_TO_RADIANS) / 2);
setLeadDistance(lead_distance_m * SG_METER_TO_FEET);
SG_LOG(SG_AI, SG_BULK, "Setting Leaddistance " << (lead_distance_ft * SG_FEET_TO_METER) << " Turnradius " << turn_radius_m << " Speed " << speed_mps << " Half turn Angle " << (leadInAngle) / 2);
if (lead_distance_ft > 1000) {
SG_LOG(SG_AI, SG_BULK, "Excessive leaddistance possible direction change " << lead_distance_ft << " leadInAngle " << leadInAngle << " inbound " << inbound << " outbound " << outbound << " at " << current->getName());
}
}
/*
if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) { if ((lead_distance_ft > (3*turn_radius)) && (current->on_ground == false)) {
SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound SG_LOG(SG_AI, SG_ALERT, "Warning: Lead-in distance is large. Inbound = " << inbound
<< ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle << ". Turn radius = " << turn_radius); << ". Outbound = " << outbound << ". Lead in angle = " << leadInAngle << ". Turn radius = " << turn_radius);
@ -491,60 +487,61 @@ void FGAIFlightPlan::setLeadDistance(double speed,
}*/ }*/
} }
void FGAIFlightPlan::setLeadDistance(double distance_ft){ void FGAIFlightPlan::setLeadDistance(double distance_ft)
lead_distance_ft = distance_ft; {
if (lead_distance_ft>10000) { lead_distance_ft = distance_ft;
SG_LOG(SG_AI, SG_BULK, "Excessive Leaddistance " << distance_ft); if (lead_distance_ft > 10000) {
} SG_LOG(SG_AI, SG_BULK, "Excessive Leaddistance " << distance_ft);
}
} }
double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const double FGAIFlightPlan::getBearing(FGAIWaypoint* first, FGAIWaypoint* second) const
{ {
return SGGeodesy::courseDeg(first->getPos(), second->getPos()); return SGGeodesy::courseDeg(first->getPos(), second->getPos());
} }
double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const double FGAIFlightPlan::getBearing(const SGGeod& aPos, FGAIWaypoint* wp) const
{ {
return SGGeodesy::courseDeg(aPos, wp->getPos()); return SGGeodesy::courseDeg(aPos, wp->getPos());
} }
void FGAIFlightPlan::deleteWaypoints() void FGAIFlightPlan::deleteWaypoints()
{ {
for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); ++i) for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); ++i)
delete (*i); delete (*i);
waypoints.clear(); waypoints.clear();
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
} }
// Delete all waypoints except the last, // Delete all waypoints except the last,
// which we will recycle as the first waypoint in the next leg; // which we will recycle as the first waypoint in the next leg;
void FGAIFlightPlan::resetWaypoints() void FGAIFlightPlan::resetWaypoints()
{ {
if (waypoints.begin() == waypoints.end()) if (waypoints.begin() == waypoints.end())
return; return;
FGAIWaypoint* wpt = new FGAIWaypoint; FGAIWaypoint* wpt = new FGAIWaypoint;
wpt_vector_iterator i = waypoints.end(); wpt_vector_iterator i = waypoints.end();
--i; --i;
wpt->setName((*i)->getName()); wpt->setName((*i)->getName());
wpt->setPos((*i)->getPos()); wpt->setPos((*i)->getPos());
wpt->setCrossat((*i)->getCrossat()); wpt->setCrossat((*i)->getCrossat());
wpt->setGear_down((*i)->getGear_down()); wpt->setGear_down((*i)->getGear_down());
wpt->setFlaps((*i)->getFlaps()); wpt->setFlaps((*i)->getFlaps());
wpt->setSpoilers((*i)->getSpoilers()); wpt->setSpoilers((*i)->getSpoilers());
wpt->setSpeedBrakes((*i)->getSpeedBrakes()); wpt->setSpeedBrakes((*i)->getSpeedBrakes());
wpt->setBeaconLight((*i)->getBeaconLight()); wpt->setBeaconLight((*i)->getBeaconLight());
wpt->setLandingLight((*i)->getLandingLight()); wpt->setLandingLight((*i)->getLandingLight());
wpt->setNavLight((*i)->getNavLight()); wpt->setNavLight((*i)->getNavLight());
wpt->setStrobeLight((*i)->getStrobeLight()); wpt->setStrobeLight((*i)->getStrobeLight());
wpt->setTaxiLight((*i)->getTaxiLight()); wpt->setTaxiLight((*i)->getTaxiLight());
wpt->setFinished(false); wpt->setFinished(false);
wpt->setOn_ground((*i)->getOn_ground()); wpt->setOn_ground((*i)->getOn_ground());
SG_LOG(SG_AI, SG_DEBUG, "Recycling waypoint " << wpt->getName()); SG_LOG(SG_AI, SG_DEBUG, "Recycling waypoint " << wpt->getName());
deleteWaypoints(); deleteWaypoints();
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
} }
void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt) void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt)
@ -552,56 +549,57 @@ void FGAIFlightPlan::addWaypoint(FGAIWaypoint* wpt)
pushBackWaypoint(wpt); pushBackWaypoint(wpt);
} }
void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint *wpt) void FGAIFlightPlan::pushBackWaypoint(FGAIWaypoint* wpt)
{ {
if (!wpt) { if (!wpt) {
SG_LOG(SG_AI, SG_WARN, "Null WPT added"); SG_LOG(SG_AI, SG_WARN, "Null WPT added");
} }
size_t pos = wpt_iterator - waypoints.begin(); size_t pos = wpt_iterator - waypoints.begin();
if (waypoints.size()>0) { if (waypoints.size() > 0) {
double dist = SGGeodesy::distanceM( waypoints.back()->getPos(), wpt->getPos()); double dist = SGGeodesy::distanceM(waypoints.back()->getPos(), wpt->getPos());
if( dist == 0 ) { if (dist == 0) {
SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added "); SG_LOG(SG_AI, SG_DEBUG, "Double WP : \t" << wpt->getName() << " not added ");
} else { } else {
waypoints.push_back(wpt);
SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
}
} else {
waypoints.push_back(wpt); waypoints.push_back(wpt);
SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed()); SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed());
} }
} else { // std::vector::push_back invalidates waypoints
waypoints.push_back(wpt); // so we should restore wpt_iterator after push_back
SG_LOG(SG_AI, SG_BULK, "Added WP : \t" << std::setprecision(12) << wpt->getName() << "\t" << wpt->getPos() << "\t" << wpt->getSpeed()); // (or it could be an index in the vector)
} wpt_iterator = waypoints.begin() + pos;
// std::vector::push_back invalidates waypoints
// so we should restore wpt_iterator after push_back
// (or it could be an index in the vector)
wpt_iterator = waypoints.begin() + pos;
} }
// Start flightplan over from the beginning // Start flightplan over from the beginning
void FGAIFlightPlan::restart() void FGAIFlightPlan::restart()
{ {
wpt_iterator = waypoints.begin(); wpt_iterator = waypoints.begin();
} }
int FGAIFlightPlan::getRouteIndex(int i) { int FGAIFlightPlan::getRouteIndex(int i) const
if ((i > 0) && (i < (int)waypoints.size())) { {
return waypoints[i]->getRouteIndex(); if ((i > 0) && (i < (int)waypoints.size())) {
} return waypoints[i]->getRouteIndex();
else } else
return 0; return 0;
} }
double FGAIFlightPlan::checkTrackLength(const string& wptName) const { double FGAIFlightPlan::checkTrackLength(const string& wptName) const
{
// skip the first two waypoints: first one is behind, second one is partially done; // skip the first two waypoints: first one is behind, second one is partially done;
double trackDistance = 0; double trackDistance = 0;
wpt_vector_iterator wptvec = waypoints.begin(); wpt_vector_iterator wptvec = waypoints.begin();
++wptvec; ++wptvec;
++wptvec; ++wptvec;
while ((wptvec != waypoints.end())) { while ((wptvec != waypoints.end())) {
if (*wptvec!=nullptr && (!((*wptvec)->contains(wptName)))) { if (*wptvec != nullptr && (!((*wptvec)->contains(wptName)))) {
break; break;
} }
trackDistance += (*wptvec)->getTrackLength(); trackDistance += (*wptvec)->getTrackLength();
++wptvec; ++wptvec;
} }
if (wptvec == waypoints.end()) { if (wptvec == waypoints.end()) {
trackDistance = 0; // name not found trackDistance = 0; // name not found
@ -609,7 +607,7 @@ double FGAIFlightPlan::checkTrackLength(const string& wptName) const {
return trackDistance; return trackDistance;
} }
void FGAIFlightPlan::shortenToFirst(unsigned int number, string name) void FGAIFlightPlan::shortenToFirst(unsigned int number, const std::string& name)
{ {
while (waypoints.size() > number + 3) { while (waypoints.size() > number + 3) {
eraseLastWaypoint(); eraseLastWaypoint();
@ -619,12 +617,12 @@ void FGAIFlightPlan::shortenToFirst(unsigned int number, string name)
void FGAIFlightPlan::setGate(const ParkingAssignment& pka) void FGAIFlightPlan::setGate(const ParkingAssignment& pka)
{ {
gate = pka; gate = pka;
} }
FGParking* FGAIFlightPlan::getParkingGate() FGParking* FGAIFlightPlan::getParkingGate() const
{ {
return gate.parking(); return gate.parking();
} }
FGAirportRef FGAIFlightPlan::departureAirport() const FGAirportRef FGAIFlightPlan::departureAirport() const

14
src/AIModel/AIFlightPlan.hxx
View file

@ -93,9 +93,9 @@ public:
const std::string& getName() { return name; }; const std::string& getName() { return name; };
const SGGeod& getPos() { return pos; }; const SGGeod& getPos() { return pos; };
double getLatitude(); double getLatitude() const;
double getLongitude(); double getLongitude() const;
double getAltitude(); double getAltitude() const;
double getSpeed() { return speed; }; double getSpeed() { return speed; };
double getCrossat() { return crossat; }; double getCrossat() { return crossat; };
@ -198,7 +198,7 @@ public:
void restart(void); void restart(void);
int getNrOfWayPoints() { return waypoints.size(); } int getNrOfWayPoints() { return waypoints.size(); }
int getRouteIndex(int i); // returns the AI related index of this current routes. int getRouteIndex(int i) const; // returns the AI related index of this current routes.
const std::string& getRunway() { return activeRunway; } const std::string& getRunway() { return activeRunway; }
bool isActive(time_t time) { return time >= this->getStartTime(); } bool isActive(time_t time) { return time >= this->getStartTime(); }
@ -220,12 +220,12 @@ public:
void setSID(FGAIFlightPlan* fp) { sid = fp; }; void setSID(FGAIFlightPlan* fp) { sid = fp; };
FGAIFlightPlan* getSID() { return sid; }; FGAIFlightPlan* getSID() { return sid; };
FGAIWaypoint* getWayPoint(int i) { return waypoints[i]; }; FGAIWaypoint* getWayPoint(int i) { return waypoints[i]; };
FGAIWaypoint* getLastWaypoint(); FGAIWaypoint* getLastWaypoint() const;
void shortenToFirst(unsigned int number, std::string name); void shortenToFirst(unsigned int number, const std::string& name);
void setGate(const ParkingAssignment& pka); void setGate(const ParkingAssignment& pka);
FGParking* getParkingGate(); FGParking* getParkingGate() const;
FGAirportRef departureAirport() const; FGAirportRef departureAirport() const;
FGAirportRef arrivalAirport() const; FGAirportRef arrivalAirport() const;