FGFS-4.1/flightgear
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
e4411e8a6900451029598264df8eb78f72171f08
flightgear/src/Navaids/route.cxx
T
Your Name 4d531f8044 first commit
2022-10-20 20:29:11 +08:00

613 lines
17 KiB
C++
Raw Blame History

// route.cxx - classes supporting waypoints and route structures
// Written by James Turner, started 2009.
//
// Copyright (C) 2009 Curtis L. Olson
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#include "config.h"
#include "route.hxx"
// std
#include <map>
#include <fstream>
// SimGear
#include <simgear/structure/exception.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/magvar/magvar.hxx>
#include <simgear/timing/sg_time.hxx>
#include <simgear/io/iostreams/sgstream.hxx>
#include <simgear/misc/strutils.hxx>
#include <simgear/props/props_io.hxx>
// FlightGear
#include <Main/globals.hxx>
#include "Main/fg_props.hxx"
#include <Navaids/procedure.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/LevelDXML.hxx>
#include <Airports/airport.hxx>
#include <Navaids/airways.hxx>
using std::string;
using std::vector;
using std::endl;
using std::fstream;
namespace flightgear {
const double NO_MAG_VAR = -1000.0; // an impossible mag-var value
bool isMachRestrict(RouteRestriction rr)
{
return (rr == SPEED_RESTRICT_MACH) || (rr == SPEED_COMPUTED_MACH);
}
namespace {
double magvarDegAt(const SGGeod& pos)
{
double jd = globals->get_time_params()->getJD();
return sgGetMagVar(pos, jd) * SG_RADIANS_TO_DEGREES;
}
WayptRef intersectionFromString(FGPositionedRef p1,
const SGGeod& basePosition,
const double magvar,
const string_list& pieces)
{
assert(pieces.size() == 4);
// navid/radial/navid/radial notation
FGPositionedRef p2 = FGPositioned::findClosestWithIdent(pieces[2], basePosition);
if (!p2) {
SG_LOG(SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces[2]);
return {};
}
double r1 = atof(pieces[1].c_str()),
r2 = atof(pieces[3].c_str());
r1 += magvar;
r2 += magvar;
SGGeod intersection;
bool ok = SGGeodesy::radialIntersection(p1->geod(), r1, p2->geod(), r2, intersection);
if (!ok) {
SG_LOG(SG_NAVAID, SG_INFO, "no valid intersection for:" << pieces[0] << "/" << pieces[2]);
return {};
}
std::string name = p1->ident() + "-" + p2->ident();
return new BasicWaypt(intersection, name, nullptr);
}
WayptRef viaFromString(const SGGeod& basePosition, const std::string& target)
{
assert(target.find("VIA-") == 0);
string_list pieces(simgear::strutils::split(target.substr(4), "/"));
if (pieces.size() != 2) {
SG_LOG(SG_NAVAID, SG_WARN, "Malformed VIA specification string:" << target);
return {};
}
// TO navaid is pieces[1]
FGPositionedRef nav = FGPositioned::findClosestWithIdent(pieces[1], basePosition, nullptr);
if (!nav) {
SG_LOG(SG_NAVAID, SG_WARN, "TO navaid:" << pieces[3] << " unknown");
return {};
}
// airway ident is pieces[1]
AirwayRef airway = Airway::findByIdentAndNavaid(pieces[0], nav);
if (!airway) {
SG_LOG(SG_NAVAID, SG_WARN, "Unknown airway:" << pieces[0]);
return {};
}
return new Via(nullptr, airway, nav);
}
} // namespace
Waypt::Waypt(RouteBase* aOwner) :
_owner(aOwner),
_magVarDeg(NO_MAG_VAR)
{
}
Waypt::~Waypt()
{
}
std::string Waypt::ident() const
{
return {};
}
bool Waypt::flag(WayptFlag aFlag) const
{
return ((_flags & aFlag) != 0);
}
void Waypt::setFlag(WayptFlag aFlag, bool aV)
{
if (aFlag == 0) {
throw sg_range_exception("invalid waypoint flag set");
}
_flags = (_flags & ~aFlag);
if (aV) _flags |= aFlag;
}
bool Waypt::matches(Waypt* aOther) const
{
assert(aOther);
if (ident() != aOther->ident()) { // cheap check first
return false;
}
return matches(aOther->position());
}
bool Waypt::matches(FGPositioned* aPos) const
{
if (!aPos)
return false;
// if w ehave no source, match on position and ident
if (!source()) {
return (ident() == aPos->ident()) && matches(aPos->geod());
}
return (aPos == source());
}
bool Waypt::matches(const SGGeod& aPos) const
{
double d = SGGeodesy::distanceM(position(), aPos);
return (d < 100.0); // 100 metres seems plenty
}
void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict)
{
_altitudeFt = aAlt;
_altRestrict = aRestrict;
}
void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict)
{
_speed = aSpeed;
_speedRestrict = aRestrict;
}
double Waypt::speedKts() const
{
assert(_speedRestrict != SPEED_RESTRICT_MACH);
return speed();
}
double Waypt::speedMach() const
{
assert(_speedRestrict == SPEED_RESTRICT_MACH);
return speed();
}
double Waypt::magvarDeg() const
{
if (_magVarDeg == NO_MAG_VAR) {
// derived classes with a default pos must override this method
assert(!(position() == SGGeod()));
double jd = globals->get_time_params()->getJD();
_magVarDeg = sgGetMagVar(position(), jd) * SG_RADIANS_TO_DEGREES;
}
return _magVarDeg;
}
double Waypt::headingRadialDeg() const
{
return 0.0;
}
std::string Waypt::icaoDescription() const
{
return ident();
}
///////////////////////////////////////////////////////////////////////////
// persistence
static RouteRestriction restrictionFromString(const char* aStr)
{
std::string l = simgear::strutils::lowercase(aStr);
if (l == "at") return RESTRICT_AT;
if (l == "above") return RESTRICT_ABOVE;
if (l == "below") return RESTRICT_BELOW;
if (l == "none") return RESTRICT_NONE;
if (l == "mach") return SPEED_RESTRICT_MACH;
if (l.empty()) return RESTRICT_NONE;
throw sg_io_exception("unknown restriction specification:" + l,
"Route restrictFromString");
}
const char* restrictionToString(RouteRestriction aRestrict)
{
switch (aRestrict) {
case RESTRICT_AT: return "at";
case RESTRICT_BELOW: return "below";
case RESTRICT_ABOVE: return "above";
case RESTRICT_NONE: return "none";
case SPEED_RESTRICT_MACH: return "mach";
default:
throw sg_exception("invalid route restriction",
"Route restrictToString");
}
}
WayptRef Waypt::createInstance(RouteBase* aOwner, const std::string& aTypeName)
{
WayptRef r;
if (aTypeName == "basic") {
r = new BasicWaypt(aOwner);
} else if (aTypeName == "navaid") {
r = new NavaidWaypoint(aOwner);
} else if (aTypeName == "offset-navaid") {
r = new OffsetNavaidWaypoint(aOwner);
} else if (aTypeName == "hold") {
r = new Hold(aOwner);
} else if (aTypeName == "runway") {
r = new RunwayWaypt(aOwner);
} else if (aTypeName == "hdgToAlt") {
r = new HeadingToAltitude(aOwner);
} else if (aTypeName == "dmeIntercept") {
r = new DMEIntercept(aOwner);
} else if (aTypeName == "radialIntercept") {
r = new RadialIntercept(aOwner);
} else if (aTypeName == "vectors") {
r = new ATCVectors(aOwner);
} else if (aTypeName == "discontinuity") {
r = new Discontinuity(aOwner);
} else if (aTypeName == "via") {
r = new Via(aOwner);
}
if (!r || (r->type() != aTypeName)) {
throw sg_exception("broken factory method for type:" + aTypeName,
"Waypt::createInstance");
}
return r;
}
WayptRef Waypt::createFromProperties(RouteBase* aOwner, SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("type")) {
SG_LOG(SG_GENERAL, SG_WARN, "Bad waypoint node: missing type");
return {};
}
flightgear::AirwayRef via;
if (aProp->hasChild("airway")) {
auto level = Airway::Both;
if (aProp->hasValue("network")) {
level = static_cast<flightgear::Airway::Level>(aProp->getIntValue("network"));
}
via = flightgear::Airway::findByIdent(aProp->getStringValue("airway"), level);
if (via) {
// override owner if we are from an airway
aOwner = via.get();
}
}
WayptRef nd(createInstance(aOwner, aProp->getStringValue("type")));
if (nd->initFromProperties(aProp)) {
return nd;
}
SG_LOG(SG_GENERAL, SG_WARN, "failed to create waypoint, trying basic");
// if we failed to make the waypoint, try again making a basic waypoint.
// this handles the case where a navaid waypoint is missing, for example
// we also reject navaids that don't look correct (too far form the specified
// lat-lon, eg see https://sourceforge.net/p/flightgear/codetickets/1814/ )
// and again fallback to here.
WayptRef bw(new BasicWaypt(aOwner));
if (bw->initFromProperties(aProp)) {
return bw;
}
return {}; // total failure
}
WayptRef Waypt::fromLatLonString(RouteBase* aOwner, const std::string& target)
{
// permit lat,lon/radial/offset format
string_list pieces(simgear::strutils::split(target, "/"));
if ((pieces.size() != 1) && (pieces.size() != 3)) {
return {};
}
SGGeod g;
const bool defaultToLonLat = true; // parseStringAsGeod would otherwise default to lat,lon
if (!simgear::strutils::parseStringAsGeod(pieces[0], &g, defaultToLonLat)) {
return {};
}
if (pieces.size() == 3) {
// process offset
const double bearing = std::stod(pieces[1]);
const double distanceNm = std::stod(pieces[2]);
g = SGGeodesy::direct(g, bearing, distanceNm * SG_NM_TO_METER);
}
// build a short name
const int lonDeg = static_cast<int>(g.getLongitudeDeg());
const int latDeg = static_cast<int>(g.getLatitudeDeg());
char buf[32];
char ew = (lonDeg < 0) ? 'W' : 'E';
char ns = (latDeg < 0) ? 'S' : 'N';
snprintf(buf, 32, "%c%03d%c%03d", ew, std::abs(lonDeg), ns, std::abs(latDeg));
return new BasicWaypt(g, buf, aOwner);
}
WayptRef Waypt::createFromString(RouteBase* aOwner, const std::string& s, const SGGeod& aVicinity)
{
auto vicinity = aVicinity;
if (!vicinity.isValid()) {
vicinity = globals->get_aircraft_position();
}
auto target = simgear::strutils::uppercase(s);
// extract altitude
double altFt = 0.0;
RouteRestriction altSetting = RESTRICT_NONE;
size_t pos = target.find('@');
if (pos != string::npos) {
altFt = std::stof(target.substr(pos + 1));
target = target.substr(0, pos);
if (fgGetString("/sim/startup/units") == "meter") {
altFt *= SG_METER_TO_FEET;
}
altSetting = RESTRICT_AT;
}
// check for lon,lat
WayptRef wpt = fromLatLonString(aOwner, target);
const double magvar = magvarDegAt(vicinity);
if (wpt) {
// already handled in the lat/lon test above
} else if (target.find("VIA-") == 0) {
wpt = viaFromString(vicinity, target);
} else {
FGPositioned::TypeFilter filter({FGPositioned::Type::AIRPORT, FGPositioned::Type::HELIPORT,
FGPositioned::Type::SEAPORT, FGPositioned::Type::NDB, FGPositioned::Type::VOR,
FGPositioned::Type::FIX, FGPositioned::Type::WAYPOINT});
string_list pieces(simgear::strutils::split(target, "/"));
FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), vicinity, &filter);
if (!p) {
SG_LOG(SG_NAVAID, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
return {};
}
if (pieces.size() == 1) {
wpt = new NavaidWaypoint(p, aOwner);
} else if (pieces.size() == 3) {
// navaid/radial/distance-nm notation
double radial = atof(pieces[1].c_str()),
distanceNm = atof(pieces[2].c_str());
radial += magvar;
wpt = new OffsetNavaidWaypoint(p, aOwner, radial, distanceNm);
} else if (pieces.size() == 2) {
FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
if (!apt) {
SG_LOG(SG_NAVAID, SG_INFO, "Waypoint is not an airport:" << pieces.front());
return {};
}
if (!apt->hasRunwayWithIdent(pieces[1])) {
SG_LOG(SG_NAVAID, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
return {};
}
FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
wpt = new NavaidWaypoint(runway, aOwner);
} else if (pieces.size() == 4) {
wpt = intersectionFromString(p, vicinity, magvar, pieces);
}
}
if (!wpt) {
SG_LOG(SG_NAVAID, SG_INFO, "Unable to parse waypoint:" << target);
return {};
}
if (altSetting != RESTRICT_NONE) {
wpt->setAltitude(altFt, altSetting);
}
return wpt;
}
void Waypt::saveAsNode(SGPropertyNode* n) const
{
n->setStringValue("type", type());
writeToProperties(n);
}
bool Waypt::initFromProperties(SGPropertyNode_ptr aProp)
{
if (aProp->hasChild("generated")) {
setFlag(WPT_GENERATED, aProp->getBoolValue("generated"));
}
if (aProp->hasChild("overflight")) {
setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight"));
}
if (aProp->hasChild("arrival")) {
setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival"));
}
if (aProp->hasChild("approach")) {
setFlag(WPT_APPROACH, aProp->getBoolValue("approach"));
}
if (aProp->hasChild("departure")) {
setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure"));
}
if (aProp->hasChild("miss")) {
setFlag(WPT_MISS, aProp->getBoolValue("miss"));
}
if (aProp->hasChild("airway")) {
setFlag(WPT_VIA, true);
}
if (aProp->hasChild("alt-restrict")) {
_altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict").c_str());
_altitudeFt = aProp->getDoubleValue("altitude-ft");
}
if (aProp->hasChild("speed-restrict")) {
_speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict").c_str());
_speed = aProp->getDoubleValue("speed");
}
return true;
}
void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const
{
if (flag(WPT_OVERFLIGHT)) {
aProp->setBoolValue("overflight", true);
}
if (flag(WPT_DEPARTURE)) {
aProp->setBoolValue("departure", true);
}
if (flag(WPT_ARRIVAL)) {
aProp->setBoolValue("arrival", true);
}
if (flag(WPT_APPROACH)) {
aProp->setBoolValue("approach", true);
}
if (flag(WPT_VIA) && _owner) {
flightgear::AirwayRef awy = (flightgear::Airway*) _owner;
aProp->setStringValue("airway", awy->ident());
aProp->setIntValue("network", awy->level());
}
if (flag(WPT_MISS)) {
aProp->setBoolValue("miss", true);
}
if (flag(WPT_GENERATED)) {
aProp->setBoolValue("generated", true);
}
if (_altRestrict != RESTRICT_NONE) {
aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict));
aProp->setDoubleValue("altitude-ft", _altitudeFt);
}
if (_speedRestrict != RESTRICT_NONE) {
aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict));
aProp->setDoubleValue("speed", _speed);
}
}
RouteBase::~RouteBase()
{
}
void RouteBase::dumpRouteToKML(const WayptVec& aRoute, const std::string& aName)
{
SGPath p = "/Users/jmt/Desktop/" + aName + ".kml";
sg_ofstream f(p);
if (!f.is_open()) {
SG_LOG(SG_NAVAID, SG_WARN, "unable to open:" << p);
return;
}
// pre-amble
f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
"<Document>\n";
dumpRouteToKMLLineString(aName, aRoute, f);
// post-amble
f << "</Document>\n"
"</kml>" << endl;
f.close();
}
void RouteBase::dumpRouteToKMLLineString(const std::string& aIdent,
const WayptVec& aRoute, std::ostream& aStream)
{
// preamble
aStream << "<Placemark>\n";
aStream << "<name>" << aIdent << "</name>\n";
aStream << "<LineString>\n";
aStream << "<tessellate>1</tessellate>\n";
aStream << "<coordinates>\n";
// waypoints
for (unsigned int i=0; i<aRoute.size(); ++i) {
SGGeod pos = aRoute[i]->position();
aStream << pos.getLongitudeDeg() << "," << pos.getLatitudeDeg() << " " << endl;
}
// postable
aStream << "</coordinates>\n"
"</LineString>\n"
"</Placemark>\n" << endl;
}
void RouteBase::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt)
{
assert(aApt);
try {
NavdataVisitor visitor(aApt, aPath);
readXML(aPath, visitor);
} catch (sg_io_exception& ex) {
SG_LOG(SG_NAVAID, SG_WARN, "failure parsing procedures: " << aPath <<
"\n\t" << ex.getMessage() << "\n\tat:" << ex.getLocation().asString());
} catch (sg_exception& ex) {
SG_LOG(SG_NAVAID, SG_WARN, "failure parsing procedures: " << aPath <<
"\n\t" << ex.getMessage());
}
}
} // of namespace flightgear
Reference in New Issue View Git Blame Copy Permalink