groundOffset = 0;
fp = 0;
+ controller = 0;
+ prevController = 0;
dt_count = 0;
dt_elev_count = 0;
use_perf_vs = true;
AccelTo( props->getDoubleValue("controls/flight/target-spd" ) );
}
+ if (controller)
+ {
+ controller->update(getID(),
+ pos.getLatitudeDeg(),
+ pos.getLongitudeDeg(),
+ hdg,
+ speed,
+ altitude_ft);
+ //if (controller->hasInstruction(getID()))
+ // {
+ processATC(controller->getInstruction(getID()));
+ // }
+ }
double turn_radius_ft;
double turn_circum_ft;
void FGAIAircraft::ProcessFlightPlan( double dt, time_t now ) {
bool eraseWaypoints;
if (trafficRef) {
-// FGAirport *arr;
-// FGAirport *dep;
eraseWaypoints = true;
-// cerr << trafficRef->getRegistration();
-// cerr << "Departure airport " << endl;
-// dep = trafficRef->getDepartureAirport();
-// if (dep)
-// cerr << dep->getId() << endl;
-// cerr << "Arrival airport " << endl;
-// arr = trafficRef->getArrivalAirport();
-// if (arr)
-// cerr << arr->getId() <<endl << endl;;
} else
eraseWaypoints = false;
- //cerr << "Processing Flightplan" << endl;
+
FGAIFlightPlan::waypoint* prev = 0; // the one behind you
FGAIFlightPlan::waypoint* curr = 0; // the one ahead
FGAIFlightPlan::waypoint* next = 0; // the next plus 1
next = fp->getNextWaypoint();
dt_count += dt;
- if (!prev) { //beginning of flightplan, do this initialization once
- //setBank(0.0);
- spinCounter = 0;
- tempReg = "";
- //prev_dist_to_go = HUGE;
- //cerr << "Before increment " << curr-> name << endl;
- fp->IncrementWaypoint(eraseWaypoints);
- //prev = fp->getPreviousWaypoint(); //first waypoint
- //curr = fp->getCurrentWaypoint(); //second waypoint
- //next = fp->getNextWaypoint(); //third waypoint (might not exist!)
- //cerr << "After increment " << prev-> name << endl;
- if (!(fp->getNextWaypoint()) && trafficRef)
- loadNextLeg();
+ ///////////////////////////////////////////////////////////////////////////
+ // Initialize the flightplan
+ //////////////////////////////////////////////////////////////////////////
+ if (!prev) {
- //cerr << "After load " << prev-> name << endl;
- prev = fp->getPreviousWaypoint(); //first waypoint
- curr = fp->getCurrentWaypoint(); //second waypoint
- next = fp->getNextWaypoint(); //third waypoint (might not exist!)
- //cerr << "After load " << prev-> name << endl;
- setLatitude(prev->latitude);
- setLongitude(prev->longitude);
- setSpeed(prev->speed);
- setAltitude(prev->altitude);
-
- if (prev->speed > 0.0)
- setHeading(fp->getBearing(prev->latitude, prev->longitude, curr));
- else
- setHeading(fp->getBearing(curr->latitude, curr->longitude, prev));
-
- // If next doesn't exist, as in incrementally created flightplans for
- // AI/Trafficmanager created plans,
- // Make sure lead distance is initialized otherwise
- if (next)
- fp->setLeadDistance(speed, hdg, curr, next);
-
- if (curr->crossat > -1000.0) { //use a calculated descent/climb rate
- use_perf_vs = false;
- tgt_vs = (curr->crossat - prev->altitude)
- / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
- / 6076.0 / prev->speed*60.0);
- tgt_altitude_ft = curr->crossat;
- } else {
- use_perf_vs = true;
- tgt_altitude_ft = prev->altitude;
- }
- alt_lock = hdg_lock = true;
- no_roll = prev->on_ground;
- if (no_roll) {
- Transform(); // make sure aip is initialized.
- getGroundElev(60.1); // make sure it's exectuted first time around, so force a large dt value
- //getGroundElev(60.1); // Need to do this twice.
- //cerr << trafficRef->getRegistration() << " Setting altitude to " << tgt_altitude << endl;
- doGroundAltitude(); //(tgt_altitude);
- }
- prevSpeed = 0;
- //cout << "First waypoint: " << prev->name << endl;
- //cout << " Target speed: " << tgt_speed << endl;
- //cout << " Target altitude: " << tgt_altitude << endl;
- //cout << " Target heading: " << tgt_heading << endl << endl;
- //cerr << "Done Flightplan init" << endl;
- return;
+ spinCounter = 0;
+ tempReg = "";
+ fp->IncrementWaypoint(eraseWaypoints);
+ if (!(fp->getNextWaypoint()) && trafficRef)
+ loadNextLeg();
+
+ prev = fp->getPreviousWaypoint(); //first waypoint
+ curr = fp->getCurrentWaypoint(); //second waypoint
+ next = fp->getNextWaypoint(); //third waypoint (might not exist!)
+
+ setLatitude(prev->latitude);
+ setLongitude(prev->longitude);
+ setSpeed(prev->speed);
+ setAltitude(prev->altitude);
+
+ if (prev->speed > 0.0)
+ setHeading(fp->getBearing(prev->latitude, prev->longitude, curr));
+ else
+ setHeading(fp->getBearing(curr->latitude, curr->longitude, prev));
+
+ // If next doesn't exist, as in incrementally created flightplans for
+ // AI/Trafficmanager created plans,
+ // Make sure lead distance is initialized otherwise
+ if (next)
+ fp->setLeadDistance(speed, hdg, curr, next);
+
+ if (curr->crossat > -1000.0) { //use a calculated descent/climb rate
+ use_perf_vs = false;
+ tgt_vs = (curr->crossat - prev->altitude)
+ / (fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr)
+ / 6076.0 / prev->speed*60.0);
+ tgt_altitude_ft = curr->crossat;
+ } else {
+ use_perf_vs = true;
+ tgt_altitude_ft = prev->altitude;
+ }
+ alt_lock = hdg_lock = true;
+ no_roll = prev->on_ground;
+ if (no_roll) {
+ Transform(); // make sure aip is initialized.
+ getGroundElev(60.1); // make sure it's exectuted first time around, so force a large dt value
+ doGroundAltitude();
+ }
+ // Make sure to announce the aircraft's position
+ announcePositionToController();
+ prevSpeed = 0;
+ return;
} // end of initialization
-
- // let's only process the flight plan every 100 ms.
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Check Execution time (currently once every 100 ms
+ ///////////////////////////////////////////////////////////////////////////
if ((dt_count < 0.1) || (now < fp->getStartTime())) {
- //cerr << "done fp dt" << endl;
- return;
+ //cerr << "done fp dt" << endl;
+ return;
} else {
- dt_count = 0;
+ dt_count = 0;
}
// check to see if we've reached the lead point for our next turn
double dist_to_go = fp->getDistanceToGo(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr);
lead_dist = fabs(2*speed); //don't skip over the waypoint
//cerr << "Extending lead distance to " << lead_dist << endl;
}
-// FGAirport * apt = trafficRef->getDepartureAirport();
-// if ((dist_to_go > prev_dist_to_go) && trafficRef && apt) {
-// if (apt->getId() == string("EHAM"))
-// cerr << "Alert: " << trafficRef->getRegistration() << " is moving away from waypoint " << curr->name << endl
-// << "Target heading : " << tgt_heading << "act heading " << hdg << " Tgt speed : " << tgt_speed << endl
-// << "Lead distance : " << lead_dist << endl
-// << "Distance to go: " << dist_to_go << endl;
-// }
-
- prev_dist_to_go = dist_to_go;
- //cerr << "2" << endl;
- //if (no_roll)
- // lead_dist = 10.0;
- //cout << "Leg : " << (fp->getLeg()-1) << ". dist_to_go: " << dist_to_go << ", lead_dist: "
- // << lead_dist << ", tgt_speed " << tgt_speed << ", tgt_heading " << tgt_heading
- // << " speed " << speed << " hdg " << hdg << ". Altitude " << altitude << " TAget alt :"
- // << tgt_altitude << endl;
+
+ //prev_dist_to_go = dist_to_go;
if ( dist_to_go < lead_dist ) {
- //prev_dist_to_go = HUGE;
- // For traffic manager generated aircraft:
- // check if the aircraft flies of of user range. And adjust the
- // Current waypoint's elevation according to Terrain Elevation
if (curr->finished) { //end of the flight plan
if (fp->getRepeat())
fp->restart();
else
setDie(true);
-
- //cerr << "Done die end of fp" << endl;
return;
}
- // we've reached the lead-point for the waypoint ahead
- //cerr << "4" << endl;
- //cerr << "Situation after lead point" << endl;
- //cerr << "Prviious: " << prev->name << endl;
- //cerr << "Current : " << curr->name << endl;
- //cerr << "Next : " << next->name << endl;
if (next) {
tgt_heading = fp->getBearing(curr, next);
spinCounter = 0;
next = fp->getNextWaypoint();
// Now that we have incremented the waypoints, excute some traffic manager specific code
- // based on the name of the waypoint we just passed.
if (trafficRef) {
double userLatitude = fgGetDouble("/position/latitude-deg");
double userLongitude = fgGetDouble("/position/longitude-deg");
user.CourseAndDistance(current, &course, &distance);
if ((distance * SG_METER_TO_NM) > TRAFFICTOAIDIST) {
setDie(true);
- //cerr << "done fp die out of range" << endl;
return;
}
FGAirport * dep = trafficRef->getDepartureAirport();
FGAirport * arr = trafficRef->getArrivalAirport();
- // At parking the beginning of the airport
if (!( dep && arr)) {
setDie(true);
return;
}
-
- //if ((dep->getId() == string("EHAM") || (arr->getId() == string("EHAM")))) {
- // cerr << trafficRef->getRegistration()
- // << " Enroute from " << dep->getId()
- // << " to " << arr->getId()
- // << " just crossed " << prev->name
- // << " Assigned rwy " << fp->getRunwayId()
- // << " " << fp->getRunway() << endl;
- // }
- //if ((dep->getId() == string("EHAM")) && (prev->name == "park2")) {
- // cerr << "Schiphol ground "
- // << trafficRef->getCallSign();
- // if (trafficRef->getHeavy())
- // cerr << "Heavy";
- // cerr << ", is type "
- // << trafficRef->getAircraft()
- // << " ready to go. IFR to "
- // << arr->getId() <<endl;
- // }
-
+ // This waypoint marks the fact that the aircraft has passed the initial taxi
+ // departure waypoint, so it can release the parking.
if (prev->name == "park2")
dep->getDynamics()->releaseParking(fp->getGate());
-
- // Some debug messages, specific to testing the Logical networks.
- // if ((arr->getId() == string("EHAM")) && (prev->name == "Center")) {
- // cerr << "Schiphol ground "
- // << trafficRef->getRegistration() << " "
- // << trafficRef->getCallSign();
- // if (trafficRef->getHeavy())
- // cerr << "Heavy";
- // cerr << ", arriving from " << dep->getName() ;
- // cerr << " landed runway "
- // << fp->getRunway()
- // << " request taxi to gate "
- // << arr->getDynamics()->getParkingName(fp->getGate())
- // << endl;
- // }
- if (prev->name == "END")
- fp->setTime(trafficRef->getDepartureTime());
- //cerr << "5" << endl;
+ // This is the last taxi waypoint, and marks the the end of the flight plan
+ // so, the schedule should update and wait for the next departure time.
+ if (prev->name == "END") {
+ // make sure to wait at least 20 minutes at parking to prevent "nervous" taxi behavior
+ // delayed aircraft.
+ time_t nextDeparture = trafficRef->getDepartureTime();
+ if (nextDeparture < (now+1200)) {
+ nextDeparture = now + 1200;
+ }
+ fp->setTime(trafficRef->getDepartureTime());
+ }
+ announcePositionToController();
+
}
if (next) {
}
}
+void FGAIAircraft::initializeFlightPlan()
+{
+
+}
+
bool FGAIAircraft::_getGearDown() const {
return ((props->getFloatValue("position/altitude-agl-ft") < 900.0)
altitude_ft += 0.1 * ((tgt_altitude_ft+groundOffset) - altitude_ft);
}
+
+void FGAIAircraft::announcePositionToController()
+{
+ if (trafficRef) {
+ //FGTaxiRoute *taxiRoute = fp->getTaxiRoute();
+
+ int leg = fp->getLeg();
+
+ // For starters, I'll only do this for departure and arrival taxi. The mechanism
+ // could be extended to include any controller however.
+ //int node, currentTaxiSegment;
+ //if (taxiRoute->next(&node, ¤tTaxiSegment)) {
+ if (fp->getCurrentWaypoint()->routeIndex != 0) {
+ //char buffer[10];
+ //snprintf (buffer, 10, "%d", node);
+ switch (leg) {
+ case 3:
+ cerr << trafficRef->getRegistration()
+ << " taxiing to runway at segment "
+ << fp->getCurrentWaypoint()->routeIndex
+ << endl;
+ //cerr << "Match check between taxisegment and taxiroute : " << node << " "
+ // << fp->getCurrentWaypoint()->name << endl;
+ if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists())
+ controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork();
+ break;
+ case 9:
+ cerr << trafficRef->getRegistration()
+ << " taxiing to parking at segment "
+ << fp->getCurrentWaypoint()->routeIndex
+ << endl;
+ if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists())
+ controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork();
+ break;
+ default:
+ controller = 0;
+ break;
+ }
+ } else {
+ //fp->deleteTaxiRoute();
+ controller = 0;
+ }
+ if ((controller != prevController) && (prevController != 0)) {
+ prevController->signOff(getID());
+ cerr << trafficRef->getRegistration()
+ << " signing off " << endl;
+ }
+ prevController = controller;
+ if (controller) {
+ controller->announcePosition(getID(), fp, fp->getCurrentWaypoint()->routeIndex,
+ _getLatitude(), _getLongitude(), hdg, speed, altitude_ft,
+ trafficRef->getRadius());
+ }
+ }
+}
+
+
+void FGAIAircraft::processATC(FGATCInstruction instruction)
+{
+ //cerr << "Processing ATC instruction (not Implimented yet)" << endl;
+ if (instruction.getHoldPattern ()) {
+ }
+ if (instruction.getHoldPosition ()) {
+ }
+ if (instruction.getChangeSpeed ()) {
+ AccelTo(instruction.getSpeed());
+ }else {
+ if (fp) AccelTo(fp->getPreviousWaypoint()->speed);
+ }
+ if (instruction.getChangeHeading ()) {
+ hdg_lock = false;
+ TurnTo(instruction.getHeading());
+ } else {
+ if (fp) {hdg_lock = true;}
+ }
+ if (instruction.getChangeAltitude()) {
+ }
+
+}
void SetPerformance(const PERF_STRUCT *ps);
void setFlightPlan(const std::string& fp, bool repat = false);
void SetFlightPlan(FGAIFlightPlan *f);
+ void initializeFlightPlan();
FGAIFlightPlan* GetFlightPlan() const { return fp; };
void AccelTo(double speed);
void PitchTo(double angle);
void setAcType(const string& ac) { acType = ac; };
void setCompany(const string& comp) { company = comp;};
+ void announcePositionToController();
+ void processATC(FGATCInstruction instruction);
+
inline void SetTanker(bool setting) { isTanker = setting; };
virtual const char* getTypeString(void) const { return "aircraft"; }
private:
FGAISchedule *trafficRef;
+ FGATCController *controller, *prevController;
bool hdg_lock;
bool alt_lock;
FGAIFlightPlan::~FGAIFlightPlan()
{
deleteWaypoints();
- //waypoints.clear();
- //while (waypoints.begin() != waypoints.end())
- // {
- // delete *(waypoints.begin());
- // waypoints.erase (waypoints.begin());
- // }
if (taxiRoute)
delete taxiRoute;
}
{
wpt_iterator = waypoints.begin();
}
+
+
+void FGAIFlightPlan::deleteTaxiRoute()
+{
+ if (taxiRoute)
+ delete taxiRoute;
+ taxiRoute = 0;
+}
+
+
+int FGAIFlightPlan::getRouteIndex(int i) {
+ if ((i > 0) && (i < waypoints.size())) {
+ return waypoints[i]->routeIndex;
+ }
+ else
+ return 0;
+}
bool gear_down;
bool flaps_down;
bool on_ground;
+ int routeIndex; // For AI/ATC purposes;
} waypoint;
FGAIFlightPlan(const string& filename);
void setRepeat(bool r) { repeat = r; };
bool getRepeat(void) const { return repeat; };
void restart(void);
+ int getNrOfWayPoints() { return waypoints.size(); };
+ int getRouteIndex(int i); // returns the AI related index of this current routes.
+ FGTaxiRoute *getTaxiRoute() { return taxiRoute; };
+ void deleteTaxiRoute();
- int getNrOfWayPoints() { return waypoints.end() - waypoints.begin(); };
private:
FGRunway rwy;
int currWpt = wpt_iterator - waypoints.begin();
switch(legNr)
{
- case 1:
+ case 1:
createPushBack(firstFlight,dep, latitude, longitude,
radius, fltType, aircraftType, airline);
break;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+
waypoints.push_back(wpt);
- // Add park twice, because it uses park once for initialization and once
- // to trigger the departure ATC message
geo_direct_wgs_84 ( 0, lat, lon, heading,
10,
&lat2, &lon2, &az2 );
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
geo_direct_wgs_84 ( 0, lat, lon, heading,
2.2*radius,
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
//Add the runway startpoint;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
} else {
int node;
isPushBackPoint = false;
taxiRoute->next(&node);
}
- }
- else {
+ } else {
//chop off the first two waypoints, because
// those have already been created
// by create pushback
taxiRoute->next(&node);
}
}
- while(taxiRoute->next(&node))
+ int route;
+ while(taxiRoute->next(&node, &route))
{
//FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd();
+ char buffer[10];
+ snprintf (buffer, 10, "%d", node);
FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
//ids.pop_back();
wpt = new waypoint;
- wpt->name = "taxiway"; // fixme: should be the name of the taxiway
+ wpt->name = string(buffer); // fixme: should be the name of the taxiway
wpt->latitude = tn->getLatitude();
wpt->longitude = tn->getLongitude();
// Elevation is currently disregarded when on_ground is true
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = route;
waypoints.push_back(wpt);
}
//cerr << endl;
// finally, rewind the taxiRoute object to the point where we started
+ // generating the Flightplan, for AI use.
+ // This is a bit tricky, because the
taxiRoute->first();
if (firstFlight) {
- for (int i = 0; i < nrWaypointsToSkip-2; i++) {
+ for (int i = 0; i < nrWaypointsToSkip-1; i++) {
taxiRoute->next(&node);
}
} else {
int size = taxiRoute->size();
if (size > 2) {
- taxiRoute->next(&node);
- taxiRoute->next(&node);
+ //taxiRoute->next(&node);
+ //taxiRoute->next(&node);
+ //taxiRoute->next(&node);
}
}
- }
+ } // taxiRoute not empty
}
else
{
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
//Add the runway startpoint;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
}
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
//Add the runway startpoint;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
} else {
int node;
int size = taxiRoute->size();
// Omit the last two waypoints, as
// those are created by createParking()
+ int route;
for (int i = 0; i < size-2; i++)
{
- taxiRoute->next(&node);
+ taxiRoute->next(&node, &route);
+ char buffer[10];
+ snprintf (buffer, 10, "%d", node);
//FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
wpt = new waypoint;
- wpt->name = "taxiway"; // fixme: should be the name of the taxiway
+ //wpt->name = "taxiway"; // fixme: should be the name of the taxiway
+ wpt->name = string(buffer);// fixme: should be the name of the taxiway
wpt->latitude = tn->getLatitude();
wpt->longitude = tn->getLongitude();
wpt->altitude = apt->getElevation();
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = route;
waypoints.push_back(wpt);
}
+ //taxiRoute->first();
+ //taxiRoute->next(&node);
}
}
else
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
wpt = new waypoint;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
//waypoint* wpt;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
lat = lat2;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= false;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
// Three thousand ft. Slowing down to 160 kts
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
/*******************************************************************
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
//Full stop at the runway centerpoint
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading,
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
geo_direct_wgs_84 ( 0, lat, lon, heading,
0.1 *radius,
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
wpt = new waypoint;
wpt->flaps_down= true;
wpt->finished = false;
wpt->on_ground = true;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
wpt->flaps_down= false;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
wpt->flaps_down= false;
wpt->finished = false;
wpt->on_ground = false;
+ wpt->routeIndex = 0;
waypoints.push_back(wpt);
}
//#include <Main/fg_props.hxx>
//#include <Airports/runways.hxx>
+#include <AIModel/AIFlightPlan.hxx>
+
//#include STL_STRING
#include "groundnetwork.hxx"
0);
first.CourseAndDistance(second, &course, &length);
}
-
+/***************************************************************************
+ * FGTaxiRoute
+ **************************************************************************/
bool FGTaxiRoute::next(int *nde)
{
//for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
if (currNode == nodes.end())
return false;
*nde = *(currNode);
+ if (currNode != nodes.begin()) // make sure route corresponds to the end node
+ currRoute++;
currNode++;
- currRoute++;
return true;
};
if (currNode == nodes.end())
return false;
*nde = *(currNode);
- *rte = *(currRoute);
+ //*rte = *(currRoute);
+ if (currNode != nodes.begin()) // Make sure route corresponds to the end node
+ {
+ *rte = *(currRoute);
+ currRoute++;
+ }
+ else
+ {
+ // If currNode points to the first node, this means the aircraft is not on the taxi node
+ // yet. Make sure to return a unique identifyer in this situation though, because otherwise
+ // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
+ // taxi route or not. the negative of the preceding route seems a logical choice, as it is
+ // unique for any starting location.
+ // Note that this is probably just a temporary fix until I get Parking / tower control working.
+ *rte = -1 * *(currRoute);
+ }
currNode++;
- currRoute++;
return true;
};
+
+void FGTaxiRoute::rewind(int route)
+{
+ int currPoint;
+ int currRoute;
+ first();
+ do {
+ if (!(next(&currPoint, &currRoute))) {
+ SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
+ << " goal " << route);
+ }
+ } while (currRoute != route);
+}
+
+/***************************************************************************
+ * FGTrafficRecord
+ **************************************************************************/
+void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route)
+{
+
+ currentPos = pos;
+ if (intentions.size()) {
+ if (*intentions.begin() != pos) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions");
+ cerr << "Pos : " << pos << " Curr " << *(intentions.begin()) << endl;
+ for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) {
+ cerr << (*i) << " ";
+ }
+ cerr << endl;
+ }
+ intentions.erase(intentions.begin());
+ } else {
+ //int legNr, routeNr;
+ //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
+ int size = route->getNrOfWayPoints();
+ cerr << "Setting pos" << pos << " ";
+ cerr << "setting intentions ";
+ for (int i = 0; i < size; i++) {
+ int val = route->getRouteIndex(i);
+
+ if ((val) && (val != pos))
+ {
+ intentions.push_back(val);
+ cerr << val<< " ";
+ }
+ }
+ cerr << endl;
+ //while (route->next(&legNr, &routeNr)) {
+ //intentions.push_back(routeNr);
+ //}
+ //route->rewind(currentPos);
+ }
+ //exit(1);
+}
+
+bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other)
+{
+ bool result = false;
+ //cerr << "Start check 1" << endl;
+ if (currentPos == other.currentPos)
+ {
+ //cerr << "Check Position and intentions: current matches" << endl;
+ result = true;
+ }
+ // else if (other.intentions.size())
+// {
+// cerr << "Start check 2" << endl;
+// intVecIterator i = other.intentions.begin();
+// while (!((i == other.intentions.end()) || ((*i) == currentPos)))
+// i++;
+// if (i != other.intentions.end()) {
+// cerr << "Check Position and intentions: current matches other.intentions" << endl;
+// result = true;
+// }
+ else if (intentions.size()) {
+ //cerr << "Start check 3" << endl;
+ intVecIterator i = intentions.begin();
+ while (!((i == intentions.end()) || ((*i) == other.currentPos)))
+ i++;
+ if (i != intentions.end()) {
+ //cerr << "Check Position and intentions: .other.current matches" << endl;
+ result = true;
+ }
+ }
+ //cerr << "Done !!" << endl;
+ return result;
+}
+
+void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
+ double spd, double alt)
+{
+ latitude = lat;
+ longitude = lon;
+ heading = hdg;
+ speed = spd;
+ altitude = alt;
+}
+
/***************************************************************************
* FGGroundNetwork()
**************************************************************************/
foundRoute = false;
totalDistance = 0;
maxDistance = 0;
+ currTraffic = activeTraffic.begin();
}
void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
void FGGroundNetwork::init()
{
hasNetwork = true;
- int index = 0;
+ int index = 1;
FGTaxiSegmentVectorIterator i = segments.begin();
while(i != segments.end()) {
//cerr << "initializing node " << i->getIndex() << endl;
}
//exit(1);
}
+
+
+void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
+ double lat, double lon, double heading,
+ double speed, double alt, double radius)
+{
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id alread has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ while ((i->getId() != id) && i != activeTraffic.end()) {
+ i++;
+ }
+ }
+ // Add a new TrafficRecord if no one exsists for this aircraft.
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ FGTrafficRecord rec;
+ rec.setId(id);
+ rec.setPositionAndIntentions(currentPosition, intendedRoute);
+ rec.setPositionAndHeading(lat, lon, heading, speed, alt);
+ rec.setRadius(radius); // only need to do this when creating the record.
+ activeTraffic.push_back(rec);
+ } else {
+ i->setPositionAndIntentions(currentPosition, intendedRoute);
+ i->setPositionAndHeading(lat, lon, heading, speed, alt);
+ }
+}
+
+void FGGroundNetwork::signOff(int id) {
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id alread has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ while ((i->getId() != id) && i != activeTraffic.end()) {
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
+ } else {
+ i = activeTraffic.erase(i);
+ }
+}
+
+void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) {
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ TrafficVectorIterator current, closest;
+ if (activeTraffic.size()) {
+ while ((i->getId() != id) && i != activeTraffic.end()) {
+ i++;
+ }
+ }
+ // update position of the current aircraft
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
+ } else {
+ i->setPositionAndHeading(lat, lon, heading, speed, alt);
+ current = i;
+ }
+
+ // Scan for a speed adjustment change. Find the nearest aircraft that is in front
+ // and adjust speed when we get too close. Only do this when current position and/or
+ // intentions of the current aircraft match current taxiroute position of the proximate
+ // aircraft.
+ double mindist = HUGE;
+ if (activeTraffic.size())
+ {
+ double course, dist, bearing, minbearing;
+
+ //TrafficVector iterator closest;
+ for (TrafficVectorIterator i = activeTraffic.begin();
+ i != activeTraffic.end(); i++)
+ {
+ if (i != current) {
+ SGWayPoint curr (lon,
+ lat,
+ alt);
+ SGWayPoint other (i->getLongitude (),
+ i->getLatitude (),
+ i->getAltitude ());
+ other.CourseAndDistance(curr, &course, &dist);
+ bearing = fabs(heading-course);
+ if (bearing > 180)
+ bearing = 360-bearing;
+ if ((dist < mindist) && (bearing < 60.0))
+ {
+ mindist = dist;
+ closest = i;
+ minbearing = bearing;
+ }
+ }
+ }
+ //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
+ // << " course : " << course << endl;
+ current->clearSpeedAdjustment();
+ // Only clear the heading adjustment at positive speeds, otherwise the waypoint following
+ // code wreaks havoc
+ if (speed > 0.2)
+ current->clearHeadingAdjustment();
+ // All clear
+ if (mindist > 100)
+ {
+ //current->clearSpeedAdjustment();
+ //current->clearHeadingAdjustment();
+ }
+ else
+ {
+ if (current->getId() == closest->getWaitsForId())
+ return;
+ else
+ current->setWaitsForId(closest->getId());
+
+ // Getting close: Slow down to a bit less than the other aircraft
+ double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
+ if (mindist > maxAllowableDistance)
+ {
+ if (current->checkPositionAndIntentions(*closest))
+ {
+ // Adjust speed, but don't let it drop to below 1 knots
+ //if (fabs(speed) > 1)
+ if (!(current->hasHeadingAdjustment()))
+ {
+ current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
+ //cerr << "Adjusting speed to " << closest->getSpeed() * (mindist / 100) << " "
+ // << "Bearing = " << minbearing << " Distance = " << mindist
+ // << " Latitude = " <<lat << " longitude = " << lon << endl;
+ //<< " Latitude = " <<closest->getLatitude()
+ //<< " longitude = " << closest->getLongitude()
+ // << endl;
+ }
+ else
+ {
+ double newSpeed = (maxAllowableDistance-mindist);
+ current->setSpeedAdjustment(newSpeed);
+ }
+ }
+ }
+ else
+ {
+ if (!(current->hasHeadingAdjustment()))
+ {
+ double newSpeed;
+ if (mindist > 10) {
+ newSpeed = 0.01;
+ current->setSpeedAdjustment(newSpeed);
+ } else {
+ newSpeed = -1 * (maxAllowableDistance-mindist);
+ current->setSpeedAdjustment(newSpeed);
+ current->setHeadingAdjustment(heading);
+ // if (mindist < 5) {
+ // double bank_sense = 0;
+ // current->setSpeedAdjustment(-0.1);
+ // // Do a heading adjustment
+ // double diff = fabs(heading - bearing);
+ // if (diff > 180)
+ // diff = fabs(diff - 360);
+
+ // double sum = heading + diff;
+ // if (sum > 360.0)
+ // sum -= 360.0;
+ // if (fabs(sum - bearing) < 1.0) {
+ // bank_sense = -1.0; // turn left for evasive action
+ // } else {
+ // bank_sense = 1.0; // turn right for evasive action
+ // }
+ // double newHeading = heading + bank_sense;
+ // if (newHeading < 0) newHeading += 360;
+ // if (newHeading > 360) newHeading -= 360;
+ // current->setHeadingAdjustment(newHeading);
+ // //cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading
+ // // << endl;
+ // cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId()
+ // << endl;
+ // }
+ }
+ }
+ }
+ }
+ }
+}
+
+bool FGGroundNetwork::hasInstruction(int id)
+{
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ while ((i->getId() != id) && i != activeTraffic.end()) {
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
+ } else {
+ return i->hasInstruction();
+ }
+}
+
+FGATCInstruction FGGroundNetwork::getInstruction(int id)
+{
+ TrafficVectorIterator i = activeTraffic.begin();
+ // Search search if the current id has an entry
+ // This might be faster using a map instead of a vector, but let's start by taking a safe route
+ if (activeTraffic.size()) {
+ while ((i->getId() != id) && i != activeTraffic.end()) {
+ i++;
+ }
+ }
+ if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
+ SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
+ } else {
+ return i->getInstruction();
+ }
+}
+
+
+
+
+/***************************************************************************
+ * FGATCInstruction
+ *
+ * This class is really out of place here, and should be combined with
+ * FGATC controller and go into it's own file / directory
+ * I'm leaving it for now though, because I'm testing this stuff quite
+ * heavily.
+ **************************************************************************/
+FGATCInstruction::FGATCInstruction()
+{
+ holdPattern = false;
+ holdPosition = false;
+ changeSpeed = false;
+ changeHeading = false;
+ changeAltitude = false;
+
+ double speed = 0;
+ double heading = 0;
+ double alt = 0;
+}
+
+bool FGATCInstruction::hasInstruction()
+{
+ return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
+}
+
+
#include <simgear/compiler.h>
+
#include STL_STRING
#include <vector>
SG_USING_STD(string);
SG_USING_STD(vector);
+
#include "parking.hxx"
+//#include <AIModel/AIBase.hxx>
+
+
class FGTaxiSegment; // forward reference
+class FGAIFlightPlan; // forward reference
typedef vector<FGTaxiSegment> FGTaxiSegmentVector;
typedef vector<FGTaxiSegment*> FGTaxiSegmentPointerVector;
bool empty () { return nodes.begin() == nodes.end(); };
bool next(int *nde);
bool next(int *nde, int *rte);
+ void rewind(int legNr);
void first() { currNode = nodes.begin(); currRoute = routes.begin(); };
int size() { return nodes.size(); };
typedef vector<FGTaxiRoute> TaxiRouteVector;
typedef vector<FGTaxiRoute>::iterator TaxiRouteVectorIterator;
+/**************************************************************************************
+ * class FGATCInstruction
+ * like class FGATC Controller, this class definition should go into its own file
+ * and or directory... For now, just testing this stuff out though...
+ *************************************************************************************/
+class FGATCInstruction
+{
+private:
+ bool holdPattern;
+ bool holdPosition;
+ bool changeSpeed;
+ bool changeHeading;
+ bool changeAltitude;
+
+ double speed;
+ double heading;
+ double alt;
+public:
+
+ FGATCInstruction();
+ bool hasInstruction ();
+ bool getHoldPattern () { return holdPattern; };
+ bool getHoldPosition () { return holdPosition; };
+ bool getChangeSpeed () { return changeSpeed; };
+ bool getChangeHeading () { return changeHeading; };
+ bool getChangeAltitude() { return changeAltitude; };
+
+ double getSpeed () { return speed; };
+ double getHeading () { return heading; };
+ double getAlt () { return alt; };
+
+ void setHoldPattern (bool val) { holdPattern = val; };
+ void setHoldPosition (bool val) { holdPosition = val; };
+ void setChangeSpeed (bool val) { changeSpeed = val; };
+ void setChangeHeading (bool val) { changeHeading = val; };
+ void setChangeAltitude(bool val) { changeAltitude = val; };
+
+ void setSpeed (double val) { speed = val; };
+ void setHeading (double val) { heading = val; };
+ void setAlt (double val) { alt = val; };
+};
+
+
+/**************************************************************************************
+ * class FGTrafficRecord
+ *************************************************************************************/
+class FGTrafficRecord
+{
+private:
+ int id, waitsForId;
+ int currentPos;
+ intVec intentions;
+ FGATCInstruction instruction;
+ double latitude, longitude, heading, speed, altitude, radius;
+
+
+public:
+ FGTrafficRecord() {};
+
+ void setId(int val) { id = val; };
+ void setRadius(double rad) { radius = rad;};
+ void setPositionAndIntentions(int pos, FGAIFlightPlan *route);
+ int getId() { return id;};
+ FGATCInstruction getInstruction() { return instruction;};
+ bool hasInstruction() { return instruction.hasInstruction(); };
+ void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt);
+ bool checkPositionAndIntentions(FGTrafficRecord &other);
+
+ double getLatitude () { return latitude ; };
+ double getLongitude() { return longitude; };
+ double getHeading () { return heading ; };
+ double getSpeed () { return speed ; };
+ double getAltitude () { return altitude ; };
+ double getRadius () { return radius ; };
+
+ int getWaitsForId () { return waitsForId; };
+
+ void setSpeedAdjustment(double spd) { instruction.setChangeSpeed(true);
+ instruction.setSpeed(spd); };
+ void setHeadingAdjustment(double heading) { instruction.setChangeHeading(true);
+ instruction.setHeading(heading); };
+ void clearSpeedAdjustment () { instruction.setChangeSpeed (false); };
+ void clearHeadingAdjustment() { instruction.setChangeHeading(false); };
+
+ bool hasHeadingAdjustment() { return instruction.getChangeHeading(); };
+
+ void setWaitsForId(int id) { waitsForId = id; };
+
+};
+
+typedef vector<FGTrafficRecord> TrafficVector;
+typedef vector<FGTrafficRecord>::iterator TrafficVectorIterator;
+
+
+
+
+/**************************************************************************************
+ * class FGATCController
+ * NOTE: this class serves as an abstraction layer for all sorts of ATC controller,
+ * Ground and air, so eventually it should move to its own file / directory.
+ *************************************************************************************/
+class FGATCController
+{
+private:
+ double dt_count;
+public:
+ FGATCController() { dt_count = 0;};
+ virtual ~FGATCController() {};
+ virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute,
+ double lat, double lon,
+ double hdg, double spd, double alt, double radius) = 0;
+ virtual void signOff(int id) = 0;
+ virtual void update(int id, double lat, double lon,
+ double heading, double speed, double alt) = 0;
+ virtual bool hasInstruction(int id) = 0;
+ virtual FGATCInstruction getInstruction(int id) = 0;
+
+ double getDt() { return dt_count; };
+ void setDt(double dt) { dt_count = dt;};
+};
+
+
+
/**************************************************************************************
* class FGGroundNetWork
*************************************************************************************/
-class FGGroundNetwork
+class FGGroundNetwork : public FGATCController
{
private:
bool hasNetwork;
intVec nodesStack;
intVec routesStack;
TaxiRouteVector routes;
+ TrafficVector activeTraffic;
+ TrafficVectorIterator currTraffic;
bool foundRoute;
double totalDistance, maxDistance;
FGTaxiSegment *findSegment(int idx);
FGTaxiRoute findShortestRoute(int start, int end);
void trace(FGTaxiNode *, int, int, double dist);
-
+
+ virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute,
+ double lat, double lon, double hdg, double spd, double alt, double radius);
+ virtual void signOff(int id);
+ virtual void update(int id, double lat, double lon, double heading, double speed, double alt);
+ virtual bool hasInstruction(int id);
+ virtual FGATCInstruction getInstruction(int id);
};
+
#endif
#include "groundnetwork.hxx"
#include "dynamics.hxx"
+
SG_USING_STD(string);
SG_USING_STD(map);
SG_USING_STD(set);
arrivalPort = globals->get_airports()->search(arrId);
if(arrivalPort == NULL)
{
- SG_LOG( SG_GENERAL, SG_WARN, "Traffic manager could not find arrival airort : " << arrId);
+ SG_LOG( SG_GENERAL, SG_WARN, "Traffic manager could not find arrival airport : " << arrId);
return false;
}
time_t
totalTimeEnroute,
elapsedTimeEnroute,
- remainingTimeEnroute;
+ remainingTimeEnroute, deptime = 0;
double
userLatitude,
userLongitude;
{
i->adjustTime(now);
}
+ if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true)
+ deptime = now;
firstRun = false;
}
// Because this is done at every update, we only need to check the status
// of the first listed flight.
sort(flights.begin(), flights.end());
+ if (!deptime)
+ deptime = flights.begin()->getDepartureTime();
FGScheduledFlightVecIterator i = flights.begin();
if (AIManagerRef)
{
aircraft->setAltitude(i->getCruiseAlt()*100); // convert from FL to feet
aircraft->setSpeed(speed);
aircraft->setBank(0);
- aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, i->getDepartureTime(), dep,
- arr,true, radius, i->getCruiseAlt()*100, lat, lon, speed, flightType, acType, airline));
+ aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
+ dep, arr,true, radius,
+ i->getCruiseAlt()*100,
+ lat, lon, speed, flightType, acType,
+ airline));
aimgr->attach(aircraft);
int, FGScheduledFlightVec); // construct & init
FGAISchedule(const FGAISchedule &other); // copy constructor
+
+
~FGAISchedule(); //destructor
bool update(time_t now);
projects / flightgear.git / commitdiff