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