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()
80 FGAISchedule::FGAISchedule(string mdl,
91 FGScheduledFlightVec flt)
106 for (FGScheduledFlightVecIterator i = flt.begin();
109 flights.push_back(new FGScheduledFlight((*(*i))));
115 FGAISchedule::FGAISchedule(const FGAISchedule &other)
117 modelPath = other.modelPath;
118 livery = other.livery;
119 registration = other.registration;
121 flights = other.flights;
124 AIManagerRef = other.AIManagerRef;
125 acType = other.acType;
126 airline = other.airline;
127 m_class = other.m_class;
128 firstRun = other.firstRun;
129 radius = other.radius;
130 groundOffset = other.groundOffset;
131 flightType = other.flightType;
133 distanceToUser = other.distanceToUser;
137 FGAISchedule::~FGAISchedule()
139 for (FGScheduledFlightVecIterator flt = flights.begin(); flt != flights.end(); flt++)
146 bool FGAISchedule::init()
149 //SGTime* currTimeDate = globals->get_time_params();
151 //tm *temp = currTimeDate->getGmt();
153 //sgTimeFormatTime(&targetTimeDate, buffer);
154 //cout << "Scheduled Time " << buffer << endl;
155 //cout << "Time :" << time(NULL) << " SGTime : " << sgTimeGetGMT(temp) << endl;
156 for (FGScheduledFlightVecIterator i = flights.begin();
160 //i->adjustTime(now);
161 if (!((*i)->initializeAirports()))
164 //sort(flights.begin(), flights.end());
165 // Since time isn't initialized yet when this function is called,
166 // Find the closest possible airport.
167 // This should give a reasonable initialization order.
168 //setClosestDistanceToUser();
172 bool FGAISchedule::update(time_t now)
181 double courseToUser, courseToDest;
182 double distanceToDest;
188 remainingTimeEnroute, deptime = 0;
193 if (fgGetBool("/sim/traffic-manager/enabled") == false)
196 aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
197 // Before the flight status of this traffic entity is updated
198 // for the first time, we need to roll back it's flight schedule so
199 // so that all the flights are centered around this simulated week's time
200 // table. This is to avoid the situation where the first scheduled flight is
201 // in the future, causing the traffic manager to not generate traffic until
202 // simulated time has caught up with the real world time at initialization.
203 // This is to counter a more general initialization bug, caused by the fact
204 // that warp is not yet set when the schedule is initialized. This is
205 // especially a problem when using a negative time offset.
206 // i.e let's say we specify FlightGear to run with --time-offset=-24:00:00.
207 // Then the schedule will initialize using today, but we will fly yesterday.
208 // Thus, it would take a whole day of simulation before the traffic manager
213 AIManagerRef = BOGUS;
215 for (FGScheduledFlightVecIterator i = flights.begin();
219 (*i)->adjustTime(now);
221 if (fgGetBool("/sim/traffic-manager/instantaneous-action") == true)
222 deptime = now + rand() % 300; // Wait up to 5 minutes until traffic starts moving to prevent too many aircraft
223 // from cluttering the gate areas.
227 // Sort all the scheduled flights according to scheduled departure time.
228 // Because this is done at every update, we only need to check the status
229 // of the first listed flight.
230 sort(flights.begin(), flights.end(), compareScheduledFlights);
232 deptime = (*flights.begin())->getDepartureTime();
233 FGScheduledFlightVecIterator i = flights.begin();
234 SG_LOG (SG_GENERAL, SG_DEBUG,"Traffic Manager: Processing registration " << registration << " with callsign " << (*i)->getCallSign());
237 // Check if this aircraft has been released.
238 FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
239 if (tmgr->isReleased(AIManagerRef))
245 userLatitude = fgGetDouble("/position/latitude-deg");
246 userLongitude = fgGetDouble("/position/longitude-deg");
248 //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
249 // This flight entry is entirely in the past, do we need to
250 // push it forward in time to the next scheduled departure.
251 if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now))
253 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in the Past");
258 // Departure time in the past and arrival time in the future.
259 // This flight is in progress, so we need to calculate it's
260 // approximate position and -if in range- create an AIAircraft
262 //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
264 // Part of this flight is in the future.
265 if ((*i)->getArrivalTime() > now)
268 dep = (*i)->getDepartureAirport();
269 arr = (*i)->getArrivalAirport ();
273 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
274 dep->getLatitude(), 1));
275 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
276 arr->getLatitude(), 1));
277 SGVec3d _cross = cross(b, a);
279 angle = sgACos(dot(a, b));
281 // Okay, at this point we have the angle between departure and
282 // arrival airport, in degrees. From here we can interpolate the
283 // position of the aircraft by calculating the ratio between
284 // total time enroute and elapsed time enroute.
286 totalTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
287 if (now > (*i)->getDepartureTime())
289 //err << "Lat = " << lat << ", lon = " << lon << endl;
290 //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
291 elapsedTimeEnroute = now - (*i)->getDepartureTime();
292 remainingTimeEnroute = (*i)->getArrivalTime() - now;
293 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is in progress.");
297 lat = dep->getLatitude();
298 lon = dep->getLongitude();
299 elapsedTimeEnroute = 0;
300 remainingTimeEnroute = totalTimeEnroute;
301 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager: Flight is pending.");
304 angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
307 //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
308 // << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
310 sgdMakeRotMat4(matrix, angle, _cross.sg());
311 SGVec3d newPos(0, 0, 0);
312 for(int j = 0; j < 3; j++)
314 for (int k = 0; k<3; k++)
316 newPos[j] += matrix[j][k]*a[k];
320 if (now > (*i)->getDepartureTime())
322 SGGeoc geoc = SGGeoc::fromCart(newPos);
323 lat = geoc.getLatitudeDeg();
324 lon = geoc.getLongitudeDeg();
328 lat = dep->getLatitude();
329 lon = dep->getLongitude();
333 SGWayPoint current (lon,
335 (*i)->getCruiseAlt(),
336 SGWayPoint::SPHERICAL);
337 SGWayPoint user ( userLongitude,
339 (*i)->getCruiseAlt(),
340 SGWayPoint::SPHERICAL);
341 SGWayPoint dest ( arr->getLongitude(),
343 (*i)->getCruiseAlt(),
344 SGWayPoint::SPHERICAL);
345 // We really only need distance to user
346 // and course to destination
347 user.CourseAndDistance(current, &courseToUser, &distanceToUser);
348 dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
349 speed = (distanceToDest*SG_METER_TO_NM) /
350 ((double) remainingTimeEnroute/3600.0);
353 // If distance between user and simulated aircaft is less
354 // then 500nm, create this flight. At jet speeds 500 nm is roughly
355 // one hour flight time, so that would be a good approximate point
356 // to start a more detailed simulation of this aircraft.
357 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: " << registration << " is scheduled for a flight from "
358 << dep->getId() << " to " << arr->getId() << ". Current distance to user: "
359 << distanceToUser*SG_METER_TO_NM);
360 if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
362 string flightPlanName = dep->getId() + string("-") + arr->getId() +
364 SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: Creating AIModel");
366 //if ((i->getDepartureTime() < now))
368 // alt = i->getCruiseAlt() *100;
372 // alt = dep->_elevation+19;
375 // Only allow traffic to be created when the model path (or the AI version of mp) exists
376 SGPath mp(globals->get_fg_root());
379 mp.append(modelPath);
381 mp_ai.append(modelPath);
383 if (mp.exists() || mp_ai.exists())
385 FGAIAircraft *aircraft = new FGAIAircraft(this);
386 aircraft->setPerformance(m_class); //"jet_transport";
387 aircraft->setCompany(airline); //i->getAirline();
388 aircraft->setAcType(acType); //i->getAcType();
389 aircraft->setPath(modelPath.c_str());
390 //aircraft->setFlightPlan(flightPlanName);
391 aircraft->setLatitude(lat);
392 aircraft->setLongitude(lon);
393 aircraft->setAltitude((*i)->getCruiseAlt()*100); // convert from FL to feet
394 aircraft->setSpeed(speed);
395 aircraft->setBank(0);
396 aircraft->SetFlightPlan(new FGAIFlightPlan(flightPlanName, courseToDest, deptime,
397 dep, arr,true, radius,
398 (*i)->getCruiseAlt()*100,
399 lat, lon, speed, flightType, acType,
401 aimgr->attach(aircraft);
404 AIManagerRef = aircraft->getID();
405 //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
406 //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
407 //cerr << "Dep : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
408 //cerr << "Arr : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
409 //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
410 //cerr << "Total time = " << (totalTimeEnroute/3600.0) << endl;
411 //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
415 SG_LOG(SG_INPUT, SG_WARN, "TrafficManager: Could not load model " << mp.str());
421 // Both departure and arrival time are in the future, so this
422 // the aircraft is parked at the departure airport.
423 // Currently this status is mostly ignored, but in future
424 // versions, code should go here that -if within user range-
425 // positions these aircraft at parking locations at the airport.
426 if (((*i)->getDepartureTime() > now) && ((*i)->getArrivalTime() > now))
428 dep = (*i)->getDepartureAirport();
432 //cerr << "Traffic schedule got to beyond last clause" << endl;
433 // EMH: prevent a warning, should this be 'true' instead?
434 // DT: YES. Originally, this code couldn't be reached, but
435 // when the "if(!(AIManagerManager))" clause is false we
436 // fall through right to the end. This is a valid flow.
437 // the actual value is pretty innocent, only it triggers
438 // warning in TrafficManager::update().
439 // (which was added as a sanity check for myself in the first place. :-)
444 void FGAISchedule::next()
446 (*flights.begin())->update();
447 sort(flights.begin(), flights.end(), compareScheduledFlights);
450 double FGAISchedule::getSpeed()
453 double distanceToDest;
454 double speed, remainingTimeEnroute;
455 FGAirport *dep, *arr;
457 FGScheduledFlightVecIterator i = flights.begin();
458 dep = (*i)->getDepartureAirport();
459 arr = (*i)->getArrivalAirport ();
463 SGWayPoint dest ( dep->getLongitude(),
465 (*i)->getCruiseAlt(),
466 SGWayPoint::SPHERICAL);
467 SGWayPoint curr ( arr->getLongitude(),
469 (*i)->getCruiseAlt(),
470 SGWayPoint::SPHERICAL);
471 remainingTimeEnroute = (*i)->getArrivalTime() - (*i)->getDepartureTime();
472 dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
473 speed = (distanceToDest*SG_METER_TO_NM) /
474 ((double) remainingTimeEnroute/3600.0);
478 bool compareSchedules(FGAISchedule*a, FGAISchedule*b)
484 // void FGAISchedule::setClosestDistanceToUser()
494 // elapsedTimeEnroute;
496 // double userLatitude = fgGetDouble("/position/latitude-deg");
497 // double userLongitude = fgGetDouble("/position/longitude-deg");
501 // #if defined( __CYGWIN__) || defined( __MINGW32__)
502 // #define HUGE HUGE_VAL
504 // distanceToUser = HUGE;
505 // FGScheduledFlightVecIterator i = flights.begin();
506 // while (i != flights.end())
508 // dep = i->getDepartureAirport();
512 // SGWayPoint user ( userLongitude,
514 // i->getCruiseAlt());
515 // SGWayPoint current (dep->getLongitude(),
516 // dep->getLatitude(),
518 // user.CourseAndDistance(current, &course, &dist);
519 // if (dist < distanceToUser)
521 // distanceToUser = dist;
522 // //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;
526 // //return distToUser;