fly/flightgear
1
0
Fork 0
Code Activity

Fix flight-plan course and distance computations.

Browse source 
Always use the RoutePath system for course and distance computations
in the flight plan, so that dynamic segments are handled correctly.
This commit is contained in:
James Turner 2014-12-08 17:56:15 +00:00
parent 938a180c3d
commit 01622dd1f4
10 changed files with 138 additions and 80 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
src/Autopilot/route_mgr.cxx
View file

@ -51,6 +51,8 @@
#include "Navaids/positioned.hxx"
#include <Navaids/waypoint.hxx>
#include <Navaids/procedure.hxx>
#include <Navaids/routePath.hxx>
#include "Airports/airport.hxx"
#include "Airports/runways.hxx"
#include <GUI/new_gui.hxx>
@ -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

38
src/GUI/WaypointList.cxx
View file

@ -18,6 +18,7 @@
#include <Main/fg_props.hxx>
#include <Navaids/positioned.hxx>
#include <Navaids/routePath.hxx>
#include <Autopilot/route_mgr.hxx>
// 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);
}

6
src/GUI/WaypointList.hxx
View file

@ -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);

23
src/Instrumentation/gps.cxx
View file

@ -23,6 +23,8 @@
#include <Navaids/waypoint.hxx>
#include "Navaids/navrecord.hxx"
#include "Navaids/FlightPlan.hxx"
#include <Navaids/routePath.hxx>
#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) {

23
src/Navaids/FlightPlan.cxx
View file

@ -49,6 +49,7 @@
#include "Main/fg_props.hxx"
#include <Navaids/procedure.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/routePath.hxx>
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<int>(_legs.size()) - 1;
for (int l=0; l<lastLeg; ++l) {
Leg* cur = _legs[l];
Leg* next = _legs[l + 1];
_legs[l]->_courseDeg = path.computeTrackForIndex(l);
_legs[l]->_pathDistance = path.computeDistanceForIndex(l) * SG_METER_TO_NM;
_legs[l]->_distanceAlongPath = totalDistanceIncludingMissed;
std::pair<double, double> 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;
// omit misseed-approach waypoints from total distance calculation
if (!_legs[l]->waypoint()->flag(WPT_MISS)) {
_totalDistance += _legs[l]->_pathDistance;
}
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;
}
}

12
src/Navaids/route.cxx
View file

@ -139,18 +139,6 @@ double Waypt::speedMach() const
assert(_speedRestrict == SPEED_RESTRICT_MACH);
return speed();
}
std::pair<double, double>
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
{

8
src/Navaids/route.hxx
View file

@ -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<double, double> courseAndDistanceFrom(const SGGeod& aPos) const;
virtual SGGeod position() const = 0;
/**

74
src/Navaids/routePath.cxx
View file

@ -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;
}

4
src/Navaids/routePath.hxx
View file

@ -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;

11
src/Scripting/NasalPositioned.cxx
View file

@ -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));