src/ATC/AILocalTraffic.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., 675 Mass Ave, Cambridge, MA 02139, USA.
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <Main/globals.hxx>
  27 #include <Main/location.hxx>
  28 #include <Scenery/scenery.hxx>
  29 #include <simgear/math/point3d.hxx>
  30 #include <simgear/math/sg_geodesy.hxx>
  31 #include <simgear/misc/sg_path.hxx>
  32 #include <string>
  33 #include <math.h>
  34
  35 SG_USING_STD(string);
  36
  37 #include "ATCmgr.hxx"
  38 #include "AILocalTraffic.hxx"
  39 #include "ATCutils.hxx"
  40
  41 FGAILocalTraffic::FGAILocalTraffic() {
  42     //Hardwire initialisation for now - a lot of this should be read in from config eventually
  43     Vr = 70.0;
  44     best_rate_of_climb_speed = 70.0;
  45     //best_rate_of_climb;
  46     //nominal_climb_speed;
  47     //nominal_climb_rate;
  48     //nominal_circuit_speed;
  49     //min_circuit_speed;
  50     //max_circuit_speed;
  51     nominal_descent_rate = 500.0;
  52     nominal_final_speed = 65.0;
  53     //nominal_approach_speed;
  54     //stall_speed_landing_config;
  55     wind_from_hdg = 0.0;
  56     wind_speed_knots = 0.0;
  57 }
  58
  59 FGAILocalTraffic::~FGAILocalTraffic() {
  60 }
  61
  62 void FGAILocalTraffic::Init() {
  63     // Hack alert - Hardwired path!!
  64     string planepath = "Aircraft/c172/Models/c172-dpm.ac";
  65     SGPath path = globals->get_fg_root();
  66     path.append(planepath);
  67     aip.init(planepath.c_str());
  68     aip.setVisible(true);
  69     globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph());
  70
  71 #define DCL_KEMT true
  72 //#define DCL_KPAO true
  73 #ifdef DCL_KEMT
  74     // Hardwire to KEMT for now
  75     // Hardwired points at each end of KEMT runway
  76     Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
  77     Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER);
  78     Point3D takeoff_end;
  79     bool d010 = true;   // use this to change the hardwired runway direction
  80     if(d010) {
  81         rwy.threshold_pos = P010;
  82         takeoff_end = P190;
  83         rwy.hdg = 25.32;        //from default.apt
  84         patternDirection = -1;  // Left
  85         pos.setelev(rwy.threshold_pos.elev() + (-8.5 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
  86     } else {
  87         rwy.threshold_pos = P190;
  88         takeoff_end = P010;
  89         rwy.hdg = 205.32;
  90         patternDirection = 1;   // Right
  91         pos.setelev(rwy.threshold_pos.elev() + (-0.0 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
  92     }
  93 #else
  94     //KPAO - might be a better choice since its in the default scenery
  95     //Hardwire it to the default (no wind) direction
  96     Point3D threshold_end(-122.1124358, 37.45848783, 6.8 * SG_FEET_TO_METER);   // These positions are from airnav.com and don't quite seem to correspond with the sim scenery
  97     Point3D takeoff_end(-122.1176522, 37.463752, 6.7 * SG_FEET_TO_METER);
  98     rwy.threshold_pos = threshold_end;
  99     rwy.hdg = 315.0;
 100     patternDirection = 1;       // Right
 101     pos.setelev(rwy.threshold_pos.elev() + (-0.0 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
 102 #endif
 103
 104     //rwy.threshold_pos.setlat(34.081358);
 105     //rwy.threshold_pos.setlon(-118.037483);
 106     //rwy.mag_hdg = 12.0;
 107     //rwy.mag_var = 14.0;
 108     //rwy.hdg = rwy.mag_hdg + rwy.mag_var;
 109     //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER);
 110
 111     // Initial position on threshold for now
 112     // TODO - check wind / default runway
 113     pos.setlat(rwy.threshold_pos.lat());
 114     pos.setlon(rwy.threshold_pos.lon());
 115     hdg = rwy.hdg;
 116
 117     pitch = 0.0;
 118     roll = 0.0;
 119     leg = TAKEOFF_ROLL;
 120     vel = 0.0;
 121     slope = 0.0;
 122
 123     // Now set the position of the plane and then re-get the elevation!! (Didn't work - elev always returned as zero) :-(
 124     //aip.setPosition(pos.lon(), pos.lat(), pos.elev() * SG_METER_TO_FEET);
 125     //cout << "*********************** elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n';
 126
 127     // Activate the tower - this is dependent on the ATC system being initialised before the AI system
 128     AirportATC a;
 129     if(globals->get_ATC_mgr()->GetAirportATCDetails((string)"KEMT", &a)) {
 130         if(a.tower_freq) {      // Has a tower
 131             tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)"KEMT", TOWER);     // Maybe need some error checking here
 132             freq = (double)tower->get_freq() / 100.0;
 133             //cout << "***********************************AILocalTraffic freq = " << freq << '\n';
 134         } else {
 135             // Check CTAF, unicom etc
 136         }
 137     } else {
 138         cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
 139     }
 140
 141     // Set the projection for the local area
 142     ortho.Init(rwy.threshold_pos, rwy.hdg);
 143     rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);    // should come out as zero
 144     // Hardwire to KEMT for now
 145     rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 146     //cout << "*********************************************************************************\n";
 147     //cout << "*********************************************************************************\n";
 148     //cout << "*********************************************************************************\n";
 149     //cout << "end1ortho = " << rwy.end1ortho << '\n';
 150     //cout << "end2ortho = " << rwy.end2ortho << '\n';  // end2ortho.x() should be zero or thereabouts
 151
 152     Transform();
 153 }
 154
 155 // Run the internal calculations
 156 void FGAILocalTraffic::Update(double dt) {
 157     // cout << "In FGAILocalTraffic::Update\n";
 158     // Hardwire flying traffic pattern for now - eventually also needs to be able to taxi to and from runway and GA parking area.
 159     FlyTrafficPattern(dt);
 160     Transform();
 161     //cout << "elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n';
 162     // This should become if(the plane has moved) then Transform()
 163 }
 164
 165 // Fly a traffic pattern
 166 // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here.
 167 //         Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation.
 168 void FGAILocalTraffic::FlyTrafficPattern(double dt) {
 169     // Need to differentiate between in-air (IAS governed) and on-ground (vel governed)
 170     // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed.
 171     bool inAir = true;  // FIXME - possibly make into a class variable
 172
 173     static bool transmitted = false;    // FIXME - this is a hack
 174
 175     // WIND
 176     // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track,
 177     // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold.
 178
 179     //cout << "dt = " << dt << '\n';
 180     double dist = 0;
 181     // ack - I can't remember how long a rate 1 turn is meant to take.
 182     double turn_time = 60.0;    // seconds - TODO - check this guess
 183     double turn_circumference;
 184     double turn_radius;
 185     Point3D orthopos = ortho.ConvertToLocal(pos);       // ortho position of the plane
 186     //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n';
 187     //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n';
 188     switch(leg) {
 189     case TAKEOFF_ROLL:
 190         inAir = false;
 191         track = rwy.hdg;
 192         if(vel < 80.0) {
 193             double dveldt = 5.0;
 194             vel += dveldt * dt;
 195         }
 196         IAS = vel + (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots);
 197         if(IAS >= 70) {
 198             leg = CLIMBOUT;
 199             pitch = 10.0;
 200             IAS = best_rate_of_climb_speed;
 201             slope = 7.0;
 202         }
 203         break;
 204     case CLIMBOUT:
 205         track = rwy.hdg;
 206         if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) {
 207             leg = TURN1;
 208         }
 209         break;
 210     case TURN1:
 211         track += (360.0 / turn_time) * dt * patternDirection;
 212         Bank(25.0 * patternDirection);
 213         if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) {
 214             leg = CROSSWIND;
 215         }
 216         break;
 217     case CROSSWIND:
 218         LevelWings();
 219         track = rwy.hdg + (90.0 * patternDirection);
 220         if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 221             slope = 0.0;
 222             pitch = 0.0;
 223             IAS = 80.0;         // FIXME - use smooth transistion to new speed
 224         }
 225         // turn 1000m out for now
 226         if(fabs(orthopos.x()) > 980) {
 227             leg = TURN2;
 228         }
 229         break;
 230     case TURN2:
 231         track += (360.0 / turn_time) * dt * patternDirection;
 232         Bank(25.0 * patternDirection);
 233         // just in case we didn't make height on crosswind
 234         if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 235             slope = 0.0;
 236             pitch = 0.0;
 237             IAS = 80.0;         // FIXME - use smooth transistion to new speed
 238         }
 239         if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) {
 240             leg = DOWNWIND;
 241             transmitted = false;
 242             //roll = 0.0;
 243         }
 244         break;
 245     case DOWNWIND:
 246         LevelWings();
 247         track = rwy.hdg - (180 * patternDirection);     //should tend to bring track back into the 0->360 range
 248         // just in case we didn't make height on crosswind
 249         if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) {
 250             slope = 0.0;
 251             pitch = 0.0;
 252             IAS = 90.0;         // FIXME - use smooth transistion to new speed
 253         }
 254         if((orthopos.y() < 0) && (!transmitted)) {
 255             TransmitPatternPositionReport();
 256             transmitted = true;
 257         }
 258         if(orthopos.y() < -480) {
 259             slope = -4.0;       // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!)
 260             pitch = -3.0;
 261             IAS = 85.0;
 262         }
 263         if(orthopos.y() < -980) {
 264             //roll = -20;
 265             leg = TURN3;
 266             transmitted = false;
 267             IAS = 80.0;
 268         }
 269         break;
 270     case TURN3:
 271         track += (360.0 / turn_time) * dt * patternDirection;
 272         Bank(25.0 * patternDirection);
 273         if(fabs(rwy.hdg - track) < 91.0) {
 274             leg = BASE;
 275         }
 276         break;
 277     case BASE:
 278         LevelWings();
 279         if(!transmitted) {
 280             TransmitPatternPositionReport();
 281             transmitted = true;
 282         }
 283         track = rwy.hdg - (90 * patternDirection);
 284         slope = -6.0;   // FIXME - calculate to descent at 500fpm and hit the threshold
 285         pitch = -4.0;
 286         IAS = 70.0;     // FIXME - slowdown gradually
 287         // Try and arrange to turn nicely onto base
 288         turn_circumference = IAS * 0.514444 * turn_time;
 289         //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius
 290         //We'll leave it as a hack with IAS for now but it needs revisiting.
 291
 292         turn_radius = turn_circumference / (2.0 * DCL_PI);
 293         if(fabs(orthopos.x()) < (turn_radius + 50)) {
 294             leg = TURN4;
 295             transmitted = false;
 296             //roll = -20;
 297         }
 298         break;
 299     case TURN4:
 300         track += (360.0 / turn_time) * dt * patternDirection;
 301         Bank(25.0 * patternDirection);
 302         if(fabs(track - rwy.hdg) < 0.6) {
 303             leg = FINAL;
 304             vel = nominal_final_speed;
 305         }
 306         break;
 307     case FINAL:
 308         LevelWings();
 309         if(!transmitted) {
 310             TransmitPatternPositionReport();
 311             transmitted = true;
 312         }
 313         // Try and track the extended centreline
 314         track = rwy.hdg - (0.2 * orthopos.x());
 315         //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n';
 316         if(pos.elev() <= rwy.threshold_pos.elev()) {
 317             pos.setelev(rwy.threshold_pos.elev());// + (-8.5 * SG_FEET_TO_METER));  // This is a complete hack - the rendered runway takes the underlying scenery elev rather than the published runway elev so I should use height above terrain or something.
 318             slope = 0.0;
 319             pitch = 0.0;
 320             leg = LANDING_ROLL;
 321         }
 322         break;
 323     case LANDING_ROLL:
 324         inAir = false;
 325         track = rwy.hdg;
 326         double dveldt = -5.0;
 327         vel += dveldt * dt;
 328         if(vel <= 15.0) {
 329             leg = TAKEOFF_ROLL;
 330         }
 331         break;
 332     }
 333
 334     yaw = 0.0;  //yaw = f(track, wind);
 335     hdg = track + yaw;
 336     // Apply wind to ground-relative velocity if in the air
 337     if(inAir) {
 338         vel = IAS - (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots);
 339     }
 340     dist = vel * 0.514444 * dt;
 341     pos = dclUpdatePosition(pos, track, slope, dist);
 342 }
 343
 344 void FGAILocalTraffic::TransmitPatternPositionReport(void) {
 345     // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
 346     string trns = "";
 347
 348     trns += tower->get_name();
 349     trns += " Traffic ";
 350     // FIXME - add the callsign to the class variables
 351     trns += "Trainer-two-five-charlie ";
 352     if(patternDirection == 1) {
 353         trns += "right ";
 354     } else {
 355         trns += "left ";
 356     }
 357
 358     // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function.
 359     switch(leg) {       // We'll assume that transmissions in turns are intended for next leg - do pilots ever call out that they are in the turn?
 360     case TURN1:
 361         // Fall through to CROSSWIND
 362     case CROSSWIND:     // I don't think this case will be used here but it can't hurt to leave it in
 363         trns += "crosswind ";
 364         break;
 365     case TURN2:
 366         // Fall through to DOWNWIND
 367     case DOWNWIND:
 368         trns += "downwind ";
 369         break;
 370     case TURN3:
 371         // Fall through to BASE
 372     case BASE:
 373         trns += "base ";
 374         break;
 375     case TURN4:
 376         // Fall through to FINAL
 377     case FINAL:         // maybe this should include long/short final if appropriate?
 378         trns += "final ";
 379         break;
 380     default:            // Hopefully this won't be used
 381         trns += "pattern ";
 382         break;
 383     }
 384     // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
 385     trns += convertNumToSpokenString(1);
 386
 387     // And add the airport name again
 388     trns += tower->get_name();
 389
 390     Transmit(trns);
 391 }