580 lines
19 KiB
C++
580 lines
19 KiB
C++
// trafficrecord.cxx - Implementation of AIModels ATC code.
|
|
//
|
|
// Written by Durk Talsma, started September 2006.
|
|
//
|
|
// Copyright (C) 2006 Durk Talsma.
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License as
|
|
// published by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will be useful, but
|
|
// WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
// General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program; if not, write to the Free Software
|
|
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
|
//
|
|
// $Id$
|
|
|
|
#include <config.h>
|
|
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
#include <random>
|
|
|
|
#include <osg/Geode>
|
|
#include <osg/Geometry>
|
|
#include <osg/MatrixTransform>
|
|
#include <osg/Shape>
|
|
|
|
#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 <simgear/timing/sg_time.hxx>
|
|
#include <simgear/math/sg_geodesy.hxx>
|
|
|
|
#include <Scenery/scenery.hxx>
|
|
|
|
#include "trafficcontrol.hxx"
|
|
#include "atc_mgr.hxx"
|
|
#include <AIModel/AIAircraft.hxx>
|
|
#include <AIModel/AIFlightPlan.hxx>
|
|
#include <AIModel/performancedata.hxx>
|
|
#include <ATC/atc_mgr.hxx>
|
|
#include <Traffic/TrafficMgr.hxx>
|
|
#include <Airports/groundnetwork.hxx>
|
|
#include <Airports/dynamics.hxx>
|
|
#include <Airports/airport.hxx>
|
|
#include <Radio/radio.hxx>
|
|
#include <signal.h>
|
|
|
|
using std::sort;
|
|
using std::string;
|
|
|
|
/***************************************************************************
|
|
* ActiveRunway
|
|
**************************************************************************/
|
|
|
|
ActiveRunway::ActiveRunway(const std::string& r, int cc) :
|
|
rwy(r)
|
|
{
|
|
currentlyCleared = cc;
|
|
distanceToFinal = 6.0 * SG_NM_TO_METER;
|
|
};
|
|
|
|
void ActiveRunway::updateDepartureQueue()
|
|
{
|
|
departureQueue.erase(departureQueue.begin());
|
|
}
|
|
|
|
/*
|
|
* Fetch next slot for the active runway
|
|
* @param eta time of slot requested
|
|
* @return newEta: next slot available; starts at eta paramater
|
|
* and adds separation as needed
|
|
*/
|
|
time_t ActiveRunway::requestTimeSlot(time_t eta)
|
|
{
|
|
time_t newEta = 0;
|
|
// default separation - 60 seconds
|
|
time_t separation = 60;
|
|
//if (wakeCategory == "heavy_jet") {
|
|
// SG_LOG(SG_ATC, SG_DEBUG, "Heavy jet, using extra separation");
|
|
// time_t separation = 120;
|
|
//}
|
|
bool found = false;
|
|
|
|
// if the aircraft is the first arrival, add to the vector and return eta directly
|
|
if (estimatedArrivalTimes.empty()) {
|
|
estimatedArrivalTimes.push_back(eta);
|
|
SG_LOG(SG_ATC, SG_DEBUG, getRunwayName() << "Checked eta slots, using " << eta);
|
|
return eta;
|
|
} else {
|
|
// First check the already assigned slots to see where we need to fit the flight in
|
|
TimeVectorIterator i = estimatedArrivalTimes.begin();
|
|
SG_LOG(SG_ATC, SG_DEBUG, getRunwayName() << " Checking eta slots " << eta << " : " << estimatedArrivalTimes.size() << " Timediff " << (eta - globals->get_time_params()->get_cur_time()));
|
|
|
|
// is this needed - just a debug output?
|
|
for (i = estimatedArrivalTimes.begin();
|
|
i != estimatedArrivalTimes.end(); i++) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Stored time : " << (*i));
|
|
}
|
|
|
|
// if the flight is before the first scheduled slot + separation
|
|
i = estimatedArrivalTimes.begin();
|
|
if ((eta + separation) < (*i)) {
|
|
newEta = eta;
|
|
SG_LOG(SG_ATC, SG_BULK, "Storing at beginning");
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Done. New ETA : " << newEta);
|
|
slotHousekeeping(newEta);
|
|
return newEta;
|
|
}
|
|
|
|
// else, look through the rest of the slots
|
|
while ((i != estimatedArrivalTimes.end()) && (!found)) {
|
|
TimeVectorIterator j = i + 1;
|
|
|
|
// if the flight is after the last scheduled slot check if separation is needed
|
|
if (j == estimatedArrivalTimes.end()) {
|
|
if (((*i) + separation) < eta) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Storing at end");
|
|
newEta = eta;
|
|
} else {
|
|
newEta = (*i) + separation;
|
|
SG_LOG(SG_ATC, SG_BULK, "Storing at end + separation");
|
|
}
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Done. New ETA : " << newEta);
|
|
slotHousekeeping(newEta);
|
|
return newEta;
|
|
} else {
|
|
// potential slot found
|
|
// check the distance between the previous and next slots
|
|
// distance msut be greater than 2* separation
|
|
if ((((*j) - (*i)) > (separation * 2))) {
|
|
// now check whether this slot is usable:
|
|
// eta should fall between the two points
|
|
// i.e. eta > i AND eta < j
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Found potential slot after " << (*i));
|
|
if (eta > (*i) && (eta < (*j))) {
|
|
found = true;
|
|
if (eta < ((*i) + separation)) {
|
|
newEta = (*i) + separation;
|
|
SG_LOG(SG_ATC, SG_BULK, "Using original" << (*i) << " + separation ");
|
|
} else {
|
|
newEta = eta;
|
|
SG_LOG(SG_ATC, SG_BULK, "Using original after " << (*i));
|
|
}
|
|
} else if (eta < (*i)) {
|
|
found = true;
|
|
newEta = (*i) + separation;
|
|
SG_LOG(SG_ATC, SG_BULK, "Using delayed slot after " << (*i));
|
|
}
|
|
/*
|
|
if (((*j) - separation) < eta) {
|
|
found = true;
|
|
if (((*i) + separation) < eta) {
|
|
newEta = eta;
|
|
SG_LOG(SG_ATC, SG_BULK, "Using original after " << (*i));
|
|
} else {
|
|
newEta = (*i) + separation;
|
|
SG_LOG(SG_ATC, SG_BULK, "Using " << (*i) << " + separation ");
|
|
}
|
|
} */
|
|
}
|
|
}
|
|
i++;
|
|
}
|
|
}
|
|
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Done. New ETA : " << newEta);
|
|
slotHousekeeping(newEta);
|
|
return newEta;
|
|
}
|
|
|
|
void ActiveRunway::slotHousekeeping(time_t newEta)
|
|
{
|
|
// add the slot to the vector and resort the vector
|
|
estimatedArrivalTimes.push_back(newEta);
|
|
sort(estimatedArrivalTimes.begin(), estimatedArrivalTimes.end());
|
|
|
|
// do some housekeeping : remove any slots that are past
|
|
time_t now = globals->get_time_params()->get_cur_time();
|
|
|
|
TimeVectorIterator i = estimatedArrivalTimes.begin();
|
|
while (i != estimatedArrivalTimes.end()) {
|
|
if ((*i) < now) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Deleting timestamp " << (*i) << " (now = " << now << "). ");
|
|
estimatedArrivalTimes.erase(i);
|
|
i = estimatedArrivalTimes.begin();
|
|
} else {
|
|
i++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Output the contents of the departure queue vector nicely formatted*/
|
|
void ActiveRunway::printDepartureQueue()
|
|
{
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Departure queue for " << rwy << ": ");
|
|
for (auto acft : departureQueue) {
|
|
SG_LOG(SG_ATC, SG_DEBUG, " " << acft->getCallSign() << " " << acft->getTakeOffStatus());
|
|
SG_LOG(SG_ATC, SG_DEBUG, " " << acft->_getLatitude() << " " << acft->_getLongitude() << acft->getSpeed() << " " << acft->getAltitude());
|
|
}
|
|
|
|
}
|
|
|
|
/* Fetch the first aircraft in the departure queue with a certain status */
|
|
SGSharedPtr<FGAIAircraft>ActiveRunway::getFirstOfStatus(int stat) const
|
|
{
|
|
auto it = std::find_if(departureQueue.begin(), departureQueue.end(), [stat](const SGSharedPtr<FGAIAircraft>& acft) {
|
|
return acft->getTakeOffStatus() == stat;
|
|
});
|
|
|
|
if (it == departureQueue.end()) {
|
|
return {};
|
|
}
|
|
|
|
return *it;
|
|
}
|
|
|
|
SGSharedPtr<FGAIAircraft> ActiveRunway::getFirstAircraftInDepartureQueue() const
|
|
{
|
|
if (departureQueue.empty()) {
|
|
return {};
|
|
}
|
|
|
|
return departureQueue.front();
|
|
};
|
|
|
|
void ActiveRunway::addToDepartureQueue(FGAIAircraft *ac)
|
|
{
|
|
assert(ac);
|
|
assert(!ac->getDie());
|
|
departureQueue.push_back(ac);
|
|
};
|
|
|
|
|
|
/***************************************************************************
|
|
* FGTrafficRecord
|
|
**************************************************************************/
|
|
|
|
FGTrafficRecord::FGTrafficRecord():
|
|
id(0), waitsForId(0),
|
|
currentPos(0),
|
|
leg(0),
|
|
frequencyId(0),
|
|
state(0),
|
|
allowTransmission(true),
|
|
allowPushback(true),
|
|
priority(0),
|
|
timer(0),
|
|
heading(0), speed(0), altitude(0), radius(0)
|
|
{
|
|
}
|
|
|
|
FGTrafficRecord::~FGTrafficRecord()
|
|
{
|
|
}
|
|
|
|
void FGTrafficRecord::setPositionAndIntentions(int pos,
|
|
FGAIFlightPlan * route)
|
|
{
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Position: " << pos);
|
|
currentPos = pos;
|
|
if (!intentions.empty()) {
|
|
intVecIterator i = intentions.begin();
|
|
if ((*i) != currentPos) {
|
|
SG_LOG(SG_ATC, SG_ALERT,
|
|
"Error in FGTrafficRecord::setPositionAndIntentions at " << SG_ORIGIN << ", " << (*i));
|
|
}
|
|
intentions.erase(i);
|
|
} else {
|
|
//FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
|
|
int size = route->getNrOfWayPoints();
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Setting pos" << currentPos);
|
|
SG_LOG(SG_ATC, SG_DEBUG, "Setting intentions");
|
|
for (int i = 2; i < size; i++) {
|
|
int val = route->getRouteIndex(i);
|
|
intentions.push_back(val);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FGTrafficRecord::setAircraft(FGAIAircraft *ref)
|
|
{
|
|
aircraft = ref;
|
|
}
|
|
|
|
bool FGTrafficRecord::isDead() const {
|
|
if (!aircraft) {
|
|
return true;
|
|
}
|
|
return aircraft->getDie();
|
|
}
|
|
|
|
void FGTrafficRecord::clearATCController() const {
|
|
if (aircraft) {
|
|
aircraft->clearATCController();
|
|
}
|
|
}
|
|
|
|
FGAIAircraft* FGTrafficRecord::getAircraft() const
|
|
{
|
|
if(aircraft.valid()) {
|
|
return aircraft.ptr();
|
|
}
|
|
return 0;
|
|
}
|
|
/**
|
|
* Check if another aircraft is ahead of the current one, and on the same taxiway
|
|
* @return true / false if this is/isn't the case.
|
|
*/
|
|
bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord & other)
|
|
{
|
|
bool result = false;
|
|
SG_LOG(SG_ATC, SG_BULK, getCallsign() << "|checkPositionAndIntentions");
|
|
if (currentPos == other.currentPos && getId() != other.getId() ) {
|
|
SG_LOG(SG_ATC, SG_BULK, getCallsign() << "|Check Position and intentions: " << other.getCallsign() << " we are on the same taxiway; Index = " << currentPos);
|
|
result = true;
|
|
}
|
|
// else if (! other.intentions.empty())
|
|
// {
|
|
// SG_LOG(SG_ATC, SG_BULK, "Start check 2");
|
|
// intVecIterator i = other.intentions.begin();
|
|
// while (!((i == other.intentions.end()) || ((*i) == currentPos)))
|
|
// i++;
|
|
// if (i != other.intentions.end()) {
|
|
// SG_LOG(SG_ATC, SG_BULK, "Check Position and intentions: current matches other.intentions");
|
|
// result = true;
|
|
// }
|
|
else if (! intentions.empty()) {
|
|
SG_LOG(SG_ATC, SG_BULK, getCallsign() << "|Itentions " << intentions.size());
|
|
intVecIterator i = intentions.begin();
|
|
//while (!((i == intentions.end()) || ((*i) == other.currentPos)))
|
|
while (i != intentions.end()) {
|
|
if ((*i) == other.currentPos) {
|
|
break;
|
|
}
|
|
i++;
|
|
}
|
|
if (i != intentions.end()) {
|
|
SG_LOG(SG_ATC, SG_BULK, getCallsign() << "| Check Position and intentions: " << other.getCallsign()<< " matches Index = " << (*i));
|
|
result = true;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void FGTrafficRecord::setPositionAndHeading(double lat, double lon,
|
|
double hdg, double spd,
|
|
double alt)
|
|
{
|
|
this->pos = SGGeod::fromDegFt(lon, lat, alt);
|
|
heading = hdg;
|
|
speed = spd;
|
|
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.empty()) {
|
|
for (i = intentions.begin(); i != intentions.end(); i++) {
|
|
if ((*i) > 0) {
|
|
if (currentTargetNode ==
|
|
net->findSegment(*i)->getEnd()->getIndex()) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Current crosses at " << currentTargetNode);
|
|
return currentTargetNode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (! other.intentions.empty()) {
|
|
for (i = other.intentions.begin(); i != other.intentions.end();
|
|
i++) {
|
|
if ((*i) > 0) {
|
|
if (otherTargetNode ==
|
|
net->findSegment(*i)->getEnd()->getIndex()) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Other crosses at " << currentTargetNode);
|
|
return otherTargetNode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (! intentions.empty() && ! other.intentions.empty()) {
|
|
for (i = intentions.begin(); i != intentions.end(); i++) {
|
|
for (j = other.intentions.begin(); j != other.intentions.end();
|
|
j++) {
|
|
SG_LOG(SG_ATC, SG_BULK, "finding segment " << *i << " and " << *j);
|
|
if (((*i) > 0) && ((*j) > 0)) {
|
|
currentTargetNode =
|
|
net->findSegment(*i)->getEnd()->getIndex();
|
|
otherTargetNode =
|
|
net->findSegment(*j)->getEnd()->getIndex();
|
|
if (currentTargetNode == otherTargetNode) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Routes will cross at " << currentTargetNode);
|
|
return currentTargetNode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
bool FGTrafficRecord::onRoute(FGGroundNetwork * net,
|
|
FGTrafficRecord & other)
|
|
{
|
|
int node = -1, othernode = -1;
|
|
if (currentPos > 0)
|
|
node = net->findSegment(currentPos)->getEnd()->getIndex();
|
|
if (other.currentPos > 0)
|
|
othernode =
|
|
net->findSegment(other.currentPos)->getEnd()->getIndex();
|
|
if ((node == othernode) && (node != -1))
|
|
return true;
|
|
if (! other.intentions.empty()) {
|
|
for (intVecIterator i = other.intentions.begin();
|
|
i != other.intentions.end(); i++) {
|
|
if (*i > 0) {
|
|
othernode = net->findSegment(*i)->getEnd()->getIndex();
|
|
if ((node == othernode) && (node > -1))
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
//if (other.currentPos > 0)
|
|
// othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
|
|
//if (! intentions.empty())
|
|
// {
|
|
// for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
|
|
// {
|
|
// if (*i > 0)
|
|
// {
|
|
// node = net->findSegment(*i)->getEnd()->getIndex();
|
|
// if ((node == othernode) && (node > -1))
|
|
// return true;
|
|
// }
|
|
// }
|
|
// }
|
|
return false;
|
|
}
|
|
|
|
|
|
bool FGTrafficRecord::isOpposing(FGGroundNetwork * net,
|
|
FGTrafficRecord & other, int node)
|
|
{
|
|
// Check if current segment is the reverse segment for the other aircraft
|
|
FGTaxiSegment *opp;
|
|
SG_LOG(SG_ATC, SG_BULK, "Current segment " << currentPos);
|
|
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++) {
|
|
if ((opp = net->findSegment(other.currentPos)->opposite())) {
|
|
if ((*i) > 0)
|
|
if (opp->getIndex() ==
|
|
net->findSegment(*i)->getIndex()) {
|
|
if (net->findSegment(*i)->getStart()->getIndex() ==
|
|
node) {
|
|
{
|
|
SG_LOG(SG_ATC, SG_BULK, "Found the node " << node);
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (! other.intentions.empty()) {
|
|
for (intVecIterator j = other.intentions.begin();
|
|
j != other.intentions.end(); j++) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Current segment 1 " << (*i));
|
|
if ((*i) > 0) {
|
|
if ((opp = net->findSegment(*i)->opposite())) {
|
|
if (opp->getIndex() ==
|
|
net->findSegment(*j)->getIndex()) {
|
|
SG_LOG(SG_ATC, SG_BULK, "Nodes " << net->findSegment(*i)->getIndex()
|
|
<< " and " << net->findSegment(*j)->getIndex()
|
|
<< " are opposites ");
|
|
if (net->findSegment(*i)->getStart()->
|
|
getIndex() == node) {
|
|
{
|
|
SG_LOG(SG_ATC, SG_BULK, "Found the node " << node);
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool FGTrafficRecord::isActive(int margin) const
|
|
{
|
|
if (aircraft->getDie()) {
|
|
return false;
|
|
}
|
|
|
|
time_t now = globals->get_time_params()->get_cur_time();
|
|
time_t deptime = aircraft->getTrafficRef()->getDepartureTime();
|
|
return ((now + margin) > deptime);
|
|
}
|
|
|
|
|
|
void FGTrafficRecord::setSpeedAdjustment(double spd)
|
|
{
|
|
instruction.setChangeSpeed(true);
|
|
instruction.setSpeed(spd);
|
|
}
|
|
|
|
void FGTrafficRecord::setHeadingAdjustment(double heading)
|
|
{
|
|
instruction.setChangeHeading(true);
|
|
instruction.setHeading(heading);
|
|
}
|
|
|
|
bool FGTrafficRecord::pushBackAllowed() const
|
|
{
|
|
return allowPushback;
|
|
}
|
|
|
|
|
|
|
|
/***************************************************************************
|
|
* FGATCInstruction
|
|
*
|
|
**************************************************************************/
|
|
|
|
FGATCInstruction::FGATCInstruction()
|
|
{
|
|
holdPattern = false;
|
|
holdPosition = false;
|
|
changeSpeed = false;
|
|
changeHeading = false;
|
|
changeAltitude = false;
|
|
resolveCircularWait = false;
|
|
|
|
speed = 0;
|
|
heading = 0;
|
|
alt = 0;
|
|
}
|
|
|
|
bool FGATCInstruction::hasInstruction() const
|
|
{
|
|
return (holdPattern || holdPosition || changeSpeed || changeHeading
|
|
|| changeAltitude || resolveCircularWait);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|