#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());
+ // 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 {
+ //cout << "Unable to find airport details in FGAILocalTraffic::Init()\n";
+ }
+
+ // 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
+ //#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';
+ // 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 {
- // Check CTAF, unicom etc
+ 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 {
- 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();
-}
+#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;
+ 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;
+ }
+
+ 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;
}
- 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;
+
+ 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 ";
}
- break;
- case FINAL:
- LevelWings();
- if(!transmitted) {
- TransmitPatternPositionReport();
- transmitted = true;
+
+ // 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;
}
- // 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;
+ // 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;
}
- break;
- case LANDING_ROLL:
- inAir = false;
- track = rwy.hdg;
- double dveldt = -5.0;
- vel += dveldt * dt;
- if(vel <= 15.0) {
- leg = TAKEOFF_ROLL;
+ //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;
+ }
+ }
}
- 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);
+// 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::TransmitPatternPositionReport(void) {
- // airport name + "traffic" + airplane callsign + pattern direction + pattern leg + rwy + ?
- string trns = "";
+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!!
- 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);
+ Point3D orthopos = ortho.ConvertToLocal(pos); // ortho position of the plane
+ desiredTaxiHeading = GetHeadingFromTo(pos, nextTaxiNode->pos);
- // And add the airport name again
- trns += tower->get_name();
-
- Transmit(trns);
+ // 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);
+ }
}
+
#include <plib/sg.h>
#include <plib/ssg.h>
#include <simgear/math/point3d.hxx>
+#include <Main/fg_props.hxx>
#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.
-
- // 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.
-
- // 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;
-
- // environment - some of this might get moved into FGAIPlane
- double wind_from_hdg; // degrees
- double 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
-
- void FlyTrafficPattern(double dt);
-
- // TODO - need to add something to define what option we are flying - Touch and go / Stop and go / Landing properly / others?
-
- void TransmitPatternPositionReport();
-
- void CalculateStartofDescent();
+ // 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
+
+ // 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
+
+ void FlyTrafficPattern(double dt);
+
+ // TODO - need to add something to define what option we are flying - Touch and go / Stop and go / Landing properly / others?
+
+ void TransmitPatternPositionReport();
+
+ void CalculateStartofDescent();
+
+ void ExitRunway(Point3D orthopos);
+
+ void StartTaxi();
+
+ void Taxi(double dt);
+
+ void GetNextTaxiNode();
};
#endif // _FG_AILocalTraffic_HXX
projects / flightgear.git / commitdiff