src/AIModel/AIFlightPlan.cxx

   1 // FGAIFlightPlan - class for loading and storing  AI flight plans
   2 // Written by David Culp, started May 2004
   3 // - davidculp2@comcast.net
   4 //
   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.
   9 //
  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.
  14 //
  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.
  18
  19
  20 #include "AIFlightPlan.hxx"
  21 #include <simgear/misc/sg_path.hxx>
  22 #include <simgear/debug/logstream.hxx>
  23 #include <simgear/route/waypoint.hxx>
  24 #include <simgear/structure/exception.hxx>
  25 #include <simgear/constants.h>
  26 #ifdef __BORLANDC__
  27 #  define exception c_exception
  28 #endif
  29 #include <simgear/props/props.hxx>
  30 #include <Main/globals.hxx>
  31 #include <Main/fg_props.hxx>
  32
  33
  34 FGAIFlightPlan::FGAIFlightPlan(string filename)
  35 {
  36   int i;
  37   SGPath path( globals->get_fg_root() );
  38   path.append( ("/Data/AI/FlightPlans/" + filename).c_str() );
  39   SGPropertyNode root;
  40
  41   try {
  42       readProperties(path.str(), &root);
  43   } catch (const sg_exception &e) {
  44       SG_LOG(SG_GENERAL, SG_ALERT,
  45        "Error reading AI flight plan: ");
  46        cout << path.str() << endl;
  47       return;
  48   }
  49
  50   SGPropertyNode * node = root.getNode("flightplan");
  51   for (i = 0; i < node->nChildren(); i++) {
  52      //cout << "Reading waypoint " << i << endl;
  53      waypoint* wpt = new waypoint;
  54      SGPropertyNode * wpt_node = node->getChild(i);
  55      wpt->name      = wpt_node->getStringValue("name", "END");
  56      wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
  57      wpt->longitude = wpt_node->getDoubleValue("lon", 0);
  58      wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
  59      wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
  60      wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
  61      wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
  62      wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
  63      wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
  64
  65      if (wpt->name == "END") wpt->finished = true;
  66      else wpt->finished = false;
  67
  68      waypoints.push_back( wpt );
  69    }
  70
  71   wpt_iterator = waypoints.begin();
  72   //cout << waypoints.size() << " waypoints read." << endl;
  73 }
  74
  75
  76 // This is a modified version of the constructor,
  77 // Which not only reads the waypoints from a
  78 // Flight plan file, but also adds the current
  79 // Position computed by the traffic manager, as well
  80 // as setting speeds and altitude computed by the
  81 // traffic manager.
  82 FGAIFlightPlan::FGAIFlightPlan(string filename,
  83                                double lat,
  84                                double lon,
  85                                double alt,
  86                                double speed,
  87                                double course)
  88 {
  89   int i;
  90   bool useInitialWayPoint = true;
  91   SGPath path( globals->get_fg_root() );
  92   path.append( ("/Data/AI/FlightPlans/" + filename).c_str() );
  93   SGPropertyNode root;
  94
  95   try {
  96       readProperties(path.str(), &root);
  97   } catch (const sg_exception &e) {
  98       SG_LOG(SG_GENERAL, SG_ALERT,
  99        "Error reading AI flight plan: ");
 100        cout << path.str() << endl;
 101       return;
 102   }
 103
 104   SGPropertyNode * node = root.getNode("flightplan");
 105   // First waypoint is current position of the aircraft as
 106   // dictated by the traffic manager.
 107   waypoint* init_waypoint   = new waypoint;
 108   init_waypoint->name       = string("initial position");
 109   init_waypoint->latitude   = lat;
 110   init_waypoint->longitude  = lon;
 111   init_waypoint->altitude   = alt;
 112   init_waypoint->speed      = speed;
 113   init_waypoint->crossat    = - 10000;
 114   init_waypoint->gear_down  = false;
 115   init_waypoint->flaps_down = false;
 116   waypoints.push_back( init_waypoint );
 117   for (i = 0; i < node->nChildren(); i++) {
 118      //cout << "Reading waypoint " << i << endl;
 119      waypoint* wpt = new waypoint;
 120      SGPropertyNode * wpt_node = node->getChild(i);
 121      wpt->name      = wpt_node->getStringValue("name", "END");
 122      wpt->latitude  = wpt_node->getDoubleValue("lat", 0);
 123      wpt->longitude = wpt_node->getDoubleValue("lon", 0);
 124      wpt->altitude  = wpt_node->getDoubleValue("alt", 0);
 125      wpt->speed     = wpt_node->getDoubleValue("ktas", 0);
 126      //wpt->speed     = speed;
 127      wpt->crossat   = wpt_node->getDoubleValue("crossat", -10000);
 128      wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
 129      wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
 130
 131      if (wpt->name == "END") wpt->finished = true;
 132      else wpt->finished = false;
 133      // discard this waypoint if it's bearing differs more than
 134      // 90 degrees from the course we should fly according to the
 135      // Traffic manager. Those are considered "behind" us.
 136      SGWayPoint first(init_waypoint->longitude,
 137                       init_waypoint->latitude,
 138                       init_waypoint->altitude);
 139      SGWayPoint curr (wpt->longitude,
 140                       wpt->latitude,
 141                       wpt->altitude);
 142      double crse, crsDiff;
 143      double dist;
 144      first.CourseAndDistance(curr, &crse, &dist);
 145
 146      dist *= SG_METER_TO_NM;
 147
 148      // We're only interested in the absolute value of crsDiff
 149      // wich should fall in the 0-180 deg range.
 150      crsDiff = fabs(crse-course);
 151      if (crsDiff > 180)
 152        crsDiff -= 180;
 153      // These are the threee conditions that we consder including
 154      // in our flight plan:
 155      // 1) current waypoint is less then 100 miles away OR
 156      // 2) curren waypoint is ahead of us, at any distance
 157      bool useWpt = false;
 158      if ((dist > 100.0) && (crsDiff > 90.0) && (wpt->name != string ("EOF")))
 159        {
 160          //useWpt = false;
 161          // Once we start including waypoints, we have to continue, even though
 162          // one of the following way point would suffice.
 163          // so once is the useWpt flag is set to true, we cannot reset it to false.
 164          // cerr << "Discarding waypoint: " << wpt->name
 165          //    << ": Course difference = " << crsDiff << endl;
 166        }
 167      else
 168        useWpt = true;
 169
 170      if (useWpt)
 171        {
 172          if ((dist > 100.0) && (useInitialWayPoint))
 173            {
 174              waypoints.push_back(init_waypoint);
 175              //cerr << "Using waypoint : " << init_waypoint->name <<  endl;
 176            }
 177          waypoints.push_back( wpt );
 178          //cerr << "Using waypoint : " << wpt->name
 179          //    << ": course diff : " << crsDiff
 180          //      << "distance      : " << dist << endl;
 181          useInitialWayPoint = false;
 182        }
 183      else
 184        delete wpt;
 185   }
 186
 187   wpt_iterator = waypoints.begin();
 188   //cout << waypoints.size() << " waypoints read." << endl;
 189 }
 190
 191
 192
 193
 194 FGAIFlightPlan::~FGAIFlightPlan()
 195 {
 196   waypoints.clear();
 197 }
 198
 199
 200 FGAIFlightPlan::waypoint*
 201 FGAIFlightPlan::getPreviousWaypoint( void )
 202 {
 203   if (wpt_iterator == waypoints.begin()) {
 204     return 0;
 205   } else {
 206     wpt_vector_iterator prev = wpt_iterator;
 207     return *(--prev);
 208   }
 209 }
 210
 211 FGAIFlightPlan::waypoint*
 212 FGAIFlightPlan::getCurrentWaypoint( void )
 213 {
 214   return *wpt_iterator;
 215 }
 216
 217 FGAIFlightPlan::waypoint*
 218 FGAIFlightPlan::getNextWaypoint( void )
 219 {
 220   if (wpt_iterator == waypoints.end()) {
 221     return 0;
 222   } else {
 223     wpt_vector_iterator next = wpt_iterator;
 224     return *(++next);
 225   }
 226 }
 227
 228 void FGAIFlightPlan::IncrementWaypoint( void )
 229 {
 230   wpt_iterator++;
 231 }
 232
 233 // gives distance in feet from a position to a waypoint
 234 double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp){
 235    // get size of a degree at the present latitude
 236    // this won't work over large distances
 237    double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
 238    double ft_per_deg_lon = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
 239    double lat_diff_ft = fabs(wp->latitude - lat) * ft_per_deg_lat;
 240    double lon_diff_ft = fabs(wp->longitude - lon) * ft_per_deg_lon;
 241    return sqrt((lat_diff_ft * lat_diff_ft) + (lon_diff_ft * lon_diff_ft));
 242 }
 243
 244 // sets distance in feet from a lead point to the current waypoint
 245 void FGAIFlightPlan::setLeadDistance(double speed, double bearing,
 246                                      waypoint* current, waypoint* next){
 247   double turn_radius = 0.1911 * speed * speed; // an estimate for 25 degrees bank
 248   double inbound = bearing;
 249   double outbound = getBearing(current, next);
 250   double diff = fabs(inbound - outbound);
 251   if (diff > 180.0) diff = 360.0 - diff;
 252   lead_distance = turn_radius * sin(diff * SG_DEGREES_TO_RADIANS);
 253 }
 254
 255 void FGAIFlightPlan::setLeadDistance(double distance_ft){
 256   lead_distance = distance_ft;
 257 }
 258
 259
 260 double FGAIFlightPlan::getBearing(waypoint* first, waypoint* second){
 261   return getBearing(first->latitude, first->longitude, second);
 262 }
 263
 264
 265 double FGAIFlightPlan::getBearing(double lat, double lon, waypoint* wp){
 266   double course, distance;
 267  //  double latd = lat;
 268 //   double lond = lon;
 269 //   double latt = wp->latitude;
 270 //   double lont = wp->longitude;
 271 //   double ft_per_deg_lat = 366468.96 - 3717.12 * cos(lat/SG_RADIANS_TO_DEGREES);
 272 //   double ft_per_deg_lon = 365228.16 * cos(lat/SG_RADIANS_TO_DEGREES);
 273
 274 //   if (lond < 0.0) {
 275 //     lond+=360.0;
 276 //     lont+=360;
 277 //   }
 278 //   if (lont < 0.0) {
 279 //     lond+=360.0;
 280 //     lont+=360.0;
 281 //   }
 282 //   latd+=90.0;
 283 //   latt+=90.0;
 284
 285 //   double lat_diff = (latt - latd) * ft_per_deg_lat;
 286 //   double lon_diff = (lont - lond) * ft_per_deg_lon;
 287 //   double angle = atan(fabs(lat_diff / lon_diff)) * SG_RADIANS_TO_DEGREES;
 288
 289 //   bool southerly = true;
 290 //   if (latt > latd) southerly = false;
 291 //   bool easterly = false;
 292 //   if (lont > lond) easterly = true;
 293 //   if (southerly && easterly) return 90.0 + angle;
 294 //   if (!southerly && easterly) return 90.0 - angle;
 295 //   if (southerly && !easterly) return 270.0 - angle;
 296 //   if (!southerly && !easterly) return 270.0 + angle;
 297   SGWayPoint sgWp(wp->longitude,wp->latitude, wp->altitude, SGWayPoint::WGS84, string("temp"));
 298   sgWp.CourseAndDistance(lon, lat, wp->altitude, &course, &distance);
 299   return course;
 300   // Omit a compiler warning.
 301
 302 }
 303