#include <math.h>
#include <algorithm>
+#include <fstream>
#include <osg/Geode>
#include <osg/Shape>
#include <simgear/debug/logstream.hxx>
-#include <simgear/route/waypoint.hxx>
#include <simgear/scene/material/EffectGeode.hxx>
#include <simgear/scene/material/matlib.hxx>
#include <simgear/scene/material/mat.hxx>
+#include <simgear/scene/util/OsgMath.hxx>
#include <Airports/simple.hxx>
#include <Airports/dynamics.hxx>
#include <AIModel/AIAircraft.hxx>
+#include <AIModel/performancedata.hxx>
#include <AIModel/AIFlightPlan.hxx>
#include <ATC/atc_mgr.hxx>
#include "groundnetwork.hxx"
+using std::string;
+
/***************************************************************************
* FGTaxiSegment
**************************************************************************/
// headingDiff = fabs(headingDiff - 360);
//}
+void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
+{
+ BlockListIterator i = blockTimes.begin();
+ while (i != blockTimes.end()) {
+ if (i->getId() == id)
+ break;
+ i++;
+ }
+ if (i == blockTimes.end()) {
+ blockTimes.push_back(Block(id, blockTime, now));
+ sort(blockTimes.begin(), blockTimes.end());
+ } else {
+ i->updateTimeStamps(blockTime, now);
+ }
+}
+
+// The segment has a block if any of the block times listed in the block list is
+// smaller than the current time.
+bool FGTaxiSegment::hasBlock(time_t now)
+{
+ for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
+ if (i->getBlockTime() < now)
+ return true;
+ }
+ return false;
+}
+
+void FGTaxiSegment::unblock(time_t now)
+{
+ if (blockTimes.size()) {
+ BlockListIterator i = blockTimes.begin();
+ if (i->getTimeStamp() < (now - 30)) {
+ blockTimes.erase(i);
+ }
+ }
+}
+
+
/***************************************************************************
* FGTaxiRoute
return (a.getIntentions().size() < b.getIntentions().size());
}
-FGGroundNetwork::FGGroundNetwork()
+FGGroundNetwork::FGGroundNetwork() :
+ parent(NULL)
{
hasNetwork = false;
foundRoute = false;
FGGroundNetwork::~FGGroundNetwork()
{
+// JMT 2012-09-8 - disabling the groundnet-caching as part of enabling the
+// navcache. The problem isn't the NavCache - it's that for the past few years,
+// we have not being running destructors on FGPositioned classes, and hence,
+// not running this code.
+// When I fix FGPositioned lifetimes (unloading-at-runtime support), this
+// will need to be re-visited so it can run safely during shutdown.
+#if 0
//cerr << "Running Groundnetwork Destructor " << endl;
bool saveData = false;
ofstream cachefile;
if (fgGetBool("/sim/ai/groundnet-cache")) {
- SGPath cacheData(fgGetString("/sim/fg-home"));
+ SGPath cacheData(globals->get_fg_home());
cacheData.append("ai");
string airport = parent->getId();
if (saveData) {
cachefile.close();
}
+#endif
}
void FGGroundNetwork::saveElevationCache() {
bool saveData = false;
ofstream cachefile;
if (fgGetBool("/sim/ai/groundnet-cache")) {
- SGPath cacheData(fgGetString("/sim/fg-home"));
+ SGPath cacheData(globals->get_fg_home());
cacheData.append("ai");
string airport = parent->getId();
//cerr << "Done initializing ground network" << endl;
//exit(1);
if (fgGetBool("/sim/ai/groundnet-cache")) {
- SGPath cacheData(fgGetString("/sim/fg-home"));
+ SGPath cacheData(globals->get_fg_home());
cacheData.append("ai");
string airport = parent->getId();
return itr->getAddress();
} */
- if ((idx >= 0) && (idx < nodes.size()))
+ if (idx < nodes.size())
return nodes[idx]->getAddress();
else
return 0;
}
FGTaxiNode *firstNode = findNode(start);
+ if (!firstNode)
+ {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Error in ground network. Failed to find first waypoint: " << start
+ << " at " << ((parent) ? parent->getId() : "<unknown>"));
+ return FGTaxiRoute();
+ }
firstNode->setPathScore(0);
FGTaxiNode *lastNode = findNode(end);
+ if (!lastNode)
+ {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Error in ground network. Failed to find last waypoint: " << end
+ << " at " << ((parent) ? parent->getId() : "<unknown>"));
+ return FGTaxiRoute();
+ }
FGTaxiNodeVector unvisited(*currNodesSet); // working copy
seg != best->getEndRoute(); seg++) {
if (fullSearch || (*seg)->isPushBack()) {
FGTaxiNode *tgt = (*seg)->getEnd();
+ if (!tgt)
+ {
+ SG_LOG(SG_GENERAL, SG_ALERT,
+ "Error in ground network. Found empty segment "
+ << " at " << ((parent) ? parent->getId() : "<unknown>"));
+ return FGTaxiRoute();
+ }
double alt =
best->getPathScore() + (*seg)->getLength() +
(*seg)->getPenalty(nParkings);
rec.setPositionAndHeading(lat, lon, heading, speed, alt);
rec.setRadius(radius); // only need to do this when creating the record.
rec.setAircraft(aircraft);
- activeTraffic.push_front(rec);
+ if (leg == 2) {
+ activeTraffic.push_front(rec);
+ } else {
+ activeTraffic.push_back(rec);
+ }
} else {
i->setPositionAndIntentions(currentPosition, intendedRoute);
trans_num->setIntValue(-1);
// PopupCallback(n);
//cerr << "Selected transmission message " << n << endl;
- FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
+ //FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
FGATCDialogNew::instance()->removeEntry(1);
} else {
//cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
- transmit(&(*i), msgId, msgDir, false);
+ transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
return false;
}
}
- transmit(&(*i), msgId, msgDir, true);
+ transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
i->updateState();
lastTransmission = now;
available = false;
TrafficVectorIterator current, closest, closestOnNetwork;
TrafficVectorIterator i = activeTraffic.begin();
bool otherReasonToSlowDown = false;
- bool previousInstruction;
+// bool previousInstruction;
if (activeTraffic.size()) {
//while ((i->getId() != id) && (i != activeTraffic.end()))
while (i != activeTraffic.end()) {
current = i;
//closest = current;
- previousInstruction = current->getSpeedAdjustment();
+// previousInstruction = current->getSpeedAdjustment();
double mindist = HUGE_VAL;
if (activeTraffic.size()) {
- double course, dist, bearing, minbearing, az2;
+ double course, dist, bearing, az2; // minbearing,
SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
//TrafficVector iterator closest;
closest = current;
+ closestOnNetwork = current;
for (TrafficVectorIterator i = activeTraffic.begin();
i != activeTraffic.end(); i++) {
if (i == current) {
mindist = dist;
closest = i;
closestOnNetwork = i;
- minbearing = bearing;
+// minbearing = bearing;
}
}
// << endl;
mindist = dist;
closest = i;
- minbearing = bearing;
+// minbearing = bearing;
otherReasonToSlowDown = true;
}
}
}
}
}
- if ((closest == closestOnNetwork) && (current->getPriority() < closest->getPriority()) && needBraking) {
+ if ((closest->getId() == closestOnNetwork->getId()) && (current->getPriority() < closest->getPriority()) && needBraking) {
swap(current, closest);
}
}
} else {
return;
}
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
SG_LOG(SG_GENERAL, SG_ALERT,
"AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition at " << SG_ORIGIN);
return;
}
if (current->getAircraft()->getTakeOffStatus() == 2) {
+ //cerr << current->getAircraft()->getCallSign() << ". Taxi in position and hold" << endl;
current->setHoldPosition(false);
+ current->clearSpeedAdjustment();
return;
}
bool origStatus = current->hasHoldPosition();
} else {
nx = tx;
}
- if (tx->hasBlock() || nx->hasBlock() ) {
- current->setHoldPosition(true);
- }
- }
-
-
- /* for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
- if (i->getId() != current->getId()) {
- int node = current->crosses(this, *i);
- if (node != -1) {
- FGTaxiNode *taxiNode = findNode(node);
-
- // Determine whether it's save to continue or not.
- // If we have a crossing route, there are two possibilities:
- // 1) This is an interestion
- // 2) This is oncoming two-way traffic, using the same taxiway.
- //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
-
- SGGeod other(SGGeod::
- fromDegM(i->getLongitude(), i->getLatitude(),
- i->getAltitude()));
- bool needsToWait;
- bool opposing;
- if (current->isOpposing(this, *i, node)) {
- needsToWait = true;
- opposing = true;
- //cerr << "Hold check 2 : " << node << " has opposing segment " << endl;
- // issue a "Hold Position" as soon as we're close to the offending node
- // For now, I'm doing this as long as the other aircraft doesn't
- // have a hold instruction as soon as we're within a reasonable
- // distance from the offending node.
- // This may be a bit of a conservative estimate though, as it may
- // be well possible that both aircraft can both continue to taxi
- // without crashing into each other.
- } else {
- opposing = false;
- if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
- {
- needsToWait = false;
- //cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
- // << endl;
- } else {
- needsToWait = true;
- //cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
- }
- }
+ //if (tx->hasBlock(now) || nx->hasBlock(now) ) {
+ // current->setHoldPosition(true);
+ //}
+ SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
+ SGGeod end (SGGeod::fromDeg(nx->getStart()->getLongitude(), nx->getStart()->getLatitude()));
- double dist =
- SGGeodesy::distanceM(curr, taxiNode->getGeod());
- if (!(i->hasHoldPosition())) {
-
- if ((dist < 200) && //2.5*current->getRadius()) &&
- (needsToWait) && (i->onRoute(this, *current)) &&
- //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
- (!(current->getId() == i->getWaitsForId())))
- //(!(i->getSpeedAdjustment()))) // &&
- //(!(current->getSpeedAdjustment())))
-
- {
- if (!(isUserAircraft(i->getAircraft()))) { // test code. Don't wait for the user, let the user wait for you.
- current->setHoldPosition(true);
- current->setWaitsForId(i->getId());
- }
- //cerr << "Hold check 5: " << current->getCallSign() <<" Setting Hold Position: distance to node (" << node << ") "
- // << dist << " meters. Waiting for " << i->getCallSign();
- //if (opposing)
- //cerr <<" [opposing] " << endl;
- //else
- // cerr << "[non-opposing] " << endl;
- //if (i->hasSpeefAdjustment())
- // {
- // cerr << " (which in turn waits for ) " << i->
- } else {
- //cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
- }
+ double distance = SGGeodesy::distanceM(start, end);
+ if (nx->hasBlock(now) && (distance < i->getRadius() * 4)) {
+ current->setHoldPosition(true);
+ } else {
+ intVecIterator ivi = i->getIntentions().begin();
+ while (ivi != i->getIntentions().end()) {
+ if ((*ivi) > 0) {
+ distance += segments[(*ivi)-1]->getLength();
+ if ((segments[(*ivi)-1]->hasBlock(now)) && (distance < i->getRadius() * 4)) {
+ current->setHoldPosition(true);
+ break;
}
}
+ ivi++;
}
- } */
+ }
+ }
bool currStatus = current->hasHoldPosition();
current->setHoldPosition(origStatus);
// Either a Hold Position or a resume taxi transmission has been issued
- time_t now = time(NULL) + fgGetLong("/sim/time/warp");
if ((now - lastTransmission) > 2) {
available = true;
}
if ((origStatus != currStatus) && available) {
//cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
if (currStatus == true) { // No has a hold short instruction
- transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
+ transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
//cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
current->setState(1);
} else {
- transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
+ transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
//cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
current->setState(2);
}
double lon, double elev, double hdg, double slope)
{
SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
- obj_pos = geod.makeZUpFrame();
+ obj_pos = makeZUpFrame(geod);
// hdg is not a compass heading, but a counter-clockwise rotation
// around the Z axis
obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
if (visible) {
group = new osg::Group;
FGScenery * local_scenery = globals->get_scenery();
- double elevation_meters = 0.0;
- double elevation_feet = 0.0;
+ // double elevation_meters = 0.0;
+// double elevation_feet = 0.0;
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
//for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
- double dx = 0;
+ //double dx = 0;
for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
// Handle start point
int pos = i->getCurrentPosition() - 1;
SGGeod center2 = end;
center2.setElevationM(SG_MAX_ELEVATION_M);
if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
- elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
//elevation_meters += 0.5;
}
else {
geode->addDrawable(geometry);
//osg::Node *custom_obj;
SGMaterial *mat;
- if (segments[pos]->hasBlock()) {
+ if (segments[pos]->hasBlock(now)) {
mat = matlib->find("UnidirectionalTaperRed");
} else {
mat = matlib->find("UnidirectionalTaperGreen");
SGGeod center2 = segments[k]->getStart()->getGeod();
center2.setElevationM(SG_MAX_ELEVATION_M);
if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
- elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
+// elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
//elevation_meters += 0.5;
}
else {
SGGeod center2 = segments[k]->getEnd()->getGeod();
center2.setElevationM(SG_MAX_ELEVATION_M);
if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
- elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+// elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
//elevation_meters += 0.5;
}
else {
geode->addDrawable(geometry);
//osg::Node *custom_obj;
SGMaterial *mat;
- if (segments[k]->hasBlock()) {
+ if (segments[k]->hasBlock(now)) {
mat = matlib->find("UnidirectionalTaperRed");
} else {
mat = matlib->find("UnidirectionalTaperGreen");
void FGGroundNetwork::update(double dt)
{
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
- (*tsi)->unblock();
+ (*tsi)->unblock(now);
}
int priority = 1;
//sort(activeTraffic.begin(), activeTraffic.end(), compare_trafficrecords);
i != parent->getDynamics()->getStartupController()->getActiveTraffic().end(); i++) {
i->allowPushBack();
i->setPriority(priority++);
- if (i->isActive(60)) {
+ // in meters per second;
+ double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
+ if (i->isActive(0)) {
// Check for all active aircraft whether it's current pos segment is
// an opposite of one of the departing aircraft's intentions
for (intVecIterator k = i->getIntentions().begin(); k != i->getIntentions().end(); k++) {
if ((*k) == posReverse) {
i->denyPushBack();
- segments[posReverse-1]->block();
+ segments[posReverse-1]->block(i->getId(), now, now);
}
}
}
}
// if the current aircraft is still allowed to pushback, we can start reserving a route for if by blocking all the entry taxiways.
if (i->pushBackAllowed()) {
+ double length = 0;
int pos = i->getCurrentPosition();
if (pos > 0) {
FGTaxiSegment *seg = segments[pos-1];
FGTaxiNode *node = seg->getEnd();
+ length = seg->getLength();
for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
- (*tsi)->block();
+ (*tsi)->block(i->getId(), now, now);
}
}
}
if (pos > 0) {
FGTaxiSegment *seg = segments[pos-1];
FGTaxiNode *node = seg->getEnd();
+ length += seg->getLength();
+ time_t blockTime = now + (length / vTaxi);
for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
- (*tsi)->block();
+ (*tsi)->block(i->getId(), blockTime-30, now);
}
}
}
}
}
for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
+ double length = 0;
+ double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
i->setPriority(priority++);
int pos = i->getCurrentPosition();
if (pos > 0) {
- if (segments[pos-1]->hasBlock()) {
+ length = segments[pos-1]->getLength();
+ if (segments[pos-1]->hasBlock(now)) {
//SG_LOG(SG_GENERAL, SG_ALERT, "Taxiway incursion for AI aircraft" << i->getAircraft()->getCallSign());
}
for (ivi = i->getIntentions().begin(); ivi != i->getIntentions().end(); ivi++) {
int segIndex = (*ivi);
if (segIndex > 0) {
- if (segments[segIndex-1]->hasBlock())
+ if (segments[segIndex-1]->hasBlock(now))
break;
}
}
if (pos > 0) {
FGTaxiSegment *seg = segments[pos-1];
FGTaxiNode *node = seg->getEnd();
+ length += seg->getLength();
for (FGTaxiSegmentVectorIterator tsi = segments.begin(); tsi != segments.end(); tsi++) {
if (((*tsi)->getEnd() == node) && ((*tsi) != seg)) {
- (*tsi)->block();
+ time_t blockTime = now + (length / vTaxi);
+ (*tsi)->block(i->getId(), blockTime - 30, now);
}
}
}
projects / flightgear.git / blobdiff