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
  58        if ((val) && (val != pos))
  59         {
  60           intentions.push_back(val);
  61           //cerr << val<< " ";
  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 << "Check Position and intentions: current matches" << 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 << "Check Position and intentions: .other.current matches" << 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::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node)
 182 {
 183    // Check if current segment is the reverse segment for the other aircraft
 184    FGTaxiSegment *opp;
 185    //cerr << "Current segment " << currentPos << endl;
 186    if ((currentPos > 0) && (other.currentPos > 0))
 187      {
 188        opp = net->findSegment(currentPos)->opposite();
 189        if (opp) {
 190         if (opp->getIndex() == other.currentPos)
 191           return true;
 192        }
 193
 194        for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
 195         {
 196           if (other.intentions.size())
 197             {
 198               for (intVecIterator j = other.intentions.begin(); j != other.intentions.end(); j++)
 199                 {
 200                   // cerr << "Current segment 1 " << (*i) << endl;
 201                   if ((*i) > 0) {
 202                     if (opp = net->findSegment(*i)->opposite())
 203                       {
 204                         if (opp->getIndex() ==
 205                             net->findSegment(*j)->getIndex())
 206                           {
 207                             cerr << "Nodes " << net->findSegment(*i)->getIndex()
 208                                  << " and  " << net->findSegment(*j)->getIndex()
 209                                  << " are opposites " << endl;
 210                             if (net->findSegment(*i)->getStart()->getIndex() == node) {
 211                               {
 212                                 cerr << "Found the node" << endl;
 213                                 return true;
 214                               }
 215                             }
 216                           }
 217                       }
 218                   }
 219                 }
 220             }
 221         }
 222      }
 223    return false;
 224 }
 225
 226 void FGTrafficRecord::setSpeedAdjustment(double spd)
 227 {
 228   instruction.setChangeSpeed(true);
 229   instruction.setSpeed(spd);
 230 }
 231
 232 void FGTrafficRecord::setHeadingAdjustment(double heading)
 233 {
 234   instruction.setChangeHeading(true);
 235   instruction.setHeading(heading);
 236 }
 237
 238
 239
 240 /***************************************************************************
 241  * FGATCInstruction
 242  *
 243  **************************************************************************/
 244 FGATCInstruction::FGATCInstruction()
 245 {
 246   holdPattern    = false;
 247   holdPosition   = false;
 248   changeSpeed    = false;
 249   changeHeading  = false;
 250   changeAltitude = false;
 251
 252   double speed   = 0;
 253   double heading = 0;
 254   double alt     = 0;
 255 }
 256
 257 bool FGATCInstruction::hasInstruction()
 258 {
 259   return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
 260 }
 261
 262
 263
 264 /***************************************************************************
 265  * class FGTowerController
 266  *
 267  **************************************************************************/
 268 FGTowerController::FGTowerController() :
 269   FGATCController()
 270 {
 271 }
 272
 273 //
 274 void FGTowerController::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 275                                          double lat, double lon, double heading,
 276                                          double speed, double alt, double radius, int leg)
 277 {
 278   TrafficVectorIterator i = activeTraffic.begin();
 279   // Search whether the current id alread has an entry
 280   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 281   if (activeTraffic.size()) {
 282     //while ((i->getId() != id) && i != activeTraffic.end()) {
 283     while (i != activeTraffic.end()) {
 284       if (i->getId() == id) {
 285         break;
 286       }
 287       i++;
 288     }
 289   }
 290
 291   // Add a new TrafficRecord if no one exsists for this aircraft.
 292   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 293     FGTrafficRecord rec;
 294     rec.setId(id);
 295     rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 296     rec.setRunway(intendedRoute->getRunway());
 297     rec.setLeg(leg);
 298     activeTraffic.push_back(rec);
 299   } else {
 300     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 301   }
 302 }
 303
 304 void FGTowerController::update(int id, double lat, double lon, double heading, double speed, double alt,
 305                              double dt)
 306 {
 307     TrafficVectorIterator i = activeTraffic.begin();
 308     // Search search if the current id has an entry
 309     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 310     TrafficVectorIterator current, closest;
 311     if (activeTraffic.size()) {
 312       //while ((i->getId() != id) && i != activeTraffic.end()) {
 313       while (i != activeTraffic.end()) {
 314         if (i->getId() == id) {
 315           break;
 316         }
 317         i++;
 318       }
 319     }
 320
 321 //    // update position of the current aircraft
 322     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 323       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 324     } else {
 325       i->setPositionAndHeading(lat, lon, heading, speed, alt);
 326       current = i;
 327     }
 328     setDt(getDt() + dt);
 329
 330 //    // see if we already have a clearance record for the currently active runway
 331     ActiveRunwayVecIterator rwy = activeRunways.begin();
 332     // again, a map might be more efficient here
 333     if (activeRunways.size()) {
 334       //while ((rwy->getRunwayName() != current->getRunway()) && (rwy != activeRunways.end())) {
 335       while (rwy != activeRunways.end()) {
 336         if (rwy->getRunwayName() == current->getRunway()) {
 337           break;
 338         }
 339         rwy++;
 340       }
 341     }
 342     if (rwy == activeRunways.end()) {
 343       ActiveRunway aRwy(current->getRunway(), id);
 344       activeRunways.push_back(aRwy);   // Since there are no clearance records for this runway yet
 345       current->setHoldPosition(false); // Clear the current aircraft to continue
 346     }
 347     else {
 348       // Okay, we have a clearance record for this runway, so check
 349       // whether the clearence ID matches that of the current aircraft
 350       if (id == rwy->getCleared()) {
 351         current->setHoldPosition(false);
 352       } else {
 353         current->setHoldPosition(true);
 354       }
 355     }
 356 }
 357
 358
 359 void FGTowerController::signOff(int id)
 360 {
 361   TrafficVectorIterator i = activeTraffic.begin();
 362   // Search search if the current id alread 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     if (activeTraffic.size()) {
 365       //while ((i->getId() != id) && i != activeTraffic.end()) {
 366       while (i != activeTraffic.end()) {
 367         if (i->getId() == id) {
 368           break;
 369         }
 370         i++;
 371       }
 372     }
 373     // If this aircraft has left the runway, we can clear the departure record for this runway
 374     ActiveRunwayVecIterator rwy = activeRunways.begin();
 375     if (activeRunways.size()) {
 376       //while ((rwy->getRunwayName() != i->getRunway()) && (rwy != activeRunways.end())) {
 377       while (rwy != activeRunways.end()) {
 378         if (rwy->getRunwayName() == i->getRunway()) {
 379           break;
 380         }
 381         rwy++;
 382       }
 383       if (rwy != activeRunways.end()) {
 384         rwy = activeRunways.erase(rwy);
 385       } else {
 386         SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Attempting to erase non-existing runway clearance record in FGTowerController::signoff");
 387       }
 388     }
 389     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 390       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off from tower");
 391     } else {
 392       i = activeTraffic.erase(i);
 393     }
 394 }
 395
 396 // NOTE:
 397 // IF WE MAKE TRAFFICRECORD A MEMBER OF THE BASE CLASS
 398 // THE FOLLOWING THREE FUNCTIONS: SIGNOFF, HAS INSTRUCTION AND GETINSTRUCTION CAN
 399 // BECOME DEVIRTUALIZED AND BE A MEMBER OF THE BASE ATCCONTROLLER CLASS
 400 // WHICH WOULD SIMPLIFY CODE MAINTENANCE.
 401 // Note that this function is probably obsolete
 402 bool FGTowerController::hasInstruction(int id)
 403 {
 404     TrafficVectorIterator i = activeTraffic.begin();
 405     // Search search if the current id has an entry
 406     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 407     if (activeTraffic.size())
 408       {
 409         //while ((i->getId() != id) && i != activeTraffic.end()) {
 410         while (i != activeTraffic.end()) {
 411           if (i->getId() == id) {
 412             break;
 413           }
 414         i++;
 415       }
 416     }
 417     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 418       SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 419     } else {
 420       return i->hasInstruction();
 421     }
 422   return false;
 423 }
 424
 425
 426 FGATCInstruction FGTowerController::getInstruction(int id)
 427 {
 428   TrafficVectorIterator i = activeTraffic.begin();
 429   // Search search if the current id has an entry
 430   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 431   if (activeTraffic.size()) {
 432     //while ((i->getId() != id) && i != activeTraffic.end()) {
 433     while (i != activeTraffic.end()) {
 434       if (i->getId() == id) {
 435         break;
 436       }
 437       i++;
 438     }
 439   }
 440   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 441     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
 442   } else {
 443     return i->getInstruction();
 444   }
 445   return FGATCInstruction();
 446 }
 447