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     _distanceOriginAircraftMeter(0.0),
 242     _distanceAircraftTargetMeter(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         _distanceAircraftTargetMeter    = 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     _distanceOriginAircraftMeter        = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
 282
 283     _courseAircraftToTarget                     = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 284     _distanceAircraftTargetMeter        = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 285
 286     _courseDev = -(_courseOriginToAircraft - _targetTrack);
 287
 288     bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
 289     bool isOverFlightConeArmed                          = _distanceAircraftTargetMeter < ( _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 _distanceAircraftTargetMeter;
 310   }
 311
 312   virtual double xtrackErrorNm() const
 313   {
 314           return greatCircleCrossTrackError(_distanceOriginAircraftMeter * 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 _distanceOriginAircraftMeter;
 345   double _distanceAircraftTargetMeter;
 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_RADIANS_TO_DEGREES) * _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     // note we want the outbound radial from the waypt, hence the ordering
 538     // of arguments to courseDeg
 539     double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
 540     if (fabs(r - _trueRadial) < 0.5) {
 541       setDone();
 542     }
 543   }
 544
 545   virtual double distanceToWayptM() const
 546   {
 547     return SGGeodesy::distanceM(_rnav->position(), position());
 548   }
 549
 550     virtual SGGeod position() const
 551     {
 552         SGGeoc c,
 553         geocPos = SGGeoc::fromGeod(_rnav->position()),
 554         geocWayptPos = SGGeoc::fromGeod(_waypt->position());
 555
 556         bool ok = geocRadialIntersection(geocPos, _rnav->trackDeg(),
 557                                          geocWayptPos, _trueRadial, c);
 558         if (!ok) {
 559             // try with a backwards offset from the waypt pos, in case the
 560             // procedure waypt location is too close. (eg, KSFO OCEAN SID)
 561
 562             SGGeoc navidAdjusted;
 563             SGGeodesy::advanceRadM(geocWayptPos, _trueRadial, SG_NM_TO_METER * -10, navidAdjusted);
 564
 565             ok = geocRadialIntersection(geocPos, _rnav->trackDeg(),
 566                                         navidAdjusted, _trueRadial, c);
 567             if (!ok) {
 568                 // fallback for the broken case
 569                 return _waypt->position();
 570             }
 571         }
 572
 573         return SGGeod::fromGeoc(c);
 574     }
 575 private:
 576   double _trueRadial;
 577 };
 578
 579 class DMEInterceptCtl : public WayptController
 580 {
 581 public:
 582   DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 583     WayptController(aRNAV, aWpt),
 584     _dme(NULL),
 585     _distanceNm(0.0)
 586   {
 587     if (_waypt->type() != "dmeIntercept") {
 588       throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
 589     }
 590   }
 591
 592   virtual void init()
 593   {
 594     _dme  = (DMEIntercept*) _waypt.get();
 595     _targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
 596   }
 597
 598   virtual void update(double)
 599   {
 600     _distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
 601     double d = fabs(_distanceNm - _dme->dmeDistanceNm());
 602     if (d < 0.1) {
 603       setDone();
 604     }
 605   }
 606
 607   virtual double distanceToWayptM() const
 608   {
 609     return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
 610   }
 611
 612     virtual SGGeod position() const
 613     {
 614         SGGeoc geocPos = SGGeoc::fromGeod(_rnav->position());
 615         SGGeoc navid = SGGeoc::fromGeod(_dme->position());
 616         double distRad = _dme->dmeDistanceNm() * SG_NM_TO_RAD;
 617
 618         // compute radial GC course
 619         SGGeoc bPt;
 620         SGGeodesy::advanceRadM(geocPos, _targetTrack, 100 * SG_NM_TO_RAD, bPt);
 621
 622         double dNm = pointsKnownDistanceFromGC(geocPos, bPt, navid, distRad);
 623         if (dNm < 0.0) {
 624             SG_LOG(SG_AUTOPILOT, SG_WARN, "DMEInterceptCtl::position failed");
 625             return _dme->position(); // horrible fallback
 626         }
 627
 628         return SGGeodesy::direct(_rnav->position(), _targetTrack, dNm * SG_NM_TO_METER);
 629     }
 630
 631 private:
 632   DMEIntercept* _dme;
 633   double _distanceNm;
 634 };
 635
 636 class HoldCtl : public WayptController
 637 {
 638 public:
 639   HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 640     WayptController(aRNAV, aWpt)
 641
 642   {
 643
 644   }
 645
 646   virtual void init()
 647   {
 648   }
 649
 650   virtual void update(double)
 651   {
 652     // fly inbound / outbound sides, or execute the turn
 653   #if 0
 654     if (inTurn) {
 655
 656       targetTrack += dt * turnRateSec * turnDirection;
 657       if (inbound) {
 658         if .. targetTrack has passed inbound radial, doen with this turn
 659       } else {
 660         if target track has passed reciprocal radial done with turn
 661       }
 662     } else {
 663       check time / distance elapsed
 664
 665       if (sideDone) {
 666         inTurn = true;
 667         inbound = !inbound;
 668         nextHeading = inbound;
 669         if (!inbound) {
 670           nextHeading += 180.0;
 671           SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
 672         }
 673       }
 674
 675     }
 676
 677   #endif
 678     setDone();
 679   }
 680
 681   virtual double distanceToWayptM() const
 682   {
 683     return -1.0;
 684   }
 685
 686   virtual SGGeod position() const
 687   {
 688     return _waypt->position();
 689   }
 690 };
 691
 692 class VectorsCtl : public WayptController
 693 {
 694 public:
 695   VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
 696     WayptController(aRNAV, aWpt)
 697
 698   {
 699   }
 700
 701   virtual void init()
 702   {
 703
 704   }
 705
 706   virtual void update(double)
 707   {
 708     setDone();
 709   }
 710
 711   virtual double distanceToWayptM() const
 712   {
 713     return -1.0;
 714   }
 715
 716   virtual SGGeod position() const
 717   {
 718     return _waypt->position();
 719   }
 720
 721 private:
 722 };
 723
 724 ///////////////////////////////////////////////////////////////////////////////
 725
 726 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
 727   WayptController(aRNAV, aWpt),
 728   _origin(aOrigin),
 729   _distanceAircraftTargetMeter(0.0),
 730   _courseDev(0.0),
 731   _courseAircraftToTarget(0.0)
 732 {
 733 }
 734
 735 void DirectToController::init()
 736 {
 737   if (_waypt->flag(WPT_DYNAMIC)) {
 738     throw sg_exception("can't direct-to a dynamic waypoint");
 739   }
 740
 741   _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
 742 }
 743
 744 void DirectToController::update(double)
 745 {
 746   double courseOriginToAircraft;
 747
 748   courseOriginToAircraft                = SGGeodesy::courseDeg(_origin,_rnav->position());
 749
 750   _courseAircraftToTarget               = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 751   _distanceAircraftTargetMeter  = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 752
 753   _courseDev = -(courseOriginToAircraft - _targetTrack);
 754
 755   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 756
 757   bool isMinimumOverFlightDistanceReached       = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
 758   bool isOverFlightConeArmed                            = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
 759   bool leavingOverFlightCone                            = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
 760
 761   if( isMinimumOverFlightDistanceReached ){
 762         setDone();
 763   }else{
 764                 if( isOverFlightConeArmed && leavingOverFlightCone ){
 765                                 setDone();
 766                 }
 767   }
 768 }
 769
 770 double DirectToController::distanceToWayptM() const
 771 {
 772   return _distanceAircraftTargetMeter;
 773 }
 774
 775 double DirectToController::xtrackErrorNm() const
 776 {
 777         return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 778 }
 779
 780 double DirectToController::courseDeviationDeg() const
 781 {
 782   return _courseDev;
 783 }
 784
 785 double DirectToController::trueBearingDeg() const
 786 {
 787   return _courseAircraftToTarget;
 788 }
 789
 790 SGGeod DirectToController::position() const
 791 {
 792   return _waypt->position();
 793 }
 794
 795 ///////////////////////////////////////////////////////////////////////////////
 796
 797 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
 798   WayptController(aRNAV, aWpt),
 799   _distanceAircraftTargetMeter(0.0),
 800   _courseDev(0.0),
 801   _courseAircraftToTarget(0.0)
 802 {
 803 }
 804
 805 void OBSController::init()
 806 {
 807   if (_waypt->flag(WPT_DYNAMIC)) {
 808     throw sg_exception("can't use a dynamic waypoint for OBS mode");
 809   }
 810
 811   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 812 }
 813
 814 void OBSController::update(double)
 815 {
 816   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
 817
 818   _courseAircraftToTarget               = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
 819   _distanceAircraftTargetMeter  = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
 820
 821   // _courseDev inverted we use point target as origin
 822   _courseDev = (_courseAircraftToTarget - _targetTrack);
 823   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 824 }
 825
 826 bool OBSController::toFlag() const
 827 {
 828   return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
 829 }
 830
 831 double OBSController::distanceToWayptM() const
 832 {
 833   return _distanceAircraftTargetMeter;
 834 }
 835
 836 double OBSController::xtrackErrorNm() const
 837 {
 838         return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 839 }
 840
 841 double OBSController::courseDeviationDeg() const
 842 {
 843 //  if (fabs(_courseDev) > 90.0) {
 844  //   double d = -_courseDev;
 845  //   SG_NORMALIZE_RANGE(d, -90.0, 90.0);
 846   //  return d;
 847   //}
 848
 849   return _courseDev;
 850 }
 851
 852 double OBSController::trueBearingDeg() const
 853 {
 854   return _courseAircraftToTarget;
 855 }
 856
 857 SGGeod OBSController::position() const
 858 {
 859   return _waypt->position();
 860 }
 861
 862 ///////////////////////////////////////////////////////////////////////////////
 863
 864 WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
 865 {
 866   if (!aWpt) {
 867     throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
 868   }
 869
 870   const std::string& wty(aWpt->type());
 871   if (wty == "runway") {
 872     return new RunwayCtl(aRNAV, aWpt);
 873   }
 874
 875   if (wty == "radialIntercept") {
 876     return new InterceptCtl(aRNAV, aWpt);
 877   }
 878
 879   if (wty == "dmeIntercept") {
 880     return new DMEInterceptCtl(aRNAV, aWpt);
 881   }
 882
 883   if (wty == "hdgToAlt") {
 884     return new ConstHdgToAltCtl(aRNAV, aWpt);
 885   }
 886
 887   if (wty == "vectors") {
 888     return new VectorsCtl(aRNAV, aWpt);
 889   }
 890
 891   if (wty == "hold") {
 892     return new HoldCtl(aRNAV, aWpt);
 893   }
 894
 895   return new LegWayptCtl(aRNAV, aWpt);
 896 }
 897
 898 } // of namespace flightgear
 899