GPS

	- greatCircleCrossTrackError added to LegWayptCtl, DirectToController, OBSController
	- config overflight seqence
		config/over-flight-arm-angle-deg	90
		config/over-flight-arm-distance-nm	1
		config/over-flight-distance-nm		0

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 5552fdf1c592d4aecffbad8246fbdad15e92f4f4..a50ee1ff73e2acdea0459d25c59298d5b8cbc058 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -61,7 +61,9 @@ static const char* makeTTWString(double TTW)
 GPS::Config::Config() :
   _enableTurnAnticipation(true),
   _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds
+  _overflightDistance(0.0),
   _overflightArmDistance(1.0),
+  _overflightArmAngle(90.0),
   _waypointAlertTime(30.0),
   _requireHardSurface(true),
   _cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg
@@ -80,6 +82,9 @@ void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
   aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_cdiMaxDeflectionNm));
   aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer<bool>(&_driveAutopilot));
   aOwner->tie(aCfg, "course-selectable", SGRawValuePointer<bool>(&_courseSelectable));
+  aOwner->tie(aCfg, "over-flight-distance-nm", SGRawValuePointer<double>(&_overflightDistance));
+  aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer<double>(&_overflightArmDistance));
+  aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer<double>(&_overflightArmAngle));
 }
 
 ////////////////////////////////////////////////////////////////////////////
@@ -417,16 +422,39 @@ double GPS::magvarDeg()
   return _magvar_node->getDoubleValue();
 }
 
+double GPS::overflightDistanceM()
+{
+  return _config.overflightDistanceNm() * SG_NM_TO_METER;
+}
+
 double GPS::overflightArmDistanceM()
 {
   return _config.overflightArmDistanceNm() * SG_NM_TO_METER;
 }
 
+double GPS::overflightArmAngleDeg()
+{
+  return _config.overflightArmAngleDeg();
+}
+
 double GPS::selectedMagCourse()
 {
   return _selectedCourse;
 }
 
+SGGeod GPS::previousLegWaypointPosition(bool& isValid)
+{
+       FlightPlan::Leg* leg = _route->previousLeg();
+       if (leg){
+               Waypt* waypt = leg->waypoint();
+               if(waypt){
+                       isValid = true;
+                       return waypt->position();
+               }
+       }
+       isValid = false;
+  return SGGeod();
+}
 ///////////////////////////////////////////////////////////////////////////
 
 void
@@ -653,7 +681,6 @@ void GPS::updateOverflight()
   } else if (_mode == "obs") {
     // nothing to do here, TO/FROM will update but that's fine
   }
-  
   _computeTurnData = true;
 }
 
@@ -1128,7 +1155,6 @@ void GPS::directTo()
   _wp0_position = _indicated_pos;
   _currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
   _mode = "dto";
-
   clearScratch();
   wp1Changed();
 }
@@ -1145,6 +1171,7 @@ void GPS::selectOBSMode(flightgear::Waypt* waypt)
   }
   
   _mode = "obs";
+
   _currentWaypt = waypt;
   _wp0_position = _indicated_pos;
   wp1Changed();
@@ -1162,6 +1189,7 @@ void GPS::selectLegMode()
   }
 
   _mode = "leg";  
+
   // depending on the situation, this will either get over-written 
   // in routeManagerSequenced or not; either way it does no harm to
   // set it here.

diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx
index a7d3b3be10a60f662f66db7edf59444ac816ce74..444f029d25d15aed55f5559d89929a57c834f428 100644 (file)
--- a/src/Instrumentation/gps.hxx
+++ b/src/Instrumentation/gps.hxx
@@ -79,7 +79,10 @@ public:
     virtual double vspeedFPM();
     virtual double magvarDeg();
     virtual double selectedMagCourse();
+    virtual double overflightDistanceM();
     virtual double overflightArmDistanceM();
+    virtual double overflightArmAngleDeg();
+    virtual SGGeod previousLegWaypointPosition(bool& isValid);
 
 private:
     friend class SearchFilter;
@@ -102,12 +105,20 @@ private:
        */
       double turnRateDegSec() const        { return _turnRate; }
 
+      /**
+       * Distance at which we switch to next waypoint.
+       */
+      double overflightDistanceNm() const { return _overflightDistance; }
       /**
        * Distance at which we arm overflight sequencing. Once inside this
        * distance, a change of the wp1 'TO' flag to false will be considered
        * overlight of the wp.
        */
       double overflightArmDistanceNm() const { return _overflightArmDistance; }
+      /**
+                * abs angle at which we arm overflight sequencing.
+                */
+      double overflightArmAngleDeg() const { return _overflightArmAngle; }
 
       /**
        * Time before the next WP to activate an external annunciator
@@ -134,9 +145,15 @@ private:
       // desired turn rate in degrees per second
       double _turnRate;
 
+      // distance from waypoint to arm overflight sequencing (in nm)
+      double _overflightDistance;
+
       // distance from waypoint to arm overflight sequencing (in nm)
       double _overflightArmDistance;
 
+      //abs angle from course to waypoint to arm overflight sequencing (in deg)
+      double _overflightArmAngle;
+
       // time before reaching a waypoint to trigger annunciator light/sound
       // (in seconds)
       double _waypointAlertTime;

diff --git a/src/Instrumentation/rnav_waypt_controller.cxx b/src/Instrumentation/rnav_waypt_controller.cxx
index f893b9413ccabdb7d6943d7024465c2c1fa90cd1..d0bed4ce7f68442bce20b826407978651344a234 100644 (file)
--- a/src/Instrumentation/rnav_waypt_controller.cxx
+++ b/src/Instrumentation/rnav_waypt_controller.cxx
@@ -121,6 +121,34 @@ bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double
   return true;
 }
 
+
+/**
+ * Helper function Cross track error:
+ * http://williams.best.vwh.net/avform.htm#XTE
+ *
+ * param       distanceOriginToPosition in Nautical Miles
+ * param       courseDev difference between courseOriginToAircraft(AD) and courseOriginToTarget(AB)
+ * return      XTE in Nautical Miles
+ *
+ * A(origin)
+ * B(target)
+ * D(aircraft) perhaps off course
+ *
+ *                     A-->--B
+ *                      \   /
+ *                       \ /
+ *                        D
+ */
+
+double greatCircleCrossTrackError(double distanceOriginToPosition,double courseDev){
+       double crossTrackError = asin(
+                       sin(distanceOriginToPosition * SG_NM_TO_RAD) *
+                       sin(courseDev * SG_DEGREES_TO_RADIANS)
+                        );
+       return crossTrackError * SG_RAD_TO_NM;
+}
+
+
 ////////////////////////////////////////////////////////////////////////////
 
 WayptController::~WayptController()
@@ -158,7 +186,7 @@ class BasicWayptCtl : public WayptController
 public:
   BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
     WayptController(aRNAV, aWpt),
-    _distanceM(0.0),
+    _distanceAircraftTargetMeter(0.0),
     _courseDev(0.0)
   {
     if (aWpt->flag(WPT_DYNAMIC)) {
@@ -173,30 +201,33 @@ public:
 
   virtual void update()
   {
-    double brg, az2;
-    SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
-    _courseDev = brg - _targetTrack;
+    double bearingAircraftToTarget;
+
+    bearingAircraftToTarget                    = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+    _distanceAircraftTargetMeter       = SGGeodesy::distanceM(_rnav->position(), _waypt->position());
+
+    _courseDev = bearingAircraftToTarget - _targetTrack;
     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
     
-    if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
+    if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftTargetMeter < _rnav->overflightArmDistanceM())) {
       setDone();
     }
   } 
 
   virtual double distanceToWayptM() const
   {
-    return _distanceM;
+    return _distanceAircraftTargetMeter;
   }
   
   virtual double xtrackErrorNm() const
   {
-    double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
+    double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceAircraftTargetMeter;
     return x * SG_METER_TO_NM;
   }
   
   virtual bool toFlag() const
   {
-    return (fabs(_courseDev) < 90.0);
+    return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
   }
   
   virtual double courseDeviationDeg() const
@@ -215,8 +246,134 @@ public:
   }
 
 private:
-  double _distanceM;
+  double _distanceAircraftTargetMeter;
+  double _courseDev;
+};
+
+/**
+ * Controller for leg course interception.
+ * In leg mode, we want to intercept the leg between 2 waypoints(A->B).
+ * If we can't reach the the selected waypoint leg,we going direct to B.
+ */
+
+class LegWayptCtl : public WayptController
+{
+public:
+       LegWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
+    WayptController(aRNAV, aWpt),
+    _waypointOrigin(),
+    _distanceOriginTargetMeter(0.0),
+    _distanceOriginAircraftMeter(0.0),
+    _distanceAircraftTargetMeter(0.0),
+    _courseOriginToAircraft(0.0),
+    _courseAircraftToTarget(0.0),
+    _courseDev(0.0),
+    _toFlag(true)
+  {
+    if (aWpt->flag(WPT_DYNAMIC)) {
+      throw sg_exception("LegWayptCtl doesn't work with dynamic waypoints");
+    }
+  }
+
+  virtual void init()
+  {
+       double courseOriginTarget;
+       bool isPreviousLegValid = false;
+
+       _waypointOrigin = _rnav->previousLegWaypointPosition(isPreviousLegValid);
+
+       courseOriginTarget                              = SGGeodesy::courseDeg(_waypointOrigin,_waypt->position());
+
+       _courseAircraftToTarget                 = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+       _distanceAircraftTargetMeter    = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+
+       // check reach the leg in 45Deg or going direct
+       bool canReachLeg = (fabs(courseOriginTarget -_courseAircraftToTarget) < 45.0);
+
+       if ( isPreviousLegValid && canReachLeg){
+               _targetTrack = courseOriginTarget;
+       }else{
+               _targetTrack = _courseAircraftToTarget;
+               _waypointOrigin  = _rnav->position();
+       }
+
+  }
+
+  virtual void update()
+  {
+    _courseOriginToAircraft                    = SGGeodesy::courseDeg(_waypointOrigin,_rnav->position());
+    _distanceOriginAircraftMeter       = SGGeodesy::distanceM(_waypointOrigin,_rnav->position());
+
+    _courseAircraftToTarget                    = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+    _distanceAircraftTargetMeter       = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+    _courseDev = -(_courseOriginToAircraft - _targetTrack);
+
+    bool isMinimumOverFlightDistanceReached = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
+    bool isOverFlightConeArmed                                 = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
+    bool leavingOverFlightCone                                 = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
+
+    if( isMinimumOverFlightDistanceReached ){
+       _toFlag = false;
+       setDone();
+    }else{
+               if( isOverFlightConeArmed ){
+                       _toFlag = false;
+                       if ( leavingOverFlightCone ) {
+                               setDone();
+                       }
+               }else{
+                       _toFlag = true;
+               }
+    }
+  }
+
+  virtual double distanceToWayptM() const
+  {
+    return _distanceAircraftTargetMeter;
+  }
+
+  virtual double xtrackErrorNm() const
+  {
+         return greatCircleCrossTrackError(_distanceOriginAircraftMeter * SG_METER_TO_NM, _courseDev);
+  }
+
+  virtual bool toFlag() const
+  {
+    return _toFlag;
+  }
+
+  virtual double courseDeviationDeg() const
+  {
+    return _courseDev;
+  }
+
+  virtual double trueBearingDeg() const
+  {
+    return _courseAircraftToTarget;
+  }
+
+  virtual SGGeod position() const
+  {
+    return _waypt->position();
+  }
+
+private:
+
+  /*
+   * great circle route
+   * A(from), B(to), D(position) perhaps off course
+   */
+  SGGeod _waypointOrigin;
+  double _distanceOriginTargetMeter;
+  double _distanceOriginAircraftMeter;
+  double _distanceAircraftTargetMeter;
+  double _courseOriginToAircraft;
+  double _courseAircraftToTarget;
   double _courseDev;
+  bool _toFlag;
+
 };
 
 /**
@@ -230,7 +387,7 @@ public:
   RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
     WayptController(aRNAV, aWpt),
     _runway(NULL),
-    _distanceM(0.0),
+    _distanceAircraftRunwayEnd(0.0),
     _courseDev(0.0)
   {
   }
@@ -243,16 +400,19 @@ public:
 
   virtual void update()
   {
-    double brg, az2;
+    double bearingAircraftRunwayEnd;
     // use the far end of the runway for course deviation calculations. 
     // this should do the correct thing both for takeoffs (including entering 
     // the runway at a taxiway after the threshold) and also landings.
     // seperately compute the distance to the threshold for timeToWaypt calc
-    SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM); 
-    double _courseDev = brg - _targetTrack;
+
+    bearingAircraftRunwayEnd   = SGGeodesy::courseDeg(_rnav->position(), _runway->end());
+    _distanceAircraftRunwayEnd = SGGeodesy::distanceM(_rnav->position(), _runway->end());
+
+    double _courseDev = bearingAircraftRunwayEnd - _targetTrack;
     SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
     
-    if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
+    if ((fabs(_courseDev) > _rnav->overflightArmAngleDeg()) && (_distanceAircraftRunwayEnd < _rnav->overflightArmDistanceM())) {
       setDone();
     }
   } 
@@ -264,7 +424,7 @@ public:
   
   virtual double xtrackErrorNm() const
   {
-    double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
+    double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceAircraftRunwayEnd;
     return x * SG_METER_TO_NM;
   }
 
@@ -286,7 +446,7 @@ public:
   }
 private:
   FGRunway* _runway;
-  double _distanceM;
+  double _distanceAircraftRunwayEnd;
   double _courseDev;
 };
 
@@ -554,8 +714,9 @@ private:
 DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
   WayptController(aRNAV, aWpt),
   _origin(aOrigin),
-  _distanceM(0.0),
-  _courseDev(0.0)
+  _distanceAircraftTargetMeter(0.0),
+  _courseDev(0.0),
+  _courseAircraftToTarget(0.0)
 {
 }
 
@@ -564,31 +725,44 @@ void DirectToController::init()
   if (_waypt->flag(WPT_DYNAMIC)) {
     throw sg_exception("can't direct-to a dynamic waypoint");
   }
-  
+
   _targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
 }
 
 void DirectToController::update()
 {
-  double brg, az2;
-  SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
-  _courseDev = brg - _targetTrack;
+  double courseOriginToAircraft;
+
+  courseOriginToAircraft               = SGGeodesy::courseDeg(_origin,_rnav->position());
+
+  _courseAircraftToTarget              = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+  _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+  _courseDev = -(courseOriginToAircraft - _targetTrack);
+
   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
-    
-  if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
-    setDone();
+
+  bool isMinimumOverFlightDistanceReached      = _distanceAircraftTargetMeter < _rnav->overflightDistanceM();
+  bool isOverFlightConeArmed                           = _distanceAircraftTargetMeter < ( _rnav->overflightArmDistanceM() + _rnav->overflightDistanceM() );
+  bool leavingOverFlightCone                           = (fabs(_courseAircraftToTarget - _targetTrack) > _rnav->overflightArmAngleDeg());
+
+  if( isMinimumOverFlightDistanceReached ){
+       setDone();
+  }else{
+               if( isOverFlightConeArmed && leavingOverFlightCone ){
+                               setDone();
+               }
   }
 }
 
 double DirectToController::distanceToWayptM() const
 {
-  return _distanceM;
+  return _distanceAircraftTargetMeter;
 }
 
 double DirectToController::xtrackErrorNm() const
 {
-  double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
-  return x * SG_METER_TO_NM;
+       return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 }
 
 double DirectToController::courseDeviationDeg() const
@@ -598,7 +772,7 @@ double DirectToController::courseDeviationDeg() const
 
 double DirectToController::trueBearingDeg() const
 {
-  return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+  return _courseAircraftToTarget;
 }
 
 SGGeod DirectToController::position() const
@@ -610,8 +784,9 @@ SGGeod DirectToController::position() const
 
 OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
   WayptController(aRNAV, aWpt),
-  _distanceM(0.0),
-  _courseDev(0.0)
+  _distanceAircraftTargetMeter(0.0),
+  _courseDev(0.0),
+  _courseAircraftToTarget(0.0)
 {
 }
 
@@ -627,26 +802,28 @@ void OBSController::init()
 void OBSController::update()
 {
   _targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
-  double brg, az2;
-  SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM); 
-  _courseDev = brg - _targetTrack;
+
+  _courseAircraftToTarget              = SGGeodesy::courseDeg(_rnav->position(),_waypt->position());
+  _distanceAircraftTargetMeter = SGGeodesy::distanceM(_rnav->position(),_waypt->position());
+
+  // _courseDev inverted we use point target as origin
+  _courseDev = (_courseAircraftToTarget - _targetTrack);
   SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
 }
 
 bool OBSController::toFlag() const
 {
-  return (fabs(_courseDev) < 90.0);
+  return (fabs(_courseDev) < _rnav->overflightArmAngleDeg());
 }
 
 double OBSController::distanceToWayptM() const
 {
-  return _distanceM;
+  return _distanceAircraftTargetMeter;
 }
 
 double OBSController::xtrackErrorNm() const
 {
-  double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
-  return x * SG_METER_TO_NM;
+       return greatCircleCrossTrackError(_distanceAircraftTargetMeter * SG_METER_TO_NM, _courseDev);
 }
 
 double OBSController::courseDeviationDeg() const
@@ -662,7 +839,7 @@ double OBSController::courseDeviationDeg() const
 
 double OBSController::trueBearingDeg() const
 {
-  return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
+  return _courseAircraftToTarget;
 }
 
 SGGeod OBSController::position() const
@@ -703,7 +880,7 @@ WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aW
     return new HoldCtl(aRNAV, aWpt);
   }
   
-  return new BasicWayptCtl(aRNAV, aWpt);
+  return new LegWayptCtl(aRNAV, aWpt);
 }
 
 } // of namespace flightgear

diff --git a/src/Instrumentation/rnav_waypt_controller.hxx b/src/Instrumentation/rnav_waypt_controller.hxx
index b5c4b19e1b5c3c242bb23d41681a42046cdf636e..8ff8ab853adc402b621d37c72443a8cb6340a799 100644 (file)
--- a/src/Instrumentation/rnav_waypt_controller.hxx
+++ b/src/Instrumentation/rnav_waypt_controller.hxx
@@ -59,10 +59,24 @@ public:
    */
   virtual double selectedMagCourse() = 0;
 
+  /**
+   * minimum distance to switch next waypoint.
+   */
+  virtual double overflightDistanceM() = 0;
   /**
    * minimum distance to a waypoint for overflight sequencing. 
    */
   virtual double overflightArmDistanceM() = 0;
+  /**
+     * angle for overflight sequencing.
+     */
+    virtual double overflightArmAngleDeg() = 0;
+
+  /**
+   * device leg previous waypoint position(eg, from route manager)
+   */
+  virtual SGGeod previousLegWaypointPosition(bool& isValid)= 0;
+
 };
 
 class WayptController
@@ -164,8 +178,9 @@ public:
   virtual SGGeod position() const;
 private:
   SGGeod _origin;
-  double _distanceM;
+  double _distanceAircraftTargetMeter;
   double _courseDev;
+  double _courseAircraftToTarget;
 };
 
 /**
@@ -185,8 +200,9 @@ public:
   virtual bool toFlag() const;
   virtual SGGeod position() const;
 private:
-  double _distanceM;
+  double _distanceAircraftTargetMeter;
   double _courseDev;
+  double _courseAircraftToTarget;
 };
 
 } // of namespace flightgear