1 /******************************************************************************
3 * Written by Durk Talsma, started May 5, 2004.
5 * This program is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License as
7 * published by the Free Software Foundation; either version 2 of the
8 * License, or (at your option) any later version.
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 ****************************************************************************
22 *****************************************************************************/
42 #include <simgear/compiler.h>
43 #include <simgear/math/polar3d.hxx>
44 #include <simgear/math/sg_geodesy.hxx>
45 #include <simgear/props/props.hxx>
46 #include <simgear/route/waypoint.hxx>
47 #include <simgear/structure/subsystem_mgr.hxx>
48 #include <simgear/xml/easyxml.hxx>
50 #include <AIModel/AIFlightPlan.hxx>
51 #include <AIModel/AIManager.hxx>
52 #include <AIModel/AIAircraft.hxx>
53 #include <Airports/simple.hxx>
54 #include <Main/fg_init.hxx> // That's pretty ugly, but I need fgFindAirportID
57 #include "SchedFlight.hxx"
58 #include "TrafficMgr.hxx"
62 /******************************************************************************
63 * the FGAISchedule class contains data members and code to maintain a
64 * schedule of Flights for an articically controlled aircraft.
65 *****************************************************************************/
66 FGAISchedule::FGAISchedule()
81 FGAISchedule::FGAISchedule(string mdl,
92 FGScheduledFlightVec flt)*/
93 FGAISchedule::FGAISchedule(string model,
110 flightIdentifier = flightId;
121 /*for (FGScheduledFlightVecIterator i = flt.begin();
124 flights.push_back(new FGScheduledFlight((*(*i))));*/
130 FGAISchedule::FGAISchedule(const FGAISchedule &other)
132 modelPath = other.modelPath;
133 homePort = other.homePort;
134 livery = other.livery;
135 registration = other.registration;
137 flightIdentifier = other.flightIdentifier;
138 flights = other.flights;
141 AIManagerRef = other.AIManagerRef;
142 acType = other.acType;
143 airline = other.airline;
144 m_class = other.m_class;
145 firstRun = other.firstRun;
146 radius = other.radius;
147 groundOffset = other.groundOffset;
148 flightType = other.flightType;
149 //score = other.score;
150 distanceToUser = other.distanceToUser;
151 currentDestination = other.currentDestination;
152 firstRun = other.firstRun;
156 FGAISchedule::~FGAISchedule()
158 /* for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++)
165 bool FGAISchedule::init()
168 //SGTime* currTimeDate = globals->get_time_params();
170 //tm *temp = currTimeDate->getGmt();
172 //sgTimeFormatTime(&targetTimeDate, buffer);
173 //cout << "Scheduled Time " << buffer << endl;
174 //cout << "Time :" << time(NULL) << " SGTime : " << sgTimeGetGMT(temp) << endl;
175 /*for (FGScheduledFlightVecIterator i = flights.begin();
179 //i->adjustTime(now);
180 if (!((*i)->initializeAirports()))
183 //sort(flights.begin(), flights.end());
184 // Since time isn't initialized yet when this function is called,
185 // Find the closest possible airport.
186 // This should give a reasonable initialization order.
187 //setClosestDistanceToUser();
191 bool FGAISchedule::update(time_t now)
200 double courseToUser, courseToDest;
201 double distanceToDest;
207 remainingTimeEnroute, deptime = 0;
212 if (fgGetBool("/sim/traffic-manager/enabled") == false)
215 aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
216 // Out-of-work aircraft seeks employment. Willing to work irregular hours ...
217 //cerr << "About to find a flight " << endl;
218 if (flights.empty()) {
219 //execute this loop at least once.
220 SG_LOG(SG_GENERAL, SG_INFO, "Scheduling for : " << modelPath << " " << registration << " " << homePort);
221 FGScheduledFlight *flight = 0;
223 flight = findAvailableFlight(currentDestination, flightIdentifier);
225 currentDestination = flight->getArrivalAirport()->getId();
227 dep = flight->getDepartureTime();
228 arr = flight->getArrivalTime();
229 string depT = asctime(gmtime(&dep));
230 string arrT = asctime(gmtime(&arr));
232 depT = depT.substr(0,24);
233 arrT = arrT.substr(0,24);
234 SG_LOG(SG_GENERAL, SG_INFO, " " << flight->getCallSign() << ":"
235 << " " << flight->getDepartureAirport()->getId() << ":"
236 << " " << depT << ":"
237 << " \"" << flight->getArrivalAirport()->getId() << "\"" << ":"
238 << " " << arrT << ":");
239 flights.push_back(flight);
241 } while ((currentDestination != homePort) && (flight != 0));
242 SG_LOG(SG_GENERAL, SG_INFO, cerr << " Done " << endl);
244 //cerr << " Done " << endl;
245 // No flights available for this aircraft
246 if (flights.size() == 0) {
249 // Sort all the scheduled flights according to scheduled departure time.
250 // Because this is done at every update, we only need to check the status
251 // of the first listed flight.
252 //sort(flights.begin(), flights.end(), compareScheduledFlights);
254 if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true) {
255 deptime = now + rand() % 300; // Wait up to 5 minutes until traffic starts moving to prevent too many aircraft
256 // from cluttering the gate areas.
257 cerr << "Scheduling " << registration << " for instantaneous action flight " << endl;
262 deptime = (*flights.begin())->getDepartureTime();
263 FGScheduledFlightVecIterator i = flights.begin();
264 SG_LOG (SG_GENERAL, SG_DEBUG,"Traffic Manager: Processing registration " << registration << " with callsign " << (*i)->getCallSign());
267 // Check if this aircraft has been released.
268 FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
269 if (tmgr->isReleased(AIManagerRef))
275 userLatitude = fgGetDouble("/position/latitude-deg");
276 userLongitude = fgGetDouble("/position/longitude-deg");
278 //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
279 // This flight entry is entirely in the past, do we need to
280 // push it forward in time to the next scheduled departure.
281 if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now))
283 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in the Past");
284 //cerr << modelPath << " " << registration << ": Flights from the past belong to the past :-)" << endl;
286 // Don't just update: check whether we need to load a new leg. etc.
287 // This update occurs for distant aircraft, so we can update the current leg
288 // and detach it from the current list of aircraft.
290 i = flights.erase(i);
294 // Departure time in the past and arrival time in the future.
295 // This flight is in progress, so we need to calculate it's
296 // approximate position and -if in range- create an AIAircraft
298 //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
300 // Part of this flight is in the future.
301 if ((*i)->getArrivalTime() > now)
304 dep = (*i)->getDepartureAirport();
305 arr = (*i)->getArrivalAirport ();
309 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
310 dep->getLatitude(), 1));
311 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
312 arr->getLatitude(), 1));
313 SGVec3d _cross = cross(b, a);
315 angle = sgACos(dot(a, b));
317 // Okay, at this point we have the angle between departure and
318 // arrival airport, in degrees. From here we can interpolate the
319 // position of the aircraft by calculating the ratio between
320 // total time enroute and elapsed time enroute.
322 totalTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
323 if (now > (*i)->getDepartureTime())
325 //err << "Lat = " << lat << ", lon = " << lon << endl;
326 //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
327 elapsedTimeEnroute = now - (*i)->getDepartureTime();
328 remainingTimeEnroute = (*i)->getArrivalTime() - now;
329 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in progress.");
333 lat = dep->getLatitude();
334 lon = dep->getLongitude();
335 elapsedTimeEnroute = 0;
336 remainingTimeEnroute = totalTimeEnroute;
337 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is pending.");
340 angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
343 //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
344 // << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
346 sgdMakeRotMat4(matrix, angle, _cross.sg());
347 SGVec3d newPos(0, 0, 0);
348 for(int j = 0; j < 3; j++)
350 for (int k = 0; k<3; k++)
352 newPos[j] += matrix[j][k]*a[k];
356 if (now > (*i)->getDepartureTime())
358 SGGeoc geoc = SGGeoc::fromCart(newPos);
359 lat = geoc.getLatitudeDeg();
360 lon = geoc.getLongitudeDeg();
364 lat = dep->getLatitude();
365 lon = dep->getLongitude();
369 SGWayPoint current (lon,
371 (*i)->getCruiseAlt(),
372 SGWayPoint::SPHERICAL);
373 SGWayPoint user ( userLongitude,
375 (*i)->getCruiseAlt(),
376 SGWayPoint::SPHERICAL);
377 SGWayPoint dest ( arr->getLongitude(),
379 (*i)->getCruiseAlt(),
380 SGWayPoint::SPHERICAL);
381 // We really only need distance to user
382 // and course to destination
383 user.CourseAndDistance(current, &courseToUser, &distanceToUser);
384 dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
385 speed = (distanceToDest*SG_METER_TO_NM) /
386 ((double) remainingTimeEnroute/3600.0);
389 // If distance between user and simulated aircaft is less
390 // then 500nm, create this flight. At jet speeds 500 nm is roughly
391 // one hour flight time, so that would be a good approximate point
392 // to start a more detailed simulation of this aircraft.
393 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: " << registration << " is scheduled for a flight from "
394 << dep->getId() << " to " << arr->getId() << ". Current distance to user: "
395 << distanceToUser*SG_METER_TO_NM);
396 if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDISTTOSTART)
398 string flightPlanName = dep->getId() + string("-") + arr->getId() +
400 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: Creating AIModel");
402 //if ((i->getDepartureTime() < now))
404 // alt = i->getCruiseAlt() *100;
408 // alt = dep->_elevation+19;
411 // Only allow traffic to be created when the model path (or the AI version of mp) exists
412 SGPath mp(globals->get_fg_root());
415 mp.append(modelPath);
417 mp_ai.append(modelPath);
419 if (mp.exists() || mp_ai.exists())
421 FGAIAircraft *aircraft = new FGAIAircraft(this);
422 aircraft->setPerformance(m_class); //"jet_transport";
423 aircraft->setCompany(airline); //i->getAirline();
424 aircraft->setAcType(acType); //i->getAcType();
425 aircraft->setPath(modelPath.c_str());
426 //aircraft->setFlightPlan(flightPlanName);
427 aircraft->setLatitude(lat);
428 aircraft->setLongitude(lon);
429 aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
430 aircraft->setSpeed(speed);
431 aircraft->setBank(0);
432 aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
433 dep, arr,true, radius,
434 (*i)->getCruiseAlt()*100,
435 lat, lon, speed, flightType, acType,
437 aimgr->attach(aircraft);
440 AIManagerRef = aircraft->getID();
441 //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
442 //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
443 //cerr << "Dep : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
444 //cerr << "Arr : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
445 //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
446 //cerr << "Total time = " << (totalTimeEnroute/3600.0) << endl;
447 //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
451 SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
457 // Both departure and arrival time are in the future, so this
458 // the aircraft is parked at the departure airport.
459 // Currently this status is mostly ignored, but in future
460 // versions, code should go here that -if within user range-
461 // positions these aircraft at parking locations at the airport.
462 if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
464 dep = (*i)->getDepartureAirport();
468 //cerr << "Traffic schedule got to beyond last clause" << endl;
469 // EMH: prevent a warning, should this be 'true' instead?
470 // DT: YES. Originally, this code couldn't be reached, but
471 // when the "if(!(AIManagerManager))" clause is false we
472 // fall through right to the end. This is a valid flow.
473 // the actual value is pretty innocent, only it triggers
474 // warning in TrafficManager::update().
475 // (which was added as a sanity check for myself in the first place. :-)
480 bool FGAISchedule::next()
482 FGScheduledFlightVecIterator i = flights.begin();
484 //FIXME: remove first entry,
485 // load new flights until back at home airport
486 // Lock loaded flights
487 //sort(flights.begin(), flights.end(), compareScheduledFlights);
489 i = flights.erase(i);
490 //cerr << "Next: scheduling for : " << modelPath << " " << registration << endl;
491 FGScheduledFlight *flight = findAvailableFlight(currentDestination, flightIdentifier);
493 currentDestination = flight->getArrivalAirport()->getId();
495 dep = flight->getDepartureTime();
496 arr = flight->getArrivalTime();
497 string depT = asctime(gmtime(&dep));
498 string arrT = asctime(gmtime(&arr));
500 depT = depT.substr(0,24);
501 arrT = arrT.substr(0,24);
502 //cerr << " " << flight->getCallSign() << ":"
503 // << " " << flight->getDepartureAirport()->getId() << ":"
504 // << " " << depT << ":"
505 // << " \"" << flight->getArrivalAirport()->getId() << "\"" << ":"
506 // << " " << arrT << ":" << endl;
508 flights.push_back(flight);
513 //cerr << "FGAISchedule :: next needs updating" << endl;
517 FGScheduledFlight* FGAISchedule::findAvailableFlight (const string ¤tDestination,
520 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
522 FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
523 FGScheduledFlightVecIterator fltBegin, fltEnd;
524 fltBegin = tmgr->getFirstFlight(req);
525 fltEnd = tmgr->getLastFlight(req);
528 //cerr << "Finding available flight " << endl;
530 // Traverse every registered flight
531 if (fltBegin == fltEnd) {
532 //cerr << "No Flights Scheduled for " << req << endl;
535 for (FGScheduledFlightVecIterator i = fltBegin; i != fltEnd; i++) {
536 (*i)->adjustTime(now);
537 //sort(fltBegin, fltEnd, compareScheduledFlights);
538 //cerr << counter++ << endl;
540 sort(fltBegin, fltEnd, compareScheduledFlights);
541 for (FGScheduledFlightVecIterator i = fltBegin; i != fltEnd; i++) {
544 if (!(*i)->isAvailable()) {
545 //cerr << (*i)->getCallSign() << "is no longer available" << endl;
548 if (!((*i)->getRequirement() == req)) {
551 if (!(((*i)->getArrivalAirport()) && ((*i)->getDepartureAirport()))) {
554 if (!(currentDestination.empty())) {
555 if (currentDestination != (*i)->getDepartureAirport()->getId()) {
556 //cerr << (*i)->getCallSign() << "Doesn't match destination" << endl;
557 //cerr << "Current Destination " << currentDestination << "Doesnt match flight's " <<
558 // (*i)->getArrivalAirport()->getId() << endl;
563 // So, if we actually get here, we have a winner
564 //cerr << "found flight: " << req << " : " << currentDestination << " : " <<
565 // (*i)->getArrivalAirport()->getId() << endl;
570 // if currentDestination has a value, does it match departure of next flight?
571 // is departure time later than planned arrival?
572 // is departure port valid?
573 // is arrival port valid?
574 //cerr << "Ack no flight found: " << endl;
578 double FGAISchedule::getSpeed()
581 double distanceToDest;
582 double speed, remainingTimeEnroute;
583 FGAirport *dep, *arr;
585 FGScheduledFlightVecIterator i = flights.begin();
586 dep = (*i)->getDepartureAirport();
587 arr = (*i)->getArrivalAirport ();
591 SGWayPoint dest ( dep->getLongitude(),
593 (*i)->getCruiseAlt(),
594 SGWayPoint::SPHERICAL);
595 SGWayPoint curr ( arr->getLongitude(),
597 (*i)->getCruiseAlt(),
598 SGWayPoint::SPHERICAL);
599 remainingTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
600 dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
601 speed = (distanceToDest*SG_METER_TO_NM) /
602 ((double) remainingTimeEnroute/3600.0);
604 //cerr << "Warning : calculated speed for " << (*i)->getCallSign() << " is low : " << speed << " clamping to 300" << endl;
608 //cerr << "Warning : calculated speed for " << (*i)->getCallSign() << " is high : " << speed << " clamping to 300" << endl;
614 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
616 //return (*a) < (*b);
620 // void FGAISchedule::setClosestDistanceToUser()
630 // elapsedTimeEnroute;
632 // double userLatitude = fgGetDouble("/position/latitude-deg");
633 // double userLongitude = fgGetDouble("/position/longitude-deg");
637 // #if defined( __CYGWIN__) || defined( __MINGW32__)
638 // #define HUGE HUGE_VAL
640 // distanceToUser = HUGE;
641 // FGScheduledFlightVecIterator i = flights.begin();
642 // while (i != flights.end())
644 // dep = i->getDepartureAirport();
648 // SGWayPoint user ( userLongitude,
650 // i->getCruiseAlt());
651 // SGWayPoint current (dep->getLongitude(),
652 // dep->getLatitude(),
654 // user.CourseAndDistance(current, &course, &dist);
655 // if (dist < distanceToUser)
657 // distanceToUser = dist;
658 // //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
662 // //return distToUser;