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