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