From: James Turner Date: Mon, 8 Dec 2014 17:56:15 +0000 (+0000) Subject: Fix flight-plan course and distance computations. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=01622dd1f4e990fc588a0eed5cde1c42a1b378e8;p=flightgear.git Fix flight-plan course and distance computations. Always use the RoutePath system for course and distance computations in the flight plan, so that dynamic segments are handled correctly. --- diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index fa9e588a2..ca851881e 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -51,6 +51,8 @@ #include "Navaids/positioned.hxx" #include #include +#include + #include "Airports/airport.hxx" #include "Airports/runways.hxx" #include @@ -482,11 +484,13 @@ void FGRouteMgr::update( double dt ) return; } - double courseDeg; - double distanceM; - boost::tie(courseDeg, distanceM) = leg->waypoint()->courseAndDistanceFrom(currentPos); - -// update wp0 / wp1 / wp-last + // use RoutePath to compute location of active WP + RoutePath path(_plan); + SGGeod wpPos = path.positionForIndex(_plan->currentIndex()); + double courseDeg, az2, distanceM; + SGGeodesy::inverse(currentPos, wpPos, courseDeg, az2, distanceM); + + // update wp0 / wp1 / wp-last wp0->setDoubleValue("dist", distanceM * SG_METER_TO_NM); wp0->setDoubleValue("true-bearing-deg", courseDeg); courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing @@ -507,8 +511,9 @@ void FGRouteMgr::update( double dt ) FlightPlan::Leg* nextLeg = _plan->nextLeg(); if (nextLeg) { - boost::tie(courseDeg, distanceM) = nextLeg->waypoint()->courseAndDistanceFrom(currentPos); - + wpPos = path.positionForIndex(_plan->currentIndex() + 1); + SGGeodesy::inverse(currentPos, wpPos, courseDeg, az2, distanceM); + wp1->setDoubleValue("dist", distanceM * SG_METER_TO_NM); wp1->setDoubleValue("true-bearing-deg", courseDeg); courseDeg -= magvar->getDoubleValue(); // expose magnetic bearing diff --git a/src/GUI/WaypointList.cxx b/src/GUI/WaypointList.cxx index 20183d44e..3687c58b7 100644 --- a/src/GUI/WaypointList.cxx +++ b/src/GUI/WaypointList.cxx @@ -18,6 +18,7 @@ #include
#include +#include #include // select if the widget grabs keys necessary to fly aircraft from the keyboard, @@ -78,6 +79,11 @@ public: return _fp->legAtIndex(index)->waypoint(); } + virtual flightgear::FlightPlan* flightplan() const + { + return _fp; + } + virtual void deleteAt(unsigned int index) { _fp->deleteIndex(index); @@ -372,8 +378,11 @@ void WaypointList::draw( int dx, int dy ) y -= (_scrollPx % rowHeight); // partially draw the first row _arrowWidth = legendFont.getStringWidth(">"); + + RoutePath path(_model->flightplan()); + for ( ; row <= final; ++row, y += rowHeight) { - drawRow(dx, dy, row, y); + drawRow(dx, dy, row, y, path); } // of row drawing iteration glDisable(GL_SCISSOR_TEST); @@ -392,7 +401,8 @@ void WaypointList::draw( int dx, int dy ) } } -void WaypointList::drawRow(int dx, int dy, int rowIndex, int y) +void WaypointList::drawRow(int dx, int dy, int rowIndex, int y, + const RoutePath& path) { flightgear::Waypt* wp(_model->waypointAt(rowIndex)); @@ -459,18 +469,20 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y) x += 300 + PUSTR_LGAP; if (_showLatLon) { - SGGeod p(wp->position()); - char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S'; - char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W'; - - ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c", - fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns); + // only show for non-dynamic waypoints + if (!wp->flag(WPT_DYNAMIC)) { + SGGeod p(wp->position()); + char ns = (p.getLatitudeDeg() > 0.0) ? 'N' : 'S'; + char ew = (p.getLongitudeDeg() > 0.0) ? 'E' : 'W'; + + ::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c", + fabs(p.getLongitudeDeg()), ew, fabs(p.getLatitudeDeg()), ns); + } else { + buffer[0] = 0; + } } else if (rowIndex > 0) { - double courseDeg; - double distanceM; - Waypt* prev = _model->waypointAt(rowIndex - 1); - boost::tie(courseDeg, distanceM) = wp->courseAndDistanceFrom(prev->position()); - + double courseDeg = path.computeTrackForIndex(rowIndex); + double distanceM = path.computeDistanceForIndex(rowIndex); ::snprintf(buffer, 128 - count, "%03.0f %5.1fnm", courseDeg, distanceM * SG_METER_TO_NM); } diff --git a/src/GUI/WaypointList.hxx b/src/GUI/WaypointList.hxx index 4176d751f..32ac8f274 100644 --- a/src/GUI/WaypointList.hxx +++ b/src/GUI/WaypointList.hxx @@ -15,9 +15,11 @@ // forward decls class puaScrollBar; class SGCallback; +class RoutePath; namespace flightgear { class Waypt; + class FlightPlan; } class WaypointList : public puFrame, public GUI_ID @@ -76,6 +78,8 @@ public: virtual int currentWaypoint() const = 0; virtual flightgear::Waypt* waypointAt(unsigned int index) const = 0; + virtual flightgear::FlightPlan* flightplan() const = 0; + // update notifications virtual void setUpdateCallback(SGCallback* cb) = 0; @@ -90,7 +94,7 @@ public: protected: private: - void drawRow(int dx, int dy, int rowIndex, int yOrigin); + void drawRow(int dx, int dy, int rowIndex, int yOrigin, const RoutePath& path); void handleDrag(int x, int y); void doDrop(int x, int y); diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 2e8f5bc47..cf897e254 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -23,6 +23,8 @@ #include #include "Navaids/navrecord.hxx" #include "Navaids/FlightPlan.hxx" +#include + #include "Airports/airport.hxx" #include "Airports/runways.hxx" #include "Autopilot/route_mgr.hxx" @@ -723,16 +725,18 @@ void GPS::computeTurnData() } WayptRef next = _route->nextLeg()->waypoint(); - if (next->flag(WPT_DYNAMIC) || !_config.turnAnticipationEnabled()) { + if (next->flag(WPT_DYNAMIC) || + !_config.turnAnticipationEnabled() || + next->flag(WPT_OVERFLIGHT)) + { _anticipateTurn = false; return; } _turnStartBearing = _desiredCourse; // compute next leg course - double crs, dist; - boost::tie(crs, dist) = next->courseAndDistanceFrom(_currentWaypt->position()); - + RoutePath path(_route); + double crs = path.computeTrackForIndex(_route->currentIndex() + 1); // compute offset bearing _turnAngle = crs - _turnStartBearing; @@ -795,9 +799,14 @@ void GPS::updateRouteData() { double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM; // walk all waypoints from wp2 to route end, and sum - // for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) { - //totalDistance += _routeMgr->get_waypoint(i).get_distance(); - //} + for (int i=_route->currentIndex()+1; i<_route->numLegs(); ++i) { + FlightPlan::Leg* leg = _route->legAtIndex(i); + // omit missed-approach waypoints in distance calculation + if (leg->waypoint()->flag(WPT_MISS)) + continue; + + totalDistance += leg->distanceAlongRoute(); + } _routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM); if (_last_speed_kts > 1.0) { diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index 8a407c991..3b2fe6870 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -49,6 +49,7 @@ #include "Main/fg_props.hxx" #include #include +#include using std::string; using std::vector; @@ -1186,17 +1187,21 @@ double FlightPlan::Leg::distanceAlongRoute() const void FlightPlan::rebuildLegData() { _totalDistance = 0.0; + double totalDistanceIncludingMissed = 0.0; + RoutePath path(this); + int lastLeg = static_cast(_legs.size()) - 1; for (int l=0; l_courseDeg = path.computeTrackForIndex(l); + _legs[l]->_pathDistance = path.computeDistanceForIndex(l) * SG_METER_TO_NM; + _legs[l]->_distanceAlongPath = totalDistanceIncludingMissed; + + // omit misseed-approach waypoints from total distance calculation + if (!_legs[l]->waypoint()->flag(WPT_MISS)) { + _totalDistance += _legs[l]->_pathDistance; + } - std::pair crsDist = - next->waypoint()->courseAndDistanceFrom(cur->waypoint()->position()); - _legs[l]->_courseDeg = crsDist.first; - _legs[l]->_pathDistance = crsDist.second * SG_METER_TO_NM; - _legs[l]->_distanceAlongPath = _totalDistance; - _totalDistance += crsDist.second * SG_METER_TO_NM; + totalDistanceIncludingMissed += _legs[l]->_pathDistance; } // of legs iteration // set some data on the final leg @@ -1205,7 +1210,7 @@ void FlightPlan::rebuildLegData() // waypoint _legs[lastLeg]->_courseDeg = _legs[lastLeg - 1]->_courseDeg; _legs[lastLeg]->_pathDistance = 0.0; - _legs[lastLeg]->_distanceAlongPath = _totalDistance; + _legs[lastLeg]->_distanceAlongPath = totalDistanceIncludingMissed; } } diff --git a/src/Navaids/route.cxx b/src/Navaids/route.cxx index 8d4eb4029..0697769d3 100644 --- a/src/Navaids/route.cxx +++ b/src/Navaids/route.cxx @@ -139,18 +139,6 @@ double Waypt::speedMach() const assert(_speedRestrict == SPEED_RESTRICT_MACH); return speed(); } - -std::pair -Waypt::courseAndDistanceFrom(const SGGeod& aPos) const -{ - if (flag(WPT_DYNAMIC)) { - return std::make_pair(0.0, 0.0); - } - - double course, az2, distance; - SGGeodesy::inverse(aPos, position(), course, az2, distance); - return std::make_pair(course, distance); -} double Waypt::magvarDeg() const { diff --git a/src/Navaids/route.hxx b/src/Navaids/route.hxx index 344ac1b38..657a939ef 100644 --- a/src/Navaids/route.hxx +++ b/src/Navaids/route.hxx @@ -98,13 +98,7 @@ public: RouteBase* owner() const { return _owner; } - - /** - * Return true course (in degrees) and distance (in metres) from the provided - * position to this waypoint - */ - virtual std::pair courseAndDistanceFrom(const SGGeod& aPos) const; - + virtual SGGeod position() const = 0; /** diff --git a/src/Navaids/routePath.cxx b/src/Navaids/routePath.cxx index afbd62854..904154ecb 100644 --- a/src/Navaids/routePath.cxx +++ b/src/Navaids/routePath.cxx @@ -210,18 +210,6 @@ SGGeodVec RoutePath::pathForHold(Hold* hold) const return r; } -/** - * the path context holds the state of of an imaginary aircraft traversing - * the route, and limits the rate at which heading / altitude / position can - * change - */ -class RoutePath::PathCtx -{ -public: - SGGeod pos; - double heading; -}; - bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const { if ((index < 0) || (index >= (int) _waypts.size())) { @@ -300,7 +288,21 @@ bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2); return true; } else if (w->type() == "vectors"){ - return false; + // return position of next point (which is presumably static fix/wpt) + // however, a missed approach might end with VECTORS, so tolerate that case + if (index + 1 >= _waypts.size()) { + SG_LOG(SG_NAVAID, SG_INFO, "route ends with VECTORS, no position"); + return false; + } + + WayptRef nextWp = _waypts[index+1]; + if (nextWp->flag(WPT_DYNAMIC)) { + SG_LOG(SG_NAVAID, SG_INFO, "dynamic WP following VECTORS, no position"); + return false; + } + + r = nextWp->position(); + return true; } else if (w->type() == "hold") { r = w->position(); return true; @@ -310,6 +312,39 @@ bool RoutePath::computedPositionForIndex(int index, SGGeod& r) const return false; } +double RoutePath::computeDistanceForIndex(int index) const +{ + if ((index < 0) || (index >= (int) _waypts.size())) { + throw sg_range_exception("waypt index out of range", + "RoutePath::computeDistanceForIndex"); + } + + if (index + 1 >= (int) _waypts.size()) { + // final waypoint, distance is 0 + return 0.0; + } + + WayptRef w = _waypts[index], + nextWp = _waypts[index+1]; + + // common case, both waypoints are static + if (!w->flag(WPT_DYNAMIC) && !nextWp->flag(WPT_DYNAMIC)) { + return SGGeodesy::distanceM(w->position(), nextWp->position()); + } + + + SGGeod wPos, nextPos; + bool ok = computedPositionForIndex(index, wPos), + nextOk = computedPositionForIndex(index + 1, nextPos); + if (ok && nextOk) { + return SGGeodesy::distanceM(wPos, nextPos); + } + + SG_LOG(SG_NAVAID, SG_INFO, "RoutePath::computeDistanceForIndex: unhandled arrangement:" + << w->type() << " followed by " << nextWp->type()); + return 0.0; +} + double RoutePath::computeAltitudeForIndex(int index) const { if ((index < 0) || (index >= (int) _waypts.size())) { @@ -382,17 +417,21 @@ double RoutePath::computeTrackForIndex(int index) const return rwy->headingDeg(); } -// course based upon previous and current pos - SGGeod pos, prevPos; +// final waypoint, use inbound course + if (index + 1 >= _waypts.size()) { + return computeTrackForIndex(index - 1); + } +// course based upon current and next pos + SGGeod pos, nextPos; if (!computedPositionForIndex(index, pos) || - !computedPositionForIndex(index - 1, prevPos)) + !computedPositionForIndex(index + 1, nextPos)) { SG_LOG(SG_NAVAID, SG_WARN, "unable to compute position for waypoints"); throw sg_range_exception("unable to compute position for waypoints"); } - return SGGeodesy::courseDeg(prevPos, pos); + return SGGeodesy::courseDeg(pos, nextPos); } double RoutePath::distanceForClimb(double climbFt) const @@ -412,4 +451,3 @@ double RoutePath::magVarFor(const SGGeod& geod) const double jd = globals->get_time_params()->getJD(); return sgGetMagVar(geod, jd) * SG_RADIANS_TO_DEGREES; } - diff --git a/src/Navaids/routePath.hxx b/src/Navaids/routePath.hxx index 778e0d00e..ddc15693e 100644 --- a/src/Navaids/routePath.hxx +++ b/src/Navaids/routePath.hxx @@ -44,6 +44,9 @@ public: SGGeod positionForIndex(int index) const; + double computeDistanceForIndex(int index) const; + double computeTrackForIndex(int index) const; + private: void commonInit(); @@ -53,7 +56,6 @@ private: bool computedPositionForIndex(int index, SGGeod& pos) const; double computeAltitudeForIndex(int index) const; - double computeTrackForIndex(int index) const; void interpolateGreatCircle(const SGGeod& aFrom, const SGGeod& aTo, SGGeodVec& r) const; diff --git a/src/Scripting/NasalPositioned.cxx b/src/Scripting/NasalPositioned.cxx index 5386fc81d..9d1260bd3 100644 --- a/src/Scripting/NasalPositioned.cxx +++ b/src/Scripting/NasalPositioned.cxx @@ -2299,11 +2299,12 @@ static naRef f_leg_courseAndDistanceFrom(naContext c, naRef me, int argc, naRef* SGGeod pos; geodFromArgs(args, 0, argc, pos); - - double courseDeg; - double distanceM; - boost::tie(courseDeg, distanceM) = leg->waypoint()->courseAndDistanceFrom(pos); - + + RoutePath path(leg->owner()); + SGGeod wpPos = path.positionForIndex(leg->index()); + double courseDeg, az2, distanceM; + SGGeodesy::inverse(pos, wpPos, courseDeg, az2, distanceM); + naRef result = naNewVector(c); naVec_append(result, naNum(courseDeg)); naVec_append(result, naNum(distanceM * SG_METER_TO_NM));