1 // FGAIFlightPlan - class for loading and storing AI flight plans
2 // Written by David Culp, started May 2004
3 // - davidculp2@comcast.net
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.
23 #include <simgear/misc/sg_path.hxx>
24 #include <simgear/debug/logstream.hxx>
25 #include <simgear/route/waypoint.hxx>
26 #include <simgear/math/sg_geodesy.hxx>
27 #include <simgear/structure/exception.hxx>
28 #include <simgear/constants.h>
30 # define exception c_exception
32 #include <simgear/props/props.hxx>
33 #include <Main/globals.hxx>
34 #include <Main/fg_props.hxx>
35 #include <Main/fg_init.hxx>
36 #include <Airports/simple.hxx>
37 #include <Airports/runways.hxx>
40 #include <Environment/environment_mgr.hxx>
41 #include <Environment/environment.hxx>
43 #include "AIFlightPlan.hxx"
46 FGAIFlightPlan::FGAIFlightPlan(const string& filename)
52 SGPath path( globals->get_fg_root() );
53 path.append( ("/AI/FlightPlans/" + filename).c_str() );
58 readProperties(path.str(), &root);
59 } catch (const sg_exception &e) {
60 SG_LOG(SG_GENERAL, SG_ALERT,
61 "Error reading AI flight plan: " << path.str());
62 // cout << path.str() << endl;
66 SGPropertyNode * node = root.getNode("flightplan");
67 for (i = 0; i < node->nChildren(); i++) {
68 //cout << "Reading waypoint " << i << endl;
69 waypoint* wpt = new waypoint;
70 SGPropertyNode * wpt_node = node->getChild(i);
71 wpt->name = wpt_node->getStringValue("name", "END");
72 wpt->latitude = wpt_node->getDoubleValue("lat", 0);
73 wpt->longitude = wpt_node->getDoubleValue("lon", 0);
74 wpt->altitude = wpt_node->getDoubleValue("alt", 0);
75 wpt->speed = wpt_node->getDoubleValue("ktas", 0);
76 wpt->crossat = wpt_node->getDoubleValue("crossat", -10000);
77 wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
78 wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
79 wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
81 if (wpt->name == "END") wpt->finished = true;
82 else wpt->finished = false;
84 waypoints.push_back( wpt );
87 wpt_iterator = waypoints.begin();
88 //cout << waypoints.size() << " waypoints read." << endl;
92 // This is a modified version of the constructor,
93 // Which not only reads the waypoints from a
94 // Flight plan file, but also adds the current
95 // Position computed by the traffic manager, as well
96 // as setting speeds and altitude computed by the
98 FGAIFlightPlan::FGAIFlightPlan(const std::string& p,
109 const string& fltType,
110 const string& acType,
111 const string& airline)
116 bool useInitialWayPoint = true;
117 bool useCurrentWayPoint = false;
118 SGPath path( globals->get_fg_root() );
119 path.append( "/AI/FlightPlans" );
123 // This is a bit of a hack:
124 // Normally the value of course will be used to evaluate whether
125 // or not a waypoint will be used for midair initialization of
126 // an AI aircraft. However, if a course value of 999 will be passed
127 // when an update request is received, which will by definition always be
128 // on the ground and should include all waypoints.
131 useInitialWayPoint = false;
132 useCurrentWayPoint = true;
139 readProperties(path.str(), &root);
141 SGPropertyNode * node = root.getNode("flightplan");
143 //waypoints.push_back( init_waypoint );
144 for (int i = 0; i < node->nChildren(); i++) {
145 //cout << "Reading waypoint " << i << endl;
146 waypoint* wpt = new waypoint;
147 SGPropertyNode * wpt_node = node->getChild(i);
148 wpt->name = wpt_node->getStringValue("name", "END");
149 wpt->latitude = wpt_node->getDoubleValue("lat", 0);
150 wpt->longitude = wpt_node->getDoubleValue("lon", 0);
151 wpt->altitude = wpt_node->getDoubleValue("alt", 0);
152 wpt->speed = wpt_node->getDoubleValue("ktas", 0);
153 //wpt->speed = speed;
154 wpt->crossat = wpt_node->getDoubleValue("crossat", -10000);
155 wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
156 wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
158 if (wpt->name == "END") wpt->finished = true;
159 else wpt->finished = false;
160 waypoints.push_back(wpt);
163 catch (const sg_exception &e) {
164 SG_LOG(SG_GENERAL, SG_WARN,
165 "Error reading AI flight plan: ");
166 cerr << "Errno = " << errno << endl;
169 cerr << "Reason: No such file or directory" << endl;
175 // cout << path.str() << endl;
176 // cout << "Trying to create this plan dynamically" << endl;
177 // cout << "Route from " << dep->id << " to " << arr->id << endl;
178 time_t now = time(NULL) + fgGetLong("/sim/time/warp");
179 time_t timeDiff = now-start;
181 if ((timeDiff > 300) && (timeDiff < 1200))
183 else if ((timeDiff >= 1200) && (timeDiff < 1500))
185 else if ((timeDiff >= 1500) && (timeDiff < 2000))
187 else if (timeDiff >= 2000)
190 //cerr << "Set leg to : " << leg << endl;
191 wpt_iterator = waypoints.begin();
192 create(dep,arr, leg, alt, speed, lat, lon,
193 firstLeg, radius, fltType, acType, airline);
194 wpt_iterator = waypoints.begin();
195 //cerr << "after create: " << (*wpt_iterator)->name << endl;
197 // Now that we have dynamically created a flight plan,
198 // we need to add some code that pops any waypoints already past.
202 waypoint* init_waypoint = new waypoint;
203 init_waypoint->name = string("initial position");
204 init_waypoint->latitude = entity->latitude;
205 init_waypoint->longitude = entity->longitude;
206 init_waypoint->altitude = entity->altitude;
207 init_waypoint->speed = entity->speed;
208 init_waypoint->crossat = - 10000;
209 init_waypoint->gear_down = false;
210 init_waypoint->flaps_down = false;
211 init_waypoint->finished = false;
213 wpt_vector_iterator i = waypoints.begin();
214 while (i != waypoints.end())
216 //cerr << "Checking status of each waypoint: " << (*i)->name << endl;
217 SGWayPoint first(init_waypoint->longitude,
218 init_waypoint->latitude,
219 init_waypoint->altitude);
220 SGWayPoint curr ((*i)->longitude,
223 double crse, crsDiff;
225 curr.CourseAndDistance(first, &crse, &dist);
227 dist *= SG_METER_TO_NM;
229 // We're only interested in the absolute value of crsDiff
230 // wich should fall in the 0-180 deg range.
231 crsDiff = fabs(crse-course);
233 crsDiff = 360-crsDiff;
234 // These are the three conditions that we consider including
235 // in our flight plan:
236 // 1) current waypoint is less then 100 miles away OR
237 // 2) curren waypoint is ahead of us, at any distance
239 if ((dist > 20.0) && (crsDiff > 90.0) && ((*i)->name != string ("EOF")))
242 // Once we start including waypoints, we have to continue, even though
243 // one of the following way point would suffice.
244 // so once is the useWpt flag is set to true, we cannot reset it to false.
245 //cerr << "Discarding waypoint: " << (*i)->name
246 // << ": Course difference = " << crsDiff
247 // << "Course = " << course
248 // << "crse = " << crse << endl;
251 useCurrentWayPoint = true;
253 if (useCurrentWayPoint)
255 if ((dist > 100.0) && (useInitialWayPoint))
257 //waypoints.push_back(init_waypoint);;
258 waypoints.insert(i, init_waypoint);
259 //cerr << "Using waypoint : " << init_waypoint->name << endl;
261 //if (useInitialWayPoint)
263 // (*i)->speed = dist; // A hack
265 //waypoints.push_back( wpt );
266 //cerr << "Using waypoint : " << (*i)->name
267 // << ": course diff : " << crsDiff
268 // << "Course = " << course
269 // << "crse = " << crse << endl
270 // << "distance : " << dist << endl;
271 useInitialWayPoint = false;
278 i = waypoints.erase(i);
283 //for (i = waypoints.begin(); i != waypoints.end(); i++)
284 // cerr << "Using waypoint : " << (*i)->name << endl;
285 //wpt_iterator = waypoints.begin();
286 //cout << waypoints.size() << " waypoints read." << endl;
292 FGAIFlightPlan::~FGAIFlightPlan()
296 //while (waypoints.begin() != waypoints.end())
298 // delete *(waypoints.begin());
299 // waypoints.erase (waypoints.begin());
304 FGAIFlightPlan::waypoint* const
305 FGAIFlightPlan::getPreviousWaypoint( void ) const
307 if (wpt_iterator == waypoints.begin()) {
310 wpt_vector_iterator prev = wpt_iterator;
315 FGAIFlightPlan::waypoint* const
316 FGAIFlightPlan::getCurrentWaypoint( void ) const
318 return *wpt_iterator;
321 FGAIFlightPlan::waypoint* const
322 FGAIFlightPlan::getNextWaypoint( void ) const
324 wpt_vector_iterator i = waypoints.end();
325 i--; // end() points to one element after the last one.
326 if (wpt_iterator == i) {
329 wpt_vector_iterator next = wpt_iterator;
334 void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints )
338 if (wpt_iterator == waypoints.begin())
342 delete *(waypoints.begin());
343 waypoints.erase(waypoints.begin());
344 wpt_iterator = waypoints.begin();
352 // gives distance in feet from a position to a waypoint
353 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{
354 // get size of a degree2 at the present latitude
355 // this won't work over large distances
356 double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
357 double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
358 double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
359 double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
360 return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
363 // sets distance in feet from a lead point to the current waypoint
364 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
365 waypoint* current, waypoint* next){
368 turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
372 double inbound = bearing;
373 double outbound = getBearing(current, next);
374 leadInAngle = fabs(inbound - outbound);
375 if (leadInAngle > 180.0)
376 leadInAngle = 360.0 - leadInAngle;
377 if (leadInAngle < 1.0) // To prevent lead_dist from getting so small it is skipped
380 lead_distance = turn_radius * sin(leadInAngle * SG_DEGREES_TO_RADIANS);
381 // if ((errno == EDOM) || (errno == ERANGE) || lead_distance < 1.0)
383 // cerr << "Lead Distance = " << lead_distance
384 // << "Diff = " << diff
385 // << "Turn Radius = " << turn_radius
386 // << "Speed = " << speed << endl;
390 void FGAIFlightPlan::setLeadDistance(double distance_ft){
391 lead_distance = distance_ft;
395 double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second) const{
396 return getBearing(first->latitude, first->longitude, second);
400 double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp) const{
401 double course, distance;
402 // double latd = lat;
403 // double lond = lon;
404 // double latt = wp->latitude;
405 // double lont = wp->longitude;
406 // double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES);
407 // double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES);
420 // double lat_diff = (latt - latd) * ft_per_deg_lat;
421 // double lon_diff = (lont - lond) * ft_per_deg_lon;
422 // double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES;
424 // bool southerly = true;
425 // if (latt > latd) southerly = false;
426 // bool easterly = false;
427 // if (lont > lond) easterly = true;
428 // if (southerly && easterly) return 90.0 + angle;
429 // if (!southerly && easterly) return 90.0 - angle;
430 // if (southerly && !easterly) return 270.0 - angle;
431 // if (!southerly && !easterly) return 270.0 + angle;
432 SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp"));
433 sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance);
436 // Omit a compiler warning.
437 //if ((errno == EDOM) || (errno == ERANGE))
439 // cerr << "Lon: " << wp->longitude
440 // << "Lat = " << wp->latitude
441 // << "Tgt Lon = " <<
442 // << "TgT Lat = " << speed << endl;
449 void FGAIFlightPlan::deleteWaypoints()
451 for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end();i++)
456 // Delete all waypoints except the last,
457 // which we will recycle as the first waypoint in the next leg;
458 void FGAIFlightPlan::resetWaypoints()
460 if (waypoints.begin() == waypoints.end())
464 waypoint *wpt = new waypoint;
465 wpt_vector_iterator i = waypoints.end();
467 wpt->name = (*i)->name;
468 wpt->latitude = (*i)->latitude;
469 wpt->longitude = (*i)->longitude;
470 wpt->altitude = (*i)->altitude;
471 wpt->speed = (*i)->speed;
472 wpt->crossat = (*i)->crossat;
473 wpt->gear_down = (*i)->gear_down;
474 wpt->flaps_down= (*i)->flaps_down;
475 wpt->finished = false;
476 wpt->on_ground = (*i)->on_ground;
477 //cerr << "Recycling waypoint " << wpt->name << endl;
479 waypoints.push_back(wpt);
483 // Start flightplan over from the beginning
484 void FGAIFlightPlan::restart()
486 wpt_iterator = waypoints.begin();