fly/flightgear
1
0
Fork 0
Code Activity

Changes to support AI taxiing and crabing when flying in wind. Still a work in progress.

Browse source
This commit is contained in:
daveluff 2002-12-04 20:02:03 +00:00
parent 791caf9fbc
commit 9be0f4032d
2 changed files with 704 additions and 393 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

307
src/ATC/AILocalTraffic.cxx
View file

@ -52,8 +52,12 @@ FGAILocalTraffic::FGAILocalTraffic() {
nominal_final_speed = 65.0;
//nominal_approach_speed;
//stall_speed_landing_config;
wind_from_hdg = 0.0;
wind_speed_knots = 0.0;
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() {
@ -67,9 +71,44 @@ void FGAILocalTraffic::Init() {
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
@ -81,12 +120,14 @@ void FGAILocalTraffic::Init() {
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.
}
@ -97,6 +138,7 @@ void FGAILocalTraffic::Init() {
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
@ -124,20 +166,6 @@ void FGAILocalTraffic::Init() {
//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';
} else {
// Check CTAF, unicom etc
}
} 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
@ -154,10 +182,23 @@ void FGAILocalTraffic::Init() {
// Run the internal calculations
void FGAILocalTraffic::Update(double dt) {
// cout << "In FGAILocalTraffic::Update\n";
//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()
}
@ -185,6 +226,17 @@ void FGAILocalTraffic::FlyTrafficPattern(double dt) {
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';
// 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();
switch(leg) {
case TAKEOFF_ROLL:
inAir = false;
@ -193,7 +245,7 @@ void FGAILocalTraffic::FlyTrafficPattern(double dt) {
double dveldt = 5.0;
vel += dveldt * dt;
}
IAS = vel + (cos((hdg - wind_from_hdg) * DCL_DEGREES_TO_RADIANS) * wind_speed_knots);
IAS = vel + (cos((hdg - wind_from) * DCL_DEGREES_TO_RADIANS) * wind_speed);
if(IAS >= 70) {
leg = CLIMBOUT;
pitch = 10.0;
@ -325,18 +377,65 @@ void FGAILocalTraffic::FlyTrafficPattern(double dt) {
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);
// 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);
}
@ -382,10 +481,170 @@ void FGAILocalTraffic::TransmitPatternPositionReport(void) {
break;
}
// FIXME - I've hardwired the runway call as well!! (We could work this out from rwy heading and mag deviation)
trns += convertNumToSpokenString(1);
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);
}
}

58
src/ATC/AILocalTraffic.hxx
View file

@ -33,10 +33,12 @@
#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,
@ -52,6 +54,18 @@ typedef enum PatternLeg {
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;
@ -60,6 +74,7 @@ typedef struct RunwayDetails {
double mag_hdg;
double mag_var;
double hdg; // true runway heading
int ID; // 1 -> 36
};
typedef struct StartofDescent {
@ -81,17 +96,30 @@ public:
// 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);
// Do what is necessary to land and parkup at home airport
void ReturnToBase(double dt);
private:
// 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* airport; // The ICAO code of the airport that we're operating around
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
@ -112,10 +140,11 @@ private:
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
double wind_from_hdg; // degrees
double wind_speed_knots; // knots
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)
@ -124,6 +153,21 @@ private:
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?
@ -131,6 +175,14 @@ private:
void TransmitPatternPositionReport();
void CalculateStartofDescent();
void ExitRunway(Point3D orthopos);
void StartTaxi();
void Taxi(double dt);
void GetNextTaxiNode();
};
#endif // _FG_AILocalTraffic_HXX