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