src/ATCDCL/AIGAVFRTraffic.cxx

   1 // FGAILocalTraffic - AIEntity derived class with enough logic to
   2 // fly and interact with the traffic pattern.
   3 //
   4 // Written by David Luff, started March 2002.
   5 //
   6 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <simgear/math/SGMath.hxx>
  27 #include <Airports/runways.hxx>
  28 #include <Main/globals.hxx>
  29 #include <string>
  30 #include <math.h>
  31
  32 using std::string;
  33
  34 #include "ATC.hxx"
  35 #include "ATCmgr.hxx"
  36 #include "AILocalTraffic.hxx"
  37 #include "AIGAVFRTraffic.hxx"
  38 #include "ATCutils.hxx"
  39 #include "tower.hxx"
  40
  41 // extern from Airports/simple.cxx
  42 extern SGGeod fgGetAirportPos( const std::string& id );
  43
  44 FGAIGAVFRTraffic::FGAIGAVFRTraffic() {
  45         ATC = globals->get_ATC_mgr();
  46         _towerContactedIncoming = false;
  47         _clearedStraightIn = false;
  48         _clearedDownwindEntry = false;
  49         _incoming = false;
  50         _straightIn = false;
  51         _downwindEntry = false;
  52         _climbout = false;
  53         _local = false;
  54         _established = false;
  55         _e45 = false;
  56         _entering = false;
  57         _turning = false;
  58         _cruise_climb_ias = 90.0;
  59         _cruise_ias = 110.0;
  60         patternDirection = -1.0;
  61
  62         // TESTING - REMOVE OR COMMENT OUT BEFORE COMMIT!!!
  63         //_towerContactPrinted = false;
  64 }
  65
  66 FGAIGAVFRTraffic::~FGAIGAVFRTraffic() {
  67 }
  68
  69 // We should never need to Init FGAIGAVFRTraffic in the pattern since that implies arrivel
  70 // and we can just use an FGAILocalTraffic instance for that instead.
  71
  72 // Init en-route to destID at point pt.
  73 // TODO - no idea what to do if pt is above planes ceiling due mountains!!
  74 bool FGAIGAVFRTraffic::Init(const SGGeod& pt, const string& destID, const string& callsign) {
  75         FGAILocalTraffic::Init(callsign, destID, EN_ROUTE);
  76         // TODO FIXME - to get up and running we're going to ignore elev and get FGAIMgr to
  77         // pass in known good values for the test location.  Need to fix this!!! (or at least canonically decide who has responsibility for setting elev).
  78         _enroute = true;
  79         _destID = destID;
  80         _pos = pt;
  81         _destPos = fgGetAirportPos(destID);     // TODO - check if we are within the tower catchment area already.
  82         _cruise_alt = (_destPos.getElevationM() + 2500.0) * SG_FEET_TO_METER;   // TODO look at terrain elevation as well
  83         _pos.setElevationM(_cruise_alt);
  84         // initially set waypoint as airport location
  85         _wp = _destPos;
  86         // Set the initial track
  87         track = GetHeadingFromTo(_pos, _wp);
  88         // And set the plane to keep following it.
  89         SetTrack(GetHeadingFromTo(_pos, _wp));
  90         _roll = 0.0;
  91         _pitch = 0.0;
  92         slope = 0.0;
  93         // TODO - set climbout if altitude is below normal cruising altitude?
  94         //Transform();
  95         // Assume it's OK to set the plane visible
  96         _aip.setVisible(true);
  97         //cout << "Setting visible true\n";
  98         Transform();
  99         return(true);
 100 }
 101
 102 // Init at srcID to fly to destID
 103 bool FGAIGAVFRTraffic::Init(const string& srcID, const string& destID, const string& callsign, OperatingState state) {
 104         _enroute = false;
 105         FGAILocalTraffic::Init(callsign, srcID, PARKED);
 106         return(true);
 107 }
 108
 109 void FGAIGAVFRTraffic::Update(double dt) {
 110         if(_enroute) {
 111                 //cout << "_enroute\n";
 112                 //cout << "e" << flush;
 113                 FlyPlane(dt);
 114                 //cout << "f" << flush;
 115                 Transform();
 116                 //cout << "g" << flush;
 117                 FGAIPlane::Update(dt);
 118                 //cout << "h" << flush;
 119                 responseCounter += dt;
 120
 121                 // we shouldn't really need this since there's a LOD of 10K on the whole plane anyway I think.
 122                 // There are two _aip.setVisible statements set when _local = true that can be removed if the below is removed.
 123                 if(dclGetHorizontalSeparation(_pos, SGGeod::fromDegM(fgGetDouble("/position/longitude-deg"), fgGetDouble("/position/latitude-deg"), 0.0)) > 8000) _aip.setVisible(false);
 124                 else _aip.setVisible(true);
 125
 126         } else if(_local) {
 127                 //cout << "L";
 128                 //cout << "_local\n";
 129                 FGAILocalTraffic::Update(dt);
 130         }
 131 }
 132
 133 void FGAIGAVFRTraffic::FlyPlane(double dt) {
 134         if(_climbout) {
 135                 // Check whether to level off
 136                 if(_pos.getElevationM() >= _cruise_alt) {
 137                         slope = 0.0;
 138                         _pitch = 0.0;
 139                         IAS = _cruise_ias;              // FIXME - use smooth transistion to new speed and attitude.
 140                         _climbout = false;
 141                 } else {
 142                         slope = 4.0;
 143                         _pitch = 5.0;
 144                         IAS = _cruise_climb_ias;
 145                 }
 146         } else {
 147                 // TESTING
 148                 /*
 149                 if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.1) {
 150                         if(!_towerContactPrinted) {
 151                                 if(airportID == "KSQL") {
 152                                         cout << "****************************************************************\n";
 153                                         cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
 154                                         cout << "****************************************************************\n";
 155                                 }
 156                                 _towerContactPrinted = true;
 157                         }
 158                 }
 159                 */
 160
 161                 // if distance to destination is less than 6 - 9 miles contact tower
 162                 // and prepare to become _incoming after response.
 163                 // Possibly check whether to start descent before this?
 164                 //cout << "." << flush;
 165                 //cout << "sep = " << dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 << '\n';
 166                 if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.0) {
 167                         //cout << "-" << flush;
 168                         if(!_towerContactedIncoming) {
 169                                 //cout << "_" << flush;
 170                                 GetAirportDetails(airportID);
 171                                 //cout << "L" << flush;
 172                                 if(_controlled) {
 173                                         freq = (double)tower->get_freq() / 100.0;
 174                                         tuned_station = tower;
 175                                 } else {
 176                                         freq = 122.8;   // TODO - need to get the correct CTAF/Unicom frequency if no tower
 177                                         tuned_station = NULL;
 178                                 }
 179                                 //cout << "freq = " << freq << endl;
 180                                 GetRwyDetails(airportID);
 181                                 //"@AP Tower @CS @MI miles @CD of the airport for full stop with ATIS"
 182                                 // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports!
 183                                 if(rwy.rwyID.size() == 3) {
 184                                         patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1);
 185                                 }
 186                                 if(_controlled) {
 187                                         pending_transmission = tower->get_name();
 188                                         pending_transmission += " Tower ";
 189                                 } else {
 190                                         pending_transmission = "Traffic ";
 191                                         // TODO - find some way of getting uncontrolled airport name
 192                                 }
 193                                 pending_transmission += plane.callsign;
 194                                 //char buf[10];
 195                                 int dist_miles = (int)dclGetHorizontalSeparation(_pos, _destPos) / 1600;
 196                                 //sprintf(buf, " %i ", dist_miles);
 197                                 pending_transmission += " ";
 198                                 pending_transmission += ConvertNumToSpokenDigits(dist_miles);
 199                                 if(dist_miles > 1) pending_transmission += " miles ";
 200                                 else pending_transmission += " mile ";
 201                                 pending_transmission += GetCompassDirection(GetHeadingFromTo(_destPos, _pos));
 202                                 pending_transmission += " of the airport for full stop with ATIS";
 203                                 //cout << pending_transmission << endl;
 204                                 Transmit(14);   // 14 is the callback code, NOT the timeout!
 205                                 responseCounter = 0;
 206                                 _towerContactedIncoming = true;
 207                         } else {
 208                                 //cout << "?" << flush;
 209                                 if(_clearedStraightIn && responseCounter > 5.5) {
 210                                         //cout << "5 " << flush;
 211                                         _clearedStraightIn = false;
 212                                         _straightIn = true;
 213                                         _incoming = true;
 214                                         _wp = GetPatternApproachPos();
 215                                         //_hdg = GetHeadingFromTo(_pos, _wp);   // TODO - turn properly!
 216                                         SetTrack(GetHeadingFromTo(_pos, _wp));
 217                                         slope = atan((_wp.getElevationM() - _pos.getElevationM()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES;
 218                                         double thesh_offset = 0.0;
 219                                         SGVec3d opos = ortho.ConvertToLocal(_pos);
 220                                         double angToApt = atan((_pos.getElevationM() - fgGetAirportElev(airportID)) / (opos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
 221                                         //cout << "angToApt = " << angToApt << ' ';
 222                                         slope = (angToApt > -5.0 ? 0.0 : angToApt);
 223                                         //cout << "slope = " << slope << '\n';
 224                                         pending_transmission = "Straight-in ";
 225                                         pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
 226                                         pending_transmission += " ";
 227                                         pending_transmission += plane.callsign;
 228                                         //cout << pending_transmission << '\n';
 229                                         ConditionalTransmit(4);
 230                                 } else if(_clearedDownwindEntry && responseCounter > 5.5) {
 231                                         //cout << "6" << flush;
 232                                         _clearedDownwindEntry = false;
 233                                         _downwindEntry = true;
 234                                         _incoming = true;
 235                                         _wp = GetPatternApproachPos();
 236                                         SetTrack(GetHeadingFromTo(_pos, _wp));
 237                                         slope = atan((_wp.getElevationM() - _pos.getElevationM()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES;
 238                                         //cout << "slope = " << slope << '\n';
 239                                         pending_transmission = "Report ";
 240                                         pending_transmission += (patternDirection == 1 ? "right downwind " : "left downwind ");
 241                                         pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
 242                                         pending_transmission += " ";
 243                                         pending_transmission += plane.callsign;
 244                                         //cout << pending_transmission << '\n';
 245                                         ConditionalTransmit(4);
 246                                 }
 247                         }
 248                         if(_pos.getElevationM() < (fgGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0;
 249                 }
 250         }
 251         if(_incoming) {
 252                 //cout << "i" << '\n';
 253                 SGVec3d orthopos = ortho.ConvertToLocal(_pos);
 254                 // TODO - Check whether to start descent
 255                 // become _local after the 3 mile report.
 256                 if(_pos.getElevationM() < (fgGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0;
 257                 // TODO - work out why I needed to add the above line to stop the plane going underground!!!
 258                 // (Although it's worth leaving it in as a robustness check anyway).
 259                 if(_straightIn) {
 260                         //cout << "A " << flush;
 261                         if(fabs(orthopos.x()) < 10.0 && !_established) {
 262                                 SetTrack(rwy.hdg);
 263                                 _established = true;
 264                                 //cout << "Established at " << orthopos << '\n';
 265                         }
 266                         double thesh_offset = 30.0;
 267                         //cout << "orthopos.y = " << orthopos.y() << " alt = " << _pos.getElevationM() - fgGetAirportElev(airportID) << '\n';
 268                         if(_established && (orthopos.y() > -5400.0)) {
 269                                 slope = atan((_pos.getElevationM() - fgGetAirportElev(airportID)) / (orthopos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES;
 270                                 //cout << "slope0 = " << slope << '\n';
 271                         }
 272                         //cout << "slope1 = " << slope << '\n';
 273                         if(slope > -5.5) slope = 0.0;   // ie we're too low.
 274                         //cout << "slope2 = " << slope << '\n';
 275                         slope += 0.001;         // To avoid yo-yoing with the above.
 276                         //if(_established && (orthopos.y() > -5400.0)) slope = -5.5;
 277                         if(_established && (orthopos.y() > -4800.0)) {
 278                                 pending_transmission = "3 mile final Runway ";
 279                                 pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
 280                                 pending_transmission += " ";
 281                                 pending_transmission += plane.callsign;
 282                                 //cout << pending_transmission << '\n';
 283                                 ConditionalTransmit(35);
 284                                 _local = true;
 285                                 _aip.setVisible(true);  // HACK
 286                                 _enroute = false;
 287                                 StraightInEntry(true);
 288                         }
 289                 } else if(_downwindEntry) {
 290                         //cout << "B" << flush;
 291                         if(_entering) {
 292                                 //cout << "C" << flush;
 293                                 if(_turning) {
 294                                         if(fabs(_hdg - (rwy.hdg + 180)) < 2.0) {        // TODO - use track instead of _hdg?
 295                                                 //cout << "Going Local...\n";
 296                                                 leg = DOWNWIND;
 297                                                 _local = true;
 298                                                 _aip.setVisible(true);  // HACK
 299                                                 _enroute = false;
 300                                                 _entering = false;
 301                                                 _turning = false;
 302                                                 DownwindEntry();
 303                                         }
 304                                 }
 305                                 if(fabs(orthopos.x() - (patternDirection == 1 ? 1000 : -1000)) < (_e45 ? 175 : 550)) {  // Caution - hardwired turn clearances.
 306                                         //cout << "_turning...\n";
 307                                         _turning = true;
 308                                         SetTrack(rwy.hdg + 180.0);
 309                                 }       // TODO - need to check for other traffic in the pattern and enter much more integilently than that!!!
 310                         } else {
 311                                 //cout << "D" << flush;
 312                                 //cout << '\n' << dclGetHorizontalSeparation(_wp, _pos) << '\n';
 313                                 //cout << ortho.ConvertToLocal(GetPos());
 314                                 //cout << ortho.ConvertToLocal(_wp);
 315                                 if(dclGetHorizontalSeparation(_wp, _pos) < 100.0) {
 316                                         pending_transmission = "2 miles out for ";
 317                                         pending_transmission += (patternDirection == 1 ? "right " : "left ");
 318                                         pending_transmission += "downwind Runway ";
 319                                         pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID);
 320                                         pending_transmission += " ";
 321                                         pending_transmission += plane.callsign;
 322                                         //cout << pending_transmission << '\n';
 323                                         // TODO - are we at pattern altitude??
 324                                         slope = 0.0;
 325                                         ConditionalTransmit(30);
 326                                         if(_e45) {
 327                                                 SetTrack(patternDirection == 1 ? rwy.hdg - 135.0 : rwy.hdg + 135.0);
 328                                         } else {
 329                                                 SetTrack(patternDirection == 1 ? rwy.hdg + 90.0 : rwy.hdg - 90.0);
 330                                         }
 331                                         //if(_hdg < 0.0) _hdg += 360.0;
 332                                         _entering = true;
 333                                 } else {
 334                                         SetTrack(GetHeadingFromTo(_pos, _wp));
 335                                 }
 336                         }
 337                 }
 338         } else {
 339                 // !_incoming
 340                 slope = 0.0;
 341         }
 342         // FIXME - lots of hackery in the next six lines!!!!
 343         double crab = 0.0;      // This is a placeholder for when we take wind into account.
 344         _hdg = track + crab;
 345         double vel = _cruise_ias;
 346         double dist = vel * 0.514444 * dt;
 347         _pos = dclUpdatePosition(_pos, track, slope, dist);
 348 }
 349
 350 void FGAIGAVFRTraffic::RegisterTransmission(int code) {
 351         switch(code) {
 352         case 1: // taxi request cleared
 353                 FGAILocalTraffic::RegisterTransmission(code);
 354                 break;
 355         case 2: // contact tower
 356                 FGAILocalTraffic::RegisterTransmission(code);
 357                 break;
 358         case 3: // Cleared to line up
 359                 FGAILocalTraffic::RegisterTransmission(code);
 360                 break;
 361         case 4: // cleared to take-off
 362                 FGAILocalTraffic::RegisterTransmission(code);
 363                 break;
 364         case 5: // contact ground
 365                 FGAILocalTraffic::RegisterTransmission(code);
 366                 break;
 367         case 6: // taxi to the GA parking
 368                 FGAILocalTraffic::RegisterTransmission(code);
 369                 break;
 370         case 7: // Cleared to land
 371                 FGAILocalTraffic::RegisterTransmission(code);
 372                 break;
 373         case 13: // Go around!
 374                 FGAILocalTraffic::RegisterTransmission(code);
 375                 break;
 376         case 14: // VFR approach for straight-in
 377                 responseCounter = 0;
 378                 _clearedStraightIn = true;
 379                 break;
 380         case 15: // VFR approach for downwind entry
 381                 responseCounter = 0;
 382                 _clearedDownwindEntry = true;
 383                 break;
 384         default:
 385                 SG_LOG(SG_ATC, SG_WARN, "FGAIGAVFRTraffic::RegisterTransmission(...) called with unknown code " << code);
 386                 FGAILocalTraffic::RegisterTransmission(code);
 387                 break;
 388         }
 389 }
 390
 391 // Callback handler
 392 // TODO - Really should enumerate these coded values.
 393 void FGAIGAVFRTraffic::ProcessCallback(int code) {
 394         // 1 - Request Departure from ground
 395         // 2 - Report at hold short
 396         // 10 - report crosswind
 397         // 11 - report downwind
 398         // 12 - report base
 399         // 13 - report final
 400         // 14 - Contact Tower for VFR arrival
 401         // 99 - Remove self
 402         if(code < 14) {
 403                 FGAILocalTraffic::ProcessCallback(code);
 404         } else if(code == 14) {
 405                 if(_controlled) {
 406                         tower->VFRArrivalContact(plane, this, FULL_STOP);
 407                 }
 408                 // TODO else possibly announce arrival intentions at uncontrolled airport?
 409         } else if(code == 99) {
 410                 // Might handle this different in future - hence separated from the other codes to pass to AILocalTraffic.
 411                 FGAILocalTraffic::ProcessCallback(code);
 412         }
 413 }
 414
 415 // Return an appropriate altitude to fly at based on the desired altitude and direction
 416 // whilst respecting the quadrangle rule.
 417 int FGAIGAVFRTraffic::GetQuadrangleAltitude(int dir, int des_alt) {
 418         return(8888);
 419         // TODO - implement me!
 420 }
 421
 422 // Calculates the position needed to set up for either pattern entry or straight in approach.
 423 // Currently returns one of three positions dependent on initial position wrt threshold of active rwy.
 424 // 1/ A few miles out on extended centreline for straight-in.
 425 // 2/ At an appropriate point on circuit side of rwy for a 45deg entry to downwind.
 426 // 3/ At and appropriate point on non-circuit side of rwy at take-off end for perpendicular entry to circuit overflying end-of-rwy.
 427 SGGeod FGAIGAVFRTraffic::GetPatternApproachPos() {
 428         //cout << "\n\n";
 429         //cout << "Calculating pattern approach pos for " << plane.callsign << '\n';
 430         SGVec3d orthopos = ortho.ConvertToLocal(_pos);
 431         SGVec3d tmp;
 432         //cout << "patternDirection = " << patternDirection << '\n';
 433         if(orthopos.y() >= -1000.0) {   // Note that this has to be set the same as the calculation in tower.cxx - at the moment approach type is not transmitted properly between the two.
 434                 //cout << "orthopos.x = " << orthopos.x() << '\n';
 435                 if((orthopos.x() * patternDirection) > 0.0) {   // 45 deg entry
 436                         tmp.x() = 2000 * patternDirection;
 437                         tmp.y() = (rwy.end2ortho.y() / 2.0) + 2000;
 438                         tmp.z() = (fgGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER));
 439                         _e45 = true;
 440                         //cout << "45 deg entry... ";
 441                 } else {
 442                         tmp.x() = (1000 * patternDirection * -1);
 443                         tmp.y() = (rwy.end2ortho.y());
 444                         tmp.z() = (fgGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER));
 445                         _e45 = false;
 446                         //cout << "90 deg entry... ";
 447                 }
 448         } else {
 449                 tmp.x() = 0;
 450                 tmp.y() = -5400;
 451                 tmp.z() = ((5400.0 / 6.0) + fgGetAirportElev(airportID) + 10.0);
 452                 //cout << "Straight in... ";
 453         }
 454         //cout << "Waypoint is " << tmp << '\n';
 455         //cout << ortho.ConvertFromLocal(tmp) << '\n';
 456         //cout << '\n';
 457         //exit(-1);
 458         return ortho.ConvertFromLocal(tmp);
 459 }
 460
 461 //FGAIGAVFRTraffic::
 462
 463 //FGAIGAVFRTraffic::
 464
 465 //FGAIGAVFRTraffic::