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