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
 304   double speed   = 0;
 305   double heading = 0;
 306   double alt     = 0;
 307 }
 308
 309 bool FGATCInstruction::hasInstruction()
 310 {
 311   return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
 312 }
 313
 314
 315
 316 /***************************************************************************
 317  * class FGTowerController
 318  *
 319  **************************************************************************/
 320 FGTowerController::FGTowerController() :
 321   FGATCController()
 322 {
 323 }
 324
 325 //
 326 void FGTowerController::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 327                                          double lat, double lon, double heading,
 328                                          double speed, double alt, double radius, int leg,
 329                                          string callsign)
 330 {
 331   TrafficVectorIterator i = activeTraffic.begin();
 332   // Search whether the current id alread has an entry
 333   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 334   if (activeTraffic.size()) {
 335     //while ((i->getId() != id) && i != activeTraffic.end()) {
 336     while (i != activeTraffic.end()) {
 337       if (i->getId() == id) {
 338         break;
 339       }
 340       i++;
 341     }
 342   }
 343
 344   // Add a new TrafficRecord if no one exsists for this aircraft.
 345   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 346     FGTrafficRecord rec;
 347     rec.setId(id);
 348     rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 349     rec.setRunway(intendedRoute->getRunway());
 350     rec.setLeg(leg);
 351     rec.setCallSign(callsign);
 352     activeTraffic.push_back(rec);
 353   } else {
 354     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 355   }
 356 }
 357
 358 void FGTowerController::update(int id, double lat, double lon, double heading, double speed, double alt,
 359                              double dt)
 360 {
 361     TrafficVectorIterator i = activeTraffic.begin();
 362     // Search search if the current id has an entry
 363     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 364     TrafficVectorIterator current, closest;
 365     if (activeTraffic.size()) {
 366       //while ((i->getId() != id) && i != activeTraffic.end()) {
 367       while (i != activeTraffic.end()) {
 368         if (i->getId() == id) {
 369           break;
 370         }
 371         i++;
 372       }
 373     }
 374
 375 //    // update position of the current aircraft
 376     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 377       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 378     } else {
 379       i->setPositionAndHeading(lat, lon, heading, speed, alt);
 380       current = i;
 381     }
 382     setDt(getDt() + dt);
 383
 384 //    // see if we already have a clearance record for the currently active runway
 385     ActiveRunwayVecIterator rwy = activeRunways.begin();
 386     // again, a map might be more efficient here
 387     if (activeRunways.size()) {
 388       //while ((rwy->getRunwayName() != current->getRunway()) && (rwy != activeRunways.end())) {
 389       while (rwy != activeRunways.end()) {
 390         if (rwy->getRunwayName() == current->getRunway()) {
 391           break;
 392         }
 393         rwy++;
 394       }
 395     }
 396     if (rwy == activeRunways.end()) {
 397       ActiveRunway aRwy(current->getRunway(), id);
 398       activeRunways.push_back(aRwy);   // Since there are no clearance records for this runway yet
 399       current->setHoldPosition(false); // Clear the current aircraft to continue
 400     }
 401     else {
 402       // Okay, we have a clearance record for this runway, so check
 403       // whether the clearence ID matches that of the current aircraft
 404       if (id == rwy->getCleared()) {
 405         current->setHoldPosition(false);
 406       } else {
 407         current->setHoldPosition(true);
 408       }
 409     }
 410 }
 411
 412
 413 void FGTowerController::signOff(int id)
 414 {
 415   TrafficVectorIterator i = activeTraffic.begin();
 416   // Search search if the current id alread has an entry
 417   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 418     if (activeTraffic.size()) {
 419       //while ((i->getId() != id) && i != activeTraffic.end()) {
 420       while (i != activeTraffic.end()) {
 421         if (i->getId() == id) {
 422           break;
 423         }
 424         i++;
 425       }
 426     }
 427     // If this aircraft has left the runway, we can clear the departure record for this runway
 428     ActiveRunwayVecIterator rwy = activeRunways.begin();
 429     if (activeRunways.size()) {
 430       //while ((rwy->getRunwayName() != i->getRunway()) && (rwy != activeRunways.end())) {
 431       while (rwy != activeRunways.end()) {
 432         if (rwy->getRunwayName() == i->getRunway()) {
 433           break;
 434         }
 435         rwy++;
 436       }
 437       if (rwy != activeRunways.end()) {
 438         rwy = activeRunways.erase(rwy);
 439       } else {
 440         SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff");
 441       }
 442     }
 443     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 444       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off from tower");
 445     } else {
 446       i = activeTraffic.erase(i);
 447     }
 448 }
 449
 450 // NOTE:
 451 // IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS
 452 // THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN
 453 // BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS
 454 // WHICH WOULD SIMPLIFY CODE MAINTENANCE.
 455 // Note that this function is probably obsolete
 456 bool FGTowerController::hasInstruction(int id)
 457 {
 458     TrafficVectorIterator i = activeTraffic.begin();
 459     // Search search if the current id has an entry
 460     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 461     if (activeTraffic.size())
 462       {
 463         //while ((i->getId() != id) && i != activeTraffic.end()) {
 464         while (i != activeTraffic.end()) {
 465           if (i->getId() == id) {
 466             break;
 467           }
 468         i++;
 469       }
 470     }
 471     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 472       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 473     } else {
 474       return i->hasInstruction();
 475     }
 476   return false;
 477 }
 478
 479
 480 FGATCInstruction FGTowerController::getInstruction(int id)
 481 {
 482   TrafficVectorIterator i = activeTraffic.begin();
 483   // Search search if the current id has an entry
 484   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 485   if (activeTraffic.size()) {
 486     //while ((i->getId() != id) && i != activeTraffic.end()) {
 487     while (i != activeTraffic.end()) {
 488       if (i->getId() == id) {
 489         break;
 490       }
 491       i++;
 492     }
 493   }
 494   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 495     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
 496   } else {
 497     return i->getInstruction();
 498   }
 499   return FGATCInstruction();
 500 }
 501