fly/flightgear
1
0
Fork 0
Code Activity

RoutePath fixes.

Browse source 
Should fix:

https://sourceforge.net/p/flightgear/codetickets/1703/
https://sourceforge.net/p/flightgear/codetickets/1939/

Although both of these I had trouble reproducing directly.
This commit is contained in:
James Turner 2017-03-25 15:16:21 +00:00
parent c2da881010
commit 7adb2fa851
1 changed files with 129 additions and 108 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

237
src/Navaids/routePath.cxx
View file

@ -641,9 +641,10 @@ public:
void computeDynamicPosition(int index) void computeDynamicPosition(int index)
{ {
const WayptData& previous(previousValidWaypoint(index)); auto previous(previousValidWaypoint(index));
WayptRef wpt = waypoints[index].wpt; WayptRef wpt = waypoints[index].wpt;
assert(previous.posValid); assert(previous != waypoints.end());
assert(previous->posValid);
const std::string& ty(wpt->type()); const std::string& ty(wpt->type());
if (ty == "hdgToAlt") { if (ty == "hdgToAlt") {
@ -662,17 +663,17 @@ public:
} }
double distanceM = timeToChangeSec * speedMSec; double distanceM = timeToChangeSec * speedMSec;
double hdg = h->headingDegMagnetic() + magVarFor(previous.pos); double hdg = h->headingDegMagnetic() + magVarFor(previous->pos);
waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, hdg, distanceM); waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, hdg, distanceM);
waypoints[index].posValid = true; waypoints[index].posValid = true;
} else if (ty == "radialIntercept") { } else if (ty == "radialIntercept") {
// start from previous.turnExit // start from previous.turnExit
RadialIntercept* i = (RadialIntercept*) wpt.get(); RadialIntercept* i = (RadialIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position()); SGGeoc navid = SGGeoc::fromGeod(wpt->position());
SGGeoc rGc; SGGeoc rGc;
double magVar = magVarFor(previous.pos); double magVar = magVarFor(previous->pos);
double radial = i->radialDegMagnetic() + magVar; double radial = i->radialDegMagnetic() + magVar;
double track = i->courseDegMagnetic() + magVar; double track = i->courseDegMagnetic() + magVar;
@ -687,7 +688,7 @@ public:
ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc); ok = geocRadialIntersection(prevGc, track, navidAdjusted, radial, rGc);
if (!ok) { if (!ok) {
SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:" SG_LOG(SG_NAVAID, SG_WARN, "couldn't compute interception for radial:"
<< previous.turnExitPos << " / " << track << "/" << wpt->position() << previous->turnExitPos << " / " << track << "/" << wpt->position()
<< "/" << radial); << "/" << radial);
waypoints[index].pos = wpt->position(); // horrible fallback waypoints[index].pos = wpt->position(); // horrible fallback
@ -702,7 +703,7 @@ public:
} else if (ty == "dmeIntercept") { } else if (ty == "dmeIntercept") {
DMEIntercept* di = (DMEIntercept*) wpt.get(); DMEIntercept* di = (DMEIntercept*) wpt.get();
SGGeoc prevGc = SGGeoc::fromGeod(previous.turnExitPos); SGGeoc prevGc = SGGeoc::fromGeod(previous->turnExitPos);
SGGeoc navid = SGGeoc::fromGeod(wpt->position()); SGGeoc navid = SGGeoc::fromGeod(wpt->position());
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD; double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc; SGGeoc rGc;
@ -716,12 +717,12 @@ public:
SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed"); SG_LOG(SG_NAVAID, SG_WARN, "dmeIntercept failed");
waypoints[index].pos = wpt->position(); // horrible fallback waypoints[index].pos = wpt->position(); // horrible fallback
} else { } else {
waypoints[index].pos = SGGeodesy::direct(previous.turnExitPos, crs, dNm * SG_NM_TO_METER); waypoints[index].pos = SGGeodesy::direct(previous->turnExitPos, crs, dNm * SG_NM_TO_METER);
} }
waypoints[index].posValid = true; waypoints[index].posValid = true;
} else if (ty == "vectors") { } else if (ty == "vectors") {
waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous.turnExitPos, waypoints[index].pos); waypoints[index].legCourseTrue = SGGeodesy::courseDeg(previous->turnExitPos, waypoints[index].pos);
waypoints[index].legCourseValid = true; waypoints[index].legCourseValid = true;
// no turn data // no turn data
} }
@ -914,16 +915,43 @@ public:
} }
} }
const WayptData& previousValidWaypoint(int index) const WayptDataVec::iterator previousValidWaypoint(unsigned int index)
{ {
assert(index > 0); if (index == 0) {
if (waypoints[index-1].skipped) { return waypoints.end();
return waypoints[index-2];
} }
return waypoints[index-1]; while (waypoints[--index].skipped) {
// waypoint zero should be unskippable, this assert verified that
assert(index > 0);
}
return waypoints.begin() + index;
} }
WayptDataVec::iterator previousValidWaypoint(WayptDataVec::iterator it)
{
return previousValidWaypoint(std::distance(waypoints.begin(), it));
}
WayptDataVec::iterator nextValidWaypoint(int index)
{
return nextValidWaypoint(waypoints.begin() + index);
}
WayptDataVec::iterator nextValidWaypoint(WayptDataVec::iterator it)
{
if (it == waypoints.end()) {
return it;
}
++it;
while ((it != waypoints.end()) && it->skipped) {
++it;
}
return it;
}
}; // of RoutePathPrivate class }; // of RoutePathPrivate class
RoutePath::RoutePath(const flightgear::FlightPlan* fp) : RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
@ -932,7 +960,7 @@ RoutePath::RoutePath(const flightgear::FlightPlan* fp) :
for (int l=0; l<fp->numLegs(); ++l) { for (int l=0; l<fp->numLegs(); ++l) {
Waypt *wpt = fp->legAtIndex(l)->waypoint(); Waypt *wpt = fp->legAtIndex(l)->waypoint();
if (!wpt) { if (!wpt) {
SG_LOG(SG_NAVAID, SG_ALERT, "Waypoint " << l << " of " << fp->numLegs() << "is NULL"); SG_LOG(SG_NAVAID, SG_DEV_ALERT, "Waypoint " << l << " of " << fp->numLegs() << "is NULL");
break; break;
} }
d->waypoints.push_back(WayptData(wpt)); d->waypoints.push_back(WayptData(wpt));
@ -971,27 +999,24 @@ void RoutePath::commonInit()
double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi(); double radiusM = ((360.0 / d->pathTurnRate) * gs * SG_KT_TO_MPS) / SGMiscd::twopi();
if (i > 0) { if (i > 0) {
const WayptData& prev(d->previousValidWaypoint(i)); auto prevIt = d->previousValidWaypoint(i);
d->waypoints[i].computeLegCourse(prev, radiusM); assert(prevIt != d->waypoints.end());
d->waypoints[i].computeLegCourse(*prevIt, radiusM);
d->computeDynamicPosition(i); d->computeDynamicPosition(i);
} }
if (i < (d->waypoints.size() - 1)) { auto nextIt = d->nextValidWaypoint(i);
int nextIndex = i + 1; if (nextIt != d->waypoints.end()) {
if (d->waypoints[nextIndex].skipped) { nextIt->computeLegCourse(d->waypoints[i], radiusM);
nextIndex++;
}
WayptData& next(d->waypoints[nextIndex]);
next.computeLegCourse(d->waypoints[i], radiusM);
if (next.legCourseValid) { if (nextIt->legCourseValid) {
d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, next); d->waypoints[i].computeTurn(radiusM, d->constrainLegCourses, *nextIt);
} else { } else {
// next waypoint has indeterminate course. Let's create a sharp turn // next waypoint has indeterminate course. Let's create a sharp turn
// this can happen when the following point is ATC vectors, for example. // this can happen when the following point is ATC vectors, for example.
d->waypoints[i].turnEntryPos = d->waypoints[i].pos; d->waypoints[i].turnEntryPos = d->waypoints[i].pos;
d->waypoints[i].turnExitPos = d->waypoints[i].pos; d->waypoints[i].turnExitPos = d->waypoints[i].pos;
} }
} else { } else {
// final waypt, fix up some data // final waypt, fix up some data
d->waypoints[i].turnExitPos = d->waypoints[i].pos; d->waypoints[i].turnExitPos = d->waypoints[i].pos;
@ -1030,12 +1055,12 @@ SGGeodVec RoutePath::pathForIndex(int index) const
return pathForHold((Hold*) d->waypoints[index].wpt.get()); return pathForHold((Hold*) d->waypoints[index].wpt.get());
} }
if (index > 0) { auto prevIt = d->previousValidWaypoint(index);
const WayptData& prev(d->previousValidWaypoint(index)); if (prevIt != d->waypoints.end()) {
prev.turnExitPath(r); prevIt->turnExitPath(r);
SGGeod from = prev.turnExitPos, SGGeod from = prevIt->turnExitPos,
to = w.turnEntryPos; to = w.turnEntryPos;
// compute rounding offset, we want to round towards the direction of travel // compute rounding offset, we want to round towards the direction of travel
// which depends on the east/west sign of the longitude change // which depends on the east/west sign of the longitude change
@ -1089,16 +1114,17 @@ SGGeod RoutePath::positionForIndex(int index) const
SGGeodVec RoutePath::pathForVia(Via* via, int index) const SGGeodVec RoutePath::pathForVia(Via* via, int index) const
{ {
// previous waypoint must be valid for a VIA // previous waypoint must be valid for a VIA
if ((index == 0) || !d->waypoints[index-1].posValid) { auto prevIt = d->previousValidWaypoint(index);
if (prevIt == d->waypoints.end()) {
return SGGeodVec(); return SGGeodVec();
} }
const WayptData& prev(d->waypoints[index-1]); WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
SGGeodVec r; SGGeodVec r;
WayptVec::const_iterator it; WayptVec::const_iterator it;
SGGeod legStart = prev.wpt->position(); SGGeod legStart = prevIt->wpt->position();
for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) { for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) {
// interpolate directly into the result vector // interpolate directly into the result vector
interpolateGreatCircle(legStart, (*it)->position(), r); interpolateGreatCircle(legStart, (*it)->position(), r);
@ -1110,94 +1136,90 @@ SGGeodVec RoutePath::pathForVia(Via* via, int index) const
SGGeodVec RoutePath::pathForHold(Hold* hold) const SGGeodVec RoutePath::pathForHold(Hold* hold) const
{ {
int turnSteps = 16; int turnSteps = 16;
double hdg = hold->inboundRadial(); double hdg = hold->inboundRadial();
double turnDelta = 180.0 / turnSteps; double turnDelta = 180.0 / turnSteps;
double altFt = 0.0; // FIXME double altFt = 0.0; // FIXME
double gsKts = d->groundSpeedForAltitude(altFt); double gsKts = d->groundSpeedForAltitude(altFt);
SGGeodVec r;
double az2;
double stepTime = turnDelta / d->pathTurnRate; // in seconds
double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
double legDist = hold->isDistance() ?
hold->timeOrDistance()
: gsKts * (hold->timeOrDistance() / 3600.0);
legDist *= SG_NM_TO_METER;
if (hold->isLeftHanded()) {
turnDelta = -turnDelta;
}
SGGeod pos = hold->position();
r.push_back(pos);
// turn+leg sides are a mirror SGGeodVec r;
for (int j=0; j < 2; ++j) { double az2;
// turn double stepTime = turnDelta / d->pathTurnRate; // in seconds
for (int i=0;i<turnSteps; ++i) { double stepDist = gsKts * (stepTime / 3600.0) * SG_NM_TO_METER;
hdg += turnDelta; double legDist = hold->isDistance() ?
SGGeodesy::direct(pos, hdg, stepDist, pos, az2); hold->timeOrDistance()
r.push_back(pos); : gsKts * (hold->timeOrDistance() / 3600.0);
legDist *= SG_NM_TO_METER;
if (hold->isLeftHanded()) {
turnDelta = -turnDelta;
} }
SGGeod pos = hold->position();
// leg
SGGeodesy::direct(pos, hdg, legDist, pos, az2);
r.push_back(pos); r.push_back(pos);
} // of leg+turn duplication
// turn+leg sides are a mirror
return r; for (int j=0; j < 2; ++j) {
// turn
for (int i=0;i<turnSteps; ++i) {
hdg += turnDelta;
SGGeodesy::direct(pos, hdg, stepDist, pos, az2);
r.push_back(pos);
}
// leg
SGGeodesy::direct(pos, hdg, legDist, pos, az2);
r.push_back(pos);
} // of leg+turn duplication
return r;
} }
double RoutePath::computeDistanceForIndex(int index) const double RoutePath::computeDistanceForIndex(int index) const
{ {
if ((index < 0) || (index >= (int) d->waypoints.size())) { if ((index < 0) || (index >= (int) d->waypoints.size())) {
throw sg_range_exception("waypt index out of range", throw sg_range_exception("waypt index out of range",
"RoutePath::computeDistanceForIndex"); "RoutePath::computeDistanceForIndex");
} }
if (index == 0) {
// first waypoint, distance is 0
return 0.0;
}
if (d->waypoints[index].skipped) { auto it = d->waypoints.begin() + index;
if ((index == 0) || it->skipped) {
// first waypoint, distance is 0
return 0.0; return 0.0;
} }
if (it->wpt->type() == "via") {
if (d->waypoints[index].wpt->type() == "via") { return distanceForVia(static_cast<Via*>(it->wpt.get()), index);
return distanceForVia(static_cast<Via*>(d->waypoints[index].wpt.get()), index);
} }
const WayptData& prev(d->previousValidWaypoint(index)); auto prevIt = d->previousValidWaypoint(index);
double dist = SGGeodesy::distanceM(prev.turnExitPos, assert(prevIt != d->waypoints.end());
d->waypoints[index].turnEntryPos);
dist += prev.turnDistanceM(); double dist = SGGeodesy::distanceM(prevIt->turnExitPos, it->turnEntryPos);
dist += prevIt->turnDistanceM();
if (!d->waypoints[index].flyOver) {
// add entry distance if (!it->flyOver) {
dist += d->waypoints[index].turnDistanceM(); // add entry distance
} dist += it->turnDistanceM();
}
return dist;
return dist;
} }
double RoutePath::distanceForVia(Via* via, int index) const double RoutePath::distanceForVia(Via* via, int index) const
{ {
// previous waypoint must be valid for a VIA auto prevIt = d->previousValidWaypoint(index);
if ((index == 0) || !d->waypoints[index-1].posValid) { if (prevIt == d->waypoints.end()) {
return 0.0; return 0.0;
} }
const WayptData& prev(d->waypoints[index-1]); WayptVec enrouteWaypoints = via->expandToWaypoints(prevIt->wpt);
WayptVec enrouteWaypoints = via->expandToWaypoints(prev.wpt);
double dist = 0.0; double dist = 0.0;
WayptVec::const_iterator it; WayptVec::const_iterator it;
SGGeod legStart = prev.wpt->position(); SGGeod legStart = prevIt->wpt->position();
for (it = enrouteWaypoints.begin(); it != enrouteWaypoints.end(); ++it) { for (auto wp : enrouteWaypoints) {
dist += SGGeodesy::distanceM(legStart, (*it)->position()); dist += SGGeodesy::distanceM(legStart, wp->position());
legStart = (*it)->position(); legStart = wp->position();
} }
return dist; return dist;
@ -1205,7 +1227,6 @@ double RoutePath::distanceForVia(Via* via, int index) const
double RoutePath::trackForIndex(int index) const double RoutePath::trackForIndex(int index) const
{ {
if (d->waypoints[index].skipped) if (d->waypoints[index].skipped)
return trackForIndex(index - 1); return trackForIndex(index - 1);