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 #include "Main/util.hxx" // for fgLowPass
  29
  30 extern double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist);
  31
  32 namespace flightgear
  33 {
  34
  35 // declared in routePath.cxx
  36 bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
  37
  38 /**
  39  * Helper function Cross track error:
  40  * http://williams.best.vwh.net/avform.htm#XTE
  41  *
  42  * param        distanceOriginToPosition in Nautical Miles
  43  * param        courseDev difference between courseOriginToAircraft(AD) and courseOriginToTarget(AB)
  44  * return       XTE in Nautical Miles
  45  *
  46  * A(origin)
  47  * B(target)
  48  * D(aircraft) perhaps off course
  49  *
  50  *                      A-->--B
  51  *                       \   /
  52  *                        \ /
  53  *                         D
  54  */
  55
  56 double greatCircleCrossTrackError(double distanceOriginToPosition,double courseDev){
  57         double crossTrackError = asin(
  58                         sin(distanceOriginToPosition * SG_NM_TO_RAD) *
  59                         sin(courseDev * SG_DEGREES_TO_RADIANS)
  60                          );
  61         return crossTrackError * SG_RAD_TO_NM;
  62 }
  63
  64
  65 ////////////////////////////////////////////////////////////////////////////
  66
  67 WayptController::~WayptController()
  68 {
  69 }
  70
  71 void WayptController::init()
  72 {
  73 }
  74
  75 void WayptController::setDone()
  76 {
  77   if (_isDone) {
  78     SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
  79   }
  80
  81   _isDone = true;
  82 }
  83
  84 double WayptController::timeToWaypt() const
  85 {
  86   double gs = _rnav->groundSpeedKts();
  87   if (gs < 1.0) {
  88     return -1.0; // stationary
  89   }
  90
  91     gs*= SG_KT_TO_MPS;
  92   return (distanceToWayptM() / gs);
  93 }
  94
  95 //////////////
  96
  97 class BasicWayptCtl : public WayptController
  98 {
  99 public:
 100   BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 101     WayptController(aRNAV, aWpt),
 102     _distanceAircraftTargetMeter(0.0),
 103     _courseDev(0.0)
 104   {
 105     if (aWpt->flag(WPT_DYNAMIC)) {
 106       throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
 107     }
 108   }
 109
 110   virtual void init()
 111   {
 112     _targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 113   }
 114
 115   virtual void update(double)
 116   {
 117     double bearingAircraftToTarget;
 118
 119     bearingAircraftToTarget                     = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 120     _distanceAircraftTargetMeter        = SGGeodesy::distanceM(_rnav->position(), _waypt->position());
 121
 122     _courseDev = bearingAircraftToTarget - _targetTrack;
 123     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 124
 125     if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftTargetMeter < _rnav->overflightArmDistanceM())) {
 126       setDone();
 127     }
 128   }
 129
 130   virtual double distanceToWayptM() const
 131   {
 132     return _distanceAircraftTargetMeter;
 133   }
 134
 135   virtual double xtrackErrorNm() const
 136   {
 137     double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceAircraftTargetMeter;
 138     return x * SG_METER_TO_NM;
 139   }
 140
 141   virtual bool toFlag() const
 142   {
 143     return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
 144   }
 145
 146   virtual double courseDeviationDeg() const
 147   {
 148     return _courseDev;
 149   }
 150
 151   virtual double trueBearingDeg() const
 152   {
 153     return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
 154   }
 155
 156   virtual SGGeod position() const
 157   {
 158     return _waypt->position();
 159   }
 160
 161 private:
 162   double _distanceAircraftTargetMeter;
 163   double _courseDev;
 164 };
 165
 166 /**
 167  * Controller for leg course interception.
 168  * In leg mode, we want to intercept the leg between 2 waypoints(A->B).
 169  * If we can't reach the the selected waypoint leg,we going direct to B.
 170  */
 171
 172 class LegWayptCtl : public WayptController
 173 {
 174 public:
 175         LegWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 176     WayptController(aRNAV, aWpt),
 177     _waypointOrigin(),
 178     _distanceOriginAircraftMetre(0.0),
 179     _distanceAircraftTargetMetre(0.0),
 180     _courseOriginToAircraft(0.0),
 181     _courseAircraftToTarget(0.0),
 182     _courseDev(0.0),
 183     _toFlag(true)
 184   {
 185     if (aWpt->flag(WPT_DYNAMIC)) {
 186       throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints");
 187     }
 188   }
 189
 190   virtual void init()
 191   {
 192         double courseOriginTarget;
 193         bool isPreviousLegValid = false;
 194
 195         _waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
 196
 197         courseOriginTarget                              = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
 198
 199         _courseAircraftToTarget                 = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 200         _distanceAircraftTargetMetre    = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 201
 202
 203         // check reach the leg in 45Deg or going direct
 204         bool canReachLeg = (fabs(courseOriginTarget -_courseAircraftToTarget) < 45.0);
 205
 206         if ( isPreviousLegValid && canReachLeg){
 207                 _targetTrack = courseOriginTarget;
 208         }else{
 209                 _targetTrack = _courseAircraftToTarget;
 210                 _waypointOrigin  = _rnav->position();
 211         }
 212
 213   }
 214
 215   virtual void update(double)
 216   {
 217     _courseOriginToAircraft                     = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
 218     _distanceOriginAircraftMetre        = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
 219
 220     _courseAircraftToTarget                     = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 221     _distanceAircraftTargetMetre        = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 222
 223     _courseDev = -(_courseOriginToAircraft - _targetTrack);
 224
 225     bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMetre < _rnav->overflightDistanceM();
 226     bool isOverFlightConeArmed                          = _distanceAircraftTargetMetre < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
 227     bool leavingOverFlightCone                          = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
 228
 229     if( isMinimumOverFlightDistanceReached ){
 230         _toFlag = false;
 231         setDone();
 232     }else{
 233                 if( isOverFlightConeArmed ){
 234                         _toFlag = false;
 235                         if ( leavingOverFlightCone ) {
 236                                 setDone();
 237                         }
 238                 }else{
 239                         _toFlag = true;
 240                 }
 241     }
 242   }
 243
 244   virtual double distanceToWayptM() const
 245   {
 246     return _distanceAircraftTargetMetre;
 247   }
 248
 249   virtual double xtrackErrorNm() const
 250   {
 251           return greatCircleCrossTrackError(_distanceOriginAircraftMetre * SG_METER_TO_NM, _courseDev);
 252   }
 253
 254   virtual bool toFlag() const
 255   {
 256     return _toFlag;
 257   }
 258
 259   virtual double courseDeviationDeg() const
 260   {
 261     return _courseDev;
 262   }
 263
 264   virtual double trueBearingDeg() const
 265   {
 266     return _courseAircraftToTarget;
 267   }
 268
 269   virtual SGGeod position() const
 270   {
 271     return _waypt->position();
 272   }
 273
 274 private:
 275
 276   /*
 277    * great circle route
 278    * A(from), B(to), D(position) perhaps off course
 279    */
 280   SGGeod _waypointOrigin;
 281   double _distanceOriginAircraftMetre;
 282   double _distanceAircraftTargetMetre;
 283   double _courseOriginToAircraft;
 284   double _courseAircraftToTarget;
 285   double _courseDev;
 286   bool _toFlag;
 287
 288 };
 289
 290 /**
 291  * Special controller for runways. For runways, we want very narrow deviation
 292  * constraints, and to understand that any point along the paved area is
 293  * equivalent to being 'at' the runway.
 294  */
 295 class RunwayCtl : public WayptController
 296 {
 297 public:
 298   RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 299     WayptController(aRNAV, aWpt),
 300     _runway(NULL),
 301     _distanceAircraftRunwayEnd(0.0),
 302     _courseDev(0.0)
 303   {
 304   }
 305
 306   virtual void init()
 307   {
 308     _runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
 309     _targetTrack = _runway->headingDeg();
 310   }
 311
 312   virtual void update(double)
 313   {
 314     double bearingAircraftRunwayEnd;
 315     // use the far end of the runway for course deviation calculations.
 316     // this should do the correct thing both for takeoffs (including entering
 317     // the runway at a taxiway after the threshold) and also landings.
 318     // seperately compute the distance to the threshold for timeToWaypt calc
 319
 320     bearingAircraftRunwayEnd    = SGGeodesy::courseDeg(_rnav->position(), _runway->end());
 321     _distanceAircraftRunwayEnd  = SGGeodesy::distanceM(_rnav->position(), _runway->end());
 322
 323     double _courseDev = bearingAircraftRunwayEnd - _targetTrack;
 324     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 325
 326     if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftRunwayEnd < _rnav->overflightArmDistanceM())) {
 327       setDone();
 328     }
 329   }
 330
 331   virtual double distanceToWayptM() const
 332   {
 333     return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
 334   }
 335
 336   virtual double xtrackErrorNm() const
 337   {
 338     double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceAircraftRunwayEnd;
 339     return x * SG_METER_TO_NM;
 340   }
 341
 342   virtual double courseDeviationDeg() const
 343   {
 344     return _courseDev;
 345   }
 346
 347   virtual double trueBearingDeg() const
 348   {
 349     // as in update(), use runway->end here, so the value remains
 350     // sensible whether taking off or landing.
 351     return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
 352   }
 353
 354   virtual SGGeod position() const
 355   {
 356     return _runway->threshold();
 357   }
 358 private:
 359   FGRunway* _runway;
 360   double _distanceAircraftRunwayEnd;
 361   double _courseDev;
 362 };
 363
 364 class ConstHdgToAltCtl : public WayptController
 365 {
 366 public:
 367   ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 368     WayptController(aRNAV, aWpt)
 369
 370   {
 371     if (_waypt->type() != "hdgToAlt") {
 372       throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
 373     }
 374
 375     if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
 376       throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
 377     }
 378   }
 379
 380   virtual void init()
 381   {
 382     HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
 383     _targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
 384       _filteredFPM = _lastFPM = _rnav->vspeedFPM();
 385   }
 386
 387   virtual void update(double dt)
 388   {
 389     double curAlt = _rnav->position().getElevationFt();
 390       // adjust to get a stable FPM value; bigger values mean slower
 391       // response but more stable.
 392       const double RESPONSIVENESS = 1.0;
 393
 394       _filteredFPM = fgGetLowPass(_lastFPM, _rnav->vspeedFPM(), dt * RESPONSIVENESS);
 395       _lastFPM = _rnav->vspeedFPM();
 396
 397     switch (_waypt->altitudeRestriction()) {
 398     case RESTRICT_AT:
 399     case RESTRICT_COMPUTED:
 400     {
 401       double d = curAlt - _waypt->altitudeFt();
 402       if (fabs(d) < 50.0) {
 403         SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
 404         setDone();
 405       }
 406     } break;
 407
 408     case RESTRICT_ABOVE:
 409       if (curAlt >= _waypt->altitudeFt()) {
 410         SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
 411         setDone();
 412       }
 413       break;
 414
 415     case RESTRICT_BELOW:
 416       if (curAlt <= _waypt->altitudeFt()) {
 417         SG_LOG(SG_INSTR, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
 418         setDone();
 419       }
 420       break;
 421
 422     default:
 423       break;
 424     }
 425   }
 426
 427   virtual double timeToWaypt() const
 428   {
 429     double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
 430     return (d / _filteredFPM) * 60.0;
 431   }
 432
 433   virtual double distanceToWayptM() const
 434   {
 435       // we could filter ground speed here, but it's likely stable enough,
 436       // and timeToWaypt already filters the FPM value
 437     double gsMsec = _rnav->groundSpeedKts() * SG_KT_TO_MPS;
 438     return timeToWaypt() * gsMsec;
 439   }
 440
 441   virtual SGGeod position() const
 442   {
 443     SGGeod p;
 444     double az2;
 445     SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
 446     return p;
 447   }
 448
 449 private:
 450     double _lastFPM, _filteredFPM;
 451 };
 452
 453 class InterceptCtl : public WayptController
 454 {
 455 public:
 456   InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 457     WayptController(aRNAV, aWpt),
 458     _trueRadial(0.0)
 459   {
 460     if (_waypt->type() != "radialIntercept") {
 461       throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
 462     }
 463   }
 464
 465   virtual void init()
 466   {
 467     RadialIntercept* w = (RadialIntercept*) _waypt.get();
 468       _trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
 469     _targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
 470   }
 471
 472   virtual void update(double)
 473   {
 474       SGGeoc c,
 475         geocPos = SGGeoc::fromGeod(_rnav->position()),
 476         geocWayptPos = SGGeoc::fromGeod(_waypt->position());
 477
 478       bool ok = geocRadialIntersection(geocPos, _targetTrack,
 479                                        geocWayptPos, _trueRadial, c);
 480       if (!ok) {
 481           // try with a backwards offset from the waypt pos, in case the
 482           // procedure waypt location is too close. (eg, KSFO OCEAN SID)
 483
 484           SGGeoc navidAdjusted;
 485           SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
 486
 487           ok = geocRadialIntersection(geocPos, _targetTrack,
 488                                       navidAdjusted, _trueRadial, c);
 489           if (!ok) {
 490               SG_LOG(SG_INSTR, SG_WARN, "InterceptCtl, bad intersection, skipping waypt");
 491               setDone();
 492               return;
 493           }
 494       }
 495
 496       _projectedPosition = SGGeod::fromGeoc(c);
 497
 498
 499     // note we want the outbound radial from the waypt, hence the ordering
 500     // of arguments to courseDeg
 501     double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
 502       double bearingDiff = r - _trueRadial;
 503       SG_NORMALIZE_RANGE(bearingDiff, -180.0, 180.0);
 504     if (fabs(bearingDiff) < 0.5) {
 505       setDone();
 506     }
 507   }
 508
 509   virtual double distanceToWayptM() const
 510   {
 511     return SGGeodesy::distanceM(_rnav->position(), position());
 512   }
 513
 514     virtual SGGeod position() const
 515     {
 516         return _projectedPosition;
 517     }
 518 private:
 519   double _trueRadial;
 520     SGGeod _projectedPosition;
 521 };
 522
 523 class DMEInterceptCtl : public WayptController
 524 {
 525 public:
 526   DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 527     WayptController(aRNAV, aWpt),
 528     _dme(NULL),
 529     _distanceNm(0.0)
 530   {
 531     if (_waypt->type() != "dmeIntercept") {
 532       throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
 533     }
 534   }
 535
 536   virtual void init()
 537   {
 538     _dme  = (DMEIntercept*) _waypt.get();
 539     _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
 540   }
 541
 542   virtual void update(double)
 543   {
 544     _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
 545     double d = fabs(_distanceNm - _dme->dmeDistanceNm());
 546     if (d < 0.1) {
 547       setDone();
 548     }
 549   }
 550
 551   virtual double distanceToWayptM() const
 552   {
 553     return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
 554   }
 555
 556     virtual SGGeod position() const
 557     {
 558         SGGeoc geocPos = SGGeoc::fromGeod(_rnav->position());
 559         SGGeoc navid = SGGeoc::fromGeod(_dme->position());
 560         double distRad = _dme->dmeDistanceNm() * SG_NM_TO_RAD;
 561
 562         // compute radial GC course
 563         SGGeoc bPt;
 564         SGGeodesy::advanceRadM(geocPos, _targetTrack, 100 * SG_NM_TO_RAD, bPt);
 565
 566         double dNm = pointsKnownDistanceFromGC(geocPos, bPt, navid, distRad);
 567         if (dNm < 0.0) {
 568             SG_LOG(SG_AUTOPILOT, SG_WARN, "DMEInterceptCtl::position failed");
 569             return _dme->position(); // horrible fallback
 570         }
 571
 572         return SGGeodesy::direct(_rnav->position(), _targetTrack, dNm * SG_NM_TO_METER);
 573     }
 574
 575 private:
 576   DMEIntercept* _dme;
 577   double _distanceNm;
 578 };
 579
 580 class HoldCtl : public WayptController
 581 {
 582 public:
 583   HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 584     WayptController(aRNAV, aWpt)
 585
 586   {
 587
 588   }
 589
 590   virtual void init()
 591   {
 592   }
 593
 594   virtual void update(double)
 595   {
 596     // fly inbound / outbound sides, or execute the turn
 597   #if 0
 598     if (inTurn) {
 599
 600       targetTrack += dt * turnRateSec * turnDirection;
 601       if (inbound) {
 602         if .. targetTrack has passed inbound radial, doen with this turn
 603       } else {
 604         if target track has passed reciprocal radial done with turn
 605       }
 606     } else {
 607       check time / distance elapsed
 608
 609       if (sideDone) {
 610         inTurn = true;
 611         inbound = !inbound;
 612         nextHeading = inbound;
 613         if (!inbound) {
 614           nextHeading += 180.0;
 615           SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
 616         }
 617       }
 618
 619     }
 620
 621   #endif
 622     setDone();
 623   }
 624
 625   virtual double distanceToWayptM() const
 626   {
 627     return -1.0;
 628   }
 629
 630   virtual SGGeod position() const
 631   {
 632     return _waypt->position();
 633   }
 634 };
 635
 636 class VectorsCtl : public WayptController
 637 {
 638 public:
 639   VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 640     WayptController(aRNAV, aWpt)
 641
 642   {
 643   }
 644
 645   virtual void init()
 646   {
 647
 648   }
 649
 650   virtual void update(double)
 651   {
 652     setDone();
 653   }
 654
 655   virtual double distanceToWayptM() const
 656   {
 657     return -1.0;
 658   }
 659
 660   virtual SGGeod position() const
 661   {
 662     return _waypt->position();
 663   }
 664
 665 private:
 666 };
 667
 668 ///////////////////////////////////////////////////////////////////////////////
 669
 670 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
 671   WayptController(aRNAV, aWpt),
 672   _origin(aOrigin),
 673   _distanceAircraftTargetMeter(0.0),
 674   _courseDev(0.0),
 675   _courseAircraftToTarget(0.0)
 676 {
 677 }
 678
 679 void DirectToController::init()
 680 {
 681   if (_waypt->flag(WPT_DYNAMIC)) {
 682     throw sg_exception("can't direct-to a dynamic waypoint");
 683   }
 684
 685   _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
 686 }
 687
 688 void DirectToController::update(double)
 689 {
 690   double courseOriginToAircraft;
 691
 692   courseOriginToAircraft                = SGGeodesy::courseDeg(_origin,_rnav->position());
 693
 694   _courseAircraftToTarget               = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 695   _distanceAircraftTargetMeter  = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 696
 697   _courseDev = -(courseOriginToAircraft - _targetTrack);
 698
 699   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 700
 701   bool isMinimumOverFlightDistanceReached       = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
 702   bool isOverFlightConeArmed                            = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
 703   bool leavingOverFlightCone                            = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
 704
 705   if( isMinimumOverFlightDistanceReached ){
 706         setDone();
 707   }else{
 708                 if( isOverFlightConeArmed && leavingOverFlightCone ){
 709                                 setDone();
 710                 }
 711   }
 712 }
 713
 714 double DirectToController::distanceToWayptM() const
 715 {
 716   return _distanceAircraftTargetMeter;
 717 }
 718
 719 double DirectToController::xtrackErrorNm() const
 720 {
 721         return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 722 }
 723
 724 double DirectToController::courseDeviationDeg() const
 725 {
 726   return _courseDev;
 727 }
 728
 729 double DirectToController::trueBearingDeg() const
 730 {
 731   return _courseAircraftToTarget;
 732 }
 733
 734 SGGeod DirectToController::position() const
 735 {
 736   return _waypt->position();
 737 }
 738
 739 ///////////////////////////////////////////////////////////////////////////////
 740
 741 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
 742   WayptController(aRNAV, aWpt),
 743   _distanceAircraftTargetMeter(0.0),
 744   _courseDev(0.0),
 745   _courseAircraftToTarget(0.0)
 746 {
 747 }
 748
 749 void OBSController::init()
 750 {
 751   if (_waypt->flag(WPT_DYNAMIC)) {
 752     throw sg_exception("can't use a dynamic waypoint for OBS mode");
 753   }
 754
 755   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 756 }
 757
 758 void OBSController::update(double)
 759 {
 760   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 761
 762   _courseAircraftToTarget               = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 763   _distanceAircraftTargetMeter  = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 764
 765   // _courseDev inverted we use point target as origin
 766   _courseDev = (_courseAircraftToTarget - _targetTrack);
 767   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 768 }
 769
 770 bool OBSController::toFlag() const
 771 {
 772   return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
 773 }
 774
 775 double OBSController::distanceToWayptM() const
 776 {
 777   return _distanceAircraftTargetMeter;
 778 }
 779
 780 double OBSController::xtrackErrorNm() const
 781 {
 782         return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 783 }
 784
 785 double OBSController::courseDeviationDeg() const
 786 {
 787 //  if (fabs(_courseDev) > 90.0) {
 788  //   double d = -_courseDev;
 789  //   SG_NORMALIZE_RANGE(d, -90.0, 90.0);
 790   //  return d;
 791   //}
 792
 793   return _courseDev;
 794 }
 795
 796 double OBSController::trueBearingDeg() const
 797 {
 798   return _courseAircraftToTarget;
 799 }
 800
 801 SGGeod OBSController::position() const
 802 {
 803   return _waypt->position();
 804 }
 805
 806 ///////////////////////////////////////////////////////////////////////////////
 807
 808 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
 809 {
 810   if (!aWpt) {
 811     throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
 812   }
 813
 814   const std::string& wty(aWpt->type());
 815   if (wty == "runway") {
 816     return new RunwayCtl(aRNAV, aWpt);
 817   }
 818
 819   if (wty == "radialIntercept") {
 820     return new InterceptCtl(aRNAV, aWpt);
 821   }
 822
 823   if (wty == "dmeIntercept") {
 824     return new DMEInterceptCtl(aRNAV, aWpt);
 825   }
 826
 827   if (wty == "hdgToAlt") {
 828     return new ConstHdgToAltCtl(aRNAV, aWpt);
 829   }
 830
 831   if (wty == "vectors") {
 832     return new VectorsCtl(aRNAV, aWpt);
 833   }
 834
 835   if (wty == "hold") {
 836     return new HoldCtl(aRNAV, aWpt);
 837   }
 838
 839   return new LegWayptCtl(aRNAV, aWpt);
 840 }
 841
 842 } // of namespace flightgear
 843