Adding some more intelligence to the AI system step 2: Added a system to
detect interfering taxi routes and added a "hold position" instruction to make one of two conflicting aircraft wait until it's route is cleared again
This commit is contained in:
parent
7c445ccb73
commit
8de6a69e3e
4 changed files with 358 additions and 75 deletions
|
@ -86,6 +86,8 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) :
|
||||||
alt_lock = false;
|
alt_lock = false;
|
||||||
roll = 0;
|
roll = 0;
|
||||||
headingChangeRate = 0.0;
|
headingChangeRate = 0.0;
|
||||||
|
|
||||||
|
holdPos = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -226,7 +228,7 @@ void FGAIAircraft::Run(double dt) {
|
||||||
pos.getLongitudeDeg(),
|
pos.getLongitudeDeg(),
|
||||||
hdg,
|
hdg,
|
||||||
speed,
|
speed,
|
||||||
altitude_ft);
|
altitude_ft, dt);
|
||||||
//if (controller->hasInstruction(getID()))
|
//if (controller->hasInstruction(getID()))
|
||||||
// {
|
// {
|
||||||
processATC(controller->getInstruction(getID()));
|
processATC(controller->getInstruction(getID()));
|
||||||
|
@ -979,20 +981,20 @@ void FGAIAircraft::announcePositionToController()
|
||||||
//snprintf (buffer, 10, "%d", node);
|
//snprintf (buffer, 10, "%d", node);
|
||||||
switch (leg) {
|
switch (leg) {
|
||||||
case 3:
|
case 3:
|
||||||
cerr << trafficRef->getRegistration()
|
//cerr << trafficRef->getRegistration()
|
||||||
<< " taxiing to runway at segment "
|
// << " taxiing to runway at segment "
|
||||||
<< fp->getCurrentWaypoint()->routeIndex
|
// << fp->getCurrentWaypoint()->routeIndex
|
||||||
<< endl;
|
// << endl;
|
||||||
//cerr << "Match check between taxisegment and taxiroute : " << node << " "
|
//cerr << "Match check between taxisegment and taxiroute : " << node << " "
|
||||||
// << fp->getCurrentWaypoint()->name << endl;
|
// << fp->getCurrentWaypoint()->name << endl;
|
||||||
if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists())
|
if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists())
|
||||||
controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork();
|
controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork();
|
||||||
break;
|
break;
|
||||||
case 9:
|
case 9:
|
||||||
cerr << trafficRef->getRegistration()
|
//cerr << trafficRef->getRegistration()
|
||||||
<< " taxiing to parking at segment "
|
// << " taxiing to parking at segment "
|
||||||
<< fp->getCurrentWaypoint()->routeIndex
|
// << fp->getCurrentWaypoint()->routeIndex
|
||||||
<< endl;
|
// << endl;
|
||||||
if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists())
|
if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists())
|
||||||
controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork();
|
controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork();
|
||||||
break;
|
break;
|
||||||
|
@ -1006,8 +1008,8 @@ void FGAIAircraft::announcePositionToController()
|
||||||
}
|
}
|
||||||
if ((controller != prevController) && (prevController != 0)) {
|
if ((controller != prevController) && (prevController != 0)) {
|
||||||
prevController->signOff(getID());
|
prevController->signOff(getID());
|
||||||
cerr << trafficRef->getRegistration()
|
//cerr << trafficRef->getRegistration()
|
||||||
<< " signing off " << endl;
|
// << " signing off " << endl;
|
||||||
}
|
}
|
||||||
prevController = controller;
|
prevController = controller;
|
||||||
if (controller) {
|
if (controller) {
|
||||||
|
@ -1024,12 +1026,28 @@ void FGAIAircraft::processATC(FGATCInstruction instruction)
|
||||||
//cerr << "Processing ATC instruction (not Implimented yet)" << endl;
|
//cerr << "Processing ATC instruction (not Implimented yet)" << endl;
|
||||||
if (instruction.getHoldPattern ()) {
|
if (instruction.getHoldPattern ()) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Hold Position
|
||||||
if (instruction.getHoldPosition ()) {
|
if (instruction.getHoldPosition ()) {
|
||||||
}
|
if (!holdPos) {
|
||||||
if (instruction.getChangeSpeed ()) {
|
if (trafficRef)
|
||||||
AccelTo(instruction.getSpeed());
|
cerr << trafficRef->getCallSign() << "Holding Position " << endl;
|
||||||
}else {
|
holdPos = true;
|
||||||
if (fp) AccelTo(fp->getPreviousWaypoint()->speed);
|
}
|
||||||
|
AccelTo(0.25);
|
||||||
|
} else {
|
||||||
|
if (holdPos) {
|
||||||
|
if (trafficRef)
|
||||||
|
cerr << trafficRef->getCallSign() << " Resuming Taxi " << endl;
|
||||||
|
holdPos = false;
|
||||||
|
}
|
||||||
|
// Change speed Instruction. This can only be excecuted when there is no
|
||||||
|
// Hold position instruction.
|
||||||
|
if (instruction.getChangeSpeed ()) {
|
||||||
|
AccelTo(instruction.getSpeed());
|
||||||
|
}else {
|
||||||
|
if (fp) AccelTo(fp->getPreviousWaypoint()->speed);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (instruction.getChangeHeading ()) {
|
if (instruction.getChangeHeading ()) {
|
||||||
hdg_lock = false;
|
hdg_lock = false;
|
||||||
|
|
|
@ -124,6 +124,8 @@ private:
|
||||||
double prevSpeed;
|
double prevSpeed;
|
||||||
double prev_dist_to_go;
|
double prev_dist_to_go;
|
||||||
|
|
||||||
|
bool holdPos;
|
||||||
|
|
||||||
bool _getGearDown() const;
|
bool _getGearDown() const;
|
||||||
bool reachedWaypoint;
|
bool reachedWaypoint;
|
||||||
string callsign; // The callsign of this tanker.
|
string callsign; // The callsign of this tanker.
|
||||||
|
|
|
@ -61,6 +61,7 @@ FGTaxiNode::FGTaxiNode()
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
FGTaxiSegment::FGTaxiSegment()
|
FGTaxiSegment::FGTaxiSegment()
|
||||||
{
|
{
|
||||||
|
oppositeDirection = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
|
void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
|
||||||
|
@ -262,6 +263,102 @@ void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
|
||||||
altitude = alt;
|
altitude = alt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other)
|
||||||
|
{
|
||||||
|
if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this)))
|
||||||
|
return -1;
|
||||||
|
intVecIterator i, j;
|
||||||
|
int currentTargetNode = 0, otherTargetNode = 0;
|
||||||
|
if (currentPos > 0)
|
||||||
|
currentTargetNode = net->findSegment(currentPos )->getEnd()->getIndex(); // OKAY,...
|
||||||
|
if (other.currentPos > 0)
|
||||||
|
otherTargetNode = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,...
|
||||||
|
if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0)
|
||||||
|
return currentTargetNode;
|
||||||
|
if (intentions.size())
|
||||||
|
{
|
||||||
|
for (i = intentions.begin(); i != intentions.end(); i++)
|
||||||
|
{
|
||||||
|
if (currentTargetNode == net->findSegment(*i)->getEnd()->getIndex())
|
||||||
|
{
|
||||||
|
cerr << "Current crosses at " << currentTargetNode <<endl;
|
||||||
|
return currentTargetNode;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (other.intentions.size())
|
||||||
|
{
|
||||||
|
for (i = other.intentions.begin(); i != other.intentions.end(); i++)
|
||||||
|
{
|
||||||
|
if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex())
|
||||||
|
{
|
||||||
|
cerr << "Other crosses at " << currentTargetNode <<endl;
|
||||||
|
return otherTargetNode;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (intentions.size() && other.intentions.size())
|
||||||
|
{
|
||||||
|
for (i = intentions.begin(); i != intentions.end(); i++)
|
||||||
|
{
|
||||||
|
for (j = other.intentions.begin(); j != other.intentions.end(); j++)
|
||||||
|
{
|
||||||
|
//cerr << "finding segment " << *i << " and " << *j << endl;
|
||||||
|
currentTargetNode = net->findSegment(*i)->getEnd()->getIndex();
|
||||||
|
otherTargetNode = net->findSegment(*j)->getEnd()->getIndex();
|
||||||
|
if (currentTargetNode == otherTargetNode)
|
||||||
|
{
|
||||||
|
//cerr << "Routes will cross at " << currentTargetNode << endl;
|
||||||
|
return currentTargetNode;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node)
|
||||||
|
{
|
||||||
|
// Check if current segment is the reverse segment for the other aircraft
|
||||||
|
FGTaxiSegment *opp;
|
||||||
|
//cerr << "Current segment " << currentPos << endl;
|
||||||
|
if ((currentPos > 0) && (other.currentPos > 0))
|
||||||
|
{
|
||||||
|
opp = net->findSegment(currentPos)->opposite();
|
||||||
|
if (opp) {
|
||||||
|
if (opp->getIndex() == other.currentPos)
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
|
||||||
|
{
|
||||||
|
for (intVecIterator j = intentions.begin(); j != intentions.end(); j++)
|
||||||
|
{
|
||||||
|
// cerr << "Current segment 1 " << (*i) << endl;
|
||||||
|
if (opp = net->findSegment(*i)->opposite())
|
||||||
|
{
|
||||||
|
if (opp->getIndex() ==
|
||||||
|
net->findSegment(*j)->getIndex())
|
||||||
|
{
|
||||||
|
cerr << "Nodes " << net->findSegment(*i)->getIndex()
|
||||||
|
<< " and " << net->findSegment(*j)->getIndex()
|
||||||
|
<< " are opposites " << endl;
|
||||||
|
if (net->findSegment(*i)->getStart()->getIndex() == node) {
|
||||||
|
{
|
||||||
|
cerr << "Found the node" << endl;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/***************************************************************************
|
/***************************************************************************
|
||||||
* FGGroundNetwork()
|
* FGGroundNetwork()
|
||||||
**************************************************************************/
|
**************************************************************************/
|
||||||
|
@ -273,6 +370,7 @@ FGGroundNetwork::FGGroundNetwork()
|
||||||
totalDistance = 0;
|
totalDistance = 0;
|
||||||
maxDistance = 0;
|
maxDistance = 0;
|
||||||
currTraffic = activeTraffic.begin();
|
currTraffic = activeTraffic.begin();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
|
void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
|
||||||
|
@ -306,16 +404,39 @@ void FGGroundNetwork::init()
|
||||||
{
|
{
|
||||||
hasNetwork = true;
|
hasNetwork = true;
|
||||||
int index = 1;
|
int index = 1;
|
||||||
|
sort(nodes.begin(), nodes.end());
|
||||||
|
sort(segments.begin(), segments.end());
|
||||||
FGTaxiSegmentVectorIterator i = segments.begin();
|
FGTaxiSegmentVectorIterator i = segments.begin();
|
||||||
while(i != segments.end()) {
|
while(i != segments.end()) {
|
||||||
//cerr << "initializing node " << i->getIndex() << endl;
|
//cerr << "initializing node " << i->getIndex() << endl;
|
||||||
i->setStart(&nodes);
|
i->setStart(&nodes);
|
||||||
i->setEnd (&nodes);
|
i->setEnd (&nodes);
|
||||||
i->setTrackDistance();
|
i->setTrackDistance();
|
||||||
i->setIndex(index++);
|
i->setIndex(index);
|
||||||
//cerr << "Track distance = " << i->getLength() << endl;
|
//cerr << "Track distance = " << i->getLength() << endl;
|
||||||
//cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
|
//cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
|
||||||
i++;
|
i++;
|
||||||
|
index++;
|
||||||
|
}
|
||||||
|
i = segments.begin();
|
||||||
|
while(i != segments.end()) {
|
||||||
|
FGTaxiSegmentPointerVectorIterator j = i->getEnd()->getBeginRoute();
|
||||||
|
while (j != i->getEnd()->getEndRoute())
|
||||||
|
{
|
||||||
|
if ((*j)->getEnd()->getIndex() == i->getStart()->getIndex())
|
||||||
|
{
|
||||||
|
int start1 = i->getStart()->getIndex();
|
||||||
|
int end1 = i->getEnd() ->getIndex();
|
||||||
|
int start2 = (*j)->getStart()->getIndex();
|
||||||
|
int end2 = (*j)->getEnd()->getIndex();
|
||||||
|
int oppIndex = (*j)->getIndex();
|
||||||
|
cerr << "Oppossite of " << i->getIndex() << " (" << start1 << "," << end1 << ") "
|
||||||
|
<< "happens to be " << oppIndex << " (" << start2 << "," << end2 << ") " << endl;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
j++;
|
||||||
|
}
|
||||||
|
i++;
|
||||||
}
|
}
|
||||||
//exit(1);
|
//exit(1);
|
||||||
}
|
}
|
||||||
|
@ -349,19 +470,23 @@ int FGGroundNetwork::findNearestNode(double lat, double lon)
|
||||||
}
|
}
|
||||||
|
|
||||||
FGTaxiNode *FGGroundNetwork::findNode(int idx)
|
FGTaxiNode *FGGroundNetwork::findNode(int idx)
|
||||||
{
|
{ /*
|
||||||
for (FGTaxiNodeVectorIterator
|
for (FGTaxiNodeVectorIterator
|
||||||
itr = nodes.begin();
|
itr = nodes.begin();
|
||||||
itr != nodes.end(); itr++)
|
itr != nodes.end(); itr++)
|
||||||
{
|
{
|
||||||
if (itr->getIndex() == idx)
|
if (itr->getIndex() == idx)
|
||||||
return itr->getAddress();
|
return itr->getAddress();
|
||||||
}
|
}*/
|
||||||
return 0;
|
|
||||||
|
if ((idx >= 0) && (idx < nodes.size()))
|
||||||
|
return nodes[idx].getAddress();
|
||||||
|
else
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
|
FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
|
||||||
{
|
{/*
|
||||||
for (FGTaxiSegmentVectorIterator
|
for (FGTaxiSegmentVectorIterator
|
||||||
itr = segments.begin();
|
itr = segments.begin();
|
||||||
itr != segments.end(); itr++)
|
itr != segments.end(); itr++)
|
||||||
|
@ -369,7 +494,14 @@ FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
|
||||||
if (itr->getIndex() == idx)
|
if (itr->getIndex() == idx)
|
||||||
return itr->getAddress();
|
return itr->getAddress();
|
||||||
}
|
}
|
||||||
return 0;
|
*/
|
||||||
|
if ((idx > 0) && (idx <= segments.size()))
|
||||||
|
return segments[idx-1].getAddress();
|
||||||
|
else
|
||||||
|
{
|
||||||
|
cerr << "Alert: trying to find invalid segment " << idx << endl;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
|
FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
|
||||||
|
@ -583,7 +715,8 @@ void FGGroundNetwork::signOff(int id) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) {
|
void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
|
||||||
|
double dt) {
|
||||||
TrafficVectorIterator i = activeTraffic.begin();
|
TrafficVectorIterator i = activeTraffic.begin();
|
||||||
// Search search if the current id has an entry
|
// Search search if the current id has an entry
|
||||||
// This might be faster using a map instead of a vector, but let's start by taking a safe route
|
// This might be faster using a map instead of a vector, but let's start by taking a safe route
|
||||||
|
@ -601,10 +734,42 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
current = i;
|
current = i;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
setDt(getDt() + dt);
|
||||||
|
|
||||||
|
// Update every three secs, but add some randomness
|
||||||
|
// to prevent all IA objects doing this in synchrony
|
||||||
|
//if (getDt() < (3.0) + (rand() % 10))
|
||||||
|
// return;
|
||||||
|
//else
|
||||||
|
// setDt(0);
|
||||||
|
checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
|
||||||
|
checkHoldPosition (id, lat, lon, heading, speed, alt);
|
||||||
|
}
|
||||||
|
|
||||||
|
void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
|
||||||
|
double lon, double heading,
|
||||||
|
double speed, double alt)
|
||||||
|
{
|
||||||
|
|
||||||
// Scan for a speed adjustment change. Find the nearest aircraft that is in front
|
// Scan for a speed adjustment change. Find the nearest aircraft that is in front
|
||||||
// and adjust speed when we get too close. Only do this when current position and/or
|
// and adjust speed when we get too close. Only do this when current position and/or
|
||||||
// intentions of the current aircraft match current taxiroute position of the proximate
|
// intentions of the current aircraft match current taxiroute position of the proximate
|
||||||
// aircraft.
|
// aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
|
||||||
|
// instruction. See below for the hold position instruction.
|
||||||
|
TrafficVectorIterator current, closest;
|
||||||
|
TrafficVectorIterator i = activeTraffic.begin();
|
||||||
|
if (activeTraffic.size())
|
||||||
|
{
|
||||||
|
while ((i->getId() != id) && (i != activeTraffic.end()))
|
||||||
|
{
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
current = i;
|
||||||
double mindist = HUGE;
|
double mindist = HUGE;
|
||||||
if (activeTraffic.size())
|
if (activeTraffic.size())
|
||||||
{
|
{
|
||||||
|
@ -622,19 +787,19 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
i->getLatitude (),
|
i->getLatitude (),
|
||||||
i->getAltitude ());
|
i->getAltitude ());
|
||||||
other.CourseAndDistance(curr, &course, &dist);
|
other.CourseAndDistance(curr, &course, &dist);
|
||||||
bearing = fabs(heading-course);
|
bearing = fabs(heading-course);
|
||||||
if (bearing > 180)
|
if (bearing > 180)
|
||||||
bearing = 360-bearing;
|
bearing = 360-bearing;
|
||||||
if ((dist < mindist) && (bearing < 60.0))
|
if ((dist < mindist) && (bearing < 60.0))
|
||||||
{
|
{
|
||||||
mindist = dist;
|
mindist = dist;
|
||||||
closest = i;
|
closest = i;
|
||||||
minbearing = bearing;
|
minbearing = bearing;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
|
//cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
|
||||||
// << " course : " << course << endl;
|
// << " course : " << course << endl;
|
||||||
current->clearSpeedAdjustment();
|
current->clearSpeedAdjustment();
|
||||||
// Only clear the heading adjustment at positive speeds, otherwise the waypoint following
|
// Only clear the heading adjustment at positive speeds, otherwise the waypoint following
|
||||||
// code wreaks havoc
|
// code wreaks havoc
|
||||||
|
@ -655,7 +820,7 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
|
|
||||||
// Getting close: Slow down to a bit less than the other aircraft
|
// Getting close: Slow down to a bit less than the other aircraft
|
||||||
double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
|
double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
|
||||||
if (mindist > maxAllowableDistance)
|
if (mindist > maxAllowableDistance)
|
||||||
{
|
{
|
||||||
if (current->checkPositionAndIntentions(*closest))
|
if (current->checkPositionAndIntentions(*closest))
|
||||||
{
|
{
|
||||||
|
@ -673,8 +838,8 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
double newSpeed = (maxAllowableDistance-mindist);
|
double newSpeed = (maxAllowableDistance-mindist);
|
||||||
current->setSpeedAdjustment(newSpeed);
|
current->setSpeedAdjustment(newSpeed);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -685,36 +850,11 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
double newSpeed;
|
double newSpeed;
|
||||||
if (mindist > 10) {
|
if (mindist > 10) {
|
||||||
newSpeed = 0.01;
|
newSpeed = 0.01;
|
||||||
current->setSpeedAdjustment(newSpeed);
|
current->setSpeedAdjustment(newSpeed);
|
||||||
} else {
|
} else {
|
||||||
newSpeed = -1 * (maxAllowableDistance-mindist);
|
newSpeed = -1 * (maxAllowableDistance-mindist);
|
||||||
current->setSpeedAdjustment(newSpeed);
|
current->setSpeedAdjustment(newSpeed);
|
||||||
current->setHeadingAdjustment(heading);
|
current->setHeadingAdjustment(heading);
|
||||||
// if (mindist < 5) {
|
|
||||||
// double bank_sense = 0;
|
|
||||||
// current->setSpeedAdjustment(-0.1);
|
|
||||||
// // Do a heading adjustment
|
|
||||||
// double diff = fabs(heading - bearing);
|
|
||||||
// if (diff > 180)
|
|
||||||
// diff = fabs(diff - 360);
|
|
||||||
|
|
||||||
// double sum = heading + diff;
|
|
||||||
// if (sum > 360.0)
|
|
||||||
// sum -= 360.0;
|
|
||||||
// if (fabs(sum - bearing) < 1.0) {
|
|
||||||
// bank_sense = -1.0; // turn left for evasive action
|
|
||||||
// } else {
|
|
||||||
// bank_sense = 1.0; // turn right for evasive action
|
|
||||||
// }
|
|
||||||
// double newHeading = heading + bank_sense;
|
|
||||||
// if (newHeading < 0) newHeading += 360;
|
|
||||||
// if (newHeading > 360) newHeading -= 360;
|
|
||||||
// current->setHeadingAdjustment(newHeading);
|
|
||||||
// //cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading
|
|
||||||
// // << endl;
|
|
||||||
// cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId()
|
|
||||||
// << endl;
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -722,13 +862,116 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void FGGroundNetwork::checkHoldPosition(int id, double lat,
|
||||||
|
double lon, double heading,
|
||||||
|
double speed, double alt)
|
||||||
|
{
|
||||||
|
// Check for "Hold position instruction".
|
||||||
|
// The hold position should be issued under the following conditions:
|
||||||
|
// 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
|
||||||
|
// 2) For taxiing aircraft that use one taxiway in opposite directions
|
||||||
|
// 3) For crossing or merging taxiroutes.
|
||||||
|
|
||||||
|
TrafficVectorIterator current, closest;
|
||||||
|
TrafficVectorIterator i = activeTraffic.begin();
|
||||||
|
if (activeTraffic.size())
|
||||||
|
{
|
||||||
|
while ((i->getId() != id) && i != activeTraffic.end())
|
||||||
|
{
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return ;
|
||||||
|
}
|
||||||
|
current = i;
|
||||||
|
current->setHoldPosition(false);
|
||||||
|
double course, dist, bearing, minbearing;
|
||||||
|
for (i = activeTraffic.begin();
|
||||||
|
i != activeTraffic.end(); i++)
|
||||||
|
{
|
||||||
|
if (i != current)
|
||||||
|
{
|
||||||
|
int node = current->crosses(this, *i);
|
||||||
|
if (node != -1)
|
||||||
|
{
|
||||||
|
// 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;
|
||||||
|
SGWayPoint nodePos(findNode(node)->getLongitude (),
|
||||||
|
findNode(node)->getLatitude (),
|
||||||
|
alt);
|
||||||
|
SGWayPoint curr (lon,
|
||||||
|
lat,
|
||||||
|
alt);
|
||||||
|
|
||||||
|
SGWayPoint other (i->getLongitude (),
|
||||||
|
i->getLatitude (),
|
||||||
|
i->getAltitude ());
|
||||||
|
//other.CourseAndDistance(curr, &course, &dist);
|
||||||
|
bool needsToWait;
|
||||||
|
if (current->isOpposing(this, *i, node))
|
||||||
|
{
|
||||||
|
needsToWait = true;
|
||||||
|
//cerr << "Hold check 2 : " << id << " 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
|
||||||
|
{
|
||||||
|
other.CourseAndDistance(nodePos, &course, &dist);
|
||||||
|
if (dist > 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 << " nm" << endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
curr.CourseAndDistance(nodePos, &course, &dist);
|
||||||
|
if (!(i->hasHoldPosition()))
|
||||||
|
{
|
||||||
|
|
||||||
|
if ((dist < 2.5*current->getRadius()) &&
|
||||||
|
(needsToWait) &&
|
||||||
|
(!(current->getId() == i->getWaitsForId())) &&
|
||||||
|
(!(current->getSpeedAdjustment())))
|
||||||
|
|
||||||
|
{
|
||||||
|
current->setHoldPosition(true);
|
||||||
|
//cerr << "Hold check 5: " << id <<" Setting Hold Position: distance to node : " << dist << " nm"<< endl;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//cerr << "Hold check 6: " << id << " No need to hold yet: Distance to node : " << dist << " nm"<< endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
bool FGGroundNetwork::hasInstruction(int id)
|
bool FGGroundNetwork::hasInstruction(int id)
|
||||||
{
|
{
|
||||||
TrafficVectorIterator i = activeTraffic.begin();
|
TrafficVectorIterator i = activeTraffic.begin();
|
||||||
// Search search if the current id has an entry
|
// Search search if the current id has an entry
|
||||||
// This might be faster using a map instead of a vector, but let's start by taking a safe route
|
// This might be faster using a map instead of a vector, but let's start by taking a safe route
|
||||||
if (activeTraffic.size()) {
|
if (activeTraffic.size())
|
||||||
while ((i->getId() != id) && i != activeTraffic.end()) {
|
{
|
||||||
|
while ((i->getId() != id) && i != activeTraffic.end()) {
|
||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -76,6 +76,7 @@ public:
|
||||||
FGTaxiNode *getAddress() { return this;};
|
FGTaxiNode *getAddress() { return this;};
|
||||||
FGTaxiSegmentPointerVectorIterator getBeginRoute() { return next.begin(); };
|
FGTaxiSegmentPointerVectorIterator getBeginRoute() { return next.begin(); };
|
||||||
FGTaxiSegmentPointerVectorIterator getEndRoute() { return next.end(); };
|
FGTaxiSegmentPointerVectorIterator getEndRoute() { return next.end(); };
|
||||||
|
bool operator<(const FGTaxiNode &other) const { return index < other.index; };
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef vector<FGTaxiNode> FGTaxiNodeVector;
|
typedef vector<FGTaxiNode> FGTaxiNodeVector;
|
||||||
|
@ -93,25 +94,32 @@ private:
|
||||||
FGTaxiNode *start;
|
FGTaxiNode *start;
|
||||||
FGTaxiNode *end;
|
FGTaxiNode *end;
|
||||||
int index;
|
int index;
|
||||||
|
FGTaxiSegment *oppositeDirection;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
FGTaxiSegment();
|
FGTaxiSegment();
|
||||||
FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int);
|
//FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int);
|
||||||
|
|
||||||
void setIndex (int val) { index = val; };
|
void setIndex (int val) { index = val; };
|
||||||
void setStartNodeRef (int val) { startNode = val; };
|
void setStartNodeRef (int val) { startNode = val; };
|
||||||
void setEndNodeRef (int val) { endNode = val; };
|
void setEndNodeRef (int val) { endNode = val; };
|
||||||
|
|
||||||
|
void setOpposite(FGTaxiSegment *opp) { oppositeDirection = opp; };
|
||||||
|
|
||||||
void setStart(FGTaxiNodeVector *nodes);
|
void setStart(FGTaxiNodeVector *nodes);
|
||||||
void setEnd (FGTaxiNodeVector *nodes);
|
void setEnd (FGTaxiNodeVector *nodes);
|
||||||
void setTrackDistance();
|
void setTrackDistance();
|
||||||
|
|
||||||
FGTaxiNode * getEnd() { return end;};
|
FGTaxiNode * getEnd() { return end;};
|
||||||
|
FGTaxiNode * getStart() { return start; };
|
||||||
double getLength() { return length; };
|
double getLength() { return length; };
|
||||||
int getIndex() { return index; };
|
int getIndex() { return index; };
|
||||||
|
|
||||||
FGTaxiSegment *getAddress() { return this;};
|
FGTaxiSegment *getAddress() { return this;};
|
||||||
|
|
||||||
|
bool operator<(const FGTaxiSegment &other) const { return index < other.index; };
|
||||||
|
FGTaxiSegment *opposite() { return oppositeDirection; };
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -194,6 +202,7 @@ public:
|
||||||
void setAlt (double val) { alt = val; };
|
void setAlt (double val) { alt = val; };
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class FGGroundNetwork;
|
||||||
|
|
||||||
/**************************************************************************************
|
/**************************************************************************************
|
||||||
* class FGTrafficRecord
|
* class FGTrafficRecord
|
||||||
|
@ -219,6 +228,10 @@ public:
|
||||||
bool hasInstruction() { return instruction.hasInstruction(); };
|
bool hasInstruction() { return instruction.hasInstruction(); };
|
||||||
void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt);
|
void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt);
|
||||||
bool checkPositionAndIntentions(FGTrafficRecord &other);
|
bool checkPositionAndIntentions(FGTrafficRecord &other);
|
||||||
|
int crosses (FGGroundNetwork *, FGTrafficRecord &other);
|
||||||
|
bool isOpposing (FGGroundNetwork *, FGTrafficRecord &other, int node);
|
||||||
|
|
||||||
|
bool getSpeedAdjustment() { return instruction.getChangeSpeed(); };
|
||||||
|
|
||||||
double getLatitude () { return latitude ; };
|
double getLatitude () { return latitude ; };
|
||||||
double getLongitude() { return longitude; };
|
double getLongitude() { return longitude; };
|
||||||
|
@ -237,6 +250,8 @@ public:
|
||||||
void clearHeadingAdjustment() { instruction.setChangeHeading(false); };
|
void clearHeadingAdjustment() { instruction.setChangeHeading(false); };
|
||||||
|
|
||||||
bool hasHeadingAdjustment() { return instruction.getChangeHeading(); };
|
bool hasHeadingAdjustment() { return instruction.getChangeHeading(); };
|
||||||
|
bool hasHoldPosition() { return instruction.getHoldPosition(); };
|
||||||
|
void setHoldPosition (bool inst) { instruction.setHoldPosition(inst); };
|
||||||
|
|
||||||
void setWaitsForId(int id) { waitsForId = id; };
|
void setWaitsForId(int id) { waitsForId = id; };
|
||||||
|
|
||||||
|
@ -265,7 +280,7 @@ public:
|
||||||
double hdg, double spd, double alt, double radius) = 0;
|
double hdg, double spd, double alt, double radius) = 0;
|
||||||
virtual void signOff(int id) = 0;
|
virtual void signOff(int id) = 0;
|
||||||
virtual void update(int id, double lat, double lon,
|
virtual void update(int id, double lat, double lon,
|
||||||
double heading, double speed, double alt) = 0;
|
double heading, double speed, double alt, double dt) = 0;
|
||||||
virtual bool hasInstruction(int id) = 0;
|
virtual bool hasInstruction(int id) = 0;
|
||||||
virtual FGATCInstruction getInstruction(int id) = 0;
|
virtual FGATCInstruction getInstruction(int id) = 0;
|
||||||
|
|
||||||
|
@ -296,6 +311,11 @@ private:
|
||||||
|
|
||||||
void printRoutingError(string);
|
void printRoutingError(string);
|
||||||
|
|
||||||
|
void checkSpeedAdjustment(int id, double lat, double lon,
|
||||||
|
double heading, double speed, double alt);
|
||||||
|
void checkHoldPosition(int id, double lat, double lon,
|
||||||
|
double heading, double speed, double alt);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
FGGroundNetwork();
|
FGGroundNetwork();
|
||||||
|
|
||||||
|
@ -314,7 +334,7 @@ public:
|
||||||
virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute,
|
virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute,
|
||||||
double lat, double lon, double hdg, double spd, double alt, double radius);
|
double lat, double lon, double hdg, double spd, double alt, double radius);
|
||||||
virtual void signOff(int id);
|
virtual void signOff(int id);
|
||||||
virtual void update(int id, double lat, double lon, double heading, double speed, double alt);
|
virtual void update(int id, double lat, double lon, double heading, double speed, double alt, double dt);
|
||||||
virtual bool hasInstruction(int id);
|
virtual bool hasInstruction(int id);
|
||||||
virtual FGATCInstruction getInstruction(int id);
|
virtual FGATCInstruction getInstruction(int id);
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in a new issue