diff --git a/src/ATC/AILocalTraffic.cxx b/src/ATC/AILocalTraffic.cxx index 131844316..b5eed063e 100644 --- a/src/ATC/AILocalTraffic.cxx +++ b/src/ATC/AILocalTraffic.cxx @@ -39,353 +39,612 @@ SG_USING_STD(string); #include "ATCutils.hxx" FGAILocalTraffic::FGAILocalTraffic() { - //Hardwire initialisation for now - a lot of this should be read in from config eventually - Vr = 70.0; - best_rate_of_climb_speed = 70.0; - //best_rate_of_climb; - //nominal_climb_speed; - //nominal_climb_rate; - //nominal_circuit_speed; - //min_circuit_speed; - //max_circuit_speed; - nominal_descent_rate = 500.0; - nominal_final_speed = 65.0; - //nominal_approach_speed; - //stall_speed_landing_config; - wind_from_hdg = 0.0; - wind_speed_knots = 0.0; + //Hardwire initialisation for now - a lot of this should be read in from config eventually + Vr = 70.0; + best_rate_of_climb_speed = 70.0; + //best_rate_of_climb; + //nominal_climb_speed; + //nominal_climb_rate; + //nominal_circuit_speed; + //min_circuit_speed; + //max_circuit_speed; + nominal_descent_rate = 500.0; + nominal_final_speed = 65.0; + //nominal_approach_speed; + //stall_speed_landing_config; + nominalTaxiSpeed = 8.0; + taxiTurnRadius = 8.0; + // Init the property nodes + wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true); + wind_speed_knots = fgGetNode("/environment/wind-speed-kts", true); + circuitsToFly = 0; } FGAILocalTraffic::~FGAILocalTraffic() { } void FGAILocalTraffic::Init() { - // Hack alert - Hardwired path!! - string planepath = "Aircraft/c172/Models/c172-dpm.ac"; - SGPath path = globals->get_fg_root(); - path.append(planepath); - aip.init(planepath.c_str()); - aip.setVisible(true); - globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph()); - -#define DCL_KEMT true -//#define DCL_KPAO true -#ifdef DCL_KEMT - // Hardwire to KEMT for now - // Hardwired points at each end of KEMT runway - Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER); - Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER); - Point3D takeoff_end; - bool d010 = true; // use this to change the hardwired runway direction - if(d010) { - rwy.threshold_pos = P010; - takeoff_end = P190; - rwy.hdg = 25.32; //from default.apt - patternDirection = -1; // Left - 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. - } else { - rwy.threshold_pos = P190; - takeoff_end = P010; - rwy.hdg = 205.32; - patternDirection = 1; // Right - 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. - } -#else - //KPAO - might be a better choice since its in the default scenery - //Hardwire it to the default (no wind) direction - 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 - Point3D takeoff_end(-122.1176522, 37.463752, 6.7 * SG_FEET_TO_METER); - rwy.threshold_pos = threshold_end; - rwy.hdg = 315.0; - patternDirection = 1; // Right - 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. -#endif - - //rwy.threshold_pos.setlat(34.081358); - //rwy.threshold_pos.setlon(-118.037483); - //rwy.mag_hdg = 12.0; - //rwy.mag_var = 14.0; - //rwy.hdg = rwy.mag_hdg + rwy.mag_var; - //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER); - - // Initial position on threshold for now - // TODO - check wind / default runway - pos.setlat(rwy.threshold_pos.lat()); - pos.setlon(rwy.threshold_pos.lon()); - hdg = rwy.hdg; - - pitch = 0.0; - roll = 0.0; - leg = TAKEOFF_ROLL; - vel = 0.0; - slope = 0.0; - - // Now set the position of the plane and then re-get the elevation!! (Didn't work - elev always returned as zero) :-( - //aip.setPosition(pos.lon(), pos.lat(), pos.elev() * SG_METER_TO_FEET); - //cout << "*********************** elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n'; - - // Activate the tower - this is dependent on the ATC system being initialised before the AI system - AirportATC a; - if(globals->get_ATC_mgr()->GetAirportATCDetails((string)"KEMT", &a)) { - if(a.tower_freq) { // Has a tower - tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)"KEMT", TOWER); // Maybe need some error checking here - freq = (double)tower->get_freq() / 100.0; - //cout << "***********************************AILocalTraffic freq = " << freq << '\n'; + // Hack alert - Hardwired path!! + string planepath = "Aircraft/c172/Models/c172-dpm.ac"; + SGPath path = globals->get_fg_root(); + path.append(planepath); + aip.init(planepath.c_str()); + aip.setVisible(true); + globals->get_scenery()->get_scene_graph()->addKid(aip.getSceneGraph()); + // is it OK to leave it like this until the first time transform is called? + // Really ought to be started in a parking space unless otherwise specified? + + // Find the tower frequency - this is dependent on the ATC system being initialised before the AI system + // FIXME - ATM this is hardwired. + airportID = "KEMT"; + AirportATC a; + if(globals->get_ATC_mgr()->GetAirportATCDetails((string)airportID, &a)) { + if(a.tower_freq) { // Has a tower + tower = (FGTower*)globals->get_ATC_mgr()->GetATCPointer((string)airportID, TOWER); // Maybe need some error checking here + freq = (double)tower->get_freq() / 100.0; + //cout << "***********************************AILocalTraffic freq = " << freq << '\n'; + } else { + // Check CTAF, unicom etc + } } else { - // Check CTAF, unicom etc + //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n"; } - } else { - cout << "Unable to find airport details in FGAILocalTraffic::Init()\n"; - } - // Set the projection for the local area - ortho.Init(rwy.threshold_pos, rwy.hdg); - rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero - // Hardwire to KEMT for now - rwy.end2ortho = ortho.ConvertToLocal(takeoff_end); - //cout << "*********************************************************************************\n"; - //cout << "*********************************************************************************\n"; - //cout << "*********************************************************************************\n"; - //cout << "end1ortho = " << rwy.end1ortho << '\n'; - //cout << "end2ortho = " << rwy.end2ortho << '\n'; // end2ortho.x() should be zero or thereabouts - - Transform(); + // Initiallise the FGAirportData structure + // This needs a complete overhaul soon - what happens if we have 2 AI planes at same airport - they don't both need a structure + // This needs to be handled by the ATC manager or similar so only one set of physical data per airport is instantiated + // ie. TODO TODO FIXME FIXME + airport.Init(); + } +// Commands to do something from higher level logic +void FGAILocalTraffic::FlyCircuits(int numCircuits, bool tag) { + circuitsToFly += numCircuits; + touchAndGo = tag; + + //At the moment we'll assume that we are always finished previous circuits when called, + //And just teleport to the threshold to start. + //This is a hack though, we need to check where we are and taxi out if appropriate. + operatingState = IN_PATTERN; +#define DCL_KEMT true + //#define DCL_KPAO true +#ifdef DCL_KEMT + // Hardwire to KEMT for now + // Hardwired points at each end of KEMT runway + Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER); + Point3D P190(-118.032308, 34.090456, 299.395263 * SG_FEET_TO_METER); + Point3D takeoff_end; + bool d010 = true; // use this to change the hardwired runway direction + if(d010) { + rwy.threshold_pos = P010; + takeoff_end = P190; + rwy.hdg = 25.32; //from default.apt + rwy.ID = 1; + patternDirection = -1; // Left + 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. + } else { + rwy.threshold_pos = P190; + takeoff_end = P010; + rwy.hdg = 205.32; + rwy.ID = 19; + patternDirection = 1; // Right + 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. + } +#else + //KPAO - might be a better choice since its in the default scenery + //Hardwire it to the default (no wind) direction + 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 + Point3D takeoff_end(-122.1176522, 37.463752, 6.7 * SG_FEET_TO_METER); + rwy.threshold_pos = threshold_end; + rwy.hdg = 315.0; + rwy.ID = ??? + patternDirection = 1; // Right + 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. +#endif + + //rwy.threshold_pos.setlat(34.081358); + //rwy.threshold_pos.setlon(-118.037483); + //rwy.mag_hdg = 12.0; + //rwy.mag_var = 14.0; + //rwy.hdg = rwy.mag_hdg + rwy.mag_var; + //rwy.threshold_pos.setelev(296 * SG_FEET_TO_METER); + + // Initial position on threshold for now + // TODO - check wind / default runway + pos.setlat(rwy.threshold_pos.lat()); + pos.setlon(rwy.threshold_pos.lon()); + hdg = rwy.hdg; + + pitch = 0.0; + roll = 0.0; + leg = TAKEOFF_ROLL; + vel = 0.0; + slope = 0.0; + + // Now set the position of the plane and then re-get the elevation!! (Didn't work - elev always returned as zero) :-( + //aip.setPosition(pos.lon(), pos.lat(), pos.elev() * SG_METER_TO_FEET); + //cout << "*********************** elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n'; + + // Set the projection for the local area + ortho.Init(rwy.threshold_pos, rwy.hdg); + rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos); // should come out as zero + // Hardwire to KEMT for now + rwy.end2ortho = ortho.ConvertToLocal(takeoff_end); + //cout << "*********************************************************************************\n"; + //cout << "*********************************************************************************\n"; + //cout << "*********************************************************************************\n"; + //cout << "end1ortho = " << rwy.end1ortho << '\n'; + //cout << "end2ortho = " << rwy.end2ortho << '\n'; // end2ortho.x() should be zero or thereabouts + + Transform(); +} + // Run the internal calculations void FGAILocalTraffic::Update(double dt) { - // cout << "In FGAILocalTraffic::Update\n"; - // Hardwire flying traffic pattern for now - eventually also needs to be able to taxi to and from runway and GA parking area. - FlyTrafficPattern(dt); - Transform(); - //cout << "elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n'; - // This should become if(the plane has moved) then Transform() + //std::cout << "In FGAILocalTraffic::Update\n"; + // Hardwire flying traffic pattern for now - eventually also needs to be able to taxi to and from runway and GA parking area. + switch(operatingState) { + case IN_PATTERN: + FlyTrafficPattern(dt); + Transform(); + break; + case TAXIING: + Taxi(dt); + Transform(); + break; + case PARKED: + // Do nothing + break; + default: + break; + } + //cout << "elev in FGAILocalTraffic = " << aip.getFGLocation()->get_cur_elev_m() << '\n'; + // This should become if(the plane has moved) then Transform() } // Fly a traffic pattern // FIXME - far too much of the mechanics of turning, rolling, accellerating, descending etc is in here. // Move it out to FGAIPlane and have FlyTrafficPattern just specify what to do, not the implementation. void FGAILocalTraffic::FlyTrafficPattern(double dt) { - // Need to differentiate between in-air (IAS governed) and on-ground (vel governed) - // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed. - bool inAir = true; // FIXME - possibly make into a class variable + // Need to differentiate between in-air (IAS governed) and on-ground (vel governed) + // Take-off is an interesting case - we are on the ground but takeoff speed is IAS governed. + bool inAir = true; // FIXME - possibly make into a class variable + + static bool transmitted = false; // FIXME - this is a hack - static bool transmitted = false; // FIXME - this is a hack + // WIND + // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track, + // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold. + + //cout << "dt = " << dt << '\n'; + double dist = 0; + // ack - I can't remember how long a rate 1 turn is meant to take. + double turn_time = 60.0; // seconds - TODO - check this guess + double turn_circumference; + double turn_radius; + Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane + //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n'; + //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n'; - // WIND - // Wind has two effects - a mechanical one in that IAS translates to a different vel, and the hdg != track, - // but also a piloting effect, in that the AI must be able to descend at a different rate in order to hit the threshold. + // HACK FOR TESTING - REMOVE + //cout << "Calling ExitRunway..." << endl; + //ExitRunway(orthopos); + //return; + // END HACK + + //wind + double wind_from = wind_from_hdg->getDoubleValue(); + double wind_speed = wind_speed_knots->getDoubleValue(); - //cout << "dt = " << dt << '\n'; - double dist = 0; - // ack - I can't remember how long a rate 1 turn is meant to take. - double turn_time = 60.0; // seconds - TODO - check this guess - double turn_circumference; - double turn_radius; - Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane - //cout << "runway elev = " << rwy.threshold_pos.elev() << ' ' << rwy.threshold_pos.elev() * SG_METER_TO_FEET << '\n'; - //cout << "elev = " << pos.elev() << ' ' << pos.elev() * SG_METER_TO_FEET << '\n'; - switch(leg) { - case TAKEOFF_ROLL: - inAir = false; - track = rwy.hdg; - if(vel < 80.0) { - double dveldt = 5.0; - vel += dveldt * dt; - } - IAS = vel + (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots); - if(IAS >= 70) { - leg = CLIMBOUT; - pitch = 10.0; - IAS = best_rate_of_climb_speed; - slope = 7.0; - } - break; - case CLIMBOUT: - track = rwy.hdg; - if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) { - leg = TURN1; - } - break; - case TURN1: - track += (360.0 / turn_time) * dt * patternDirection; - Bank(25.0 * patternDirection); - if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) { - leg = CROSSWIND; - } - break; - case CROSSWIND: - LevelWings(); - track = rwy.hdg + (90.0 * patternDirection); - if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { - slope = 0.0; - pitch = 0.0; - IAS = 80.0; // FIXME - use smooth transistion to new speed - } - // turn 1000m out for now - if(fabs(orthopos.x()) > 980) { - leg = TURN2; - } - break; - case TURN2: - track += (360.0 / turn_time) * dt * patternDirection; - Bank(25.0 * patternDirection); - // just in case we didn't make height on crosswind - if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { - slope = 0.0; - pitch = 0.0; - IAS = 80.0; // FIXME - use smooth transistion to new speed - } - if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) { - leg = DOWNWIND; - transmitted = false; - //roll = 0.0; - } - break; - case DOWNWIND: - LevelWings(); - track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range - // just in case we didn't make height on crosswind - if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { - slope = 0.0; - pitch = 0.0; - IAS = 90.0; // FIXME - use smooth transistion to new speed - } - if((orthopos.y() < 0) && (!transmitted)) { - TransmitPatternPositionReport(); - transmitted = true; - } - if(orthopos.y() < -480) { - slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!) - pitch = -3.0; - IAS = 85.0; - } - if(orthopos.y() < -980) { - //roll = -20; - leg = TURN3; - transmitted = false; - IAS = 80.0; - } - break; - case TURN3: - track += (360.0 / turn_time) * dt * patternDirection; - Bank(25.0 * patternDirection); - if(fabs(rwy.hdg - track) < 91.0) { - leg = BASE; - } - break; - case BASE: - LevelWings(); - if(!transmitted) { - TransmitPatternPositionReport(); - transmitted = true; - } - track = rwy.hdg - (90 * patternDirection); - slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold - pitch = -4.0; - IAS = 70.0; // FIXME - slowdown gradually - // Try and arrange to turn nicely onto base - turn_circumference = IAS * 0.514444 * turn_time; - //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius - //We'll leave it as a hack with IAS for now but it needs revisiting. - - turn_radius = turn_circumference / (2.0 * DCL_PI); - if(fabs(orthopos.x()) < (turn_radius + 50)) { - leg = TURN4; - transmitted = false; - //roll = -20; - } - break; - case TURN4: - track += (360.0 / turn_time) * dt * patternDirection; - Bank(25.0 * patternDirection); - if(fabs(track - rwy.hdg) < 0.6) { - leg = FINAL; - vel = nominal_final_speed; - } - break; - case FINAL: - LevelWings(); - if(!transmitted) { - TransmitPatternPositionReport(); - transmitted = true; - } - // Try and track the extended centreline - track = rwy.hdg - (0.2 * orthopos.x()); - //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n'; - if(pos.elev() <= rwy.threshold_pos.elev()) { - 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. - slope = 0.0; - pitch = 0.0; - leg = LANDING_ROLL; - } - break; - case LANDING_ROLL: - inAir = false; - track = rwy.hdg; - double dveldt = -5.0; - vel += dveldt * dt; - if(vel <= 15.0) { - leg = TAKEOFF_ROLL; - } - break; + switch(leg) { + case TAKEOFF_ROLL: + inAir = false; + track = rwy.hdg; + if(vel < 80.0) { + double dveldt = 5.0; + vel += dveldt * dt; + } + IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed); + if(IAS >= 70) { + leg = CLIMBOUT; + pitch = 10.0; + IAS = best_rate_of_climb_speed; + slope = 7.0; + } + break; + case CLIMBOUT: + track = rwy.hdg; + if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 600) { + leg = TURN1; + } + break; + case TURN1: + track += (360.0 / turn_time) * dt * patternDirection; + Bank(25.0 * patternDirection); + if((track < (rwy.hdg - 89.0)) || (track > (rwy.hdg + 89.0))) { + leg = CROSSWIND; + } + break; + case CROSSWIND: + LevelWings(); + track = rwy.hdg + (90.0 * patternDirection); + if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { + slope = 0.0; + pitch = 0.0; + IAS = 80.0; // FIXME - use smooth transistion to new speed + } + // turn 1000m out for now + if(fabs(orthopos.x()) > 980) { + leg = TURN2; + } + break; + case TURN2: + track += (360.0 / turn_time) * dt * patternDirection; + Bank(25.0 * patternDirection); + // just in case we didn't make height on crosswind + if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { + slope = 0.0; + pitch = 0.0; + IAS = 80.0; // FIXME - use smooth transistion to new speed + } + if((track < (rwy.hdg - 179.0)) || (track > (rwy.hdg + 179.0))) { + leg = DOWNWIND; + transmitted = false; + //roll = 0.0; + } + break; + case DOWNWIND: + LevelWings(); + track = rwy.hdg - (180 * patternDirection); //should tend to bring track back into the 0->360 range + // just in case we didn't make height on crosswind + if((pos.elev() - rwy.threshold_pos.elev()) * SG_METER_TO_FEET > 1000) { + slope = 0.0; + pitch = 0.0; + IAS = 90.0; // FIXME - use smooth transistion to new speed + } + if((orthopos.y() < 0) && (!transmitted)) { + TransmitPatternPositionReport(); + transmitted = true; + } + if(orthopos.y() < -480) { + slope = -4.0; // FIXME - calculate to descent at 500fpm and hit the threshold (taking wind into account as well!!) + pitch = -3.0; + IAS = 85.0; + } + if(orthopos.y() < -980) { + //roll = -20; + leg = TURN3; + transmitted = false; + IAS = 80.0; + } + break; + case TURN3: + track += (360.0 / turn_time) * dt * patternDirection; + Bank(25.0 * patternDirection); + if(fabs(rwy.hdg - track) < 91.0) { + leg = BASE; + } + break; + case BASE: + LevelWings(); + if(!transmitted) { + TransmitPatternPositionReport(); + transmitted = true; + } + track = rwy.hdg - (90 * patternDirection); + slope = -6.0; // FIXME - calculate to descent at 500fpm and hit the threshold + pitch = -4.0; + IAS = 70.0; // FIXME - slowdown gradually + // Try and arrange to turn nicely onto base + turn_circumference = IAS * 0.514444 * turn_time; + //Hmmm - this is an interesting one - ground vs airspeed in relation to turn radius + //We'll leave it as a hack with IAS for now but it needs revisiting. + + turn_radius = turn_circumference / (2.0 * DCL_PI); + if(fabs(orthopos.x()) < (turn_radius + 50)) { + leg = TURN4; + transmitted = false; + //roll = -20; + } + break; + case TURN4: + track += (360.0 / turn_time) * dt * patternDirection; + Bank(25.0 * patternDirection); + if(fabs(track - rwy.hdg) < 0.6) { + leg = FINAL; + vel = nominal_final_speed; + } + break; + case FINAL: + LevelWings(); + if(!transmitted) { + TransmitPatternPositionReport(); + transmitted = true; + } + // Try and track the extended centreline + track = rwy.hdg - (0.2 * orthopos.x()); + //cout << "orthopos.x() = " << orthopos.x() << " hdg = " << hdg << '\n'; + if(pos.elev() <= rwy.threshold_pos.elev()) { + 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. + slope = 0.0; + pitch = 0.0; + leg = LANDING_ROLL; + } + break; + case LANDING_ROLL: + inAir = false; + track = rwy.hdg; + double dveldt = -5.0; + vel += dveldt * dt; + // FIXME - differentiate between touch and go and full stops + if(vel <= 15.0) { + //cout << "Vel <= 15.0, circuitsToFly = " << circuitsToFly << endl; + if(circuitsToFly <= 0) { + //cout << "Calling ExitRunway..." << endl; + ExitRunway(orthopos); + return; + } else { + //cout << "Taking off again..." << endl; + leg = TAKEOFF_ROLL; + --circuitsToFly; + } + } + break; } - yaw = 0.0; //yaw = f(track, wind); - hdg = track + yaw; - // Apply wind to ground-relative velocity if in the air - if(inAir) { - vel = IAS - (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots); - } - dist = vel * 0.514444 * dt; - pos = dclUpdatePosition(pos, track, slope, dist); + if(inAir) { + // FIXME - at the moment this is a bit screwy + // The velocity correction is applied based on the relative headings. + // Then the heading is changed based on the velocity. + // Which comes first, the chicken or the egg? + // Does it really matter? + + // Apply wind to ground-relative velocity if in the air + vel = IAS - (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed); + //crab = f(track, wind, vel); + // The vector we need to fly is our desired vector minus the wind vector + // TODO - we probably ought to use plib's built in vector types and operations for this + // ie. There's almost *certainly* a better way to do this! + double gxx = vel * sin(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity x component wrt ground + double gyy = vel * cos(track * DCL_DEGREES_TO_RADIANS); // Plane desired velocity y component wrt ground + double wxx = wind_speed * sin((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity x component + double wyy = wind_speed * cos((wind_from + 180.0) * DCL_DEGREES_TO_RADIANS); // Wind velocity y component + double axx = gxx - wxx; // Plane in-air velocity x component + double ayy = gyy - wyy; // Plane in-air velocity y component + // Now we want the angle between gxx and axx (which is the crab) + double maga = sqrt(axx*axx + ayy*ayy); + double magg = sqrt(gxx*gxx + gyy*gyy); + crab = acos((axx*gxx + ayy*gyy) / (maga * magg)); + // At this point this works except we're getting the modulus of the angle + cout << "crab = " << crab << '\n'; + + // Make sure both headings are in the 0->360 circle in order to get sane differences + dclBoundHeading(wind_from); + dclBoundHeading(track); + if(track > wind_from) { + if((track - wind_from) <= 180) { + crab *= -1.0; + } + } else { + if((wind_from - track) >= 180) { + crab *= -1.0; + } + } + } else { // on the ground - crab dosen't apply + crab = 0.0; + } + + hdg = track + crab; + dist = vel * 0.514444 * dt; + pos = dclUpdatePosition(pos, track, slope, dist); } void FGAILocalTraffic::TransmitPatternPositionReport(void) { - // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ? - string trns = ""; - - trns += tower->get_name(); - trns += " Traffic "; - // FIXME - add the callsign to the class variables - trns += "Trainer-two-five-charlie "; - if(patternDirection == 1) { - trns += "right "; - } else { - trns += "left "; - } - - // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function. - 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? - case TURN1: - // Fall through to CROSSWIND - case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in - trns += "crosswind "; - break; - case TURN2: - // Fall through to DOWNWIND - case DOWNWIND: - trns += "downwind "; - break; - case TURN3: - // Fall through to BASE - case BASE: - trns += "base "; - break; - case TURN4: - // Fall through to FINAL - case FINAL: // maybe this should include long/short final if appropriate? - trns += "final "; - break; - default: // Hopefully this won't be used - trns += "pattern "; - break; - } - // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation) - trns += convertNumToSpokenString(1); - - // And add the airport name again - trns += tower->get_name(); - - Transmit(trns); + // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ? + string trns = ""; + + trns += tower->get_name(); + trns += " Traffic "; + // FIXME - add the callsign to the class variables + trns += "Trainer-two-five-charlie "; + if(patternDirection == 1) { + trns += "right "; + } else { + trns += "left "; + } + + // We could probably get rid of this whole switch statement and just pass a string containing the leg from the FlyPattern function. + 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? + case TURN1: + // Fall through to CROSSWIND + case CROSSWIND: // I don't think this case will be used here but it can't hurt to leave it in + trns += "crosswind "; + break; + case TURN2: + // Fall through to DOWNWIND + case DOWNWIND: + trns += "downwind "; + break; + case TURN3: + // Fall through to BASE + case BASE: + trns += "base "; + break; + case TURN4: + // Fall through to FINAL + case FINAL: // maybe this should include long/short final if appropriate? + trns += "final "; + break; + default: // Hopefully this won't be used + trns += "pattern "; + break; + } + // FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation) + trns += ConvertRwyNumToSpokenString(1); + + // And add the airport name again + trns += tower->get_name(); + + Transmit(trns); } + +void FGAILocalTraffic::ExitRunway(Point3D orthopos) { + //cout << "In ExitRunway" << endl; + //cout << "Runway ID is " << rwy.ID << endl; + node_array_type exitNodes = airport.GetExits(rwy.ID); //I suppose we ought to have some fallback for rwy with no defined exits? + //cout << "Got exits" << endl; + //cout << "Size of exits array is " << exitNodes.size() << endl; + //Find the next exit from orthopos.y + double d; + double dist = 100000; //ie. longer than any runway in existance + double backdist = 100000; + node_array_iterator nItr = exitNodes.begin(); + node* rwyExit = *(exitNodes.begin()); + int gateID; //This might want to be more persistant at some point + while(nItr != exitNodes.end()) { + d = ortho.ConvertToLocal((*nItr)->pos).y() - ortho.ConvertToLocal(pos).y(); //FIXME - consider making orthopos a class variable + if(d > 0.0) { + if(d < dist) { + dist = d; + rwyExit = *nItr; + } + } else { + if(fabs(d) < backdist) { + backdist = d; + //TODO - need some logic here that if we don't get a forward exit we turn round and store the backwards one + } + } + ++nItr; + } + //cout << "Calculated dist, dist = " << dist << endl; + // GetNodeList(exitNode->parking) and add to from here to exit node + gateID = airport.GetRandomGateID(); + //cout << "gateID = " << gateID << endl; + in_dest = airport.GetGateNode(gateID); + //cout << "in_dest got..." << endl; + path = airport.GetPath(rwyExit, in_dest); //TODO - need to convert a and b to actual nodes!! + //cout << "path got..." << endl; + //cout << "Size of path is " << path.size() << endl; + taxiState = TD_INBOUND; + StartTaxi(); +} + +// Set the class variable nextTaxiNode to the next node in the path +// and update taxiPathPos, the class variable path iterator position +// TODO - maybe should return error codes to the calling function if we fail here +void FGAILocalTraffic::GetNextTaxiNode() { + //cout << "GetNextTaxiNode called " << endl; + //cout << "taxiPathPos = " << taxiPathPos << endl; + ground_network_path_iterator pathItr = path.begin() + taxiPathPos; + if(pathItr == path.end()) { + //cout << "ERROR IN AILocalTraffic::GetNextTaxiNode - no more nodes in path" << endl; + } else { + if((*pathItr)->struct_type == NODE) { + //cout << "ITS A NODE" << endl; + //*pathItr = new node; + nextTaxiNode = (node*)*pathItr; + ++taxiPathPos; + //delete pathItr; + } else { + //cout << "ITS NOT A NODE" << endl; + //The first item in found must have been an arc + //Assume for now that it was straight + pathItr++; + taxiPathPos++; + if(pathItr == path.end()) { + //cout << "ERROR IN AILocalTraffic::GetNextTaxiNode - path ended with an arc" << endl; + } else if((*pathItr)->struct_type == NODE) { + nextTaxiNode = (node*)*pathItr; + ++taxiPathPos; + } else { + // OOPS - two non-nodes in a row - that shouldn't happen ATM + //cout << "ERROR IN AILocalTraffic::GetNextTaxiNode - two non-nodes in sequence" << endl; + } + } + } +} + +// StartTaxi - set up the taxiing state - call only at the start of taxiing +void FGAILocalTraffic::StartTaxi() { + //cout << "StartTaxi called" << endl; + operatingState = TAXIING; + taxiPathPos = 0; + + //Set the desired heading + //Assume we are aiming for first node on path + //Eventually we may need to consider the fact that we might start on a curved arc and + //not be able to head directly for the first node. + GetNextTaxiNode(); // sets the class variable nextTaxiNode to the next taxi node! + desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos); + //cout << "First taxi heading is " << desiredTaxiHeading << endl; +} + +void FGAILocalTraffic::Taxi(double dt) { + //cout << "Taxi called" << endl; + // Logic - if we are further away from next point than turn radius then head for it + // If we have reached turning point then get next point and turn onto that heading + // Look out for the finish!! + + Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane + desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos); + + // HACK ALERT! - for now we will taxi at constant speed for straights and turns + + // Remember that hdg is always equal to track when taxiing so we don't have to consider them both + double dist_to_go = dclGetHorizontalSeparation(pos, nextTaxiNode->pos); // we may be able to do this more cheaply using orthopos + //cout << "dist_to_go = " << dist_to_go << endl; + if((nextTaxiNode->type == GATE) && (dist_to_go <= 0.1)) { + // park up + //taxiing = false; + //parked = true; + operatingState = PARKED; + } else if((dist_to_go > taxiTurnRadius) || (nextTaxiNode->type == GATE)) { + // if the turn radius is r, and speed is s, then in a time dt we turn through + // ((s.dt)/(PI.r)) x 180 degrees + // or alternatively (s.dt)/r radians + //cout << "hdg = " << hdg << " desired taxi heading = " << desiredTaxiHeading << '\n'; + if(fabs(hdg - desiredTaxiHeading) > 0.1) { + // Which is the quickest direction to turn onto heading? + if(desiredTaxiHeading > hdg) { + if((desiredTaxiHeading - hdg) <= 180) { + // turn right + hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0; + // TODO - check that increments are less than the delta that we check for the right direction + // Probably need to reduce convergence speed as convergence is reached + } else { + hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0; + } + } else { + if((hdg - desiredTaxiHeading) <= 180) { + // turn left + hdg -= ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0; + // TODO - check that increments are less than the delta that we check for the right direction + // Probably need to reduce convergence speed as convergence is reached + } else { + hdg += ((nominalTaxiSpeed * 0.514444 * dt) / (taxiTurnRadius * DCL_PI)) * 180.0; + } + } + } + double vel = nominalTaxiSpeed; + //cout << "vel = " << vel << endl; + double dist = vel * 0.514444 * dt; + //cout << "dist = " << dist << endl; + double track = hdg; + //cout << "track = " << track << endl; + double slope = 0.0; + pos = dclUpdatePosition(pos, track, slope, dist); + //cout << "Updated position...\n"; + // FIXME - HACK in absense of proper ground elevation determination + // Linearly interpolate altitude when taxiing between N and S extremes of orthopos + pos.setelev((287.5 + ((299.3 - 287.5) * fabs(orthopos.y() / 1000.0))) * SG_FEET_TO_METER); + } else { + // Time to turn (we've already checked it's not the end we're heading for). + // set the target node to be the next node which will prompt automatically turning onto + // the right heading in the stuff above, with the usual provisos applied. + GetNextTaxiNode(); + // For now why not just recursively call this function? + Taxi(dt); + } +} + diff --git a/src/ATC/AILocalTraffic.hxx b/src/ATC/AILocalTraffic.hxx index 79e4bded2..64b1b58f3 100644 --- a/src/ATC/AILocalTraffic.hxx +++ b/src/ATC/AILocalTraffic.hxx @@ -33,104 +33,156 @@ #include #include #include +#include
#include "tower.hxx" #include "AIPlane.hxx" #include "ATCProjection.hxx" +#include "ground.hxx" typedef enum PatternLeg { - TAKEOFF_ROLL, - CLIMBOUT, - TURN1, - CROSSWIND, - TURN2, - DOWNWIND, - TURN3, - BASE, - TURN4, - FINAL, - LANDING_ROLL + TAKEOFF_ROLL, + CLIMBOUT, + TURN1, + CROSSWIND, + TURN2, + DOWNWIND, + TURN3, + BASE, + TURN4, + FINAL, + LANDING_ROLL +}; + +typedef enum TaxiState { + TD_INBOUND, + TD_OUTBOUND, + TD_NONE +}; + +typedef enum OperatingState { + IN_PATTERN, + TAXIING, + PARKED }; // perhaps we could use an FGRunway instead of this typedef struct RunwayDetails { - Point3D threshold_pos; - Point3D end1ortho; // ortho projection end1 (the threshold ATM) - Point3D end2ortho; // ortho projection end2 (the take off end in the current hardwired scheme) - double mag_hdg; - double mag_var; - double hdg; // true runway heading + Point3D threshold_pos; + Point3D end1ortho; // ortho projection end1 (the threshold ATM) + Point3D end2ortho; // ortho projection end2 (the take off end in the current hardwired scheme) + double mag_hdg; + double mag_var; + double hdg; // true runway heading + int ID; // 1 -> 36 }; typedef struct StartofDescent { - PatternLeg leg; - double orthopos_x; - double orthopos_y; + PatternLeg leg; + double orthopos_x; + double orthopos_y; }; class FGAILocalTraffic : public FGAIPlane { - + public: - - FGAILocalTraffic(); - ~FGAILocalTraffic(); - - // Initialise - void Init(); - - // Run the internal calculations - void Update(double dt); - + + FGAILocalTraffic(); + ~FGAILocalTraffic(); + + // Initialise + void Init(); + + // Run the internal calculations + void Update(double dt); + + // Go out and practice circuits + void FlyCircuits(int numCircuits, bool tag); + protected: - - // Attempt to enter the traffic pattern in a reasonably intelligent manner - void EnterTrafficPattern(double dt); - + + // Attempt to enter the traffic pattern in a reasonably intelligent manner + void EnterTrafficPattern(double dt); + + // Do what is necessary to land and parkup at home airport + void ReturnToBase(double dt); + private: - FGATCAlignedProjection ortho; // Orthogonal mapping of the local area with the threshold at the origin - // and the runway aligned with the y axis. + // High-level stuff + OperatingState operatingState; + int circuitsToFly; //Number of circuits still to do in this session NOT INCLUDING THE CURRENT ONE + bool touchAndGo; //True if circuits should be flown touch and go, false for full stop + + // Its possible that this might be moved out to the ground/airport class at some point. + FGATCAlignedProjection ortho; // Orthogonal mapping of the local area with the threshold at the origin + // and the runway aligned with the y axis. + + // Airport/runway/pattern details + char* airportID; // The ICAO code of the airport that we're operating around + FGGround airport; // FIXME FIXME FIXME This is a complete hardwired cop-out at the moment - we need to connect to the correct ground in the same way we do to the tower. + FGTower* tower; // A pointer to the tower control. + RunwayDetails rwy; + double patternDirection; // 1 for right, -1 for left (This is double because we multiply/divide turn rates + // with it to get RH/LH turns - DON'T convert it to int under ANY circumstances!! + double glideAngle; // Assumed to be visual glidepath angle for FGAILocalTraffic - can be found at www.airnav.com + // Its conceivable that patternDirection and glidePath could be moved into the RunwayDetails structure. + + // Performance characteristics of the plane in knots and ft/min - some of this might get moved out into FGAIPlane + double Vr; + double best_rate_of_climb_speed; + double best_rate_of_climb; + double nominal_climb_speed; + double nominal_climb_rate; + double nominal_circuit_speed; + double min_circuit_speed; + double max_circuit_speed; + double nominal_descent_rate; + double nominal_approach_speed; + double nominal_final_speed; + double stall_speed_landing_config; + double nominal_taxi_speed; + + // environment - some of this might get moved into FGAIPlane + SGPropertyNode* wind_from_hdg; //degrees + SGPropertyNode* wind_speed_knots; //knots + + // Pattern details that (may) change + int numInPattern; // Number of planes in the pattern (this might get more complicated if high performance GA aircraft fly a higher pattern eventually) + int numAhead; // More importantly - how many of them are ahead of us? + double distToNext; // And even more importantly, how near are we getting to the one immediately ahead? + PatternLeg leg; // Out current position in the pattern + StartofDescent SoD; // Start of descent calculated wrt wind, pattern size & altitude, glideslope etc - // Airport/runway/pattern details - char* airport; // The ICAO code of the airport that we're operating around - FGTower* tower; // A pointer to the tower control. - RunwayDetails rwy; - double patternDirection; // 1 for right, -1 for left (This is double because we multiply/divide turn rates - // with it to get RH/LH turns - DON'T convert it to int under ANY circumstances!! - double glideAngle; // Assumed to be visual glidepath angle for FGAILocalTraffic - can be found at www.airnav.com - // Its conceivable that patternDirection and glidePath could be moved into the RunwayDetails structure. + // Taxiing details + // At the moment this assumes that all taxiing in is to gates (a loose term that includes + // any permitted parking spot) and that all taxiing out is to runways. + bool parked; + bool taxiing; + TaxiState taxiState; + double desiredTaxiHeading; + double taxiTurnRadius; + double nominalTaxiSpeed; + Gate* in_dest; + ground_network_path_type path; // a path through the ground network for the plane to taxi + int taxiPathPos; // position of iterator in taxi path when applicable + node* nextTaxiNode; // next node in taxi path + //Runway out_dest; //FIXME - implement this - // Performance characteristics of the plane in knots and ft/min - some of this might get moved out into FGAIPlane - double Vr; - double best_rate_of_climb_speed; - double best_rate_of_climb; - double nominal_climb_speed; - double nominal_climb_rate; - double nominal_circuit_speed; - double min_circuit_speed; - double max_circuit_speed; - double nominal_descent_rate; - double nominal_approach_speed; - double nominal_final_speed; - double stall_speed_landing_config; + void FlyTrafficPattern(double dt); - // environment - some of this might get moved into FGAIPlane - double wind_from_hdg; // degrees - double wind_speed_knots; // knots + // TODO - need to add something to define what option we are flying - Touch and go / Stop and go / Landing properly / others? - // Pattern details that (may) change - int numInPattern; // Number of planes in the pattern (this might get more complicated if high performance GA aircraft fly a higher pattern eventually) - int numAhead; // More importantly - how many of them are ahead of us? - double distToNext; // And even more importantly, how near are we getting to the one immediately ahead? - PatternLeg leg; // Out current position in the pattern - StartofDescent SoD; // Start of descent calculated wrt wind, pattern size & altitude, glideslope etc + void TransmitPatternPositionReport(); - void FlyTrafficPattern(double dt); + void CalculateStartofDescent(); - // TODO - need to add something to define what option we are flying - Touch and go / Stop and go / Landing properly / others? + void ExitRunway(Point3D orthopos); - void TransmitPatternPositionReport(); + void StartTaxi(); - void CalculateStartofDescent(); + void Taxi(double dt); + + void GetNextTaxiNode(); }; #endif // _FG_AILocalTraffic_HXX