fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/Navaids/route.cxx

713 lines
19 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.
#ifdef HAVE_CONFIG_H
# include "config.h"
#endif
#include "route.hxx"
// std
#include <map>
#include <fstream>
// Boost
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/algorithm/string.hpp>
// SimGear
#include <simgear/structure/exception.hxx>
#include <simgear/xml/easyxml.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/magvar/magvar.hxx>
#include <simgear/timing/sg_time.hxx>
// FlightGear
#include <Main/globals.hxx>
#include <Navaids/procedure.hxx>
#include <Navaids/waypoint.hxx>
#include <Airports/simple.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
Waypt::Waypt(Route* aOwner) :
_altitudeFt(0.0),
_speed(0.0),
_altRestrict(RESTRICT_NONE),
_speedRestrict(RESTRICT_NONE),
_owner(aOwner),
_flags(0),
_magVarDeg(NO_MAG_VAR)
{
}
std::string Waypt::ident() const
{
return "";
}
bool Waypt::flag(WayptFlag aFlag) const
{
return ((_flags & aFlag) != 0);
}
void Waypt::setFlag(WayptFlag aFlag, bool aV)
{
_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(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();
}
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
{
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);
}
return _magVarDeg;
}
///////////////////////////////////////////////////////////////////////////
// persistence
static RouteRestriction restrictionFromString(const char* aStr)
{
std::string l = boost::to_lower_copy(std::string(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");
}
static 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");
}
}
Waypt* Waypt::createInstance(Route* aOwner, const std::string& aTypeName)
{
Waypt* r = NULL;
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);
}
if (!r || (r->type() != aTypeName)) {
throw sg_exception("broken factory method for type:" + aTypeName,
"Waypt::createInstance");
}
return r;
}
WayptRef Waypt::createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("type")) {
throw sg_io_exception("bad props node, no type provided",
"Waypt::createFromProperties");
}
WayptRef nd(createInstance(aOwner, aProp->getStringValue("type")));
nd->initFromProperties(aProp);
return nd;
}
void Waypt::saveAsNode(SGPropertyNode* n) const
{
n->setStringValue("type", type());
writeToProperties(n);
}
void 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("departure")) {
setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure"));
}
if (aProp->hasChild("miss")) {
setFlag(WPT_MISS, aProp->getBoolValue("miss"));
}
if (aProp->hasChild("alt-restrict")) {
_altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict"));
_altitudeFt = aProp->getDoubleValue("altitude-ft");
}
if (aProp->hasChild("speed-restrict")) {
_speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict"));
_speed = aProp->getDoubleValue("speed");
}
}
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_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);
}
}
void Route::dumpRouteToFile(const WayptVec& aRoute, const std::string& aName)
{
SGPath p = "/Users/jmt/Desktop/" + aName + ".kml";
std::fstream f;
f.open(p.str().c_str(), fstream::out | fstream::app);
if (!f.is_open()) {
SG_LOG(SG_GENERAL, SG_WARN, "unable to open:" << p.str());
return;
}
// pre-amble
f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
"<Document>\n";
dumpRouteToLineString(aName, aRoute, f);
// post-amble
f << "</Document>\n"
"</kml>" << endl;
f.close();
}
void Route::dumpRouteToLineString(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;
}
///////////////////////////////////////////////////////////////////////////
class NavdataVisitor : public XMLVisitor {
public:
NavdataVisitor(FGAirport* aApt, const SGPath& aPath);
protected:
virtual void startXML ();
virtual void endXML ();
virtual void startElement (const char * name, const XMLAttributes &atts);
virtual void endElement (const char * name);
virtual void data (const char * s, int len);
virtual void pi (const char * target, const char * data);
virtual void warning (const char * message, int line, int column);
virtual void error (const char * message, int line, int column);
private:
Waypt* buildWaypoint();
void processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts);
void finishApproach();
void finishSid();
void finishStar();
FGAirport* _airport;
SGPath _path;
string _text; ///< last element text value
SID* _sid;
STAR* _star;
Approach* _approach;
WayptVec _waypoints; ///< waypoint list for current approach/sid/star
WayptVec _transWaypts; ///< waypoint list for current transition
string _wayptName;
string _wayptType;
string _ident; // id of segment under construction
string _transIdent;
double _longitude, _latitude, _altitude, _speed;
RouteRestriction _altRestrict;
double _holdRadial; // inbound hold radial, or -1 if radial is 'inbound'
double _holdTD; ///< hold time (seconds) or distance (nm), based on flag below
bool _holdRighthanded;
bool _holdDistance; // true, TD is distance in nm; false, TD is time in seconds
double _course, _radial, _dmeDistance;
};
void Route::loadAirportProcedures(const SGPath& aPath, FGAirport* aApt)
{
assert(aApt);
try {
NavdataVisitor visitor(aApt, aPath);
readXML(aPath.str(), visitor);
} catch (sg_io_exception& ex) {
SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
"\n\t" << ex.getMessage() << "\n\tat:" << ex.getLocation().asString());
} catch (sg_exception& ex) {
SG_LOG(SG_GENERAL, SG_WARN, "failured parsing procedures: " << aPath.str() <<
"\n\t" << ex.getMessage());
}
}
NavdataVisitor::NavdataVisitor(FGAirport* aApt, const SGPath& aPath):
_airport(aApt),
_path(aPath),
_sid(NULL),
_star(NULL),
_approach(NULL)
{
}
void NavdataVisitor::startXML()
{
}
void NavdataVisitor::endXML()
{
}
void NavdataVisitor::startElement(const char* name, const XMLAttributes &atts)
{
_text.clear();
string tag(name);
if (tag == "Airport") {
string icao(atts.getValue("ICAOcode"));
if (_airport->ident() != icao) {
throw sg_format_exception("Airport and ICAO mismatch", icao, _path.str());
}
} else if (tag == "Sid") {
string ident(atts.getValue("Name"));
_sid = new SID(ident);
_waypoints.clear();
processRunways(_sid, atts);
} else if (tag == "Star") {
string ident(atts.getValue("Name"));
_star = new STAR(ident);
_waypoints.clear();
processRunways(_star, atts);
} else if ((tag == "Sid_Waypoint") ||
(tag == "App_Waypoint") ||
(tag == "Star_Waypoint") ||
(tag == "AppTr_Waypoint") ||
(tag == "SidTr_Waypoint") ||
(tag == "RwyTr_Waypoint"))
{
// reset waypoint data
_speed = 0.0;
_altRestrict = RESTRICT_NONE;
_altitude = 0.0;
} else if (tag == "Approach") {
_ident = atts.getValue("Name");
_waypoints.clear();
_approach = new Approach(_ident);
} else if ((tag == "Sid_Transition") ||
(tag == "App_Transition") ||
(tag == "Star_Transition")) {
_transIdent = atts.getValue("Name");
_transWaypts.clear();
} else if (tag == "RunwayTransition") {
_transIdent = atts.getValue("Runway");
_transWaypts.clear();
} else {
}
}
void NavdataVisitor::processRunways(ArrivalDeparture* aProc, const XMLAttributes &atts)
{
string v("All");
if (atts.hasAttribute("Runways")) {
v = atts.getValue("Runways");
}
if (v == "All") {
for (unsigned int r=0; r<_airport->numRunways(); ++r) {
aProc->addRunway(_airport->getRunwayByIndex(r));
}
return;
}
vector<string> rwys;
boost::split(rwys, v, boost::is_any_of(" ,"));
for (unsigned int r=0; r<rwys.size(); ++r) {
FGRunway* rwy = _airport->getRunwayByIdent(rwys[r]);
aProc->addRunway(rwy);
}
}
void NavdataVisitor::endElement(const char* name)
{
string tag(name);
if ((tag == "Sid_Waypoint") ||
(tag == "App_Waypoint") ||
(tag == "Star_Waypoint"))
{
_waypoints.push_back(buildWaypoint());
} else if ((tag == "AppTr_Waypoint") ||
(tag == "SidTr_Waypoint") ||
(tag == "RwyTr_Waypoint") ||
(tag == "StarTr_Waypoint"))
{
_transWaypts.push_back(buildWaypoint());
} else if (tag == "Sid_Transition") {
assert(_sid);
// SID waypoints are stored backwards, to share code with STARs
std::reverse(_transWaypts.begin(), _transWaypts.end());
Transition* t = new Transition(_transIdent, _sid, _transWaypts);
_sid->addTransition(t);
} else if (tag == "Star_Transition") {
assert(_star);
Transition* t = new Transition(_transIdent, _star, _transWaypts);
_star->addTransition(t);
} else if (tag == "App_Transition") {
assert(_approach);
Transition* t = new Transition(_transIdent, _approach, _transWaypts);
_approach->addTransition(t);
} else if (tag == "RunwayTransition") {
ArrivalDeparture* ad;
if (_sid) {
// SID waypoints are stored backwards, to share code with STARs
std::reverse(_transWaypts.begin(), _transWaypts.end());
ad = _sid;
} else {
ad = _star;
}
Transition* t = new Transition(_transIdent, ad, _transWaypts);
FGRunwayRef rwy = _airport->getRunwayByIdent(_transIdent);
ad->addRunwayTransition(rwy, t);
} else if (tag == "Approach") {
finishApproach();
} else if (tag == "Sid") {
finishSid();
} else if (tag == "Star") {
finishStar();
} else if (tag == "Longitude") {
_longitude = atof(_text.c_str());
} else if (tag == "Latitude") {
_latitude = atof(_text.c_str());
} else if (tag == "Name") {
_wayptName = _text;
} else if (tag == "Type") {
_wayptType = _text;
} else if (tag == "Speed") {
_speed = atoi(_text.c_str());
} else if (tag == "Altitude") {
_altitude = atof(_text.c_str());
} else if (tag == "AltitudeRestriction") {
if (_text == "at") {
_altRestrict = RESTRICT_AT;
} else if (_text == "above") {
_altRestrict = RESTRICT_ABOVE;
} else if (_text == "below") {
_altRestrict = RESTRICT_BELOW;
} else {
throw sg_format_exception("Unrecognized altitude restriction", _text);
}
} else if (tag == "Hld_Rad_or_Inbd") {
if (_text == "Inbd") {
_holdRadial = -1.0;
}
} else if (tag == "Hld_Time_or_Dist") {
_holdDistance = (_text == "Dist");
} else if (tag == "Hld_Rad_value") {
_holdRadial = atof(_text.c_str());
} else if (tag == "Hld_Turn") {
_holdRighthanded = (_text == "Right");
} else if (tag == "Hld_td_value") {
_holdTD = atof(_text.c_str());
} else if (tag == "Hdg_Crs_value") {
_course = atof(_text.c_str());
} else if (tag == "DMEtoIntercept") {
_dmeDistance = atof(_text.c_str());
} else if (tag == "RadialtoIntercept") {
_radial = atof(_text.c_str());
} else {
}
}
Waypt* NavdataVisitor::buildWaypoint()
{
Waypt* wp = NULL;
if (_wayptType == "Normal") {
// new LatLonWaypoint
SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
wp = new BasicWaypt(pos, _wayptName, NULL);
} else if (_wayptType == "Runway") {
string ident = _wayptName.substr(2);
FGRunwayRef rwy = _airport->getRunwayByIdent(ident);
wp = new RunwayWaypt(rwy, NULL);
} else if (_wayptType == "Hold") {
SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
Hold* h = new Hold(pos, _wayptName, NULL);
wp = h;
if (_holdRighthanded) {
h->setRightHanded();
} else {
h->setLeftHanded();
}
if (_holdDistance) {
h->setHoldDistance(_holdTD);
} else {
h->setHoldTime(_holdTD * 60.0);
}
if (_holdRadial >= 0.0) {
h->setHoldRadial(_holdRadial);
}
} else if (_wayptType == "Vectors") {
wp = new ATCVectors(NULL, _airport);
} else if ((_wayptType == "Intc") || (_wayptType == "VorRadialIntc")) {
SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
wp = new RadialIntercept(NULL, _wayptName, pos, _course, _radial);
} else if (_wayptType == "DmeIntc") {
SGGeod pos(SGGeod::fromDeg(_longitude, _latitude));
wp = new DMEIntercept(NULL, _wayptName, pos, _course, _dmeDistance);
} else if (_wayptType == "ConstHdgtoAlt") {
wp = new HeadingToAltitude(NULL, _wayptName, _course);
} else {
SG_LOG(SG_GENERAL, SG_ALERT, "implement waypoint type:" << _wayptType);
throw sg_format_exception("Unrecognized waypt type", _wayptType);
}
assert(wp);
if ((_altitude > 0.0) && (_altRestrict != RESTRICT_NONE)) {
wp->setAltitude(_altitude,_altRestrict);
}
if (_speed > 0.0) {
wp->setSpeed(_speed, RESTRICT_AT); // or _BELOW?
}
return wp;
}
void NavdataVisitor::finishApproach()
{
WayptVec::iterator it;
FGRunwayRef rwy;
// find the runway node
for (it = _waypoints.begin(); it != _waypoints.end(); ++it) {
FGPositionedRef navid = (*it)->source();
if (!navid) {
continue;
}
if (navid->type() == FGPositioned::RUNWAY) {
rwy = (FGRunway*) navid.get();
break;
}
}
if (!rwy) {
throw sg_format_exception("Malformed approach, no runway waypt", _ident);
}
WayptVec primary(_waypoints.begin(), it);
// erase all points up to and including the runway, to leave only the
// missed segments
_waypoints.erase(_waypoints.begin(), ++it);
_approach->setRunway(rwy);
_approach->setPrimaryAndMissed(primary, _waypoints);
_airport->addApproach(_approach);
_approach = NULL;
}
void NavdataVisitor::finishSid()
{
// reverse order, because that's how we deal with commonality between
// STARs and SIDs. SID::route undoes this
std::reverse(_waypoints.begin(), _waypoints.end());
_sid->setCommon(_waypoints);
_airport->addSID(_sid);
_sid = NULL;
}
void NavdataVisitor::finishStar()
{
_star->setCommon(_waypoints);
_airport->addSTAR(_star);
_star = NULL;
}
void NavdataVisitor::data (const char * s, int len)
{
_text += string(s, len);
}
void NavdataVisitor::pi (const char * target, const char * data) {
//cout << "Processing instruction " << target << ' ' << data << endl;
}
void NavdataVisitor::warning (const char * message, int line, int column) {
SG_LOG(SG_IO, SG_WARN, "Warning: " << message << " (" << line << ',' << column << ')');
}
void NavdataVisitor::error (const char * message, int line, int column) {
SG_LOG(SG_IO, SG_ALERT, "Error: " << message << " (" << line << ',' << column << ')');
}
} // of namespace flightgear
View git blame Copy permalink