src/Airports/trafficcontrol.cxx

   1 // trafficrecord.cxx - Implementation of AIModels ATC code.
   2 //
   3 // Written by Durk Talsma, started September 2006.
   4 //
   5 // Copyright (C) 2006 Durk Talsma.
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include "trafficcontrol.hxx"
  28 #include <AIModel/AIFlightPlan.hxx>
  29
  30
  31 /***************************************************************************
  32  * FGTrafficRecord
  33  **************************************************************************/
  34 void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route)
  35 {
  36
  37    currentPos = pos;
  38    if (intentions.size()) {
  39      intVecIterator i = intentions.begin();
  40      if ((*i) != pos) {
  41        SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions");
  42        //cerr << "Pos : " << pos << " Curr " << *(intentions.begin())  << endl;
  43        for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) {
  44         //cerr << (*i) << " ";
  45        }
  46        //cerr << endl;
  47      }
  48      intentions.erase(i);
  49    } else {
  50      //int legNr, routeNr;
  51      //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
  52      int size = route->getNrOfWayPoints();
  53      //cerr << "Setting pos" << pos << " ";
  54      //cerr << "setting intentions ";
  55      for (int i = 0; i < size; i++) {
  56        int val = route->getRouteIndex(i);
  57        //cerr << val<< " ";
  58        if ((val) && (val != pos))
  59         {
  60           intentions.push_back(val);
  61           //cerr << "[set] ";
  62         }
  63      }
  64      //cerr << endl;
  65      //while (route->next(&legNr, &routeNr)) {
  66      //intentions.push_back(routeNr);
  67      //}
  68      //route->rewind(currentPos);
  69    }
  70    //exit(1);
  71 }
  72
  73 bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other)
  74 {
  75    bool result = false;
  76    //cerr << "Start check 1" << endl;
  77    if (currentPos == other.currentPos)
  78      {
  79        //cerr << callsign << ": Check Position and intentions: we are on the same taxiway" << other.callsign << "Index = " << currentPos << endl;
  80        result = true;
  81      }
  82   //  else if (other.intentions.size())
  83  //     {
  84  //       cerr << "Start check 2" << endl;
  85  //       intVecIterator i = other.intentions.begin();
  86  //       while (!((i == other.intentions.end()) || ((*i) == currentPos)))
  87  //     i++;
  88  //       if (i != other.intentions.end()) {
  89  //     cerr << "Check Position and intentions: current matches other.intentions" << endl;
  90  //     result = true;
  91  //       }
  92    else if (intentions.size()) {
  93      //cerr << "Start check 3" << endl;
  94      intVecIterator i = intentions.begin();
  95      //while (!((i == intentions.end()) || ((*i) == other.currentPos)))
  96      while (i != intentions.end()) {
  97        if ((*i) == other.currentPos) {
  98          break;
  99        }
 100        i++;
 101      }
 102      if (i != intentions.end()) {
 103        //cerr << callsign << ": Check Position and intentions: .other.current matches" << other.callsign << "Index = " << (*i) << endl;
 104        result = true;
 105      }
 106    }
 107    //cerr << "Done !!" << endl;
 108    return result;
 109 }
 110
 111 void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
 112                                             double spd, double alt)
 113 {
 114   latitude = lat;
 115   longitude = lon;
 116   heading = hdg;
 117   speed = spd;
 118   altitude = alt;
 119 }
 120
 121 int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other)
 122 {
 123    if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this)))
 124      return -1;
 125    intVecIterator i, j;
 126    int currentTargetNode = 0, otherTargetNode = 0;
 127    if (currentPos > 0)
 128      currentTargetNode = net->findSegment(currentPos      )->getEnd()->getIndex(); // OKAY,...
 129    if (other.currentPos > 0)
 130      otherTargetNode   = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,...
 131    if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0)
 132      return currentTargetNode;
 133    if (intentions.size())
 134      {
 135        for (i = intentions.begin(); i != intentions.end(); i++)
 136         {
 137           if ((*i) > 0) {
 138             if ((currentTargetNode == net->findSegment(*i)->getEnd()->getIndex()))
 139               {
 140                 //cerr << "Current crosses at " << currentTargetNode <<endl;
 141                 return currentTargetNode;
 142               }
 143           }
 144         }
 145      }
 146    if (other.intentions.size())
 147      {
 148        for (i = other.intentions.begin(); i != other.intentions.end(); i++)
 149         {
 150           if ((*i) > 0) {
 151             if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex())
 152               {
 153                 //cerr << "Other crosses at " << currentTargetNode <<endl;
 154                 return otherTargetNode;
 155               }
 156           }
 157         }
 158      }
 159    if (intentions.size() && other.intentions.size())
 160      {
 161        for (i = intentions.begin(); i != intentions.end(); i++)
 162         {
 163           for (j = other.intentions.begin(); j != other.intentions.end(); j++)
 164             {
 165               //cerr << "finding segment " << *i << " and " << *j << endl;
 166               if (((*i) > 0) && ((*j) > 0)) {
 167                 currentTargetNode = net->findSegment(*i)->getEnd()->getIndex();
 168                 otherTargetNode   = net->findSegment(*j)->getEnd()->getIndex();
 169                 if (currentTargetNode == otherTargetNode)
 170                   {
 171                     //cerr << "Routes will cross at " << currentTargetNode << endl;
 172                     return currentTargetNode;
 173                   }
 174               }
 175             }
 176         }
 177      }
 178   return -1;
 179 }
 180
 181 bool FGTrafficRecord::onRoute(FGGroundNetwork *net, FGTrafficRecord &other)
 182 {
 183   int node = -1, othernode = -1;
 184   if (currentPos >0)
 185     node = net->findSegment(currentPos)->getEnd()->getIndex();
 186   if (other.currentPos > 0)
 187     othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
 188   if ((node == othernode) && (node != -1))
 189     return true;
 190   if (other.intentions.size())
 191     {
 192       for (intVecIterator i = other.intentions.begin(); i != other.intentions.end(); i++)
 193         {
 194           if (*i > 0)
 195             {
 196               othernode = net->findSegment(*i)->getEnd()->getIndex();
 197               if ((node == othernode) && (node > -1))
 198                 return true;
 199             }
 200         }
 201     }
 202   //if (other.currentPos > 0)
 203   //  othernode = net->findSegment(other.currentPos)->getEnd()->getIndex();
 204   //if (intentions.size())
 205   //  {
 206   //    for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
 207   //    {
 208   //      if (*i > 0)
 209   //        {
 210   //          node = net->findSegment(*i)->getEnd()->getIndex();
 211   //          if ((node == othernode) && (node > -1))
 212   //            return true;
 213   //        }
 214   //    }
 215   //  }
 216   return false;
 217 }
 218
 219
 220 bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node)
 221 {
 222    // Check if current segment is the reverse segment for the other aircraft
 223    FGTaxiSegment *opp;
 224    //cerr << "Current segment " << currentPos << endl;
 225    if ((currentPos > 0) && (other.currentPos > 0))
 226      {
 227        opp = net->findSegment(currentPos)->opposite();
 228        if (opp) {
 229         if (opp->getIndex() == other.currentPos)
 230           return true;
 231        }
 232
 233        for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
 234         {
 235           if (opp = net->findSegment(other.currentPos)->opposite())
 236             {
 237               if ((*i) > 0)
 238                 if (opp->getIndex() == net->findSegment(*i)->getIndex())
 239                   {
 240                     if (net->findSegment(*i)->getStart()->getIndex() == node) {
 241                       {
 242                         //cerr << "Found the node " << node << endl;
 243                         return true;
 244                       }
 245                     }
 246                   }
 247             }
 248           if (other.intentions.size())
 249             {
 250               for (intVecIterator j = other.intentions.begin(); j != other.intentions.end(); j++)
 251                 {
 252                   // cerr << "Current segment 1 " << (*i) << endl;
 253                   if ((*i) > 0) {
 254                     if (opp = net->findSegment(*i)->opposite())
 255                       {
 256                         if (opp->getIndex() ==
 257                             net->findSegment(*j)->getIndex())
 258                           {
 259                             //cerr << "Nodes " << net->findSegment(*i)->getIndex()
 260                             //   << " and  " << net->findSegment(*j)->getIndex()
 261                             //   << " are opposites " << endl;
 262                             if (net->findSegment(*i)->getStart()->getIndex() == node) {
 263                               {
 264                                 //cerr << "Found the node " << node << endl;
 265                                 return true;
 266                               }
 267                             }
 268                           }
 269                       }
 270                   }
 271                 }
 272             }
 273         }
 274      }
 275    return false;
 276 }
 277
 278 void FGTrafficRecord::setSpeedAdjustment(double spd)
 279 {
 280   instruction.setChangeSpeed(true);
 281   instruction.setSpeed(spd);
 282 }
 283
 284 void FGTrafficRecord::setHeadingAdjustment(double heading)
 285 {
 286   instruction.setChangeHeading(true);
 287   instruction.setHeading(heading);
 288 }
 289
 290
 291
 292 /***************************************************************************
 293  * FGATCInstruction
 294  *
 295  **************************************************************************/
 296 FGATCInstruction::FGATCInstruction()
 297 {
 298   holdPattern    = false;
 299   holdPosition   = false;
 300   changeSpeed    = false;
 301   changeHeading  = false;
 302   changeAltitude = false;
 303   resolveCircularWait = false;
 304
 305   double speed   = 0;
 306   double heading = 0;
 307   double alt     = 0;
 308 }
 309
 310 bool FGATCInstruction::hasInstruction()
 311 {
 312   return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude || resolveCircularWait);
 313 }
 314
 315
 316
 317 /***************************************************************************
 318  * class FGTowerController
 319  *
 320  **************************************************************************/
 321 FGTowerController::FGTowerController() :
 322   FGATCController()
 323 {
 324 }
 325
 326 //
 327 void FGTowerController::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 328                                          double lat, double lon, double heading,
 329                                          double speed, double alt, double radius, int leg,
 330                                          string callsign)
 331 {
 332   TrafficVectorIterator i = activeTraffic.begin();
 333   // Search whether the current id alread has an entry
 334   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 335   if (activeTraffic.size()) {
 336     //while ((i->getId() != id) && i != activeTraffic.end()) {
 337     while (i != activeTraffic.end()) {
 338       if (i->getId() == id) {
 339         break;
 340       }
 341       i++;
 342     }
 343   }
 344
 345   // Add a new TrafficRecord if no one exsists for this aircraft.
 346   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 347     FGTrafficRecord rec;
 348     rec.setId(id);
 349     rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 350     rec.setRunway(intendedRoute->getRunway());
 351     rec.setLeg(leg);
 352     rec.setCallSign(callsign);
 353     activeTraffic.push_back(rec);
 354   } else {
 355     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 356   }
 357 }
 358
 359 void FGTowerController::update(int id, double lat, double lon, double heading, double speed, double alt,
 360                              double dt)
 361 {
 362     TrafficVectorIterator i = activeTraffic.begin();
 363     // Search search if the current id has an entry
 364     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 365     TrafficVectorIterator current, closest;
 366     if (activeTraffic.size()) {
 367       //while ((i->getId() != id) && i != activeTraffic.end()) {
 368       while (i != activeTraffic.end()) {
 369         if (i->getId() == id) {
 370           break;
 371         }
 372         i++;
 373       }
 374     }
 375
 376 //    // update position of the current aircraft
 377     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 378       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 379     } else {
 380       i->setPositionAndHeading(lat, lon, heading, speed, alt);
 381       current = i;
 382     }
 383     setDt(getDt() + dt);
 384
 385 //    // see if we already have a clearance record for the currently active runway
 386     ActiveRunwayVecIterator rwy = activeRunways.begin();
 387     // again, a map might be more efficient here
 388     if (activeRunways.size()) {
 389       //while ((rwy->getRunwayName() != current->getRunway()) && (rwy != activeRunways.end())) {
 390       while (rwy != activeRunways.end()) {
 391         if (rwy->getRunwayName() == current->getRunway()) {
 392           break;
 393         }
 394         rwy++;
 395       }
 396     }
 397     if (rwy == activeRunways.end()) {
 398       ActiveRunway aRwy(current->getRunway(), id);
 399       activeRunways.push_back(aRwy);   // Since there are no clearance records for this runway yet
 400       current->setHoldPosition(false); // Clear the current aircraft to continue
 401     }
 402     else {
 403       // Okay, we have a clearance record for this runway, so check
 404       // whether the clearence ID matches that of the current aircraft
 405       if (id == rwy->getCleared()) {
 406         current->setHoldPosition(false);
 407       } else {
 408         current->setHoldPosition(true);
 409       }
 410     }
 411 }
 412
 413
 414 void FGTowerController::signOff(int id)
 415 {
 416   TrafficVectorIterator i = activeTraffic.begin();
 417   // Search search if the current id alread has an entry
 418   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 419     if (activeTraffic.size()) {
 420       //while ((i->getId() != id) && i != activeTraffic.end()) {
 421       while (i != activeTraffic.end()) {
 422         if (i->getId() == id) {
 423           break;
 424         }
 425         i++;
 426       }
 427     }
 428     // If this aircraft has left the runway, we can clear the departure record for this runway
 429     ActiveRunwayVecIterator rwy = activeRunways.begin();
 430     if (activeRunways.size()) {
 431       //while ((rwy->getRunwayName() != i->getRunway()) && (rwy != activeRunways.end())) {
 432       while (rwy != activeRunways.end()) {
 433         if (rwy->getRunwayName() == i->getRunway()) {
 434           break;
 435         }
 436         rwy++;
 437       }
 438       if (rwy != activeRunways.end()) {
 439         rwy = activeRunways.erase(rwy);
 440       } else {
 441         SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff");
 442       }
 443     }
 444     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 445       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off from tower");
 446     } else {
 447       i = activeTraffic.erase(i);
 448     }
 449 }
 450
 451 // NOTE:
 452 // IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS
 453 // THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN
 454 // BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS
 455 // WHICH WOULD SIMPLIFY CODE MAINTENANCE.
 456 // Note that this function is probably obsolete
 457 bool FGTowerController::hasInstruction(int id)
 458 {
 459     TrafficVectorIterator i = activeTraffic.begin();
 460     // Search search if the current id has an entry
 461     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 462     if (activeTraffic.size())
 463       {
 464         //while ((i->getId() != id) && i != activeTraffic.end()) {
 465         while (i != activeTraffic.end()) {
 466           if (i->getId() == id) {
 467             break;
 468           }
 469         i++;
 470       }
 471     }
 472     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 473       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 474     } else {
 475       return i->hasInstruction();
 476     }
 477   return false;
 478 }
 479
 480
 481 FGATCInstruction FGTowerController::getInstruction(int id)
 482 {
 483   TrafficVectorIterator i = activeTraffic.begin();
 484   // Search search if the current id has an entry
 485   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 486   if (activeTraffic.size()) {
 487     //while ((i->getId() != id) && i != activeTraffic.end()) {
 488     while (i != activeTraffic.end()) {
 489       if (i->getId() == id) {
 490         break;
 491       }
 492       i++;
 493     }
 494   }
 495   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 496     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
 497   } else {
 498     return i->getInstruction();
 499   }
 500   return FGATCInstruction();
 501 }
 502