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