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., 675 Mass Ave, Cambridge, MA 02139, USA.
20 ****************************************************************************
22 *****************************************************************************/
35 #include <simgear/compiler.h>
36 #include <simgear/math/polar3d.hxx>
37 #include <simgear/math/sg_geodesy.hxx>
38 #include <simgear/props/props.hxx>
39 #include <simgear/route/waypoint.hxx>
40 #include <simgear/structure/subsystem_mgr.hxx>
41 #include <simgear/xml/easyxml.hxx>
43 #include <AIModel/AIFlightPlan.hxx>
44 #include <AIModel/AIManager.hxx>
45 #include <AIModel/AIAircraft.hxx>
46 #include <Airports/simple.hxx>
47 #include <Main/fg_init.hxx> // That's pretty ugly, but I need fgFindAirportID
50 #include "SchedFlight.hxx"
51 #include "TrafficMgr.hxx"
55 /******************************************************************************
56 * the FGAISchedule class contains data members and code to maintain a
57 * schedule of Flights for an articically controlled aircraft.
58 *****************************************************************************/
59 FGAISchedule::FGAISchedule()
72 FGAISchedule::FGAISchedule(string mdl,
82 FGScheduledFlightVec flt)
97 for (FGScheduledFlightVecIterator i = flt.begin();
100 flights.push_back(FGScheduledFlight((*i)));
105 FGAISchedule::FGAISchedule(const FGAISchedule &other)
107 modelPath = other.modelPath;
108 livery = other.livery;
109 registration = other.registration;
111 flights = other.flights;
114 AIManagerRef = other.AIManagerRef;
115 acType = other.acType;
116 airline = other.airline;
117 m_class = other.m_class;
118 firstRun = other.firstRun;
119 radius = other.radius;
120 groundOffset = other.groundOffset;
121 flightType = other.flightType;
122 distanceToUser = other.distanceToUser;
126 FGAISchedule::~FGAISchedule()
131 bool FGAISchedule::init()
134 //SGTime* currTimeDate = globals->get_time_params();
136 //tm *temp = currTimeDate->getGmt();
138 //sgTimeFormatTime(&targetTimeDate, buffer);
139 //cout << "Scheduled Time " << buffer << endl;
140 //cout << "Time :" << time(NULL) << " SGTime : " << sgTimeGetGMT(temp) << endl;
141 for (FGScheduledFlightVecIterator i = flights.begin();
145 //i->adjustTime(now);
146 if (!(i->initializeAirports()))
149 //sort(flights.begin(), flights.end());
150 // Since time isn't initialized yet when this function is called,
151 // Find the closest possible airport.
152 // This should give a reasonable initialization order.
153 setClosestDistanceToUser();
157 bool FGAISchedule::update(time_t now)
169 double courseToUser, courseToDest;
170 double distanceToDest;
177 remainingTimeEnroute;
182 if (fgGetBool("/sim/traffic-manager/enabled") == false)
185 aimgr = (FGAIManager *) globals-> get_subsystem("ai_model");
186 // Before the flight status of this traffic entity is updated
187 // for the first time, we need to roll back it's flight schedule so
188 // so that all the flights are centered around this simulated week's time
189 // table. This is to avoid the situation where the first scheduled flight is
190 // in the future, causing the traffic manager to not generate traffic until
191 // simulated time has caught up with the real world time at initialization.
192 // This is to counter a more general initialization bug, caused by the fact
193 // that warp is not yet set when the schedule is initialized. This is
194 // especially a problem when using a negative time offset.
195 // i.e let's say we specify FlightGear to run with --time-offset=-24:00:00.
196 // Then the schedule will initialize using today, but we will fly yesterday.
197 // Thus, it would take a whole day of simulation before the traffic manager
201 for (FGScheduledFlightVecIterator i = flights.begin();
210 // Sort all the scheduled flights according to scheduled departure time.
211 // Because this is done at every update, we only need to check the status
212 // of the first listed flight.
213 sort(flights.begin(), flights.end());
214 FGScheduledFlightVecIterator i = flights.begin();
217 // Check if this aircraft has been released.
218 FGTrafficManager *tmgr = (FGTrafficManager *) globals->get_subsystem("Traffic Manager");
219 if (tmgr->isReleased(AIManagerRef))
225 userLatitude = fgGetDouble("/position/latitude-deg");
226 userLongitude = fgGetDouble("/position/longitude-deg");
228 //cerr << "Estimated minimum distance to user: " << distanceToUser << endl;
229 // This flight entry is entirely in the past, do we need to
230 // push it forward in time to the next scheduled departure.
231 if ((i->getDepartureTime() < now) && (i->getArrivalTime() < now))
237 // Departure time in the past and arrival time in the future.
238 // This flight is in progress, so we need to calculate it's
239 // approximate position and -if in range- create an AIAircraft
241 //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
244 // Part of this flight is in the future.
245 if (i->getArrivalTime() > now)
247 dep = i->getDepartureAirport();
248 arr = i->getArrivalAirport ();
252 temp = sgPolarToCart3d(Point3D(dep->getLongitude() *
253 SG_DEGREES_TO_RADIANS,
255 SG_DEGREES_TO_RADIANS,
261 temp = sgPolarToCart3d(Point3D(arr->getLongitude() *
262 SG_DEGREES_TO_RADIANS,
264 SG_DEGREES_TO_RADIANS,
271 sgdVectorProductVec3(cross,b,a);
273 angle = sgACos(sgdScalarProductVec3(a,b));
275 // Okay, at this point we have the angle between departure and
276 // arrival airport, in degrees. From here we can interpolate the
277 // position of the aircraft by calculating the ratio between
278 // total time enroute and elapsed time enroute.
280 totalTimeEnroute = i->getArrivalTime() - i->getDepartureTime();
281 if (now > i->getDepartureTime())
283 //err << "Lat = " << lat << ", lon = " << lon << endl;
284 //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
285 elapsedTimeEnroute = now - i->getDepartureTime();
286 remainingTimeEnroute = i->getArrivalTime() - now;
290 lat = dep->getLatitude();
291 lon = dep->getLongitude();
292 elapsedTimeEnroute = 0;
293 remainingTimeEnroute = totalTimeEnroute;
296 angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
299 //cout << "a = " << a[0] << " " << a[1] << " " << a[2]
300 // << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;
301 sgdMakeRotMat4(matrix, angle, cross);
302 for(int j = 0; j < 3; j++)
305 for (int k = 0; k<3; k++)
307 newPos[j] += matrix[j][k]*a[k];
311 temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
312 if (now > i->getDepartureTime())
314 //cerr << "Lat = " << lat << ", lon = " << lon << endl;
315 //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
316 lat = temp.lat() * SG_RADIANS_TO_DEGREES;
317 lon = temp.lon() * SG_RADIANS_TO_DEGREES;
321 lat = dep->getLatitude();
322 lon = dep->getLongitude();
326 SGWayPoint current (lon,
329 SGWayPoint user ( userLongitude,
332 SGWayPoint dest ( arr->getLongitude(),
335 // We really only need distance to user
336 // and course to destination
337 user.CourseAndDistance(current, &courseToUser, &distanceToUser);
338 dest.CourseAndDistance(current, &courseToDest, &distanceToDest);
339 speed = (distanceToDest*SG_METER_TO_NM) /
340 ((double) remainingTimeEnroute/3600.0);
343 // If distance between user and simulated aircaft is less
344 // then 500nm, create this flight. At jet speeds 500 nm is roughly
345 // one hour flight time, so that would be a good approximate point
346 // to start a more detailed simulation of this aircraft.
347 //cerr << registration << " is currently enroute from "
348 // << dep->_id << " to " << arr->_id << "distance : "
349 // << distanceToUser*SG_METER_TO_NM << endl;
350 if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
352 string flightPlanName = dep->getId() + string("-") + arr->getId() +
355 //if ((i->getDepartureTime() < now))
357 // alt = i->getCruiseAlt() *100;
361 // alt = dep->_elevation+19;
364 // Fixme: A non-existent model path results in an
365 // abort, due to an unhandled exeption, in fg main loop.
366 FGAIAircraft *aircraft = new FGAIAircraft(this);
367 aircraft->setPerformance(m_class); //"jet_transport";
368 aircraft->setCompany(airline); //i->getAirline();
369 aircraft->setAcType(acType); //i->getAcType();
370 aircraft->setPath(modelPath.c_str());
371 aircraft->setFlightPlan(flightPlanName);
372 aircraft->setLatitude(lat);
373 aircraft->setLongitude(lon);
374 aircraft->setAltitude(i->getCruiseAlt()*100); // convert from FL to feet
375 aircraft->setSpeed(speed);
376 aircraft->setBank(0);
377 aircraft->SetFlightPlan(new FGAIFlightPlan(modelPath, courseToDest, i->getDepartureTime(), dep,
378 arr,true, radius, i->getCruiseAlt()*100, lat, lon, speed, flightType, acType, airline));
379 aimgr->attach(aircraft);
382 AIManagerRef = aircraft->getID();
383 //cerr << "Class: " << m_class << ". acType: " << acType << ". Airline: " << airline << ". Speed = " << speed << ". From " << dep->getId() << " to " << arr->getId() << ". Time Fraction = " << (remainingTimeEnroute/(double) totalTimeEnroute) << endl;
384 //cerr << "Latitude : " << lat << ". Longitude : " << lon << endl;
385 //cerr << "Dep : " << dep->getLatitude()<< ", "<< dep->getLongitude() << endl;
386 //cerr << "Arr : " << arr->getLatitude()<< ", "<< arr->getLongitude() << endl;
387 //cerr << "Time remaining = " << (remainingTimeEnroute/3600.0) << endl;
388 //cerr << "Total time = " << (totalTimeEnroute/3600.0) << endl;
389 //cerr << "Distance remaining = " << distanceToDest*SG_METER_TO_NM << endl;
395 // Both departure and arrival time are in the future, so this
396 // the aircraft is parked at the departure airport.
397 // Currently this status is mostly ignored, but in future
398 // versions, code should go here that -if within user range-
399 // positions these aircraft at parking locations at the airport.
400 if ((i->getDepartureTime() > now) && (i->getArrivalTime() > now))
402 dep = i->getDepartureAirport();
406 //cerr << "Traffic schedule got to beyond last clause" << endl;
407 // EMH: prevent a warning, should this be 'true' instead?
408 // DT: YES. Originally, this code couldn't be reached, but
409 // when the "if(!(AIManagerManager))" clause is false we
410 // fall through right to the end. This is a valid flow.
411 // the actual value is pretty innocent, only it triggers
412 // warning in TrafficManager::update().
413 // (which was added as a sanity check for myself in the first place. :-)
418 void FGAISchedule::next()
420 flights.begin()->update();
421 sort(flights.begin(), flights.end());
424 double FGAISchedule::getSpeed()
426 double courseToUser, courseToDest;
427 double distanceToUser, distanceToDest;
428 double speed, remainingTimeEnroute;
429 FGAirport *dep, *arr;
431 FGScheduledFlightVecIterator i = flights.begin();
432 dep = i->getDepartureAirport();
433 arr = i->getArrivalAirport ();
437 SGWayPoint dest ( dep->getLongitude(),
440 SGWayPoint curr ( arr->getLongitude(),
443 remainingTimeEnroute = i->getArrivalTime() - i->getDepartureTime();
444 dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
445 speed = (distanceToDest*SG_METER_TO_NM) /
446 ((double) remainingTimeEnroute/3600.0);
451 void FGAISchedule::setClosestDistanceToUser()
463 double userLatitude = fgGetDouble("/position/latitude-deg");
464 double userLongitude = fgGetDouble("/position/longitude-deg");
468 #if defined( __CYGWIN__) || defined( __MINGW32__)
469 #define HUGE HUGE_VAL
471 distanceToUser = HUGE;
472 FGScheduledFlightVecIterator i = flights.begin();
473 while (i != flights.end())
475 dep = i->getDepartureAirport();
479 SGWayPoint user ( userLongitude,
482 SGWayPoint current (dep->getLongitude(),
485 user.CourseAndDistance(current, &course, &dist);
486 if (dist < distanceToUser)
488 distanceToUser = dist;
489 //cerr << "Found closest distance to user for " << registration << " to be " << distanceToUser << " at airport " << dep->getId() << endl;