src/Instrumentation/rnav_waypt_controller.cxx

   1 // rnav_waypt_controller.cxx - Waypoint-specific behaviours for RNAV systems
   2 // Written by James Turner, started 2009.
   3 //
   4 // Copyright (C) 2009  Curtis L. Olson
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19
  20 #include "rnav_waypt_controller.hxx"
  21
  22 #include <simgear/sg_inlines.h>
  23 #include <simgear/structure/exception.hxx>
  24
  25 #include <Airports/runways.hxx>
  26
  27 namespace flightgear
  28 {
  29
  30 const double KNOTS_TO_METRES_PER_SECOND = SG_NM_TO_METER / 3600.0;
  31
  32 double pmod(double x, double y)
  33 {
  34   if (x < 0.0) {
  35     return -fmod(x, y);
  36   } else {
  37     return fmod(x,y);
  38   }
  39 }
  40
  41 // implementation of
  42 // http://williams.best.vwh.net/avform.htm#Intersection
  43 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
  44 {
  45   double crs13 = r1 * SG_DEGREES_TO_RADIANS;
  46   double crs23 = r2 * SG_DEGREES_TO_RADIANS;
  47   double dst12 = SGGeodesy::distanceRad(a, b);
  48
  49   //IF sin(lon2-lon1)<0
  50   // crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
  51   // crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
  52   // ELSE
  53   // crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
  54   // crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
  55   // ENDIF
  56
  57
  58  // double diffLon = b.getLongitudeRad() - a.getLongitudeRad();
  59
  60   double sinLat1 = sin(a.getLatitudeRad());
  61   double cosLat1 = cos(a.getLatitudeRad());
  62  // double sinLat2 = sin(b.getLatitudeRad());
  63   //double cosLat2 = cos(b.getLatitudeRad());
  64   double sinDst12 = sin(dst12);
  65   double cosDst12 = cos(dst12);
  66
  67   double crs12 = SGGeodesy::courseRad(a, b),
  68     crs21 = SGGeodesy::courseRad(b, a);
  69
  70   double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
  71   double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
  72
  73  /*
  74   if (sin(diffLon) < 0.0) {
  75     crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1));
  76     crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2));
  77   } else {
  78     crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1));
  79     crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2));
  80   }
  81   */
  82
  83   double ang1 = SGMiscd::normalizeAngle2(crs13-crs12);
  84   double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
  85
  86   if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
  87     SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections");
  88     return false;
  89   }
  90
  91   if ((sin(ang1)*sin(ang2))<0.0) {
  92     SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous");
  93     return false;
  94   }
  95
  96   ang1 = fabs(ang1);
  97   ang2 = fabs(ang2);
  98
  99   //ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12))
 100   //dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3))
 101   //lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13))
 102
 103   //lon3=mod(lon1-dlon+pi,2*pi)-pi
 104
 105   double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12);
 106   double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3));
 107
 108   SGGeoc pt3;
 109   SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);
 110
 111   double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));
 112
 113   //dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3))
 114   double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3)));
 115   double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon);
 116
 117   result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
 118   //result = pt3;
 119   return true;
 120 }
 121
 122 ////////////////////////////////////////////////////////////////////////////
 123
 124 WayptController::~WayptController()
 125 {
 126 }
 127
 128 void WayptController::init()
 129 {
 130 }
 131
 132 void WayptController::setDone()
 133 {
 134   if (_isDone) {
 135     SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
 136   }
 137
 138   _isDone = true;
 139 }
 140
 141 double WayptController::timeToWaypt() const
 142 {
 143   double gs = _rnav->groundSpeedKts();
 144   if (gs < 1.0) {
 145     return -1.0; // stationary
 146   }
 147
 148   gs*= KNOTS_TO_METRES_PER_SECOND;
 149   return (distanceToWayptM() / gs);
 150 }
 151
 152 //////////////
 153
 154 class BasicWayptCtl : public WayptController
 155 {
 156 public:
 157   BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 158     WayptController(aRNAV, aWpt)
 159   {
 160     if (aWpt->flag(WPT_DYNAMIC)) {
 161       throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
 162     }
 163   }
 164
 165   virtual void init()
 166   {
 167     _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 168   }
 169
 170   virtual void update()
 171   {
 172     double brg, az2;
 173     SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
 174     _courseDev = brg - _targetTrack;
 175     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 176
 177     if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
 178       setDone();
 179     }
 180   }
 181
 182   virtual double distanceToWayptM() const
 183   {
 184     return _distanceM;
 185   }
 186
 187   virtual double xtrackErrorNm() const
 188   {
 189     double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
 190     return x * SG_METER_TO_NM;
 191   }
 192
 193   virtual bool toFlag() const
 194   {
 195     return (fabs(_courseDev) < 90.0);
 196   }
 197
 198   virtual double courseDeviationDeg() const
 199   {
 200     return _courseDev;
 201   }
 202
 203   virtual double trueBearingDeg() const
 204   {
 205     return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 206   }
 207
 208   virtual SGGeod position() const
 209   {
 210     return _waypt->position();
 211   }
 212
 213 private:
 214   double _distanceM;
 215   double _courseDev;
 216 };
 217
 218 /**
 219  * Special controller for runways. For runways, we want very narrow deviation
 220  * contraints, and to understand that any point along the paved area is
 221  * equivalent to being 'at' the runway.
 222  */
 223 class RunwayCtl : public WayptController
 224 {
 225 public:
 226   RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 227     WayptController(aRNAV, aWpt)
 228   {
 229   }
 230
 231   virtual void init()
 232   {
 233     _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
 234     _targetTrack = _runway->headingDeg();
 235   }
 236
 237   virtual void update()
 238   {
 239     double brg, az2;
 240     // use the far end of the runway for course deviation calculations.
 241     // this should do the correct thing both for takeoffs (including entering
 242     // the runway at a taxiway after the threshold) and also landings.
 243     // seperately compute the distance to the threshold for timeToWaypt calc
 244     SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM);
 245     double _courseDev = brg - _targetTrack;
 246     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 247
 248     if (fabs(_courseDev) > 90.0) {
 249       setDone();
 250     }
 251   }
 252
 253   virtual double distanceToWayptM() const
 254   {
 255     return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
 256   }
 257
 258   virtual double xtrackErrorNm() const
 259   {
 260     double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
 261     return x * SG_METER_TO_NM;
 262   }
 263
 264   virtual double courseDeviationDeg() const
 265   {
 266     return _courseDev;
 267   }
 268
 269   virtual double trueBearingDeg() const
 270   {
 271     // as in update(), use runway->end here, so the value remains
 272     // sensible whether taking off or landing.
 273     return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
 274   }
 275
 276   virtual SGGeod position() const
 277   {
 278     return _runway->threshold();
 279   }
 280 private:
 281   FGRunway* _runway;
 282   double _distanceM;
 283   double _courseDev;
 284 };
 285
 286 class ConstHdgToAltCtl : public WayptController
 287 {
 288 public:
 289   ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 290     WayptController(aRNAV, aWpt)
 291
 292   {
 293     if (_waypt->type() != "hdgToAlt") {
 294       throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
 295     }
 296
 297     if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
 298       throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
 299     }
 300   }
 301
 302   virtual void init()
 303   {
 304     HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
 305     _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
 306   }
 307
 308   virtual void update()
 309   {
 310     double curAlt = _rnav->position().getElevationFt();
 311
 312     switch (_waypt->altitudeRestriction()) {
 313     case RESTRICT_AT: {
 314       double d = curAlt - _waypt->altitudeFt();
 315       if (fabs(d) < 50.0) {
 316         SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
 317         setDone();
 318       }
 319     } break;
 320
 321     case RESTRICT_ABOVE:
 322       if (curAlt >= _waypt->altitudeFt()) {
 323         SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
 324         setDone();
 325       }
 326       break;
 327
 328     case RESTRICT_BELOW:
 329       if (curAlt <= _waypt->altitudeFt()) {
 330         SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
 331         setDone();
 332       }
 333       break;
 334
 335     case RESTRICT_NONE:
 336       assert(false);
 337       break;
 338     }
 339   }
 340
 341   virtual double timeToWaypt() const
 342   {
 343     double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
 344     return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
 345   }
 346
 347   virtual double distanceToWayptM() const
 348   {
 349     double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
 350     return timeToWaypt() * gsMsec;
 351   }
 352
 353   virtual SGGeod position() const
 354   {
 355     SGGeod p;
 356     double az2;
 357     SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
 358     return p;
 359   }
 360 };
 361
 362 class InterceptCtl : public WayptController
 363 {
 364 public:
 365   InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 366     WayptController(aRNAV, aWpt)
 367
 368   {
 369     if (_waypt->type() != "radialIntercept") {
 370       throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
 371     }
 372   }
 373
 374   virtual void init()
 375   {
 376     RadialIntercept* w = (RadialIntercept*) _waypt.get();
 377     _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
 378     _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
 379   }
 380
 381   virtual void update()
 382   {
 383     // note we want the outbound radial from the waypt, hence the ordering
 384     // of arguments to courseDeg
 385     double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
 386     SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
 387     if (fabs(r - _trueRadial) < 0.5) {
 388       SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
 389       setDone();
 390     }
 391   }
 392
 393   virtual double distanceToWayptM() const
 394   {
 395     return SGGeodesy::distanceM(_rnav->position(), position());
 396   }
 397
 398   virtual SGGeod position() const
 399   {
 400     SGGeoc c;
 401     geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(),
 402       SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
 403     return SGGeod::fromGeoc(c);
 404   }
 405 private:
 406   double _trueRadial;
 407 };
 408
 409 class DMEInterceptCtl : public WayptController
 410 {
 411 public:
 412   DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 413     WayptController(aRNAV, aWpt)
 414
 415   {
 416     if (_waypt->type() != "dmeIntercept") {
 417       throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
 418     }
 419   }
 420
 421   virtual void init()
 422   {
 423     _dme  = (DMEIntercept*) _waypt.get();
 424     _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
 425   }
 426
 427   virtual void update()
 428   {
 429     _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
 430     double d = fabs(_distanceNm - _dme->dmeDistanceNm());
 431     if (d < 0.1) {
 432       SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
 433       setDone();
 434     }
 435   }
 436
 437   virtual double distanceToWayptM() const
 438   {
 439     return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
 440   }
 441
 442   virtual SGGeod position() const
 443   {
 444     SGGeod p;
 445     double az2;
 446     SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
 447     return p;
 448   }
 449
 450 private:
 451   DMEIntercept* _dme;
 452   double _distanceNm;
 453 };
 454
 455 class HoldCtl : public WayptController
 456 {
 457 public:
 458   HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 459     WayptController(aRNAV, aWpt)
 460
 461   {
 462
 463   }
 464
 465   virtual void init()
 466   {
 467   }
 468
 469   virtual void update()
 470   {
 471     // fly inbound / outbound sides, or execute the turn
 472   #if 0
 473     if (inTurn) {
 474
 475       targetTrack += dt * turnRateSec * turnDirection;
 476       if (inbound) {
 477         if .. targetTrack has passed inbound radial, doen with this turn
 478       } else {
 479         if target track has passed reciprocal radial done with turn
 480       }
 481     } else {
 482       check time / distance elapsed
 483
 484       if (sideDone) {
 485         inTurn = true;
 486         inbound = !inbound;
 487         nextHeading = inbound;
 488         if (!inbound) {
 489           nextHeading += 180.0;
 490           SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
 491         }
 492       }
 493
 494     }
 495
 496   #endif
 497     setDone();
 498   }
 499
 500   virtual double distanceToWayptM() const
 501   {
 502     return -1.0;
 503   }
 504
 505   virtual SGGeod position() const
 506   {
 507     return _waypt->position();
 508   }
 509 };
 510
 511 class VectorsCtl : public WayptController
 512 {
 513 public:
 514   VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 515     WayptController(aRNAV, aWpt)
 516
 517   {
 518   }
 519
 520   virtual void init()
 521   {
 522
 523   }
 524
 525   virtual void update()
 526   {
 527     setDone();
 528   }
 529
 530   virtual double distanceToWayptM() const
 531   {
 532     return -1.0;
 533   }
 534
 535   virtual SGGeod position() const
 536   {
 537     return _waypt->position();
 538   }
 539
 540 private:
 541 };
 542
 543 ///////////////////////////////////////////////////////////////////////////////
 544
 545 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
 546   WayptController(aRNAV, aWpt),
 547   _origin(aOrigin)
 548 {
 549 }
 550
 551 void DirectToController::init()
 552 {
 553   if (_waypt->flag(WPT_DYNAMIC)) {
 554     throw sg_exception("can't direct-to a dynamic waypoint");
 555   }
 556
 557   _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
 558 }
 559
 560 void DirectToController::update()
 561 {
 562   double brg, az2;
 563   SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
 564   _courseDev = brg - _targetTrack;
 565   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 566
 567   if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
 568     setDone();
 569   }
 570 }
 571
 572 double DirectToController::distanceToWayptM() const
 573 {
 574   return _distanceM;
 575 }
 576
 577 double DirectToController::xtrackErrorNm() const
 578 {
 579   double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
 580   return x * SG_METER_TO_NM;
 581 }
 582
 583 double DirectToController::courseDeviationDeg() const
 584 {
 585   return _courseDev;
 586 }
 587
 588 double DirectToController::trueBearingDeg() const
 589 {
 590   return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 591 }
 592
 593 SGGeod DirectToController::position() const
 594 {
 595   return _waypt->position();
 596 }
 597
 598 ///////////////////////////////////////////////////////////////////////////////
 599
 600 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
 601   WayptController(aRNAV, aWpt)
 602 {
 603 }
 604
 605 void OBSController::init()
 606 {
 607   if (_waypt->flag(WPT_DYNAMIC)) {
 608     throw sg_exception("can't use a dynamic waypoint for OBS mode");
 609   }
 610
 611   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 612 }
 613
 614 void OBSController::update()
 615 {
 616   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 617   double brg, az2;
 618   SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
 619   _courseDev = brg - _targetTrack;
 620   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 621 }
 622
 623 bool OBSController::toFlag() const
 624 {
 625   return (fabs(_courseDev) < 90.0);
 626 }
 627
 628 double OBSController::distanceToWayptM() const
 629 {
 630   return _distanceM;
 631 }
 632
 633 double OBSController::xtrackErrorNm() const
 634 {
 635   double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
 636   return x * SG_METER_TO_NM;
 637 }
 638
 639 double OBSController::courseDeviationDeg() const
 640 {
 641 //  if (fabs(_courseDev) > 90.0) {
 642  //   double d = -_courseDev;
 643  //   SG_NORMALIZE_RANGE(d, -90.0, 90.0);
 644   //  return d;
 645   //}
 646
 647   return _courseDev;
 648 }
 649
 650 double OBSController::trueBearingDeg() const
 651 {
 652   return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 653 }
 654
 655 SGGeod OBSController::position() const
 656 {
 657   return _waypt->position();
 658 }
 659
 660 ///////////////////////////////////////////////////////////////////////////////
 661
 662 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
 663 {
 664   if (!aWpt) {
 665     throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
 666   }
 667
 668   const std::string& wty(aWpt->type());
 669   if (wty == "runway") {
 670     return new RunwayCtl(aRNAV, aWpt);
 671   }
 672
 673   if (wty == "radialIntercept") {
 674     return new InterceptCtl(aRNAV, aWpt);
 675   }
 676
 677   if (wty == "dmeIntercept") {
 678     return new DMEInterceptCtl(aRNAV, aWpt);
 679   }
 680
 681   if (wty == "hdgToAlt") {
 682     return new ConstHdgToAltCtl(aRNAV, aWpt);
 683   }
 684
 685   if (wty == "vectors") {
 686     return new VectorsCtl(aRNAV, aWpt);
 687   }
 688
 689   if (wty == "hold") {
 690     return new HoldCtl(aRNAV, aWpt);
 691   }
 692
 693   return new BasicWayptCtl(aRNAV, aWpt);
 694 }
 695
 696 } // of namespace flightgear
 697