src/Instrumentation/testgps.cxx

   1
   2 #include <fstream>
   3
   4 #include <simgear/misc/sg_path.hxx>
   5 #include <simgear/structure/exception.hxx>
   6
   7 #include <Main/fg_init.hxx>
   8 #include <Main/globals.hxx>
   9 #include <Main/fg_props.hxx>
  10
  11 #include <Instrumentation/gps.hxx>
  12 #include <Autopilot/route_mgr.hxx>
  13 #include <Environment/environment_mgr.hxx>
  14 #include <Navaids/airways.hxx>
  15 #include <Navaids/waypoint.hxx>
  16 #include <Navaids/procedure.hxx>
  17
  18 using std::string;
  19 using namespace flightgear;
  20
  21 char *homedir = ::getenv( "HOME" );
  22 char *hostname = ::getenv( "HOSTNAME" );
  23 bool free_hostname = false;
  24
  25 void testSetPosition(const SGGeod& aPos)
  26 {
  27   fgSetDouble("/position/longitude-deg", aPos.getLongitudeDeg());
  28   fgSetDouble("/position/latitude-deg", aPos.getLatitudeDeg());
  29   fgSetDouble("/position/altitude-ft", aPos.getElevationFt());
  30 }
  31
  32 void printScratch(SGPropertyNode* scratch)
  33 {
  34   if (!scratch->getBoolValue("valid", false)) {
  35     SG_LOG(SG_GENERAL, SG_ALERT, "Scratch is invalid.");
  36     return;
  37   }
  38
  39   SG_LOG(SG_GENERAL, SG_ALERT, "Scratch:" <<
  40     scratch->getStringValue("ident") << "/" << scratch->getStringValue("name"));
  41
  42   SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("longitude-deg")
  43     << " " << scratch->getDoubleValue("latitude-deg") << " @ " << scratch->getDoubleValue("altitude-ft"));
  44
  45   SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("true-bearing-deg") <<
  46     " (" << scratch->getDoubleValue("mag-bearing-deg") << " magnetic) " << scratch->getDoubleValue("distance-nm"));
  47
  48   if (scratch->hasChild("result-index")) {
  49     SG_LOG(SG_GENERAL, SG_ALERT, "\tresult-index:" << scratch->getIntValue("result-index"));
  50   }
  51
  52   if (scratch->hasChild("route-index")) {
  53     SG_LOG(SG_GENERAL, SG_ALERT, "\troute-index:" << scratch->getIntValue("route-index"));
  54   }
  55 }
  56
  57 void printRoute(const WayptVec& aRoute)
  58 {
  59   SG_LOG(SG_GENERAL, SG_INFO, "route size=" << aRoute.size());
  60   for (unsigned int r=0; r<aRoute.size();++r) {
  61     Waypt* w = aRoute[r];
  62     SG_LOG(SG_GENERAL, SG_ALERT, "\t" << r << ": " << w->ident() << " "
  63       << w->owner()->ident());
  64   }
  65 }
  66
  67 void createDummyRoute(FGRouteMgr* rm)
  68 {
  69   SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
  70   rmInput->setStringValue("UW");
  71   rmInput->setStringValue("TLA/347/13");
  72   rmInput->setStringValue("TLA");
  73   rmInput->setStringValue("HAVEN");
  74   rmInput->setStringValue("NEW/305/29");
  75   rmInput->setStringValue("NEW");
  76   rmInput->setStringValue("OTR");
  77 }
  78
  79 int main(int argc, char* argv[])
  80 {
  81
  82 try{
  83   globals = new FGGlobals;
  84
  85   fgInitFGRoot(argc, argv);
  86   if (!fgInitConfig(argc, argv) ) {
  87     SG_LOG( SG_GENERAL, SG_ALERT, "Config option parsing failed" );
  88     exit(-1);
  89   }
  90
  91
  92   fgInitNav();
  93   fgSetDouble("/environment/magnetic-variation-deg", 0.0);
  94
  95   Airway::load();
  96
  97   SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
  98
  99   const FGAirport* egph = fgFindAirportID("EGPH");
 100   SG_LOG(SG_GENERAL, SG_ALERT, "egph: cart location:" << egph->cart());
 101
 102   FGAirport::AirportFilter af;
 103   FGPositioned::List l = FGPositioned::findClosestN(egph->geod(), 20, 2000.0, &af);
 104   for (unsigned int i=0; i<l.size(); ++i) {
 105     SG_LOG(SG_GENERAL, SG_ALERT, "\t" << l[i]->ident() << "/" << l[i]->name());
 106   }
 107
 108   //l = FGPositioned::findWithinRange(egph->geod(), 500.0, &af);
 109   //for (unsigned int i=0; i<l.size(); ++i) {
 110   //  SG_LOG(SG_GENERAL, SG_ALERT, "\t" << l[i]->ident() << "/" << l[i]->name());
 111   //}
 112
 113
 114   FGRouteMgr* rm = new FGRouteMgr;
 115   globals->add_subsystem( "route-manager", rm );
 116
 117  // FGEnvironmentMgr* envMgr = new FGEnvironmentMgr;
 118  // globals->add_subsystem("environment", envMgr);
 119  // envMgr->init();
 120
 121   fgSetBool("/sim/realism/simple-gps", true);
 122
 123   // _realismSimpleGps
 124
 125   SGPropertyNode* nd = fgGetNode("/instrumentation/gps", true);
 126   GPS* gps = new GPS(nd);
 127   globals->add_subsystem("gps", gps);
 128
 129   const FGAirport* egph = fgFindAirportID("EGPH");
 130   testSetPosition(egph->geod());
 131
 132   // startup the route manager
 133   rm->init();
 134
 135   nd->setBoolValue("serviceable", true);
 136   fgSetBool("/systems/electrical/outputs/gps", true);
 137
 138   gps->init();
 139   SGPropertyNode* scratch  = nd->getChild("scratch", 0, true);
 140   SGPropertyNode* wp = nd->getChild("wp", 0, true);
 141   SGPropertyNode* wp1 = wp->getChild("wp", 1, true);
 142
 143   // update a few times
 144   gps->update(0.05);
 145   gps->update(0.05);
 146   gps->update(0.05);
 147
 148   scratch->setStringValue("query", "TL");
 149   scratch->setStringValue("type", "Vor");
 150   scratch->setBoolValue("exact", false);
 151   nd->setStringValue("command", "search");
 152   printScratch(scratch);
 153
 154   nd->setStringValue("command", "next");
 155   printScratch(scratch);
 156
 157   nd->setStringValue("command", "next");
 158   printScratch(scratch);
 159
 160 // alphanumeric sort, partial matching
 161   nd->setDoubleValue("config/min-runway-length-ft", 5000.0);
 162   scratch->setBoolValue("exact", false);
 163   scratch->setBoolValue("order-by-distance", false);
 164   scratch->setStringValue("query", "KS");
 165   scratch->setStringValue("type", "apt");
 166
 167   nd->setStringValue("command", "search");
 168   printScratch(scratch);
 169
 170   nd->setStringValue("command", "next");
 171   printScratch(scratch);
 172
 173   nd->setStringValue("command", "next");
 174   printScratch(scratch);
 175
 176 // alphanumeric sort, explicit matching
 177   scratch->setBoolValue("exact", true);
 178   scratch->setBoolValue("order-by-distance", true);
 179   scratch->setStringValue("type", "vor");
 180   scratch->setStringValue("query", "DCS");
 181
 182   nd->setStringValue("command", "search");
 183   printScratch(scratch);
 184
 185   nd->setStringValue("command", "next");
 186   printScratch(scratch);
 187
 188 // search on totally missing
 189   scratch->setBoolValue("exact", true);
 190   scratch->setBoolValue("order-by-distance", true);
 191   scratch->setStringValue("query", "FOFOFOFOF");
 192   nd->setStringValue("command", "search");
 193   printScratch(scratch);
 194
 195 // nearest
 196   scratch->setStringValue("type", "apt");
 197   scratch->setIntValue("max-results", 10);
 198   nd->setStringValue("command", "nearest");
 199   printScratch(scratch);
 200
 201   nd->setStringValue("command", "next");
 202   printScratch(scratch);
 203
 204   nd->setStringValue("command", "next");
 205   printScratch(scratch);
 206
 207 // direct to
 208   nd->setStringValue("command", "direct");
 209   SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
 210     << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
 211     << " " << wp1->getDoubleValue("latitude-deg"));
 212
 213 // OBS mode
 214   scratch->setStringValue("query", "UW");
 215   scratch->setBoolValue("order-by-distance", true);
 216   nd->setStringValue("command", "search");
 217   printScratch(scratch);
 218
 219   nd->setStringValue("command", "obs");
 220   SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
 221     << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
 222     << " " << wp1->getDoubleValue("latitude-deg"));
 223
 224 // load route waypoints
 225   createDummyRoute(rm);
 226
 227   scratch->setIntValue("route-index", 5);
 228   nd->setStringValue("command", "load-route-wpt");
 229   printScratch(scratch);
 230
 231   nd->setStringValue("command", "next");
 232   printScratch(scratch);
 233
 234   nd->setStringValue("command", "next");
 235   printScratch(scratch);
 236
 237   scratch->setIntValue("route-index", 2);
 238   nd->setStringValue("command", "load-route-wpt");
 239   nd->setStringValue("command", "direct");
 240   SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
 241     << wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
 242     << " " << wp1->getDoubleValue("latitude-deg"));
 243
 244 // route editing
 245   SGGeod pos = egph->geod();
 246   scratch->setStringValue("ident", "FOOBAR");
 247   scratch->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
 248   scratch->setDoubleValue("latitude-deg", pos.getLatitudeDeg());
 249   nd->setStringValue("command", "define-user-wpt");
 250   printScratch(scratch);
 251
 252 // airways
 253   FGPositioned::TypeFilter vorFilt(FGPositioned::VOR);
 254   FGPositionedRef tla = FGPositioned::findClosestWithIdent("TLA", pos, &vorFilt);
 255   FGPositionedRef big = FGPositioned::findClosestWithIdent("BIG", pos, &vorFilt);
 256   FGPositionedRef pol = FGPositioned::findClosestWithIdent("POL", pos, &vorFilt);
 257
 258   const FGAirport* eddm = fgFindAirportID("EDDM");
 259   FGPositionedRef mun = FGPositioned::findClosestWithIdent("MUN",
 260     eddm->geod(), &vorFilt);
 261
 262   const FGAirport* ksfo = fgFindAirportID("KSFO");
 263   FGPositionedRef sfo = FGPositioned::findClosestWithIdent("SFO",
 264     ksfo->geod(), &vorFilt);
 265
 266
 267   WayptRef awy1 = new NavaidWaypoint(tla, NULL);
 268   WayptRef awy2 = new NavaidWaypoint(big, NULL);
 269   WayptRef awy3 = new NavaidWaypoint(pol, NULL);
 270   WayptRef awy4 = new NavaidWaypoint(mun, NULL);
 271   WayptRef awy5 = new NavaidWaypoint(sfo, NULL);
 272
 273   WayptRef awy6 = new NavaidWaypoint(
 274     (FGPositioned*) fgFindAirportID("KJFK"), NULL);
 275
 276   SGPath p("/Users/jmt/Desktop/airways.kml");
 277   std::fstream f;
 278   f.open(p.str().c_str(), fstream::out | fstream::trunc);
 279
 280 // pre-amble
 281   f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
 282       "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
 283     "<Document>\n";
 284
 285   WayptVec route;
 286   Airway::highLevel()->route(awy1, awy3, route);
 287   Route::dumpRouteToLineString("egph-egcc", route, f);
 288
 289   Airway::lowLevel()->route(awy1, awy2, route);
 290   Route::dumpRouteToLineString("egph-big", route, f);
 291
 292   Airway::lowLevel()->route(awy2, awy4, route);
 293   Route::dumpRouteToLineString("big-mun", route, f);
 294
 295   Airway::highLevel()->route(awy4, awy5, route);
 296   Route::dumpRouteToLineString("mun-sfo", route, f);
 297
 298   Airway::lowLevel()->route(awy5, awy6, route);
 299   Route::dumpRouteToLineString("sfo-jfk", route, f);
 300
 301   // post-amble
 302   f << "</Document>\n"
 303     "</kml>" << endl;
 304   f.close();
 305
 306 // procedures
 307   SGPath op("/Users/jmt/Desktop/procedures.kml");
 308   f.open(op.str().c_str(), fstream::out | fstream::trunc);
 309
 310   FGAirport* eham = (FGAirport*) fgFindAirportID("EHAM");
 311   FGPositioned::TypeFilter fixFilt(FGPositioned::FIX);
 312
 313   WayptVec approach;
 314   FGPositionedRef redfa = FGPositioned::findClosestWithIdent("REDFA",
 315     eham->geod(), &fixFilt);
 316   bool ok = eham->buildApproach(new NavaidWaypoint(redfa, NULL),
 317     eham->getRunwayByIdent("18R"), approach);
 318   if (!ok ) {
 319     SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
 320   }
 321
 322
 323   FGAirport* egll = (FGAirport*) fgFindAirportID("EGLL");
 324   WayptVec approach2;
 325   ok = egll->buildApproach(new NavaidWaypoint(big, NULL),
 326     egll->getRunwayByIdent("27R"), approach2);
 327   if (!ok ) {
 328     SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
 329   }
 330
 331 // pre-amble
 332   f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
 333       "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
 334     "<Document>\n";
 335
 336   Route::dumpRouteToLineString("REDFA 18R", approach, f);
 337   Route::dumpRouteToLineString("EGLL 27R", approach2, f);
 338
 339   // post-amble
 340   f << "</Document>\n"
 341     "</kml>" << endl;
 342   f.close();
 343
 344
 345   return EXIT_SUCCESS;
 346
 347
 348
 349 } catch (sg_exception& ex) {
 350   SG_LOG(SG_GENERAL, SG_ALERT, "exception:" << ex.getFormattedMessage());
 351 }
 352   return EXIT_FAILURE;
 353 }