fly/flightgear
1
0
Fork 0
Code Activity

Merge branch 'next' of git://gitorious.org/fg/flightgear into next

Browse source
This commit is contained in:
ThorstenB 2010-10-20 20:43:15 +02:00
commit 622f71c01c
45 changed files with 6251 additions and 949 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
projects/VC90/FlightGear/FlightGear.vcproj
View file

@ -4203,6 +4203,46 @@
RelativePath="..\..\..\src\Navaids\positioned.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\airways.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\airways.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\procedure.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\procedure.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\route.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\route.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\routePath.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\routePath.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\waypoint.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Navaids\waypoint.hxx"
>
</File>
</Filter>
<Filter
Name="Lib_Network"
@ -4855,6 +4895,14 @@
RelativePath="..\..\..\src\Instrumentation\gps.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Instrumentation\rnav_waypt_controller.cxx"
>
</File>
<File
RelativePath="..\..\..\src\Instrumentation\rnav_waypt_controller.hxx"
>
</File>
<File
RelativePath="..\..\..\src\Instrumentation\groundradar.cxx"
>

1
src/AIModel/AIAircraft.cxx
View file

@ -29,6 +29,7 @@
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/simple.hxx>
#include <string>
#include <math.h>

25
src/AIModel/AIBase.cxx
View file

@ -194,18 +194,27 @@ void FGAIBase::Transform() {
}
bool FGAIBase::init(bool search_in_AI_path) {
osg::ref_ptr<osgDB::ReaderWriter::Options> opt=
new osgDB::ReaderWriter::Options(*osgDB::Registry::instance()->getOptions());
string f;
if(search_in_AI_path)
{
SGPath ai_path(globals->get_fg_root());
ai_path.append("AI");
opt->getDatabasePathList().push_front(ai_path.str());
// setup a modified Options strucutre, with only the $fg-root/AI defined;
// we'll check that first, then give the normal search logic a chance.
// this ensures that models in AI/ are preferred to normal models, where
// both exist.
osg::ref_ptr<osgDB::ReaderWriter::Options>
opt(osg::clone(osgDB::Registry::instance()->getOptions(), osg::CopyOp::SHALLOW_COPY));
SGPath ai_path(globals->get_fg_root(), "AI");
opt->setDatabasePath(ai_path.str());
f = osgDB::findDataFile(model_path, opt.get());
}
string f = osgDB::findDataFile(model_path, opt.get());
if (f.empty()) {
f = simgear::SGModelLib::findDataFile(model_path);
}
if(f.empty())
f = fgGetString("/sim/multiplay/default-model", default_model);

65
src/AIModel/AIFlightPlan.hxx
View file

@ -24,26 +24,18 @@
#include <string>
#include <Airports/simple.hxx>
#include <Navaids/awynet.hxx>
#include "AIBase.hxx"
using std::vector;
using std::string;
class FGTaxiRoute;
class FGRunway;
class FGAIAircraft;
class FGAirport;
class SGGeod;
class FGAIFlightPlan {
public:
typedef struct {
string name;
std::string name;
double latitude;
double longitude;
double altitude;
@ -56,11 +48,11 @@ public:
int routeIndex; // For AI/ATC purposes;
double time_sec;
double trackLength; // distance from previous waypoint (for AI purposes);
string time;
std::string time;
} waypoint;
FGAIFlightPlan();
FGAIFlightPlan(const string& filename);
FGAIFlightPlan(const std::string& filename);
FGAIFlightPlan(FGAIAircraft *,
const std::string& p,
double course,
@ -73,9 +65,9 @@ public:
double lat,
double lon,
double speed,
const string& fltType,
const string& acType,
const string& airline);
const std::string& fltType,
const std::string& acType,
const std::string& airline);
~FGAIFlightPlan();
waypoint* const getPreviousWaypoint( void ) const;
@ -91,18 +83,18 @@ public:
double getLeadDistance( void ) const {return lead_distance;}
double getBearing(waypoint* previous, waypoint* next) const;
double getBearing(double lat, double lon, waypoint* next) const;
double checkTrackLength(string wptName);
double checkTrackLength(std::string wptName);
time_t getStartTime() const { return start_time; }
time_t getArrivalTime() const { return arrivalTime; }
void create(FGAIAircraft *, FGAirport *dep, FGAirport *arr, int leg, double alt, double speed, double lat, double lon,
bool firstLeg, double radius, const string& fltType, const string& aircraftType, const string& airline, double distance);
bool firstLeg, double radius, const std::string& fltType, const std::string& aircraftType, const std::string& airline, double distance);
void setLeg(int val) { leg = val;}
void setTime(time_t st) { start_time = st; }
int getGate() const { return gateId; }
double getLeadInAngle() const { return leadInAngle; }
const string& getRunway() const;
const std::string& getRunway() const;
void setRepeat(bool r) { repeat = r; }
bool getRepeat(void) const { return repeat; }
@ -111,16 +103,16 @@ public:
int getRouteIndex(int i); // returns the AI related index of this current routes.
FGTaxiRoute *getTaxiRoute() { return taxiRoute; }
void deleteTaxiRoute();
string getRunway() { return activeRunway; }
std::string getRunway() { return activeRunway; }
bool isActive(time_t time) {return time >= this->getStartTime();}
void setRunway(string rwy) { activeRunway = rwy; };
string getRunwayClassFromTrafficType(string fltType);
void setRunway(std::string rwy) { activeRunway = rwy; };
std::string getRunwayClassFromTrafficType(std::string fltType);
void addWaypoint(waypoint* wpt) { waypoints.push_back(wpt); };
void setName(string n) { name = n; };
string getName() { return name; };
void setName(std::string n) { name = n; };
std::string getName() { return name; };
void setSID(FGAIFlightPlan* fp) { sid = fp;};
FGAIFlightPlan* getSID() { return sid; };
@ -128,7 +120,7 @@ public:
private:
FGRunway* rwy;
FGAIFlightPlan *sid;
typedef vector <waypoint*> wpt_vector_type;
typedef std::vector <waypoint*> wpt_vector_type;
typedef wpt_vector_type::const_iterator wpt_vector_iterator;
@ -143,26 +135,25 @@ private:
time_t arrivalTime; // For AI/ATC purposes.
int leg;
int gateId, lastNodeVisited;
string activeRunway;
FGAirRoute airRoute;
std::string activeRunway;
FGTaxiRoute *taxiRoute;
string name;
std::string name;
void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&);
void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const string&, const string&, const string&);
void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const string&);
void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const string&);
void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const string&);
void createDescent(FGAIAircraft *, FGAirport *, double latitude, double longitude, double speed, double alt,const string&, double distance);
void createLanding(FGAIAircraft *, FGAirport *, const string&);
void createPushBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&);
void createPushBackFallBack(FGAIAircraft *, bool, FGAirport*, double, double, double, const std::string&, const std::string&, const std::string&);
void createTakeOff(FGAIAircraft *, bool, FGAirport *, double, const std::string&);
void createClimb(FGAIAircraft *, bool, FGAirport *, double, double, const std::string&);
void createCruise(FGAIAircraft *, bool, FGAirport*, FGAirport*, double, double, double, double, const std::string&);
void createDescent(FGAIAircraft *, FGAirport *, double latitude, double longitude, double speed, double alt,const std::string&, double distance);
void createLanding(FGAIAircraft *, FGAirport *, const std::string&);
void createParking(FGAIAircraft *, FGAirport *, double radius);
void deleteWaypoints();
void resetWaypoints();
void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline);
void createLandingTaxi(FGAIAircraft *, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline);
void createDefaultLandingTaxi(FGAIAircraft *, FGAirport* aAirport);
void createDefaultTakeoffTaxi(FGAIAircraft *, FGAirport* aAirport, FGRunway* aRunway);
void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline);
void createTakeoffTaxi(FGAIAircraft *, bool firstFlight, FGAirport *apt, double radius, const std::string& fltType, const std::string& acType, const std::string& airline);
double getTurnRadius(double, bool);

1
src/AIModel/AIFlightPlanCreate.cxx
View file

@ -27,6 +27,7 @@
#include <simgear/props/props.hxx>
#include <simgear/props/props_io.hxx>
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
#include <Airports/dynamics.hxx>
#include "AIAircraft.hxx"

5
src/AIModel/AIFlightPlanCreateCruise.cxx
View file

@ -27,7 +27,7 @@
#include <iostream>
#include <simgear/route/waypoint.hxx>
#include <Navaids/awynet.hxx>
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
#include <Airports/dynamics.hxx>
@ -40,6 +40,7 @@
using std::iostream;
/*
void FGAIFlightPlan::evaluateRoutePart(double deplat,
double deplon,
double arrlat,
@ -97,7 +98,7 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
}
}
*/
/*
void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
FGAirport *arr, double latitude,

2
src/AIModel/AIFlightPlanCreatePushBack.cxx
View file

@ -23,6 +23,8 @@
#endif
#include <simgear/math/sg_geodesy.hxx>
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
#include <Airports/dynamics.hxx>

2
src/ATC/trafficcontrol.cxx
View file

@ -34,7 +34,7 @@
#include <Traffic/TrafficMgr.hxx>
#include <Airports/groundnetwork.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/simple.hxx>
using std::sort;

1
src/Airports/groundnetwork.cxx
View file

@ -30,6 +30,7 @@
#include <simgear/debug/logstream.hxx>
#include <simgear/route/waypoint.hxx>
#include <Airports/simple.hxx>
#include <Airports/dynamics.hxx>
#include <AIModel/AIAircraft.hxx>

30
src/Airports/runways.cxx
View file

@ -37,6 +37,9 @@
#include "runways.hxx"
#include <Airports/simple.hxx>
#include <Navaids/procedure.hxx>
using std::string;
static std::string cleanRunwayNo(const std::string& aRwyNo)
@ -168,3 +171,30 @@ void FGRunway::setReciprocalRunway(FGRunway* other)
_reciprocal = other;
}
std::vector<flightgear::SID*> FGRunway::getSIDs()
{
std::vector<flightgear::SID*> result;
for (unsigned int i=0; i<_airport->numSIDs(); ++i) {
flightgear::SID* s = _airport->getSIDByIndex(i);
if (s->isForRunway(this)) {
result.push_back(s);
}
} // of SIDs at the airport iteration
return result;
}
std::vector<flightgear::STAR*> FGRunway::getSTARs()
{
std::vector<flightgear::STAR*> result;
for (unsigned int i=0; i<_airport->numSTARs(); ++i) {
flightgear::STAR* s = _airport->getSTARByIndex(i);
if (s->isForRunway(this)) {
result.push_back(s);
}
} // of STARs at the airport iteration
return result;
}

15
src/Airports/runways.hxx
View file

@ -33,6 +33,11 @@ class FGAirport;
class FGNavRecord;
class SGPropertyNode;
namespace flightgear {
class SID;
class STAR;
}
class FGRunway : public FGRunwayBase
{
FGAirport* _airport;
@ -115,6 +120,16 @@ public:
* Helper to process property data loaded from an ICAO.threshold.xml file
*/
void processThreshold(SGPropertyNode* aThreshold);
/**
* Get SIDs (DPs) associated with this runway
*/
std::vector<flightgear::SID*> getSIDs();
/**
* Get STARs associared with this runway
*/
std::vector<flightgear::STAR*> getSTARs();
};
#endif // _FG_RUNWAYS_HXX

13
src/Airports/sidstar.hxx
View file

@ -24,6 +24,7 @@
#define _SIDSTAR_HXX_
#include <string>
#include <map>
#include <simgear/misc/sg_path.hxx>
@ -36,12 +37,10 @@
#include "runwayprefs.hxx"
using std::string;
class FGAirport;
typedef vector<FGAIFlightPlan*> FlightPlanVec;
typedef vector<FGAIFlightPlan*>::iterator FlightPlanVecIterator;
typedef std::vector<FGAIFlightPlan*> FlightPlanVec;
typedef std::vector<FGAIFlightPlan*>::iterator FlightPlanVecIterator;
typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap;
@ -49,7 +48,7 @@ typedef std::map < std::string, FlightPlanVec > FlightPlanVecMap;
class FGSidStar
{
private:
string id;
std::string id;
bool initialized;
FlightPlanVecMap data;
@ -57,9 +56,9 @@ class FGSidStar
FGSidStar(FGAirport *ap);
FGSidStar(const FGSidStar &other);
string getId() { return id; };
std::string getId() { return id; };
void load(SGPath path);
FGAIFlightPlan *getBest(string activeRunway, double heading);
FGAIFlightPlan *getBest(std::string activeRunway, double heading);
};

243
src/Airports/simple.cxx
View file

@ -45,6 +45,11 @@
#include <Airports/pavement.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/xmlloader.hxx>
#include <Navaids/procedure.hxx>
#include <Navaids/waypoint.hxx>
using std::vector;
using namespace flightgear;
// magic import of a helper which uses FGPositioned internals
extern char** searchAirportNamesAndIdents(const std::string& aFilter);
@ -187,6 +192,30 @@ FGRunway* FGAirport::findBestRunwayForHeading(double aHeading) const
return result;
}
FGRunway* FGAirport::findBestRunwayForPos(const SGGeod& aPos) const
{
loadRunways();
Runway_iterator it = mRunways.begin();
FGRunway* result = NULL;
double currentLowestDev = 180.0;
for (; it != mRunways.end(); ++it) {
double inboundCourse = SGGeodesy::courseDeg(aPos, (*it)->end());
double dev = inboundCourse - (*it)->headingDeg();
SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
dev = fabs(dev);
if (dev < currentLowestDev) { // new best match
currentLowestDev = dev;
result = *it;
}
} // of runway iteration
return result;
}
bool FGAirport::hasHardRunwayOfLengthFt(double aLengthFt) const
{
loadRunways();
@ -350,6 +379,23 @@ void FGAirport::loadTaxiways() const
}
}
void FGAirport::loadProcedures() const
{
if (mProceduresLoaded) {
return;
}
mProceduresLoaded = true;
SGPath path;
if (!XMLLoader::findAirportData(ident(), "procedures", path)) {
SG_LOG(SG_GENERAL, SG_INFO, "no procedures data available for " << ident());
return;
}
SG_LOG(SG_GENERAL, SG_INFO, ident() << ": loading procedures from " << path.str());
Route::loadAirportProcedures(path, const_cast<FGAirport*>(this));
}
void FGAirport::loadSceneryDefintions() const
{
// allow users to disable the scenery data in the short-term
@ -412,6 +458,203 @@ void FGAirport::readTowerData(SGPropertyNode* aRoot)
_tower_location = SGGeod::fromDegM(lon, lat, elevM);
}
bool FGAirport::buildApproach(Waypt* aEnroute, STAR* aSTAR, FGRunway* aRwy, WayptVec& aRoute)
{
loadProcedures();
if ((aRwy && (aRwy->airport() != this))) {
throw sg_exception("invalid parameters", "FGAirport::buildApproach");
}
if (aSTAR) {
bool ok = aSTAR->route(aRwy, aEnroute, aRoute);
if (!ok) {
SG_LOG(SG_GENERAL, SG_WARN, ident() << ": build approach, STAR " << aSTAR->ident()
<< " failed to route from transition " << aEnroute->ident());
return false;
}
} else if (aEnroute) {
// no a STAR specified, just use enroute point directly
aRoute.push_back(aEnroute);
}
if (!aRwy) {
// no runway selected yet, but we loaded the STAR, so that's fine, we're done
return true;
}
// build the approach (possibly including transition), and including the missed segment
vector<Approach*> aps;
for (unsigned int j=0; j<mApproaches.size();++j) {
if (mApproaches[j]->runway() == aRwy) {
aps.push_back(mApproaches[j]);
}
} // of approach filter by runway
if (aps.empty()) {
SG_LOG(SG_GENERAL, SG_INFO, ident() << "; no approaches defined for runway " << aRwy->ident());
// could build a fallback approach here
return false;
}
for (unsigned int k=0; k<aps.size(); ++k) {
if (aps[k]->route(aRoute.back(), aRoute)) {
return true;
}
} // of initial approach iteration
SG_LOG(SG_GENERAL, SG_INFO, ident() << ": unable to find transition to runway "
<< aRwy->ident() << ", assume vectors");
WayptRef v(new ATCVectors(NULL, this));
aRoute.push_back(v);
return aps.front()->routeFromVectors(aRoute);
}
pair<flightgear::SID*, WayptRef>
FGAirport::selectSID(const SGGeod& aDest, FGRunway* aRwy)
{
loadProcedures();
WayptRef enroute;
flightgear::SID* sid = NULL;
double d = 1e9;
for (unsigned int i=0; i<mSIDs.size(); ++i) {
if (aRwy && !mSIDs[i]->isForRunway(aRwy)) {
continue;
}
WayptRef e = mSIDs[i]->findBestTransition(aDest);
if (!e) {
continue; // strange, but let's not worry about it
}
// assert(e->isFixedPosition());
double ed = SGGeodesy::distanceM(aDest, e->position());
if (ed < d) { // new best match
enroute = e;
d = ed;
sid = mSIDs[i];
}
} // of SID iteration
if (!mSIDs.empty() && !sid) {
SG_LOG(SG_GENERAL, SG_INFO, ident() << "selectSID, no SID found (runway="
<< (aRwy ? aRwy->ident() : "no runway preference"));
}
return make_pair(sid, enroute);
}
pair<STAR*, WayptRef>
FGAirport::selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy)
{
loadProcedures();
WayptRef enroute;
STAR* star = NULL;
double d = 1e9;
for (unsigned int i=0; i<mSTARs.size(); ++i) {
if (!mSTARs[i]->isForRunway(aRwy)) {
continue;
}
SG_LOG(SG_GENERAL, SG_INFO, "STAR " << mSTARs[i]->ident() << " is valid for runway");
WayptRef e = mSTARs[i]->findBestTransition(aOrigin);
if (!e) {
continue; // strange, but let's not worry about it
}
// assert(e->isFixedPosition());
double ed = SGGeodesy::distanceM(aOrigin, e->position());
if (ed < d) { // new best match
enroute = e;
d = ed;
star = mSTARs[i];
}
} // of STAR iteration
return make_pair(star, enroute);
}
void FGAirport::addSID(flightgear::SID* aSid)
{
mSIDs.push_back(aSid);
}
void FGAirport::addSTAR(STAR* aStar)
{
mSTARs.push_back(aStar);
}
void FGAirport::addApproach(Approach* aApp)
{
mApproaches.push_back(aApp);
}
unsigned int FGAirport::numSIDs() const
{
loadProcedures();
return mSIDs.size();
}
flightgear::SID* FGAirport::getSIDByIndex(unsigned int aIndex) const
{
loadProcedures();
return mSIDs[aIndex];
}
flightgear::SID* FGAirport::findSIDWithIdent(const std::string& aIdent) const
{
loadProcedures();
for (unsigned int i=0; i<mSIDs.size(); ++i) {
if (mSIDs[i]->ident() == aIdent) {
return mSIDs[i];
}
}
return NULL;
}
unsigned int FGAirport::numSTARs() const
{
loadProcedures();
return mSTARs.size();
}
STAR* FGAirport::getSTARByIndex(unsigned int aIndex) const
{
loadProcedures();
return mSTARs[aIndex];
}
STAR* FGAirport::findSTARWithIdent(const std::string& aIdent) const
{
loadProcedures();
for (unsigned int i=0; i<mSTARs.size(); ++i) {
if (mSTARs[i]->ident() == aIdent) {
return mSTARs[i];
}
}
return NULL;
}
unsigned int FGAirport::numApproaches() const
{
loadProcedures();
return mApproaches.size();
}
Approach* FGAirport::getApproachByIndex(unsigned int aIndex) const
{
loadProcedures();
return mApproaches[aIndex];
}
// get airport elevation
double fgGetAirportElev( const string& id )
{

65
src/Airports/simple.hxx
View file

@ -45,6 +45,19 @@ typedef SGSharedPtr<FGRunway> FGRunwayPtr;
typedef SGSharedPtr<FGTaxiway> FGTaxiwayPtr;
typedef SGSharedPtr<FGPavement> FGPavementPtr;
namespace flightgear {
class SID;
class STAR;
class Approach;
class Waypt;
typedef SGSharedPtr<Waypt> WayptRef;
typedef std::vector<WayptRef> WayptVec;
}
/***************************************************************************************
*
**************************************************************************************/
@ -85,6 +98,15 @@ public:
FGRunway* getRunwayByIdent(const std::string& aIdent) const;
FGRunway* findBestRunwayForHeading(double aHeading) const;
/**
* return the most likely target runway based on a position.
* Specifically, return the runway for which the course from aPos
* to the runway end, mostly closely matches the runway heading.
* This is a good approximation of which runway the position is on or
* aiming towards.
*/
FGRunway* findBestRunwayForPos(const SGGeod& aPos) const;
/**
* Useful predicate for FMS/GPS/NAV displays and similar - check if this
* aiport has a hard-surfaced runway of at least the specified length.
@ -143,6 +165,26 @@ public:
double mMinLengthFt;
};
void setProcedures(const std::vector<flightgear::SID*>& aSids,
const std::vector<flightgear::STAR*>& aStars,
const std::vector<flightgear::Approach*>& aApproaches);
void addSID(flightgear::SID* aSid);
void addSTAR(flightgear::STAR* aStar);
void addApproach(flightgear::Approach* aApp);
unsigned int numSIDs() const;
flightgear::SID* getSIDByIndex(unsigned int aIndex) const;
flightgear::SID* findSIDWithIdent(const std::string& aIdent) const;
unsigned int numSTARs() const;
flightgear::STAR* getSTARByIndex(unsigned int aIndex) const;
flightgear::STAR* findSTARWithIdent(const std::string& aIdent) const;
unsigned int numApproaches() const;
flightgear::Approach* getApproachByIndex(unsigned int aIndex) const;
/**
* Syntactic wrapper around FGPositioned::findClosest - find the closest
* match for filter, and return it cast to FGAirport. The default filter
@ -169,6 +211,23 @@ public:
* matches in a format suitable for use by a puaList.
*/
static char** searchNamesAndIdents(const std::string& aFilter);
bool buildApproach(flightgear::Waypt* aEnroute, flightgear::STAR* aSTAR,
FGRunway* aRwy, flightgear::WayptVec& aRoute);
/**
* Given a destiation point, select the best SID and transition waypt from
* this airport. Returns (NULL,NULL) is no SIDs are defined, otherwise the
* best SID/transition is that which is closest to the destination point.
*/
std::pair<flightgear::SID*, flightgear::WayptRef> selectSID(const SGGeod& aDest, FGRunway* aRwy);
/**
* Select a STAR and enroute transition waypt, given an origin (departure) position.
* returns (NULL, NULL) is no suitable STAR is exists
*/
std::pair<flightgear::STAR*, flightgear::WayptRef> selectSTAR(const SGGeod& aOrigin, FGRunway* aRwy);
private:
typedef std::vector<FGRunwayPtr>::const_iterator Runway_iterator;
/**
@ -203,13 +262,19 @@ private:
void loadRunways() const;
void loadTaxiways() const;
void loadProcedures() const;
mutable bool mRunwaysLoaded;
mutable bool mTaxiwaysLoaded;
mutable bool mProceduresLoaded;
std::vector<FGRunwayPtr> mRunways;
std::vector<FGTaxiwayPtr> mTaxiways;
std::vector<FGPavementPtr> mPavements;
std::vector<flightgear::SID*> mSIDs;
std::vector<flightgear::STAR*> mSTARs;
std::vector<flightgear::Approach*> mApproaches;
};
// find basic airport location info from airport database

1333
src/Autopilot/route_mgr.cxx
View file

File diff suppressed because it is too large Load diff

171
src/Autopilot/route_mgr.hxx
View file

@ -28,9 +28,11 @@
#include <simgear/route/waypoint.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
#include <Navaids/route.hxx>
// forward decls
class SGRoute;
class SGPath;
class PropertyWatcher;
class FGAirport;
typedef SGSharedPtr<FGAirport> FGAirportRef;
@ -42,9 +44,91 @@ typedef SGSharedPtr<FGAirport> FGAirportRef;
class FGRouteMgr : public SGSubsystem
{
public:
FGRouteMgr();
~FGRouteMgr();
void init ();
void postinit ();
void bind ();
void unbind ();
void update (double dt);
void insertWayptAtIndex(flightgear::Waypt* aWpt, int aIndex);
flightgear::WayptRef removeWayptAtIndex(int index);
void clearRoute();
typedef enum {
ROUTE_HIGH_AIRWAYS, ///< high-level airways routing
ROUTE_LOW_AIRWAYS, ///< low-level airways routing
ROUTE_VOR ///< VOR-VOR routing
} RouteType;
/**
* Insert waypoints from index-1 to index. In practice this means you can
* 'fill in the gaps' between defined waypoints. If index=0, the departure
* airport is used as index-1; if index is -1, the destination airport is
* used as the final waypoint.
*/
bool routeToIndex(int index, RouteType aRouteType);
void autoRoute();
bool isRouteActive() const;
int currentIndex() const
{ return _currentIndex; }
flightgear::Waypt* currentWaypt() const;
flightgear::Waypt* nextWaypt() const;
flightgear::Waypt* previousWaypt() const;
const flightgear::WayptVec& waypts() const
{ return _route; }
int numWaypts() const
{ return _route.size(); }
flightgear::Waypt* wayptAtIndex(int index) const;
/**
* Find a waypoint in the route, by position, and return its index, or
* -1 if no matching waypoint was found in the route.
*/
int findWayptIndex(const SGGeod& aPos) const;
/**
* Activate a built route. This checks for various mandatory pieces of
* data, such as departure and destination airports, and creates waypoints
* for them on the route structure.
*
* returns true if the route was activated successfully, or false if the
* route could not be activated for some reason
*/
bool activate();
/**
* Step to the next waypoint on the active route
*/
void sequence();
/**
* Set the current waypoint to the specified index.
*/
void jumpToIndex(int index);
void saveRoute();
void loadRoute();
/**
* Helper command to setup current airport/runway if necessary
*/
void initAtPosition();
private:
SGRoute* _route;
flightgear::WayptVec _route;
int _currentIndex;
time_t _takeoffTime;
time_t _touchdownTime;
FGAirportRef _departure;
@ -77,6 +161,8 @@ private:
SGPropertyNode_ptr _pathNode;
SGPropertyNode_ptr _currentWpt;
/// integer property corresponding to the RouteType enum
SGPropertyNode_ptr _routingType;
/**
* Signal property to notify people that the route was edited
@ -111,10 +197,17 @@ private:
* - airport-id/runway-id
* - navaid/radial-deg/offset-nm
*/
SGWayPoint* make_waypoint(const string& target);
flightgear::WayptRef waypointFromString(const std::string& target);
void departureChanged();
void buildDeparture(flightgear::WayptRef enroute, flightgear::WayptVec& wps);
void arrivalChanged();
void buildArrival(flightgear::WayptRef enroute, flightgear::WayptVec& wps);
/**
* Helper to keep various pieces of state in sync when the SGRoute is
* Helper to keep various pieces of state in sync when the route is
* modified (waypoints added, inserted, removed). Notably, this fires the
* 'edited' signal.
*/
@ -138,6 +231,17 @@ private:
void loadPlainTextRoute(const SGPath& path);
void loadVersion1XMLRoute(SGPropertyNode_ptr routeData);
void loadVersion2XMLRoute(SGPropertyNode_ptr routeData);
void loadXMLRouteHeader(SGPropertyNode_ptr routeData);
flightgear::WayptRef parseVersion1XMLWaypt(SGPropertyNode* aWP);
/**
* Predicate for helping the UI - test if at least one waypoint was
* entered by the user (as opposed to being generated by the route-manager)
*/
bool haveUserWaypoints() const;
// tied getters and setters
const char* getDepartureICAO() const;
const char* getDepartureName() const;
@ -146,64 +250,9 @@ private:
const char* getDestinationICAO() const;
const char* getDestinationName() const;
void setDestinationICAO(const char* aIdent);
public:
FGRouteMgr();
~FGRouteMgr();
void init ();
void postinit ();
void bind ();
void unbind ();
void update (double dt);
bool build ();
void new_waypoint( const string& tgt_alt, int n = -1 );
void add_waypoint( const SGWayPoint& wp, int n = -1 );
SGWayPoint pop_waypoint( int i = 0 );
SGWayPoint get_waypoint( int i ) const;
int size() const;
bool isRouteActive() const;
int currentWaypoint() const;
/**
* Find a waypoint in the route, by position, and return its index, or
* -1 if no matching waypoint was found in the route.
*/
int findWaypoint(const SGGeod& aPos) const;
/**
* Activate a built route. This checks for various mandatory pieces of
* data, such as departure and destination airports, and creates waypoints
* for them on the route structure.
*
* returns true if the route was activated successfully, or false if the
* route could not be activated for some reason
*/
bool activate();
/**
* Step to the next waypoint on the active route
*/
void sequence();
/**
*
*/
void jumpToIndex(int index);
/**
*
*/
void setWaypointTargetAltitudeFt(unsigned int index, int altFt);
void saveRoute();
void loadRoute();
PropertyWatcher* _departureWatcher;
PropertyWatcher* _arrivalWatcher;
};

81
src/GUI/MapWidget.cxx
View file

@ -25,6 +25,7 @@
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
#include <Main/fg_os.hxx> // fgGetKeyModifiers()
#include <Navaids/routePath.hxx>
const char* RULER_LEGEND_KEY = "ruler-legend";
@ -657,77 +658,73 @@ void MapWidget::paintAircraftLocation(const SGGeod& aircraftPos)
void MapWidget::paintRoute()
{
if (_route->size() < 2) {
if (_route->numWaypts() < 2) {
return;
}
// first pass, draw the actual line
RoutePath path(_route->waypts());
// first pass, draw the actual lines
glLineWidth(2.0);
glBegin(GL_LINE_STRIP);
SGVec2d prev = project(_route->get_waypoint(0).get_target());
glVertex2d(prev.x(), prev.y());
for (int w=1; w < _route->size(); ++w) {
for (int w=0; w<_route->numWaypts(); ++w) {
SGGeodVec gv(path.pathForIndex(w));
if (gv.empty()) {
continue;
}
SGVec2d p = project(_route->get_waypoint(w).get_target());
if (w < _route->currentWaypoint()) {
if (w < _route->currentIndex()) {
glColor4f(0.5, 0.5, 0.5, 0.7);
} else {
glColor4f(1.0, 0.0, 1.0, 1.0);
}
glVertex2d(p.x(), p.y());
flightgear::WayptRef wpt(_route->wayptAtIndex(w));
if (wpt->flag(flightgear::WPT_MISS)) {
glEnable(GL_LINE_STIPPLE);
glLineStipple(1, 0x00FF);
}
glBegin(GL_LINE_STRIP);
for (unsigned int i=0; i<gv.size(); ++i) {
SGVec2d p = project(gv[i]);
glVertex2d(p.x(), p.y());
}
glEnd();
glDisable(GL_LINE_STIPPLE);
}
glEnd();
glLineWidth(1.0);
// second pass, draw waypoint symbols and data
for (int w=0; w < _route->size(); ++w) {
const SGWayPoint& wpt(_route->get_waypoint(w));
SGVec2d p = project(wpt.get_target());
for (int w=0; w < _route->numWaypts(); ++w) {
flightgear::WayptRef wpt(_route->wayptAtIndex(w));
SGGeod g = path.positionForIndex(w);
if (g == SGGeod()) {
continue; // Vectors or similar
}
SGVec2d p = project(g);
glColor4f(1.0, 0.0, 1.0, 1.0);
circleAtAlt(p, 8, 12, 5);
std::ostringstream legend;
legend << wpt.get_id();
if (wpt.get_target_alt() > -9990.0) {
legend << '\n' << SGMiscd::roundToInt(wpt.get_target_alt()) << '\'';
legend << wpt->ident();
if (wpt->altitudeRestriction() != flightgear::RESTRICT_NONE) {
legend << '\n' << SGMiscd::roundToInt(wpt->altitudeFt()) << '\'';
}
if (wpt.get_speed() > 0.0) {
legend << '\n' << SGMiscd::roundToInt(wpt.get_speed()) << "Kts";
if (wpt->speedRestriction() != flightgear::RESTRICT_NONE) {
legend << '\n' << SGMiscd::roundToInt(wpt->speedKts()) << "Kts";
}
MapData* d = getOrCreateDataForKey(reinterpret_cast<void*>(w * 2));
d->setText(legend.str());
d->setLabel(wpt.get_id());
d->setLabel(wpt->ident());
d->setAnchor(p);
d->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15);
d->setPriority(w < _route->currentWaypoint() ? 9000 : 12000);
d->setPriority(w < _route->currentIndex() ? 9000 : 12000);
if (w > 0) {
SGVec2d legMid = (prev + p) * 0.5;
std::ostringstream legLegend;
double track = wpt.get_track();
if (_magneticHeadings) {
track -= _magVar->get_magvar(); // show magnetic track for leg
}
legLegend << SGMiscd::roundToInt(track) << " "
<< SGMiscd::roundToInt(wpt.get_distance() * SG_METER_TO_NM) << "Nm";
MapData* ld = getOrCreateDataForKey(reinterpret_cast<void*>(w * 2 + 1));
ld->setText(legLegend.str());
ld->setAnchor(legMid);
ld->setOffset(MapData::VALIGN_TOP | MapData::HALIGN_CENTER, 15);
ld->setPriority(w < _route->currentWaypoint() ? 8000 : 11000);
} // of draw leg data
prev = p;
} // of second waypoint iteration
}

218
src/GUI/WaypointList.cxx
View file

@ -7,6 +7,8 @@
#include "WaypointList.hxx"
#include <algorithm>
#include <boost/tuple/tuple.hpp>
#include <plib/puAux.h>
#include <simgear/route/waypoint.hxx>
@ -16,14 +18,18 @@
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
#include <Navaids/positioned.hxx>
#include <Autopilot/route_mgr.hxx>
using namespace flightgear;
enum {
SCROLL_NO = 0,
SCROLL_UP,
SCROLL_DOWN
};
static const double BLINK_TIME = 0.3;
static const int DRAG_START_DISTANCE_PX = 5;
class RouteManagerWaypointModel :
@ -48,37 +54,38 @@ public:
// implement WaypointList::Model
virtual unsigned int numWaypoints() const
{
return _rm->size();
return _rm->numWaypts();
}
virtual int currentWaypoint() const
{
return _rm->currentWaypoint();
return _rm->currentIndex();
}
virtual SGWayPoint waypointAt(unsigned int index) const
virtual flightgear::Waypt* waypointAt(unsigned int index) const
{
return _rm->get_waypoint(index);
if (index >= numWaypoints()) {
return NULL;
}
return _rm->wayptAtIndex(index);
}
virtual void deleteAt(unsigned int index)
{
_rm->pop_waypoint(index);
}
virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt)
{
_rm->setWaypointTargetAltitudeFt(index, altFt);
_rm->removeWayptAtIndex(index);
}
virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int destIndex)
{
SG_LOG(SG_GENERAL, SG_INFO, "moveWaypoint: from " << srcIndex << " to " << destIndex);
if (destIndex > srcIndex) {
--destIndex;
}
SGWayPoint wp = _rm->pop_waypoint(srcIndex);
_rm->add_waypoint(wp, destIndex);
WayptRef w(_rm->removeWayptAtIndex(srcIndex));
SG_LOG(SG_GENERAL, SG_INFO, "wpt:" << w->ident());
_rm->insertWayptAtIndex(w, destIndex);
}
virtual void setUpdateCallback(SGCallback* cb)
@ -129,13 +136,16 @@ WaypointList::WaypointList(int x, int y, int width, int height) :
_showLatLon(false),
_model(NULL),
_updateCallback(NULL),
_scrollCallback(NULL)
_scrollCallback(NULL),
_blink(false)
{
// pretend to be a list, so fgPopup doesn't mess with our mouse events
type |= PUCLASS_LIST;
setModel(new RouteManagerWaypointModel());
setSize(width, height);
setValue(-1);
_blinkTimer.stamp();
}
WaypointList::~WaypointList()
@ -230,6 +240,11 @@ void WaypointList::handleDrag(int x, int y)
}
_dragSourceRow = rowForY(y - abox.min[1]);
Waypt* wp = _model->waypointAt(_dragSourceRow);
if (!wp || wp->flag(WPT_GENERATED)) {
return; // don't allow generated points to be dragged
}
_dragging = true;
_dragScroll = SCROLL_NO;
}
@ -255,20 +270,26 @@ void WaypointList::doDrop(int x, int y)
_dragging = false;
puDeactivateWidget();
SG_LOG(SG_GENERAL, SG_INFO, "doDrop");
if ((y < abox.min[1]) || (y >= abox.max[1])) {
SG_LOG(SG_GENERAL, SG_INFO, "y out of bounds:" << y);
return;
}
if (_dragSourceRow != _dragTargetRow) {
_model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow);
// keep row indexes linged up when moving an item down the list
if (_dragSourceRow < _dragTargetRow) {
--_dragTargetRow;
}
setSelected(_dragTargetRow);
if (_dragSourceRow == _dragTargetRow) {
SG_LOG(SG_GENERAL, SG_INFO, "source and target row match");
return;
}
_model->moveWaypointToIndex(_dragSourceRow, _dragTargetRow);
// keep row indexes linged up when moving an item down the list
if (_dragSourceRow < _dragTargetRow) {
--_dragTargetRow;
}
setSelected(_dragTargetRow);
}
void WaypointList::invokeDownCallback(void)
@ -302,6 +323,12 @@ void WaypointList::draw( int dx, int dy )
doDragScroll();
}
double dt = (SGTimeStamp::now() - _blinkTimer).toSecs();
if (dt > BLINK_TIME) {
_blinkTimer.stamp();
_blink = !_blink;
}
glEnable(GL_SCISSOR_TEST);
GLint sx = (int) abox.min[0],
sy = abox.min[1];
@ -321,6 +348,7 @@ void WaypointList::draw( int dx, int dy )
y -= (_scrollPx % rowHeight); // partially draw the first row
_arrowWidth = legendFont.getStringWidth(">");
for ( ; row <= final; ++row, y += rowHeight) {
drawRow(dx, dy, row, y);
} // of row drawing iteration
@ -343,6 +371,8 @@ void WaypointList::draw( int dx, int dy )
void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
{
flightgear::Waypt* wp(_model->waypointAt(rowIndex));
bool isSelected = (rowIndex == getSelected());
bool isCurrent = (rowIndex == _model->currentWaypoint());
bool isDragSource = (_dragging && (rowIndex == _dragSourceRow));
@ -351,62 +381,100 @@ void WaypointList::drawRow(int dx, int dy, int rowIndex, int y)
bkgBox.min[1] = abox.max[1] - y;
bkgBox.max[1] = bkgBox.min[1] + rowHeightPx();
puColour currentColor;
puSetColor(currentColor, 1.0, 1.0, 0.0, 0.5);
puColour col;
puFont* f = &legendFont;
bool drawBox = false;
if (wp->flag(WPT_MISS)) {
drawBox = true;
puSetColor(col, 1.0, 0.0, 0.0, 0.3); // red
} else if (wp->flag(WPT_ARRIVAL)) {
drawBox = true;
puSetColor(col, 0.0, 0.0, 0.0, 0.3);
} else if (wp->flag(WPT_DEPARTURE)) {
drawBox = true;
puSetColor(col, 0.0, 0.0, 0.0, 0.3);
}
if (isDragSource) {
// draw later, on *top* of text string
} else if (isCurrent) {
bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &currentColor, false, 0);
} else if (isSelected) { // -PLAIN means selected, apparently
bkgBox.draw(dx, dy, -PUSTYLE_PLAIN, colour, false, 0);
} else if (drawBox) {
bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0);
}
if (isCurrent) {
glColor4f (1.0, 0.5, 0.0, 1.0) ;
} else {
glColor4fv ( colour [ PUCOL_LEGEND ] ) ;
}
int xx = dx + abox.min[0] + PUSTR_LGAP;
int yy = dy + abox.max[1] - y ;
yy += 4; // center text in row height
// row textual data
const SGWayPoint wp(_model->waypointAt(rowIndex));
char buffer[128];
int count = ::snprintf(buffer, 128, "%03d %-5s", rowIndex, wp.get_id().c_str());
if (wp.get_name().size() > 0 && (wp.get_name() != wp.get_id())) {
// append name if present, and different to id
::snprintf(buffer + count, 128 - count, " (%s)", wp.get_name().c_str());
if (isCurrent) {
f->drawString(">", xx, yy);
}
glColor4fv ( colour [ PUCOL_LEGEND ] ) ;
drawClippedString(legendFont, buffer, xx, yy, 300);
int x = xx;
x += _arrowWidth + PUSTR_LGAP;
if (_showLatLon) {
char ns = (wp.get_target_lat() > 0.0) ? 'N' : 'S';
char ew = (wp.get_target_lon() > 0.0) ? 'E' : 'W';
::snprintf(buffer, 128 - count, "%4.2f%c %4.2f%c",
fabs(wp.get_target_lon()), ew, fabs(wp.get_target_lat()), ns);
} else {
::snprintf(buffer, 128 - count, "%03.0f %5.1fnm",
wp.get_track(), wp.get_distance() * SG_METER_TO_NM);
// row textual data
char buffer[128];
int count = ::snprintf(buffer, 128, "%03d %-5s", rowIndex, wp->ident().c_str());
FGPositioned* src = wp->source();
if (src && !src->name().empty() && (src->name() != wp->ident())) {
// append name if present, and different to id
::snprintf(buffer + count, 128 - count, " (%s)", src->name().c_str());
}
legendFont.drawString(buffer, xx + 300 + PUSTR_LGAP, yy);
drawClippedString(legendFont, buffer, x, yy, 300);
x += 300 + PUSTR_LGAP;
int altFt = (int) wp.get_target_alt() * SG_METER_TO_FEET;
if (altFt > -9990) {
int altHundredFt = (altFt + 50) / 100; // round to nearest 100ft
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);
} else if (rowIndex > 0) {
double courseDeg;
double distanceM;
Waypt* prev = _model->waypointAt(rowIndex - 1);
boost::tie(courseDeg, distanceM) = wp->courseAndDistanceFrom(prev->position());
::snprintf(buffer, 128 - count, "%03.0f %5.1fnm",
courseDeg, distanceM * SG_METER_TO_NM);
}
f->drawString(buffer, x, yy);
x += 100 + PUSTR_LGAP;
if (wp->altitudeRestriction() != RESTRICT_NONE) {
int altHundredFt = (wp->altitudeFt() + 50) / 100; // round to nearest 100ft
if (altHundredFt < 100) {
count = ::snprintf(buffer, 128, "%d'", altHundredFt * 100);
} else { // display as a flight-level
count = ::snprintf(buffer, 128, "FL%d", altHundredFt);
}
legendFont.drawString(buffer, xx + 400 + PUSTR_LGAP, yy);
f->drawString(buffer, x, yy);
} // of valid wp altitude
x += 60 + PUSTR_LGAP;
if (wp->speedRestriction() != RESTRICT_NONE) {
count = ::snprintf(buffer, 126, "%dKts", (int) wp->speedKts());
f->drawString(buffer, x, yy);
}
if (isDragSource) {
puSetColor(currentColor, 1.0, 0.5, 0.0, 0.5);
bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &currentColor, false, 0);
puSetColor(col, 1.0, 0.5, 0.0, 0.5);
bkgBox.draw(dx, dy, PUSTYLE_PLAIN, &col, false, 0);
}
}
@ -613,22 +681,34 @@ int WaypointList::checkKey (int key, int updown )
case '-':
if (getSelected() >= 0) {
int newAlt = wayptAltFtHundreds(getSelected()) - 10;
if (newAlt < 0) {
_model->setWaypointTargetAltitudeFt(getSelected(), -9999);
} else {
_model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100);
Waypt* wp = _model->waypointAt(getSelected());
if (wp->flag(WPT_GENERATED)) {
break;
}
if (wp->altitudeRestriction() != RESTRICT_NONE) {
int curAlt = (static_cast<int>(wp->altitudeFt()) + 50) / 100;
if (curAlt <= 0) {
wp->setAltitude(0, RESTRICT_NONE);
} else {
wp->setAltitude((curAlt - 10) * 100, wp->altitudeRestriction());
}
}
}
break;
case '=':
if (getSelected() >= 0) {
int newAlt = wayptAltFtHundreds(getSelected()) + 10;
if (newAlt < 0) {
_model->setWaypointTargetAltitudeFt(getSelected(), 0);
flightgear::Waypt* wp = _model->waypointAt(getSelected());
if (wp->flag(WPT_GENERATED)) {
break;
}
if (wp->altitudeRestriction() == RESTRICT_NONE) {
wp->setAltitude(1000, RESTRICT_AT);
} else {
_model->setWaypointTargetAltitudeFt(getSelected(), newAlt * 100);
int curAlt = (static_cast<int>(wp->altitudeFt()) + 50) / 100;
wp->setAltitude((curAlt + 10) * 100, wp->altitudeRestriction());
}
}
break;
@ -636,6 +716,11 @@ int WaypointList::checkKey (int key, int updown )
case 0x7f: // delete
if (getSelected() >= 0) {
int index = getSelected();
Waypt* wp = _model->waypointAt(getSelected());
if (wp->flag(WPT_GENERATED)) {
break;
}
_model->deleteAt(index);
setSelected(index - 1);
}
@ -648,17 +733,6 @@ int WaypointList::checkKey (int key, int updown )
return TRUE ;
}
int WaypointList::wayptAltFtHundreds(int index) const
{
double alt = _model->waypointAt(index).get_target_alt();
if (alt < -9990.0) {
return -9999;
}
int altFt = (int) alt * SG_METER_TO_FEET;
return (altFt + 50) / 100; // round to nearest 100ft
}
void WaypointList::modelUpdateCallback()
{
// local stuff

16
src/GUI/WaypointList.hxx
View file

@ -14,9 +14,12 @@
// forward decls
class puaScrollBar;
class SGWayPoint;
class SGCallback;
namespace flightgear {
class Waypt;
}
class WaypointList : public puFrame, public GUI_ID
{
public:
@ -71,14 +74,13 @@ public:
virtual unsigned int numWaypoints() const = 0;
virtual int currentWaypoint() const = 0;
virtual SGWayPoint waypointAt(unsigned int index) const = 0;
virtual flightgear::Waypt* waypointAt(unsigned int index) const = 0;
// update notifications
virtual void setUpdateCallback(SGCallback* cb) = 0;
// editing operations
virtual void deleteAt(unsigned int index) = 0;
virtual void setWaypointTargetAltitudeFt(unsigned int index, int altFt) = 0;
virtual void moveWaypointToIndex(unsigned int srcIndex, unsigned int dstIndex) = 0;
};
@ -121,9 +123,7 @@ private:
int numFullyVisibleRows() const;
int firstFullyVisibleRow() const;
int lastFullyVisibleRow() const;
int wayptAltFtHundreds(int index) const;
void modelUpdateCallback();
int _scrollPx; // scroll ammount (in pixels)
@ -141,6 +141,10 @@ private:
Model* _model;
SGCallback* _updateCallback;
SGCallback* _scrollCallback;
SGTimeStamp _blinkTimer;
bool _blink;
int _arrowWidth;
};
class ScrolledWaypointList : public puGroup

33
src/GUI/dialog.cxx
View file

@ -1339,8 +1339,41 @@ fgList::update()
void fgComboBox::update()
{
if (_inHit) {
return;
}
std::string curValue(getStringValue());
fgValueList::update();
newList(_list);
int currentItem = puaComboBox::getCurrentItem();
// look for the previous value, in the new list
for (int i = 0; _list[i] != 0; i++) {
if (_list[i] == curValue) {
// don't set the current item again; this is important to avoid
// recursion here if the combo callback ultimately causes another dialog-update
if (i != currentItem) {
puaComboBox::setCurrentItem(i);
}
return;
}
} // of list values iteration
// cound't find current item, default to first
if (currentItem != 0) {
puaComboBox::setCurrentItem(0);
}
}
int fgComboBox::checkHit(int b, int up, int x, int y)
{
_inHit = true;
int r = puaComboBox::checkHit(b, up, x, y);
_inHit = false;
return r;
}
// end of dialog.cxx

10
src/GUI/dialog.hxx
View file

@ -259,9 +259,17 @@ public:
class fgComboBox : public fgValueList, public puaComboBox {
public:
fgComboBox(int x1, int y1, int x2, int y2, SGPropertyNode *p, bool editable) :
fgValueList(p), puaComboBox(x1, y1, x2, y2, _list, editable) {}
fgValueList(p),
puaComboBox(x1, y1, x2, y2, _list, editable),
_inHit(false)
{}
void update();
virtual int checkHit(int b, int up, int x, int y);
private:
bool _inHit;
};
class fgSelectBox : public fgValueList, public puaSelectBox {

5
src/Instrumentation/Makefile.am
View file

@ -31,7 +31,8 @@ libInstrumentation_a_SOURCES = \
tacan.cxx tacan.hxx mk_viii.cxx mk_viii.hxx \
dclgps.cxx dclgps.hxx \
render_area_2d.cxx render_area_2d.hxx \
groundradar.cxx groundradar.hxx \
agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx
groundradar.cxx groundradar.hxx \
agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \
rnav_waypt_controller.cxx rnav_waypt_controller.hxx
INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src -I$(top_builddir)/src

434
src/Instrumentation/gps.cxx
View file

@ -9,6 +9,8 @@
#include "gps.hxx"
#include <boost/tuple/tuple.hpp>
#include <memory>
#include <set>
#include <cstring>
@ -17,6 +19,7 @@
#include "Main/globals.hxx" // for get_subsystem
#include "Main/util.hxx" // for fgLowPass
#include "Navaids/positioned.hxx"
#include <Navaids/waypoint.hxx>
#include "Navaids/navrecord.hxx"
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
@ -29,6 +32,7 @@
using std::auto_ptr;
using std::string;
using namespace flightgear;
///////////////////////////////////////////////////////////////////
@ -227,6 +231,9 @@ GPS::GPS ( SGPropertyNode *node) :
string branch = "/instrumentation/" + _name;
_gpsNode = fgGetNode(branch.c_str(), _num, true );
_scratchNode = _gpsNode->getChild("scratch", 0, true);
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
_currentWayptNode = wp_node->getChild("wp", 1, true);
}
GPS::~GPS ()
@ -255,17 +262,9 @@ GPS::init ()
_northSouthVelocity = _gpsNode->getChild("ns-velocity-msec", 0, true);
// waypoints
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
// for compatability, alias selected course down to wp/wp[1]/desired-course-deg
SGPropertyNode* wp1Crs = wp1_node->getChild("desired-course-deg", 0, true);
SGPropertyNode* wp1Crs = _currentWayptNode->getChild("desired-course-deg", 0, true);
wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true));
// _true_wp1_bearing_error_node =
// wp1_node->getChild("true-bearing-error-deg", 0, true);
// _magnetic_wp1_bearing_error_node =
// wp1_node->getChild("magnetic-bearing-error-deg", 0, true);
_tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true);
@ -299,10 +298,10 @@ GPS::init ()
// navradio slaving properties
SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true);
toFlag->alias(wp1_node->getChild("to-flag"));
toFlag->alias(_currentWayptNode->getChild("to-flag"));
SGPropertyNode* fromFlag = _gpsNode->getChild("from-flag", 0, true);
fromFlag->alias(wp1_node->getChild("from-flag"));
fromFlag->alias(_currentWayptNode->getChild("from-flag"));
// autopilot drive properties
_apDrivingFlag = fgGetNode("/autopilot/settings/gps-driving-true-heading", true);
@ -357,49 +356,39 @@ GPS::bind()
tie(_scratchNode, "mag-bearing-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchMagBearing, NULL));
tie(_scratchNode, "has-next", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchHasNext, NULL));
_scratchValid = false;
// waypoint data (including various historical things)
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft");
tie(wp0_node, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP0Ident, NULL));
tie(wp0_node, "name", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP0Name, NULL));
tieSGGeodReadOnly(wp1_node, _wp1_position, "longitude-deg", "latitude-deg", "altitude-ft");
tie(wp1_node, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Ident, NULL));
tie(wp1_node, "name", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Name, NULL));
tie(wp1_node, "distance-nm", SGRawValueMethods<GPS, double>
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
SGPropertyNode* wp0_node = wp_node->getChild("wp", 0, true);
tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft");
tie(_currentWayptNode, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Ident, NULL));
tie(_currentWayptNode, "distance-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Distance, NULL));
tie(wp1_node, "bearing-true-deg", SGRawValueMethods<GPS, double>
tie(_currentWayptNode, "bearing-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Bearing, NULL));
tie(wp1_node, "bearing-mag-deg", SGRawValueMethods<GPS, double>
tie(_currentWayptNode, "bearing-mag-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1MagBearing, NULL));
tie(wp1_node, "TTW-sec", SGRawValueMethods<GPS, double>
tie(_currentWayptNode, "TTW-sec", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1TTW, NULL));
tie(wp1_node, "TTW", SGRawValueMethods<GPS, const char*>
tie(_currentWayptNode, "TTW", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1TTWString, NULL));
tie(wp1_node, "course-deviation-deg", SGRawValueMethods<GPS, double>
tie(_currentWayptNode, "course-deviation-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseDeviation, NULL));
tie(wp1_node, "course-error-nm", SGRawValueMethods<GPS, double>
tie(_currentWayptNode, "course-error-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseErrorNm, NULL));
tie(wp1_node, "to-flag", SGRawValueMethods<GPS, bool>
tie(_currentWayptNode, "to-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1ToFlag, NULL));
tie(wp1_node, "from-flag", SGRawValueMethods<GPS, bool>
tie(_currentWayptNode, "from-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1FromFlag, NULL));
// leg properties (only valid in DTO/LEG modes, not OBS)
tie(wp_node, "leg-distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getLegDistance, NULL));
tie(wp_node, "leg-true-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegCourse, NULL));
tie(wp_node, "leg-mag-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegMagCourse, NULL));
tie(wp_node, "alt-dist-ratio", SGRawValueMethods<GPS, double>(*this, &GPS::getAltDistanceRatio, NULL));
// navradio slaving properties
tie(_gpsNode, "cdi-deflection", SGRawValueMethods<GPS,double>
@ -426,12 +415,11 @@ GPS::clearOutput()
_last_vertical_speed = 0.0;
_last_true_track = 0.0;
_lastEWVelocity = _lastNSVelocity = 0.0;
_currentWaypt = _prevWaypt = NULL;
_legDistanceNm = -1.0;
_raim_node->setDoubleValue(0.0);
_indicated_pos = SGGeod();
_wp1DistanceM = 0.0;
_wp1TrueBearing = 0.0;
_wp1_position = SGGeod();
_odometer_node->setDoubleValue(0);
_trip_odometer_node->setDoubleValue(0);
_tracking_bug_node->setDoubleValue(0);
@ -455,61 +443,28 @@ GPS::update (double delta_time_sec)
if (delta_time_sec <= 0.0) {
return; // paused, don't bother
}
// TODO: Add noise and other errors.
/*
// Bias and random error
double random_factor = sg_random();
double random_error = 1.4;
double error_radius = 5.1;
double bias_max_radius = 5.1;
double random_max_radius = 1.4;
bias_length += (random_factor-0.5) * 1.0e-3;
if (bias_length <= 0.0) bias_length = 0.0;
else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
bias_angle += (random_factor-0.5) * 1.0e-3;
if (bias_angle <= 0.0) bias_angle = 0.0;
else if (bias_angle >= 360.0) bias_angle = 360.0;
double random_length = random_factor * random_max_radius;
double random_angle = random_factor * 360.0;
double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
double random_x = random_length * cos(random_angle * SG_PI / 180.0);
double random_y = random_length * sin(random_angle * SG_PI / 180.0);
double error_x = bias_x + random_x;
double error_y = bias_y + random_y;
double error_length = sqrt(error_x*error_x + error_y*error_y);
double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
double lat2;
double lon2;
double az2;
geo_direct_wgs_84 ( altitude_m, latitude_deg,
longitude_deg, error_angle,
error_length, &lat2, &lon2,
&az2 );
//cout << lat2 << " " << lon2 << endl;
printf("%f %f \n", bias_length, bias_angle);
printf("%3.7f %3.7f \n", lat2, lon2);
printf("%f %f \n", error_length, error_angle);
*/
_raim_node->setDoubleValue(1.0);
_indicated_pos = _position.get();
updateBasicData(delta_time_sec);
if (_dataValid) {
if (_mode != "obs") {
updateTurn();
}
if (_wayptController.get()) {
_wayptController->update();
SGGeod p(_wayptController->position());
_currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
_currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
_currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
updateWaypoints();
_desiredCourse = getLegMagCourse();
updateTurn();
updateRouteData();
}
updateTrackingBug();
updateReferenceNavaid(delta_time_sec);
updateRouteData();
driveAutopilot();
}
@ -538,6 +493,49 @@ GPS::update (double delta_time_sec)
_lastPosValid = true;
}
///////////////////////////////////////////////////////////////////////////
// implement the RNAV interface
SGGeod GPS::position()
{
if (!_dataValid) {
return SGGeod();
}
return _indicated_pos;
}
double GPS::trackDeg()
{
return _last_true_track;
}
double GPS::groundSpeedKts()
{
return _last_speed_kts;
}
double GPS::vspeedFPM()
{
return _last_vertical_speed;
}
double GPS::magvarDeg()
{
return _magvar_node->getDoubleValue();
}
double GPS::overflightArmDistanceM()
{
return _config.overflightArmDistanceNm() * SG_NM_TO_METER;
}
double GPS::selectedMagCourse()
{
return _selectedCourse;
}
///////////////////////////////////////////////////////////////////////////
void
GPS::updateBasicData(double dt)
{
@ -601,13 +599,6 @@ GPS::updateTrackingBug()
_magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
}
void
GPS::updateWaypoints()
{
double az2;
SGGeodesy::inverse(_indicated_pos, _wp1_position, _wp1TrueBearing, az2,_wp1DistanceM);
}
void GPS::updateReferenceNavaid(double dt)
{
if (!_ref_navaid_set) {
@ -690,7 +681,9 @@ void GPS::routeActivated()
}
} else if (_mode == "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode");
selectOBSMode();
// select OBS mode, but keep current waypoint-as is
_mode = "obs";
wp1Changed();
}
}
@ -701,8 +694,8 @@ void GPS::routeManagerSequenced()
return;
}
int index = _routeMgr->currentWaypoint(),
count = _routeMgr->size();
int index = _routeMgr->currentIndex(),
count = _routeMgr->numWaypts();
if ((index < 0) || (index >= count)) {
SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index);
return;
@ -711,17 +704,15 @@ void GPS::routeManagerSequenced()
SG_LOG(SG_INSTR, SG_INFO, "GPS waypoint index is now " << index);
if (index > 0) {
SGWayPoint wp0(_routeMgr->get_waypoint(index - 1));
_wp0Ident = wp0.get_id();
_wp0Name = wp0.get_name();
_wp0_position = wp0.get_target();
_prevWaypt = _routeMgr->previousWaypt();
if (_prevWaypt->flag(WPT_DYNAMIC)) {
_wp0_position = _indicated_pos;
} else {
_wp0_position = _prevWaypt->position();
}
}
SGWayPoint wp1(_routeMgr->get_waypoint(index));
_wp1Ident = wp1.get_id();
_wp1Name = wp1.get_name();
_wp1_position = wp1.get_target();
_currentWaypt = _routeMgr->currentWaypt();
_desiredCourse = getLegMagCourse();
_desiredCourseNode->fireValueChanged();
@ -745,8 +736,8 @@ void GPS::routeFinished()
}
SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS");
// select OBS mode, but keep current waypoint-as is
_mode = "obs";
_wp0_position = _indicated_pos;
wp1Changed();
}
@ -800,7 +791,7 @@ void GPS::updateTurn()
double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius;
double deviationDeg = desiredCourse - getMagTrack();
deviationNm = copysign(deviationNm, deviationDeg);
// FXIME
// FIXME
//_wp1_course_deviation_node->setDoubleValue(deviationDeg);
//_wp1_course_error_nm_node->setDoubleValue(deviationNm);
//_cdiDeflectionNode->setDoubleValue(deviationDeg);
@ -809,26 +800,29 @@ void GPS::updateTurn()
void GPS::updateOverflight()
{
if ((_wp1DistanceM * SG_METER_TO_NM) > _config.overflightArmDistanceNm()) {
if (!_wayptController->isDone()) {
return;
}
if (getWP1ToFlag()) {
return; // still heading towards the WP
}
if (_mode == "dto") {
SG_LOG(SG_INSTR, SG_INFO, "GPS DTO reached destination point");
// check for wp1 being on active route - resume leg mode
if (_routeMgr->isRouteActive()) {
int index = _routeMgr->findWaypoint(_wp1_position);
int index = _routeMgr->findWayptIndex(_currentWaypt->position());
if (index >= 0) {
SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index);
_mode = "leg";
_routeMgr->jumpToIndex(index);
}
}
if (_mode == "dto") {
// if we didn't enter leg mode, drop back to OBS mode
// select OBS mode, but keep current waypoint-as is
_mode = "obs";
wp1Changed();
}
} else if (_mode == "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS doing overflight sequencing");
_routeMgr->sequence();
@ -867,8 +861,8 @@ void GPS::computeTurnData()
return;
}
int curIndex = _routeMgr->currentWaypoint();
if ((curIndex + 1) >= _routeMgr->size()) {
WayptRef next = _routeMgr->nextWaypt();
if (!next || next->flag(WPT_DYNAMIC)) {
_anticipateTurn = false;
return;
}
@ -880,11 +874,9 @@ void GPS::computeTurnData()
_turnStartBearing = _desiredCourse;
// compute next leg course
SGWayPoint wp1(_routeMgr->get_waypoint(curIndex)),
wp2(_routeMgr->get_waypoint(curIndex + 1));
double crs, dist;
wp2.CourseAndDistance(wp1, &crs, &dist);
boost::tie(crs, dist) = next->courseAndDistanceFrom(_currentWaypt->position());
// compute offset bearing
_turnAngle = crs - _turnStartBearing;
@ -897,9 +889,9 @@ void GPS::computeTurnData()
", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median
<< ", offset=" << offsetBearing);
SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << wp1.get_id() << "->" << wp2.get_id());
SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << _currentWaypt->ident() << "->" << next->ident());
_turnPt = _wp1_position;
_turnPt = _currentWaypt->position();
_anticipateTurn = true;
}
@ -945,10 +937,10 @@ double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const
void GPS::updateRouteData()
{
double totalDistance = _wp1DistanceM * SG_METER_TO_NM;
double totalDistance = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
// walk all waypoints from wp2 to route end, and sum
for (int i=_routeMgr->currentWaypoint()+1; i<_routeMgr->size(); ++i) {
totalDistance += _routeMgr->get_waypoint(i).get_distance();
for (int i=_routeMgr->currentIndex()+1; i<_routeMgr->numWaypts(); ++i) {
//totalDistance += _routeMgr->get_waypoint(i).get_distance();
}
_routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);
@ -979,13 +971,36 @@ void GPS::driveAutopilot()
void GPS::wp1Changed()
{
if (_mode == "leg") {
_wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
} else if (_mode == "obs") {
_wayptController.reset(new OBSController(this, _currentWaypt));
} else if (_mode == "dto") {
_wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position));
}
_wayptController->init();
if (_mode == "obs") {
_legDistanceNm = -1.0;
} else {
_wayptController->update();
_legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
}
if (!_config.driveAutopilot()) {
return;
}
double altFt = _wp1_position.getElevationFt();
if (altFt > -9990.0) {
_apTargetAltitudeFt->setDoubleValue(altFt);
RouteRestriction ar = _currentWaypt->altitudeRestriction();
double restrictAlt = _currentWaypt->altitudeFt();
double alt = _indicated_pos.getElevationFt();
if ((ar == RESTRICT_AT) ||
((ar == RESTRICT_ABOVE) && (alt < restrictAlt)) ||
((ar == RESTRICT_BELOW) && (alt > restrictAlt)))
{
SG_LOG(SG_AUTOPILOT, SG_INFO, "current waypt has altitude set, setting on AP");
_apTargetAltitudeFt->setDoubleValue(restrictAlt);
}
}
@ -1011,16 +1026,16 @@ double GPS::getLegDistance() const
return -1;
}
return SGGeodesy::distanceNm(_wp0_position, _wp1_position);
return _legDistanceNm;
}
double GPS::getLegCourse() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return -9999.0;
}
return SGGeodesy::courseDeg(_wp0_position, _wp1_position);
return _wayptController->targetTrackDeg();
}
double GPS::getLegMagCourse() const
@ -1034,21 +1049,6 @@ double GPS::getLegMagCourse() const
return m;
}
double GPS::getAltDistanceRatio() const
{
if (!_dataValid || (_mode == "obs")) {
return 0.0;
}
double dist = SGGeodesy::distanceM(_wp0_position, _wp1_position);
if ( dist <= 0.0 ) {
return 0.0;
}
double alt_difference_m = _wp0_position.getElevationM() - _wp1_position.getElevationM();
return alt_difference_m / dist;
}
double GPS::getMagTrack() const
{
if (!_dataValid) {
@ -1086,16 +1086,12 @@ const char* GPS::getWP0Ident() const
return "";
}
return _wp0Ident.c_str();
return _prevWaypt->ident().c_str();
}
const char* GPS::getWP0Name() const
{
if (!_dataValid || (_mode != "leg")) {
return "";
}
return _wp0Name.c_str();
return "";
}
const char* GPS::getWP1Ident() const
@ -1104,56 +1100,49 @@ const char* GPS::getWP1Ident() const
return "";
}
return _wp1Ident.c_str();
return _currentWaypt->ident().c_str();
}
const char* GPS::getWP1Name() const
{
if (!_dataValid) {
return "";
}
return _wp1Name.c_str();
return "";
}
double GPS::getWP1Distance() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return -1.0;
}
return _wp1DistanceM * SG_METER_TO_NM;
return _wayptController->distanceToWayptM() * SG_METER_TO_NM;
}
double GPS::getWP1TTW() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return -1.0;
}
if (_last_speed_kts < 1.0) {
return -1.0;
}
return (getWP1Distance() / _last_speed_kts) * 3600.0;
return _wayptController->timeToWaypt();
}
const char* GPS::getWP1TTWString() const
{
if (!_dataValid) {
double t = getWP1TTW();
if (t <= 0.0) {
return "";
}
return makeTTWString(getWP1TTW());
return makeTTWString(t);
}
double GPS::getWP1Bearing() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return -9999.0;
}
return _wp1TrueBearing;
return _wayptController->trueBearingDeg();
}
double GPS::getWP1MagBearing() const
@ -1162,56 +1151,41 @@ double GPS::getWP1MagBearing() const
return -9999.0;
}
double magBearing = _wp1TrueBearing - _magvar_node->getDoubleValue();
double magBearing = _wayptController->trueBearingDeg() - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(magBearing, 0.0, 360.0);
return magBearing;
}
double GPS::getWP1CourseDeviation() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return 0.0;
}
double dev = getWP1MagBearing() - _desiredCourse;
SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
if (fabs(dev) > 90.0) {
// When the course is away from the waypoint,
// it makes sense to change the sign of the deviation.
dev *= -1.0;
SG_NORMALIZE_RANGE(dev, -90.0, 90.0);
}
return dev;
return _wayptController->courseDeviationDeg();
}
double GPS::getWP1CourseErrorNm() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return 0.0;
}
double radDev = getWP1CourseDeviation() * SG_DEGREES_TO_RADIANS;
double course_error_m = sin(radDev) * _wp1DistanceM;
return course_error_m * SG_METER_TO_NM;
return _wayptController->xtrackErrorNm();
}
bool GPS::getWP1ToFlag() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return false;
}
double dev = getWP1MagBearing() - _desiredCourse;
SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
return (fabs(dev) < 90.0);
return _wayptController->toFlag();
}
bool GPS::getWP1FromFlag() const
{
if (!_dataValid) {
if (!_dataValid || !_wayptController.get()) {
return false;
}
@ -1278,9 +1252,9 @@ void GPS::setCommand(const char* aCmd)
defineWaypoint();
} else if (!strcmp(aCmd, "route-insert-before")) {
int index = _scratchNode->getIntValue("index");
if (index < 0 || (_routeMgr->size() == 0)) {
index = _routeMgr->size();
} else if (index >= _routeMgr->size()) {
if (index < 0 || (_routeMgr->numWaypts() == 0)) {
index = _routeMgr->numWaypts();
} else if (index >= _routeMgr->numWaypts()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-before, bad index:" << index);
return;
}
@ -1288,9 +1262,9 @@ void GPS::setCommand(const char* aCmd)
insertWaypointAtIndex(index);
} else if (!strcmp(aCmd, "route-insert-after")) {
int index = _scratchNode->getIntValue("index");
if (index < 0 || (_routeMgr->size() == 0)) {
index = _routeMgr->size();
} else if (index >= _routeMgr->size()) {
if (index < 0 || (_routeMgr->numWaypts() == 0)) {
index = _routeMgr->numWaypts();
} else if (index >= _routeMgr->numWaypts()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-after, bad index:" << index);
return;
} else {
@ -1301,8 +1275,8 @@ void GPS::setCommand(const char* aCmd)
} else if (!strcmp(aCmd, "route-delete")) {
int index = _scratchNode->getIntValue("index");
if (index < 0) {
index = _routeMgr->size();
} else if (index >= _routeMgr->size()) {
index = _routeMgr->numWaypts();
} else if (index >= _routeMgr->numWaypts()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index);
return;
}
@ -1332,18 +1306,16 @@ bool GPS::isScratchPositionValid() const
}
void GPS::directTo()
{
_wp0_position = _indicated_pos;
if (isScratchPositionValid()) {
_wp1Ident = _scratchNode->getStringValue("ident");
_wp1Name = _scratchNode->getStringValue("name");
_wp1_position = _scratchPos;
{
if (!isScratchPositionValid()) {
return;
}
_prevWaypt = NULL;
_wp0_position = _indicated_pos;
_currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
_mode = "dto";
_desiredCourse = getLegMagCourse();
_desiredCourseNode->fireValueChanged();
clearScratch();
wp1Changed();
}
@ -1359,8 +1331,8 @@ void GPS::loadRouteWaypoint()
int index = _scratchNode->getIntValue("index", -9999);
clearScratch();
if ((index < 0) || (index >= _routeMgr->size())) { // no index supplied, use current wp
index = _routeMgr->currentWaypoint();
if ((index < 0) || (index >= _routeMgr->numWaypts())) { // no index supplied, use current wp
index = _routeMgr->currentIndex();
}
_searchIsRoute = true;
@ -1370,18 +1342,16 @@ void GPS::loadRouteWaypoint()
void GPS::setScratchFromRouteWaypoint(int aIndex)
{
assert(_searchIsRoute);
if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromRouteWaypoint: route-index out of bounds");
return;
}
_searchResultIndex = aIndex;
SGWayPoint wp(_routeMgr->get_waypoint(aIndex));
_scratchNode->setStringValue("name", wp.get_name());
_scratchNode->setStringValue("ident", wp.get_id());
_scratchPos = wp.get_target();
WayptRef wp = _routeMgr->wayptAtIndex(aIndex);
_scratchNode->setStringValue("ident", wp->ident());
_scratchPos = wp->position();
_scratchValid = true;
_scratchNode->setDoubleValue("course", wp.get_track());
_scratchNode->setIntValue("index", aIndex);
}
@ -1476,9 +1446,9 @@ void GPS::search()
auto_ptr<FGPositioned::Filter> f(createFilter(_searchType));
if (_searchNames) {
_searchResults = FGPositioned::findAllWithName(_searchQuery, f.get());
_searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact);
} else {
_searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get());
_searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact);
}
bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true);
@ -1498,7 +1468,7 @@ bool GPS::getScratchHasNext() const
{
int lastResult;
if (_searchIsRoute) {
lastResult = _routeMgr->size() - 1;
lastResult = _routeMgr->numWaypts() - 1;
} else {
lastResult = (int) _searchResults.size() - 1;
}
@ -1584,14 +1554,14 @@ void GPS::addAirportToScratch(FGAirport* aAirport)
void GPS::selectOBSMode()
{
if (isScratchPositionValid()) {
_wp1Ident = _scratchNode->getStringValue("ident");
_wp1Name = _scratchNode->getStringValue("name");
_wp1_position = _scratchPos;
if (!isScratchPositionValid()) {
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode");
_mode = "obs";
_currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
_wp0_position = _indicated_pos;
wp1Changed();
}
@ -1680,7 +1650,7 @@ void GPS::defineWaypoint()
void GPS::insertWaypointAtIndex(int aIndex)
{
// note we do allow index = routeMgr->size(), that's an append
if ((aIndex < 0) || (aIndex > _routeMgr->size())) {
if ((aIndex < 0) || (aIndex > _routeMgr->numWaypts())) {
throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds");
}
@ -1690,18 +1660,18 @@ void GPS::insertWaypointAtIndex(int aIndex)
}
string ident = _scratchNode->getStringValue("ident");
string name = _scratchNode->getStringValue("name");
_routeMgr->add_waypoint(SGWayPoint(_scratchPos, ident, name), aIndex);
WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL);
_routeMgr->insertWayptAtIndex(wpt, aIndex);
}
void GPS::removeWaypointAtIndex(int aIndex)
{
if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
if ((aIndex < 0) || (aIndex >= _routeMgr->numWaypts())) {
throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
}
_routeMgr->pop_waypoint(aIndex);
_routeMgr->removeWayptAtIndex(aIndex);
}
void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef,

30
src/Instrumentation/gps.hxx
View file

@ -8,12 +8,14 @@
#define __INSTRUMENTS_GPS_HXX 1
#include <cassert>
#include <memory>
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/math/SGMath.hxx>
#include "Navaids/positioned.hxx"
#include <Navaids/positioned.hxx>
#include <Instrumentation/rnav_waypt_controller.hxx>
// forward decls
class SGRoute;
@ -73,20 +75,29 @@ private:
* /instrumentation/gps/magnetic-bug-error-deg
*/
class GPS : public SGSubsystem
class GPS : public SGSubsystem, public flightgear::RNAV
{
public:
GPS (SGPropertyNode *node);
GPS ();
virtual ~GPS ();
// SGSubsystem interface
virtual void init ();
virtual void update (double delta_time_sec);
virtual void bind();
virtual void unbind();
// RNAV interface
virtual SGGeod position();
virtual double trackDeg();
virtual double groundSpeedKts();
virtual double vspeedFPM();
virtual double magvarDeg();
virtual double selectedMagCourse();
virtual double overflightArmDistanceM();
private:
friend class GPSListener;
friend class SearchFilter;
@ -188,7 +199,6 @@ private:
void clearOutput();
void updateBasicData(double dt);
void updateWaypoints();
void updateTrackingBug();
void updateReferenceNavaid(double dt);
@ -275,7 +285,6 @@ private:
double getLegDistance() const;
double getLegCourse() const;
double getLegMagCourse() const;
double getAltDistanceRatio() const;
double getTrueTrack() const { return _last_true_track; }
double getMagTrack() const;
@ -325,6 +334,7 @@ private:
// members
SGPropertyNode_ptr _gpsNode;
SGPropertyNode_ptr _currentWayptNode;
SGPropertyNode_ptr _magvar_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _electrical_node;
@ -379,10 +389,8 @@ private:
SGGeodProperty _position;
SGGeod _wp0_position;
SGGeod _wp1_position;
SGGeod _indicated_pos;
std::string _wp0Ident, _wp0Name, _wp1Ident, _wp1Name;
double _wp1DistanceM, _wp1TrueBearing;
double _legDistanceNm;
// scratch data
SGGeod _scratchPos;
@ -410,7 +418,11 @@ private:
SGGeod _turnPt;
SGGeod _turnCentre;
std::auto_ptr<flightgear::WayptController> _wayptController;
SGPropertyNode_ptr _realismSimpleGps; ///< should the GPS be simple or realistic?
flightgear::WayptRef _prevWaypt;
flightgear::WayptRef _currentWaypt;
// autopilot drive properties
SGPropertyNode_ptr _apDrivingFlag;

699
src/Instrumentation/rnav_waypt_controller.cxx Normal file
View file

@ -0,0 +1,699 @@
// rnav_waypt_controller.cxx - Waypoint-specific behaviours for RNAV systems
// 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 "rnav_waypt_controller.hxx"
#include <cassert>
#include <simgear/sg_inlines.h>
#include <simgear/structure/exception.hxx>
#include <Airports/runways.hxx>
namespace flightgear
{
const double KNOTS_TO_METRES_PER_SECOND = SG_NM_TO_METER / 3600.0;
double pmod(double x, double y)
{
if (x < 0.0) {
return -fmod(x, y);
} else {
return fmod(x,y);
}
}
// implementation of
// http://williams.best.vwh.net/avform.htm#Intersection
bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result)
{
double crs13 = r1 * SG_DEGREES_TO_RADIANS;
double crs23 = r2 * SG_DEGREES_TO_RADIANS;
double dst12 = SGGeodesy::distanceRad(a, b);
//IF sin(lon2-lon1)<0
// crs12=acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
// crs21=2.*pi-acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
// ELSE
// crs12=2.*pi-acos((sin(lat2)-sin(lat1)*cos(dst12))/(sin(dst12)*cos(lat1)))
// crs21=acos((sin(lat1)-sin(lat2)*cos(dst12))/(sin(dst12)*cos(lat2)))
// ENDIF
// double diffLon = b.getLongitudeRad() - a.getLongitudeRad();
double sinLat1 = sin(a.getLatitudeRad());
double cosLat1 = cos(a.getLatitudeRad());
// double sinLat2 = sin(b.getLatitudeRad());
//double cosLat2 = cos(b.getLatitudeRad());
double sinDst12 = sin(dst12);
double cosDst12 = cos(dst12);
double crs12 = SGGeodesy::courseRad(a, b),
crs21 = SGGeodesy::courseRad(b, a);
double degCrs12 = crs12 * SG_RADIANS_TO_DEGREES;
double degCrs21 = crs21 * SG_RADIANS_TO_DEGREES;
/*
if (sin(diffLon) < 0.0) {
crs12 = acos((sinLat2 - sinLat1 * cosDst12) / (sinDst12 * cosLat1));
crs21 = SGMiscd::twopi() - acos((sinLat1 - sinLat2*cosDst12)/(sinDst12*cosLat2));
} else {
crs12 = SGMiscd::twopi() - acos((sinLat2 - sinLat1 * cosDst12)/(sinDst12 * cosLat1));
crs21 = acos((sinLat1 - sinLat2 * cosDst12)/(sinDst12 * cosLat2));
}
*/
double ang1 = SGMiscd::normalizeAngle2(crs13-crs12);
double ang2 = SGMiscd::normalizeAngle2(crs21-crs23);
if ((sin(ang1) == 0.0) && (sin(ang2) == 0.0)) {
SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: infinity of intersections");
return false;
}
if ((sin(ang1)*sin(ang2))<0.0) {
SG_LOG(SG_GENERAL, SG_WARN, "geocRadialIntersection: intersection ambiguous");
return false;
}
ang1 = fabs(ang1);
ang2 = fabs(ang2);
//ang3=acos(-cos(ang1)*cos(ang2)+sin(ang1)*sin(ang2)*cos(dst12))
//dst13=atan2(sin(dst12)*sin(ang1)*sin(ang2),cos(ang2)+cos(ang1)*cos(ang3))
//lat3=asin(sin(lat1)*cos(dst13)+cos(lat1)*sin(dst13)*cos(crs13))
//lon3=mod(lon1-dlon+pi,2*pi)-pi
double ang3 = acos(-cos(ang1) * cos(ang2) + sin(ang1) * sin(ang2) * cosDst12);
double dst13 = atan2(sinDst12 * sin(ang1) * sin(ang2), cos(ang2) + cos(ang1)*cos(ang3));
SGGeoc pt3;
SGGeodesy::advanceRadM(a, crs13, dst13 * SG_RAD_TO_NM * SG_NM_TO_METER, pt3);
double lat3 = asin(sinLat1 * cos(dst13) + cosLat1 * sin(dst13) * cos(crs13));
//dlon=atan2(sin(crs13)*sin(dst13)*cos(lat1),cos(dst13)-sin(lat1)*sin(lat3))
double dlon = atan2(sin(crs13)*sin(dst13)*cosLat1, cos(dst13)- (sinLat1 * sin(lat3)));
double lon3 = SGMiscd::normalizeAngle(-a.getLongitudeRad()-dlon);
result = SGGeoc::fromRadM(-lon3, lat3, a.getRadiusM());
//result = pt3;
return true;
}
////////////////////////////////////////////////////////////////////////////
WayptController::~WayptController()
{
}
void WayptController::init()
{
}
void WayptController::setDone()
{
if (_isDone) {
SG_LOG(SG_AUTOPILOT, SG_WARN, "already done @ WayptController::setDone");
}
_isDone = true;
}
double WayptController::timeToWaypt() const
{
double gs = _rnav->groundSpeedKts();
if (gs < 1.0) {
return -1.0; // stationary
}
gs*= KNOTS_TO_METRES_PER_SECOND;
return (distanceToWayptM() / gs);
}
//////////////
class BasicWayptCtl : public WayptController
{
public:
BasicWayptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
if (aWpt->flag(WPT_DYNAMIC)) {
throw sg_exception("BasicWayptCtrl doesn't work with dynamic waypoints");
}
}
virtual void init()
{
_targetTrack = SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
}
virtual void update()
{
double brg, az2;
SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
_courseDev = brg - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
setDone();
}
}
virtual double distanceToWayptM() const
{
return _distanceM;
}
virtual double xtrackErrorNm() const
{
double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
return x * SG_METER_TO_NM;
}
virtual bool toFlag() const
{
return (fabs(_courseDev) < 90.0);
}
virtual double courseDeviationDeg() const
{
return _courseDev;
}
virtual double trueBearingDeg() const
{
return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
}
virtual SGGeod position() const
{
return _waypt->position();
}
private:
double _distanceM;
double _courseDev;
};
/**
* Special controller for runways. For runways, we want very narrow deviation
* contraints, and to understand that any point along the paved area is
* equivalent to being 'at' the runway.
*/
class RunwayCtl : public WayptController
{
public:
RunwayCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
}
virtual void init()
{
_runway = static_cast<RunwayWaypt*>(_waypt.get())->runway();
_targetTrack = _runway->headingDeg();
}
virtual void update()
{
double brg, az2;
// use the far end of the runway for course deviation calculations.
// this should do the correct thing both for takeoffs (including entering
// the runway at a taxiway after the threshold) and also landings.
// seperately compute the distance to the threshold for timeToWaypt calc
SGGeodesy::inverse(_rnav->position(), _runway->end(), brg, az2, _distanceM);
double _courseDev = brg - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
if (fabs(_courseDev) > 90.0) {
setDone();
}
}
virtual double distanceToWayptM() const
{
return SGGeodesy::distanceM(_rnav->position(), _runway->threshold());
}
virtual double xtrackErrorNm() const
{
double x = sin(_courseDev * SG_RADIANS_TO_DEGREES) * _distanceM;
return x * SG_METER_TO_NM;
}
virtual double courseDeviationDeg() const
{
return _courseDev;
}
virtual double trueBearingDeg() const
{
// as in update(), use runway->end here, so the value remains
// sensible whether taking off or landing.
return SGGeodesy::courseDeg(_rnav->position(), _runway->end());
}
virtual SGGeod position() const
{
return _runway->threshold();
}
private:
FGRunway* _runway;
double _distanceM;
double _courseDev;
};
class ConstHdgToAltCtl : public WayptController
{
public:
ConstHdgToAltCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
if (_waypt->type() != "hdgToAlt") {
throw sg_exception("invalid waypoint type", "ConstHdgToAltCtl ctor");
}
if (_waypt->altitudeRestriction() == RESTRICT_NONE) {
throw sg_exception("invalid waypoint alt restriction", "ConstHdgToAltCtl ctor");
}
}
virtual void init()
{
HeadingToAltitude* w = (HeadingToAltitude*) _waypt.get();
_targetTrack = w->headingDegMagnetic() + _rnav->magvarDeg();
}
virtual void update()
{
double curAlt = _rnav->position().getElevationFt();
switch (_waypt->altitudeRestriction()) {
case RESTRICT_AT: {
double d = curAlt - _waypt->altitudeFt();
if (fabs(d) < 50.0) {
SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, reached target altitude " << _waypt->altitudeFt());
setDone();
}
} break;
case RESTRICT_ABOVE:
if (curAlt >= _waypt->altitudeFt()) {
SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, above target altitude " << _waypt->altitudeFt());
setDone();
}
break;
case RESTRICT_BELOW:
if (curAlt <= _waypt->altitudeFt()) {
SG_LOG(SG_GENERAL, SG_INFO, "ConstHdgToAltCtl, below target altitude " << _waypt->altitudeFt());
setDone();
}
break;
case RESTRICT_NONE:
assert(false);
break;
}
}
virtual double timeToWaypt() const
{
double d = fabs(_rnav->position().getElevationFt() - _waypt->altitudeFt());
return (d / _rnav->vspeedFPM()) * 60.0; // low pass filter here, probably
}
virtual double distanceToWayptM() const
{
double gsMsec = _rnav->groundSpeedKts() * KNOTS_TO_METRES_PER_SECOND;
return timeToWaypt() * gsMsec;
}
virtual SGGeod position() const
{
SGGeod p;
double az2;
SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
return p;
}
};
class InterceptCtl : public WayptController
{
public:
InterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
if (_waypt->type() != "radialIntercept") {
throw sg_exception("invalid waypoint type", "InterceptCtl ctor");
}
}
virtual void init()
{
RadialIntercept* w = (RadialIntercept*) _waypt.get();
_trueRadial = w->radialDegMagnetic() + _rnav->magvarDeg();
_targetTrack = w->courseDegMagnetic() + _rnav->magvarDeg();
}
virtual void update()
{
// note we want the outbound radial from the waypt, hence the ordering
// of arguments to courseDeg
double r = SGGeodesy::courseDeg(_waypt->position(), _rnav->position());
SG_LOG(SG_AUTOPILOT, SG_INFO, "current radial=" << r);
if (fabs(r - _trueRadial) < 0.5) {
SG_LOG(SG_GENERAL, SG_INFO, "InterceptCtl, intercepted radial " << _trueRadial);
setDone();
}
}
virtual double distanceToWayptM() const
{
return SGGeodesy::distanceM(_rnav->position(), position());
}
virtual SGGeod position() const
{
SGGeoc c;
geocRadialIntersection(SGGeoc::fromGeod(_rnav->position()), _rnav->trackDeg(),
SGGeoc::fromGeod(_waypt->position()), _trueRadial, c);
return SGGeod::fromGeoc(c);
}
private:
double _trueRadial;
};
class DMEInterceptCtl : public WayptController
{
public:
DMEInterceptCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
if (_waypt->type() != "dmeIntercept") {
throw sg_exception("invalid waypoint type", "DMEInterceptCtl ctor");
}
}
virtual void init()
{
_dme = (DMEIntercept*) _waypt.get();
_targetTrack = _dme->courseDegMagnetic() + _rnav->magvarDeg();
}
virtual void update()
{
_distanceNm = SGGeodesy::distanceNm(_rnav->position(), _dme->position());
double d = fabs(_distanceNm - _dme->dmeDistanceNm());
if (d < 0.1) {
SG_LOG(SG_GENERAL, SG_INFO, "DMEInterceptCtl, intercepted DME " << _dme->dmeDistanceNm());
setDone();
}
}
virtual double distanceToWayptM() const
{
return fabs(_distanceNm - _dme->dmeDistanceNm()) * SG_NM_TO_METER;
}
virtual SGGeod position() const
{
SGGeod p;
double az2;
SGGeodesy::direct(_rnav->position(), _targetTrack, distanceToWayptM(), p, az2);
return p;
}
private:
DMEIntercept* _dme;
double _distanceNm;
};
class HoldCtl : public WayptController
{
public:
HoldCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
}
virtual void init()
{
}
virtual void update()
{
// fly inbound / outbound sides, or execute the turn
#if 0
if (inTurn) {
targetTrack += dt * turnRateSec * turnDirection;
if (inbound) {
if .. targetTrack has passed inbound radial, doen with this turn
} else {
if target track has passed reciprocal radial done with turn
}
} else {
check time / distance elapsed
if (sideDone) {
inTurn = true;
inbound = !inbound;
nextHeading = inbound;
if (!inbound) {
nextHeading += 180.0;
SG_NORMALIZE_RANGE(nextHeading, 0.0, 360.0);
}
}
}
#endif
setDone();
}
virtual double distanceToWayptM() const
{
return -1.0;
}
virtual SGGeod position() const
{
return _waypt->position();
}
};
class VectorsCtl : public WayptController
{
public:
VectorsCtl(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
}
virtual void init()
{
}
virtual void update()
{
setDone();
}
virtual double distanceToWayptM() const
{
return -1.0;
}
virtual SGGeod position() const
{
return _waypt->position();
}
private:
};
///////////////////////////////////////////////////////////////////////////////
DirectToController::DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin) :
WayptController(aRNAV, aWpt),
_origin(aOrigin)
{
}
void DirectToController::init()
{
if (_waypt->flag(WPT_DYNAMIC)) {
throw sg_exception("can't direct-to a dynamic waypoint");
}
_targetTrack = SGGeodesy::courseDeg(_origin, _waypt->position());
}
void DirectToController::update()
{
double brg, az2;
SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
_courseDev = brg - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
if ((fabs(_courseDev) > 90.0) && (_distanceM < _rnav->overflightArmDistanceM())) {
setDone();
}
}
double DirectToController::distanceToWayptM() const
{
return _distanceM;
}
double DirectToController::xtrackErrorNm() const
{
double x = sin(courseDeviationDeg() * SG_DEGREES_TO_RADIANS) * _distanceM;
return x * SG_METER_TO_NM;
}
double DirectToController::courseDeviationDeg() const
{
return _courseDev;
}
double DirectToController::trueBearingDeg() const
{
return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
}
SGGeod DirectToController::position() const
{
return _waypt->position();
}
///////////////////////////////////////////////////////////////////////////////
OBSController::OBSController(RNAV* aRNAV, const WayptRef& aWpt) :
WayptController(aRNAV, aWpt)
{
}
void OBSController::init()
{
if (_waypt->flag(WPT_DYNAMIC)) {
throw sg_exception("can't use a dynamic waypoint for OBS mode");
}
_targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
}
void OBSController::update()
{
_targetTrack = _rnav->selectedMagCourse() + _rnav->magvarDeg();
double brg, az2;
SGGeodesy::inverse(_rnav->position(), _waypt->position(), brg, az2, _distanceM);
_courseDev = brg - _targetTrack;
SG_NORMALIZE_RANGE(_courseDev, -180.0, 180.0);
}
bool OBSController::toFlag() const
{
return (fabs(_courseDev) < 90.0);
}
double OBSController::distanceToWayptM() const
{
return _distanceM;
}
double OBSController::xtrackErrorNm() const
{
double x = sin(_courseDev * SG_DEGREES_TO_RADIANS) * _distanceM;
return x * SG_METER_TO_NM;
}
double OBSController::courseDeviationDeg() const
{
// if (fabs(_courseDev) > 90.0) {
// double d = -_courseDev;
// SG_NORMALIZE_RANGE(d, -90.0, 90.0);
// return d;
//}
return _courseDev;
}
double OBSController::trueBearingDeg() const
{
return SGGeodesy::courseDeg(_rnav->position(), _waypt->position());
}
SGGeod OBSController::position() const
{
return _waypt->position();
}
///////////////////////////////////////////////////////////////////////////////
WayptController* WayptController::createForWaypt(RNAV* aRNAV, const WayptRef& aWpt)
{
if (!aWpt) {
throw sg_exception("Passed null waypt", "WayptController::createForWaypt");
}
const std::string& wty(aWpt->type());
if (wty == "runway") {
return new RunwayCtl(aRNAV, aWpt);
}
if (wty == "radialIntercept") {
return new InterceptCtl(aRNAV, aWpt);
}
if (wty == "dmeIntercept") {
return new DMEInterceptCtl(aRNAV, aWpt);
}
if (wty == "hdgToAlt") {
return new ConstHdgToAltCtl(aRNAV, aWpt);
}
if (wty == "vectors") {
return new VectorsCtl(aRNAV, aWpt);
}
if (wty == "hold") {
return new HoldCtl(aRNAV, aWpt);
}
return new BasicWayptCtl(aRNAV, aWpt);
}
} // of namespace flightgear

193
src/Instrumentation/rnav_waypt_controller.hxx Normal file
View file

@ -0,0 +1,193 @@
// rnav_waypt_controller.hxx - Waypoint-specific behaviours for RNAV systems
// 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.
#ifndef FG_WAYPT_CONTROLLER_HXX
#define FG_WAYPT_CONTROLLER_HXX
#include <Navaids/waypoint.hxx>
namespace flightgear
{
/**
* Abstract RNAV interface, for devices which implement an RNAV
* system - INS / GPS / FMS
*/
class RNAV
{
public:
virtual SGGeod position() = 0;
/**
* True track in degrees
*/
virtual double trackDeg() = 0;
/**
* Ground speed (along the track) in knots
*/
virtual double groundSpeedKts() = 0;
/**
* Vertical speed in ft/minute
*/
virtual double vspeedFPM()= 0;
/**
* Magnetic variation at current position
*/
virtual double magvarDeg() = 0;
/**
* device selected course (eg, from autopilot / MCP / OBS) in degrees
*/
virtual double selectedMagCourse() = 0;
/**
* minimum distance to a waypoint for overflight sequencing.
*/
virtual double overflightArmDistanceM() = 0;
};
class WayptController
{
public:
virtual ~WayptController();
virtual void init();
virtual void update() = 0;
/**
* Compute time until the waypoint is done
*/
virtual double timeToWaypt() const;
/**
* Compute distance until the waypoint is done
*/
virtual double distanceToWayptM() const = 0;
/**
* Bearing to the waypoint, if this value is meaningful.
* Default implementation returns the target track
*/
virtual double trueBearingDeg() const
{ return _targetTrack; }
virtual double targetTrackDeg() const
{ return _targetTrack; }
virtual double xtrackErrorNm() const
{ return 0.0; }
virtual double courseDeviationDeg() const
{ return 0.0; }
/**
* Position associated with the waypt. For static waypoints, this is
* simply the waypoint position itself; for dynamic points, it's the
* estimated location at which the controller will be done.
*/
virtual SGGeod position() const = 0;
/**
* Is this controller finished?
*/
bool isDone() const
{ return _isDone; }
/**
* to/from flag - true = to, false = from. Defaults to 'true' because
* nearly all waypoint controllers become done as soon as this value would
* become false.
*/
virtual bool toFlag() const
{ return true; }
/**
* Static factory method, given a waypoint, return a controller bound
* to it, of the appropriate type
*/
static WayptController* createForWaypt(RNAV* rnav, const WayptRef& aWpt);
protected:
WayptController(RNAV* aRNAV, const WayptRef& aWpt) :
_waypt(aWpt),
_rnav(aRNAV),
_isDone(false)
{ }
WayptRef _waypt;
double _targetTrack;
RNAV* _rnav;
void setDone();
private:
bool _isDone;
};
/**
* Controller supports 'directTo' (DTO) navigation to a waypoint. This
* creates a course from a starting point, to the waypoint, and reports
* deviation from that course.
*
* The controller is done when the waypoint is reached (to/from goes to 'from')
*/
class DirectToController : public WayptController
{
public:
DirectToController(RNAV* aRNAV, const WayptRef& aWpt, const SGGeod& aOrigin);
virtual void init();
virtual void update();
virtual double distanceToWayptM() const;
virtual double xtrackErrorNm() const;
virtual double courseDeviationDeg() const;
virtual double trueBearingDeg() const;
virtual SGGeod position() const;
private:
SGGeod _origin;
double _distanceM;
double _courseDev;
};
/**
*
*/
class OBSController : public WayptController
{
public:
OBSController(RNAV* aRNAV, const WayptRef& aWpt);
virtual void init();
virtual void update();
virtual double distanceToWayptM() const;
virtual double xtrackErrorNm() const;
virtual double courseDeviationDeg() const;
virtual double trueBearingDeg() const;
virtual bool toFlag() const;
virtual SGGeod position() const;
private:
double _distanceM;
double _courseDev;
};
} // of namespace flightgear
#endif

130
src/Instrumentation/testgps.cxx
View file

@ -1,4 +1,9 @@
#include <fstream>
#include <simgear/misc/sg_path.hxx>
#include <simgear/structure/exception.hxx>
#include <Main/fg_init.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
@ -6,8 +11,12 @@
#include <Instrumentation/gps.hxx>
#include <Autopilot/route_mgr.hxx>
#include <Environment/environment_mgr.hxx>
#include <Navaids/airways.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/procedure.hxx>
using std::string;
using namespace flightgear;
char *homedir = ::getenv( "HOME" );
char *hostname = ::getenv( "HOSTNAME" );
@ -45,6 +54,16 @@ void printScratch(SGPropertyNode* scratch)
}
}
void printRoute(const WayptVec& aRoute)
{
SG_LOG(SG_GENERAL, SG_INFO, "route size=" << aRoute.size());
for (unsigned int r=0; r<aRoute.size();++r) {
Waypt* w = aRoute[r];
SG_LOG(SG_GENERAL, SG_ALERT, "\t" << r << ": " << w->ident() << " "
<< w->owner()->ident());
}
}
void createDummyRoute(FGRouteMgr* rm)
{
SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
@ -59,6 +78,8 @@ void createDummyRoute(FGRouteMgr* rm)
int main(int argc, char* argv[])
{
try{
globals = new FGGlobals;
fgInitFGRoot(argc, argv);
@ -69,6 +90,9 @@ int main(int argc, char* argv[])
fgInitNav();
fgSetDouble("/environment/magnetic-variation-deg", 0.0);
Airway::load();
SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
@ -94,12 +118,15 @@ int main(int argc, char* argv[])
// globals->add_subsystem("environment", envMgr);
// envMgr->init();
fgSetBool("/sim/realism/simple-gps", true);
// _realismSimpleGps
SGPropertyNode* nd = fgGetNode("/instrumentation/gps", true);
GPS* gps = new GPS(nd);
globals->add_subsystem("gps", gps);
const FGAirport* egph = fgFindAirportID("EGPH");
testSetPosition(egph->geod());
// startup the route manager
@ -116,6 +143,7 @@ int main(int argc, char* argv[])
// update a few times
gps->update(0.05);
gps->update(0.05);
gps->update(0.05);
scratch->setStringValue("query", "TL");
scratch->setStringValue("type", "Vor");
@ -221,7 +249,105 @@ int main(int argc, char* argv[])
nd->setStringValue("command", "define-user-wpt");
printScratch(scratch);
// airways
FGPositioned::TypeFilter vorFilt(FGPositioned::VOR);
FGPositionedRef tla = FGPositioned::findClosestWithIdent("TLA", pos, &vorFilt);
FGPositionedRef big = FGPositioned::findClosestWithIdent("BIG", pos, &vorFilt);
FGPositionedRef pol = FGPositioned::findClosestWithIdent("POL", pos, &vorFilt);
const FGAirport* eddm = fgFindAirportID("EDDM");
FGPositionedRef mun = FGPositioned::findClosestWithIdent("MUN",
eddm->geod(), &vorFilt);
const FGAirport* ksfo = fgFindAirportID("KSFO");
FGPositionedRef sfo = FGPositioned::findClosestWithIdent("SFO",
ksfo->geod(), &vorFilt);
WayptRef awy1 = new NavaidWaypoint(tla, NULL);
WayptRef awy2 = new NavaidWaypoint(big, NULL);
WayptRef awy3 = new NavaidWaypoint(pol, NULL);
WayptRef awy4 = new NavaidWaypoint(mun, NULL);
WayptRef awy5 = new NavaidWaypoint(sfo, NULL);
WayptRef awy6 = new NavaidWaypoint(
(FGPositioned*) fgFindAirportID("KJFK"), NULL);
SGPath p("/Users/jmt/Desktop/airways.kml");
std::fstream f;
f.open(p.str().c_str(), fstream::out | fstream::trunc);
// pre-amble
f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
"<Document>\n";
WayptVec route;
Airway::highLevel()->route(awy1, awy3, route);
Route::dumpRouteToLineString("egph-egcc", route, f);
Airway::lowLevel()->route(awy1, awy2, route);
Route::dumpRouteToLineString("egph-big", route, f);
Airway::lowLevel()->route(awy2, awy4, route);
Route::dumpRouteToLineString("big-mun", route, f);
Airway::highLevel()->route(awy4, awy5, route);
Route::dumpRouteToLineString("mun-sfo", route, f);
Airway::lowLevel()->route(awy5, awy6, route);
Route::dumpRouteToLineString("sfo-jfk", route, f);
// post-amble
f << "</Document>\n"
"</kml>" << endl;
f.close();
// procedures
SGPath op("/Users/jmt/Desktop/procedures.kml");
f.open(op.str().c_str(), fstream::out | fstream::trunc);
FGAirport* eham = (FGAirport*) fgFindAirportID("EHAM");
FGPositioned::TypeFilter fixFilt(FGPositioned::FIX);
WayptVec approach;
FGPositionedRef redfa = FGPositioned::findClosestWithIdent("REDFA",
eham->geod(), &fixFilt);
bool ok = eham->buildApproach(new NavaidWaypoint(redfa, NULL),
eham->getRunwayByIdent("18R"), approach);
if (!ok ) {
SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
}
FGAirport* egll = (FGAirport*) fgFindAirportID("EGLL");
WayptVec approach2;
ok = egll->buildApproach(new NavaidWaypoint(big, NULL),
egll->getRunwayByIdent("27R"), approach2);
if (!ok ) {
SG_LOG(SG_GENERAL, SG_INFO, "failed to build approach");
}
// pre-amble
f << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n"
"<Document>\n";
Route::dumpRouteToLineString("REDFA 18R", approach, f);
Route::dumpRouteToLineString("EGLL 27R", approach2, f);
// post-amble
f << "</Document>\n"
"</kml>" << endl;
f.close();
return EXIT_SUCCESS;
} catch (sg_exception& ex) {
SG_LOG(SG_GENERAL, SG_ALERT, "exception:" << ex.getFormattedMessage());
}
return EXIT_FAILURE;
}

14
src/Main/fg_init.cxx
View file

@ -58,6 +58,8 @@
#include <simgear/structure/event_mgr.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/misc/sg_dir.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/interpolator.hxx>
#include <simgear/scene/material/matlib.hxx>
#include <simgear/scene/model/particles.hxx>
@ -97,6 +99,7 @@
#include <Navaids/navlist.hxx>
#include <Navaids/fix.hxx>
#include <Navaids/fixlist.hxx>
#include <Navaids/airways.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include <Scripting/NasalSys.hxx>
@ -1071,15 +1074,8 @@ fgInitNav ()
fixlist.init( p_fix ); // adds fixes to the DB in positioned.cxx
SG_LOG(SG_GENERAL, SG_INFO, " Airways");
SGPath p_awy( globals->get_fg_root() );
p_awy.append( "Navaids/awy.dat" );
FGAirwayNetwork *awyNet = new FGAirwayNetwork;
//cerr << "Loading Airways" << endl;
awyNet->load (p_awy );
awyNet->init();
//cerr << "initializing airways" << endl;
globals->set_airwaynet( awyNet );
flightgear::Airway::load();
return true;
}

6
src/Main/globals.cxx
View file

@ -45,7 +45,6 @@
#include <Model/acmodel.hxx>
#include <Model/modelmgr.hxx>
#include <MultiPlayer/multiplaymgr.hxx>
#include <Navaids/awynet.hxx>
#include <Scenery/scenery.hxx>
#include <Scenery/tilemgr.hxx>
#include <Navaids/navlist.hxx>
@ -149,9 +148,7 @@ FGGlobals::FGGlobals() :
dmelist( NULL ),
tacanlist( NULL ),
carrierlist( NULL ),
channellist( NULL ),
airwaynet( NULL )
channellist( NULL )
{
simgear::ResourceManager::instance()->addProvider(new AircraftResourceProvider());
}
@ -199,7 +196,6 @@ FGGlobals::~FGGlobals()
delete tacanlist;
delete carrierlist;
delete channellist;
delete airwaynet;
soundmgr->unbind();
delete soundmgr;

6
src/Main/globals.hxx
View file

@ -60,7 +60,6 @@ class FGAircraftModel;
class FGControls;
class FGFlightPlanDispatcher;
class FGNavList;
class FGAirwayNetwork;
class FGTACANList;
class FGModelMgr;
class FGRouteMgr;
@ -171,7 +170,6 @@ private:
FGNavList *tacanlist;
FGNavList *carrierlist;
FGTACANList *channellist;
FGAirwayNetwork *airwaynet;
/// roots of Aircraft trees
string_list fg_aircraft_dirs;
@ -322,10 +320,6 @@ public:
inline FGTACANList *get_channellist() const { return channellist; }
inline void set_channellist( FGTACANList *c ) { channellist = c; }
inline FGAirwayNetwork *get_airwaynet() const { return airwaynet; }
inline void set_airwaynet( FGAirwayNetwork *a ) { airwaynet = a; }
/**
* Save the current state as the initial state.
*/

8
src/Navaids/Makefile.am
View file

@ -5,11 +5,15 @@ noinst_LIBRARIES = libNavaids.a
libNavaids_a_SOURCES = \
navdb.hxx navdb.cxx \
fix.hxx fixlist.hxx fixlist.cxx \
awynet.hxx awynet.cxx \
navrecord.hxx navrecord.cxx \
navlist.hxx navlist.cxx \
positioned.hxx positioned.cxx \
markerbeacon.hxx markerbeacon.cxx
markerbeacon.hxx markerbeacon.cxx \
routePath.hxx routePath.cxx \
airways.hxx airways.cxx \
route.hxx route.cxx \
waypoint.hxx waypoint.cxx \
procedure.hxx procedure.cxx
#
# testnavs_SOURCES = testnavs.cxx

454
src/Navaids/airways.cxx Normal file
View file

@ -0,0 +1,454 @@
// airways.cxx - storage of airways network, and routing between nodes
// 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.
#ifndef HAVE_CONFIG_H
# include "config.h"
#endif
#include "airways.hxx"
#include <algorithm>
#include <set>
#include <simgear/sg_inlines.h>
#include <simgear/structure/exception.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/sg_path.hxx>
#include <boost/tuple/tuple.hpp>
#include <Main/globals.hxx>
#include <Navaids/positioned.hxx>
#include <Navaids/waypoint.hxx>
using std::make_pair;
using std::string;
using std::set;
using std::vector;
#define DEBUG_AWY_SEARCH 1
namespace flightgear
{
Airway::Network* Airway::static_lowLevel = NULL;
Airway::Network* Airway::static_highLevel = NULL;
//////////////////////////////////////////////////////////////////////////////
/**
* information about an edge in the network.
* Some of this information is computed lazily
*/
class AdjacentWaypoint
{
public:
AdjacentWaypoint(const FGPositionedRef& aOrigin,
const FGPositionedRef& aDest, Airway* aWay);
double distanceM() const;
const FGPositionedRef& other(const FGPositionedRef& aEnd) const;
const FGPositionedRef origin;
const FGPositionedRef destination;
const Airway* airway;
private:
void validate() const;
mutable double _distanceM;
};
class AStarOpenNode : public SGReferenced
{
public:
AStarOpenNode(FGPositionedRef aNode, double aLegDist,
Airway* aAirway,
FGPositionedRef aDest, AStarOpenNode* aPrev) :
node(aNode),
airway(aAirway),
previous(aPrev)
{
distanceFromStart = aLegDist;
if (previous) {
distanceFromStart += previous->distanceFromStart;
}
directDistanceToDestination = SGGeodesy::distanceM(node->geod(), aDest->geod());
}
virtual ~AStarOpenNode()
{
}
FGPositionedRef node;
Airway* airway;
SGSharedPtr<AStarOpenNode> previous;
double distanceFromStart; // aka 'g(x)'
double directDistanceToDestination; // aka 'h(x)'
/**
* aka 'f(x)'
*/
double totalCost() const {
return distanceFromStart + directDistanceToDestination;
}
};
typedef SGSharedPtr<AStarOpenNode> AStarOpenNodeRef;
////////////////////////////////////////////////////////////////////////////
Airway::Network* Airway::lowLevel()
{
return static_lowLevel;
}
Airway::Network* Airway::highLevel()
{
return static_highLevel;
}
Airway::Airway(const std::string& aIdent, double aTop, double aBottom) :
_ident(aIdent),
_topAltitudeFt(aTop),
_bottomAltitudeFt(aBottom)
{
}
void Airway::load()
{
static_lowLevel = new Network;
static_highLevel = new Network;
SGPath path( globals->get_fg_root() );
path.append( "Navaids/awy.dat" );
std::string identStart, identEnd, name;
double latStart, lonStart, latEnd, lonEnd;
int type, base, top;
//int airwayIndex = 0;
//FGNode *n;
sg_gzifstream in( path.str() );
if ( !in.is_open() ) {
SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
throw sg_io_exception("Could not open airways data", sg_location(path.str()));
}
// toss the first two lines of the file
in >> skipeol;
in >> skipeol;
// read in each remaining line of the file
while (!in.eof()) {
in >> identStart;
if (identStart == "99") {
break;
}
in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
in >> skipeol;
// type = 1; low-altitude
// type = 2; high-altitude
Network* net = (type == 1) ? static_lowLevel : static_highLevel;
SGGeod startPos(SGGeod::fromDeg(lonStart, latStart)),
endPos(SGGeod::fromDeg(lonEnd, latEnd));
Airway* awy = net->findAirway(name, top, base);
net->addEdge(awy, startPos, identStart, endPos, identEnd);
} // of file line iteration
}
Airway* Airway::Network::findAirway(const std::string& aName, double aTop, double aBase)
{
AirwayDict::iterator it = _airways.find(aName);
if (it == _airways.end()) {
Airway* awy = new Airway(aName, aTop, aBase);
it = _airways.insert(it, make_pair(aName, awy));
}
return it->second;
}
void Airway::Network::addEdge(Airway* aWay, const SGGeod& aStartPos,
const std::string& aStartIdent,
const SGGeod& aEndPos, const std::string& aEndIdent)
{
FGPositionedRef start = FGPositioned::findClosestWithIdent(aStartIdent, aStartPos);
FGPositionedRef end = FGPositioned::findClosestWithIdent(aEndIdent, aEndPos);
if (!start) {
SG_LOG(SG_GENERAL, SG_INFO, "unknown airways start pt: '" << aStartIdent << "'");
start = FGPositioned::createUserWaypoint(aStartIdent, aStartPos);
return;
}
if (!end) {
SG_LOG(SG_GENERAL, SG_INFO, "unknown airways end pt: '" << aEndIdent << "'");
end = FGPositioned::createUserWaypoint(aEndIdent, aEndPos);
return;
}
AdjacentWaypoint* edge = new AdjacentWaypoint(start, end, aWay);
_graph.insert(make_pair(start, edge));
_graph.insert(make_pair(end, edge));
}
//////////////////////////////////////////////////////////////////////////////
bool Airway::Network::inNetwork(const FGPositioned* aPos) const
{
FGPositioned* pos = const_cast<FGPositioned*>(aPos);
return (_graph.find(pos) != _graph.end());
}
bool Airway::Network::route(WayptRef aFrom, WayptRef aTo,
WayptVec& aPath)
{
if (!aFrom || !aTo) {
throw sg_exception("invalid waypoints to route between");
}
// find closest nodes on the graph to from/to
// if argument waypoints are directly on the graph (which is frequently the
// case), note this so we don't duplicate them in the output.
FGPositionedRef from, to;
bool exactTo, exactFrom;
boost::tie(from, exactFrom) = findClosestNode(aFrom);
boost::tie(to, exactTo) = findClosestNode(aTo);
#ifdef DEBUG_AWY_SEARCH
SG_LOG(SG_GENERAL, SG_INFO, "from:" << from->ident() << "/" << from->name());
SG_LOG(SG_GENERAL, SG_INFO, "to:" << to->ident() << "/" << to->name());
#endif
bool ok = search2(from, to, aPath);
if (!ok) {
return false;
}
if (exactTo) {
aPath.pop_back();
}
if (exactFrom) {
// edge case - if from and to are equal, which can happen, don't
// crash here. This happens routing EGPH -> EGCC; 'DCS' is common
// to the EGPH departure and EGCC STAR.
if (!aPath.empty()) {
aPath.erase(aPath.begin());
}
}
return true;
}
std::pair<FGPositionedRef, bool>
Airway::Network::findClosestNode(WayptRef aRef)
{
return findClosestNode(aRef->position());
}
class InAirwayFilter : public FGPositioned::Filter
{
public:
InAirwayFilter(Airway::Network* aNet) :
_net(aNet)
{ ; }
virtual bool pass(FGPositioned* aPos) const
{
return _net->inNetwork(aPos);
}
virtual FGPositioned::Type minType() const
{ return FGPositioned::WAYPOINT; }
virtual FGPositioned::Type maxType() const
{ return FGPositioned::NDB; }
private:
Airway::Network* _net;
};
std::pair<FGPositionedRef, bool>
Airway::Network::findClosestNode(const SGGeod& aGeod)
{
InAirwayFilter f(this);
FGPositionedRef r = FGPositioned::findClosest(aGeod, 800.0, &f);
bool exact = false;
if (r && (SGGeodesy::distanceM(aGeod, r->geod()) < 100.0)) {
exact = true; // within 100 metres, let's call that exact
}
return make_pair(r, exact);
}
/////////////////////////////////////////////////////////////////////////////
typedef vector<AStarOpenNodeRef> OpenNodeHeap;
static void buildWaypoints(AStarOpenNodeRef aNode, WayptVec& aRoute)
{
// count the route length, and hence pre-size aRoute
int count = 0;
AStarOpenNodeRef n = aNode;
for (; n != NULL; ++count, n = n->previous) {;}
aRoute.resize(count);
// run over the route, creating waypoints
for (n = aNode; n; n=n->previous) {
aRoute[--count] = new NavaidWaypoint(n->node, n->airway);
}
}
/**
* Inefficent (linear) helper to find an open node in the heap
*/
static AStarOpenNodeRef
findInOpen(const OpenNodeHeap& aHeap, FGPositioned* aPos)
{
for (unsigned int i=0; i<aHeap.size(); ++i) {
if (aHeap[i]->node == aPos) {
return aHeap[i];
}
}
return NULL;
}
class HeapOrder
{
public:
bool operator()(AStarOpenNode* a, AStarOpenNode* b)
{
return a->totalCost() > b->totalCost();
}
};
bool Airway::Network::search2(FGPositionedRef aStart, FGPositionedRef aDest,
WayptVec& aRoute)
{
typedef set<FGPositioned*> ClosedNodeSet;
typedef std::pair<AdjacencyMap::iterator,AdjacencyMap::iterator> AdjacencyMapRange;
OpenNodeHeap openNodes;
ClosedNodeSet closedNodes;
HeapOrder ordering;
openNodes.push_back(new AStarOpenNode(aStart, 0.0, NULL, aDest, NULL));
// A* open node iteration
while (!openNodes.empty()) {
std::pop_heap(openNodes.begin(), openNodes.end(), ordering);
AStarOpenNodeRef x = openNodes.back();
FGPositioned* xp = x->node;
openNodes.pop_back();
closedNodes.insert(xp);
// SG_LOG(SG_GENERAL, SG_INFO, "x:" << xp->ident() << ", f(x)=" << x->totalCost());
// check if xp is the goal; if so we're done, since there cannot be an open
// node with lower f(x) value.
if (xp == aDest) {
buildWaypoints(x, aRoute);
return true;
}
// adjacent (neighbour) iteration
AdjacencyMapRange r(_graph.equal_range(xp));
for (; r.first != r.second; ++r.first) {
AdjacentWaypoint* adj(r.first->second);
FGPositioned* yp = adj->other(xp);
if (closedNodes.count(yp)) {
continue; // closed, ignore
}
AStarOpenNodeRef y = findInOpen(openNodes, yp);
if (y) { // already open
double g = x->distanceFromStart + adj->distanceM();
if (g > y->distanceFromStart) {
// worse path, ignore
//SG_LOG(SG_GENERAL, SG_INFO, "\tabandoning " << yp->ident() <<
// " path is worse: g(y)" << y->distanceFromStart << ", g'=" << g);
continue;
}
// we need to update y. Unfortunately this means rebuilding the heap,
// since y's score can change arbitrarily
//SG_LOG(SG_GENERAL, SG_INFO, "\tfixing up previous for new path to " << yp->ident() << ", d =" << g);
y->previous = x;
y->distanceFromStart = g;
y->airway = (Airway*) adj->airway;
std::make_heap(openNodes.begin(), openNodes.end(), ordering);
} else { // not open, insert a new node for y into the heap
y = new AStarOpenNode(yp, adj->distanceM(),
(Airway*) adj->airway, aDest, x);
//SG_LOG(SG_GENERAL, SG_INFO, "\ty=" << yp->ident() << ", f(y)=" << y->totalCost());
openNodes.push_back(y);
std::push_heap(openNodes.begin(), openNodes.end(), ordering);
}
} // of neighbour iteration
} // of open node iteration
SG_LOG(SG_GENERAL, SG_INFO, "A* failed to find route");
return false;
}
//////////////////////////////////////////////////////////////////////////////
// AdjacentWaypoint definitions
AdjacentWaypoint::AdjacentWaypoint(
const FGPositionedRef& aOrigin, const FGPositionedRef& aDest, Airway* aWay) :
origin(aOrigin),
destination(aDest),
airway(aWay),
_distanceM(-1.0)
{
}
const FGPositionedRef&
AdjacentWaypoint::other(const FGPositionedRef& aEnd) const
{
return (aEnd == origin) ? destination : origin;
}
double AdjacentWaypoint::distanceM() const
{
validate();
return _distanceM;
}
void AdjacentWaypoint::validate() const
{
if (_distanceM > 0.0) {
return; // already validated
}
_distanceM = SGGeodesy::distanceM(origin->geod(), destination->geod());
}
} // of namespace flightgear

134
src/Navaids/airways.hxx Normal file
View file

@ -0,0 +1,134 @@
// airways.hxx - storage of airways network, and routing between nodes
// 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.
#ifndef FG_AIRWAYS_HXX
#define FG_AIRWAYS_HXX
#include <map>
#include <vector>
#include <Navaids/route.hxx>
class FGPositioned;
typedef SGSharedPtr<FGPositioned> FGPositionedRef;
namespace flightgear {
// forward declare some helpers
struct SearchContext;
class AdjacentWaypoint;
class InAirwayFilter;
class Airway : public Route
{
public:
virtual std::string ident() const
{ return _ident; }
static void load();
/**
* Track a network of airways
*
*/
class Network
{
public:
friend class Airway;
friend class InAirwayFilter;
/**
* Principal routing algorithm. Attempts to find the best route beween
* two points. If either point is part of the airway network (e.g, a SID
* or STAR transition), it will <em>not</em> be duplicated in the result
* path.
*
* Returns true if a route could be found, or false otherwise.
*/
bool route(WayptRef aFrom, WayptRef aTo, WayptVec& aPath);
private:
void addEdge(Airway* aWay, const SGGeod& aStartPos,
const std::string& aStartIdent,
const SGGeod& aEndPos, const std::string& aEndIdent);
Airway* findAirway(const std::string& aName, double aTop, double aBase);
typedef std::multimap<FGPositioned*, AdjacentWaypoint*> AdjacencyMap;
AdjacencyMap _graph;
typedef std::vector<AdjacentWaypoint*> AdjacentWaypointVec;
typedef std::map<std::string, Airway*> AirwayDict;
AirwayDict _airways;
bool search2(FGPositionedRef aStart, FGPositionedRef aDest, WayptVec& aRoute);
/**
* Test if a positioned item is part of this airway network or not.
*/
bool inNetwork(const FGPositioned* aRef) const;
/**
* Find the closest node on the network, to the specified waypoint
*
* May return NULL,false if no match could be found; the search is
* internally limited to avoid very poor performance; for example,
* in the middle of an ocean.
*
* The second return value indicates if the returned value is
* equal (true) or distinct (false) to the input waypoint.
* Equality here means being physically within a close tolerance,
* on the order of a hundred metres.
*/
std::pair<FGPositionedRef, bool> findClosestNode(WayptRef aRef);
/**
* Overloaded version working with a raw SGGeod
*/
std::pair<FGPositionedRef, bool> findClosestNode(const SGGeod& aGeod);
};
static Network* highLevel();
static Network* lowLevel();
private:
Airway(const std::string& aIdent, double aTop, double aBottom);
friend class Network;
static Network* static_highLevel;
static Network* static_lowLevel;
std::string _ident;
double _topAltitudeFt;
double _bottomAltitudeFt;
// high-level vs low-level flag
// ... ?
WayptVec _elements;
};
} // of namespace flightgear
#endif //of FG_AIRWAYS_HXX

27
src/Navaids/positioned.cxx
View file

@ -518,7 +518,9 @@ FGPositioned::Filter::passType(Type aTy) const
static FGPositioned::List
findAll(const NamedPositionedIndex& aIndex,
const std::string& aName, FGPositioned::Filter* aFilter)
const std::string& aName,
FGPositioned::Filter* aFilter,
bool aExact)
{
FGPositioned::List result;
if (aName.empty()) {
@ -526,9 +528,16 @@ findAll(const NamedPositionedIndex& aIndex,
}
std::string name = boost::to_upper_copy(aName);
std::string upperBoundId = name;
upperBoundId[upperBoundId.size()-1]++;
NamedPositionedIndex::const_iterator upperBound = aIndex.lower_bound(upperBoundId);
NamedPositionedIndex::const_iterator upperBound;
if (aExact) {
upperBound = aIndex.upper_bound(name);
} else {
std::string upperBoundId = name;
upperBoundId[upperBoundId.size()-1]++;
upperBound = aIndex.lower_bound(upperBoundId);
}
NamedPositionedIndex::const_iterator it = aIndex.lower_bound(name);
for (; it != upperBound; ++it) {
@ -663,7 +672,7 @@ const char* FGPositioned::nameForType(Type aTy)
FGPositionedRef
FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
{
FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter));
FGPositioned::List r(findAll(global_identIndex, aIdent, aFilter, true));
if (r.empty()) {
return FGPositionedRef();
}
@ -682,15 +691,15 @@ FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilt
}
FGPositioned::List
FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter)
FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
{
return findAll(global_identIndex, aIdent, aFilter);
return findAll(global_identIndex, aIdent, aFilter, aExact);
}
FGPositioned::List
FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter)
FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
{
return findAll(global_nameIndex, aName, aFilter);
return findAll(global_nameIndex, aName, aFilter, aExact);
}
FGPositionedRef

6
src/Navaids/positioned.hxx
View file

@ -43,6 +43,7 @@ public:
TAXIWAY,
PAVEMENT,
PARK_STAND,
WAYPOINT,
FIX,
VOR,
NDB,
@ -55,7 +56,6 @@ public:
DME,
TACAN,
OBSTACLE,
WAYPOINT, // user-defined waypoint
FREQ_GND,
FREQ_TWR,
FREQ_ATIS,
@ -160,12 +160,12 @@ public:
* Find all items with the specified ident
* @param aFilter - optional filter on items
*/
static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL);
static List findAllWithIdent(const std::string& aIdent, Filter* aFilter = NULL, bool aExact = true);
/**
* As above, but searches names instead of idents
*/
static List findAllWithName(const std::string& aName, Filter* aFilter = NULL);
static List findAllWithName(const std::string& aName, Filter* aFilter = NULL, bool aExact = true);
/**
* Sort an FGPositionedList by distance from a position

336
src/Navaids/procedure.cxx Normal file
View file

@ -0,0 +1,336 @@
// procedure.cxx - define route storing an approach, arrival or departure procedure
// 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 "procedure.hxx"
#include <cassert>
#include <algorithm> // for reverse_copy
#include <simgear/structure/exception.hxx>
#include <Navaids/waypoint.hxx>
using std::string;
namespace flightgear
{
Procedure::Procedure(const string& aIdent) :
_ident(aIdent)
{
}
Approach::Approach(const string& aIdent) :
Procedure(aIdent)
{
}
void Approach::setRunway(FGRunwayRef aRwy)
{
_runway = aRwy;
}
void Approach::setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed)
{
_primary = aPrimary;
_primary[0]->setFlag(WPT_IAF, true);
_primary[_primary.size()-1]->setFlag(WPT_FAF, true);
_missed = aMissed;
if (!_missed.empty()) {
// mark the first point as the published missed-approach point
_missed[0]->setFlag(WPT_MAP, true);
// mark all the points as being on the missed approach route
for (unsigned int i=0; i<_missed.size(); ++i) {
_missed[i]->setFlag(WPT_MISS, true);
}
}
}
void Approach::addTransition(Transition* aTrans)
{
WayptRef entry = aTrans->enroute();
_transitions[entry] = aTrans;
}
bool Approach::route(WayptRef aIAF, WayptVec& aWps)
{
WptTransitionMap::iterator it;
bool haveTrans = false;
for (it = _transitions.begin(); it != _transitions.end(); ++it) {
Transition* t= it->second;
if (t->route(aIAF, aWps)) {
haveTrans = true;
break;
}
} // of transitions iteration
if (!haveTrans) {
SG_LOG(SG_GENERAL, SG_INFO, "approach " << ident() << " has no transition " <<
"for IAF: " << aIAF->ident());
return false;
}
aWps.insert(aWps.end(), _primary.begin(), _primary.end());
aWps.push_back(new RunwayWaypt(_runway, NULL));
aWps.insert(aWps.end(), _missed.begin(), _missed.end());
return true;
}
bool Approach::routeFromVectors(WayptVec& aWps)
{
aWps.insert(aWps.end(), _primary.begin(), _primary.end());
aWps.push_back(new RunwayWaypt(_runway, NULL));
aWps.insert(aWps.end(), _missed.begin(), _missed.end());
return true;
}
//////////////////////////////////////////////////////////////////////////////
ArrivalDeparture::ArrivalDeparture(const string& aIdent) :
Procedure(aIdent)
{
}
void ArrivalDeparture::addRunway(FGRunwayRef aWay)
{
_runways[aWay] = NULL;
}
bool ArrivalDeparture::isForRunway(FGRunwayRef aWay) const
{
// null runway always passes
if (!aWay) {
return true;
}
return (_runways.count(aWay));
}
void ArrivalDeparture::addTransition(Transition* aTrans)
{
WayptRef entry = aTrans->enroute();
_enrouteTransitions[entry] = aTrans;
}
void ArrivalDeparture::addRunwayTransition(FGRunwayRef aWay, Transition* aTrans)
{
assert(aWay->ident() == aTrans->ident());
if (!isForRunway(aWay)) {
throw sg_io_exception("adding transition for unspecified runway:" + aWay->ident(), ident());
}
_runways[aWay] = aTrans;
}
void ArrivalDeparture::setCommon(const WayptVec& aWps)
{
_common = aWps;
}
bool ArrivalDeparture::commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy)
{
// assume we're routing from enroute, to the runway.
// for departures, we'll flip the result points
Transition* t = findTransitionByEnroute(aEnroute);
WayptVec::iterator firstCommon = _common.begin();
if (t) {
t->route(aEnroute, aPath);
Waypt* transEnd = t->procedureEnd();
for (; firstCommon != _common.end(); ++firstCommon) {
if ((*firstCommon)->matches(transEnd)) {
// found transition->common point, stop search
break;
}
} // of common points
// if we hit this point, the transition doesn't end (start, for a SID) on
// a common point. We assume this means we should just append the entire
// common section after the transition.
firstCommon = _common.begin();
} else {
if (aEnroute && !(*firstCommon)->matches(aEnroute)) {
return false;
}
} // of not using a transition
// append (some) common points
aPath.insert(aPath.end(), firstCommon, _common.end());
if (!aRwy) {
// no runway specified, we're done
return true;
}
RunwayTransitionMap::iterator r = _runways.find(aRwy);
assert(r != _runways.end());
if (!r->second) {
// no transitions specified. Not great, but not
// much we can do about it. Calling code will insert VECTORS to the approach
// if required, or maybe there's an approach transition defined.
return true;
}
SG_LOG(SG_GENERAL, SG_INFO, ident() << " using runway transition for " << r->first->ident());
r->second->route(NULL, aPath);
return true;
}
Transition* ArrivalDeparture::findTransitionByEnroute(Waypt* aEnroute) const
{
if (!aEnroute) {
return NULL;
}
WptTransitionMap::const_iterator eit;
for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
if (eit->second->enroute()->matches(aEnroute)) {
SG_LOG(SG_GENERAL, SG_INFO, ident() << " using enroute transition " << eit->second->ident());
return eit->second;
}
} // of enroute transition iteration
return NULL;
}
WayptRef ArrivalDeparture::findBestTransition(const SGGeod& aPos) const
{
// no transitions, that's easy
if (_enrouteTransitions.empty()) {
SG_LOG(SG_GENERAL, SG_INFO, "no enroute transitions for " << ident());
return _common.front();
}
double d = 1e9;
WayptRef w;
WptTransitionMap::const_iterator eit;
for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
WayptRef c = eit->second->enroute();
SG_LOG(SG_GENERAL, SG_INFO, "findBestTransition for " << ident() << ", looking at " << c->ident());
// assert(c->hasFixedPosition());
double cd = SGGeodesy::distanceM(aPos, c->position());
if (cd < d) { // distance to 'c' is less, new best match
d = cd;
w = c;
}
} // of transitions iteration
assert(w);
return w;
}
WayptRef ArrivalDeparture::findTransitionByName(const string& aIdent) const
{
WptTransitionMap::const_iterator eit;
for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
WayptRef c = eit->second->enroute();
if (c->ident() == aIdent) {
return c;
}
}
return NULL;
}
////////////////////////////////////////////////////////////////////////////
SID::SID(const string& aIdent) :
ArrivalDeparture(aIdent)
{
}
bool SID::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
{
if (!isForRunway(aWay)) {
SG_LOG(SG_GENERAL, SG_WARN, "SID " << ident() << " not for runway " << aWay->ident());
return false;
}
WayptVec path;
if (!commonRoute(aEnroute, path, aWay)) {
return false;
}
// SID waypoints (including transitions) are stored reversed, so we can
// re-use the routing code. This is where we fix the ordering for client code
std::back_insert_iterator<WayptVec> bi(aPath);
std::reverse_copy(path.begin(), path.end(), bi);
return true;
}
////////////////////////////////////////////////////////////////////////////
STAR::STAR(const string& aIdent) :
ArrivalDeparture(aIdent)
{
}
bool STAR::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
{
if (aWay && !isForRunway(aWay)) {
return false;
}
return commonRoute(aEnroute, aPath, aWay);
}
/////////////////////////////////////////////////////////////////////////////
Transition::Transition(const std::string& aIdent, Procedure* aPr,
const WayptVec& aWps) :
_ident(aIdent),
_parent(aPr),
_primary(aWps)
{
assert(aPr);
assert(!_primary.empty());
_primary[0]->setFlag(WPT_TRANSITION, true);
}
WayptRef Transition::enroute() const
{
assert(!_primary.empty());
return _primary[0];
}
WayptRef Transition::procedureEnd() const
{
assert(!_primary.empty());
return _primary[_primary.size() - 1];
}
bool Transition::route(Waypt* aEnroute, WayptVec& aPath)
{
if (aEnroute && !enroute()->matches(aEnroute)) {
return false;
}
aPath.insert(aPath.end(), _primary.begin(), _primary.end());
return true;
}
} // of namespace

212
src/Navaids/procedure.hxx Normal file
View file

@ -0,0 +1,212 @@
/// procedure.hxx - define route storing an approach, arrival or departure procedure
// 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.
#ifndef FG_NAVAID_PROCEDURE_HXX
#define FG_NAVAID_PROCEDURE_HXX
#include <set>
#include <Navaids/route.hxx>
#include <Airports/runways.hxx>
typedef SGSharedPtr<FGRunway> FGRunwayRef;
namespace flightgear {
// forward decls
class NavdataVisitor;
class Procedure : public Route
{
public:
virtual std::string ident() const
{ return _ident; }
protected:
Procedure(const std::string& aIdent);
std::string _ident;
};
/**
* Encapsulate a transition segment
*/
class Transition : public Route
{
public:
bool route(Waypt* aEnroute, WayptVec& aPath);
Procedure* parent() const
{ return _parent; }
/**
* Return the enroute end of the transition
*/
WayptRef enroute() const;
/**
* Return the procedure end of the transition
*/
WayptRef procedureEnd() const;
virtual std::string ident() const
{ return _ident; }
private:
friend class NavdataVisitor;
Transition(const std::string& aIdent, Procedure* aPr, const WayptVec& aWps);
std::string _ident;
Procedure* _parent;
WayptVec _primary;
};
/**
* Describe an approach procedure, including the missed approach
* segment
*/
class Approach : public Procedure
{
public:
FGRunwayRef runway()
{ return _runway; }
/**
* Build a route from a valid IAF to the runway, including the missed
* segment. Return false if no valid transition from the specified IAF
* could be found
*/
bool route(WayptRef aIAF, WayptVec& aWps);
/**
* Build route as above, but ignore transitions, and assume radar
* vectoring to the start of main approach
*/
bool routeFromVectors(WayptVec& aWps);
const WayptVec& primary() const
{ return _primary; }
const WayptVec& missed() const
{ return _missed; }
private:
friend class NavdataVisitor;
Approach(const std::string& aIdent);
void setRunway(FGRunwayRef aRwy);
void setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed);
void addTransition(Transition* aTrans);
FGRunwayRef _runway;
typedef std::map<WayptRef, Transition*> WptTransitionMap;
WptTransitionMap _transitions;
WayptVec _primary; // unify these?
WayptVec _missed;
};
class ArrivalDeparture : public Procedure
{
public:
/**
* Predicate, test if this procedure applies to the requested runway
*/
virtual bool isForRunway(FGRunwayRef aWay) const;
/**
* Find a path between the runway and enroute structure. Waypoints
* corresponding to the appropriate transitions and segments will be created.
*/
virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath) = 0;
const WayptVec& common() const
{ return _common; }
/**
* Given an enroute location, find the best enroute transition point for
* this arrival/departure. Best is currently determined as 'closest to the
* enroute location'.
*/
WayptRef findBestTransition(const SGGeod& aPos) const;
/**
* Find an enroute transition waypoint by identifier. This is necessary
* for the route-manager and similar code that that needs to talk about
* transitions in a human-meaningful way (including persistence).
*/
WayptRef findTransitionByName(const std::string& aIdent) const;
Transition* findTransitionByEnroute(Waypt* aEnroute) const;
protected:
bool commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy);
ArrivalDeparture(const std::string& aIdent);
void addRunway(FGRunwayRef aRwy);
typedef std::map<FGRunwayRef, Transition*> RunwayTransitionMap;
RunwayTransitionMap _runways;
private:
friend class NavdataVisitor;
void addTransition(Transition* aTrans);
void setCommon(const WayptVec& aWps);
void addRunwayTransition(FGRunwayRef aRwy, Transition* aTrans);
WayptVec _common;
typedef std::map<WayptRef, Transition*> WptTransitionMap;
WptTransitionMap _enrouteTransitions;
};
class SID : public ArrivalDeparture
{
public:
virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath);
private:
friend class NavdataVisitor;
SID(const std::string& aIdent);
};
class STAR : public ArrivalDeparture
{
public:
virtual bool route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath);
private:
friend class NavdataVisitor;
STAR(const std::string& aIdent);
};
} // of namespace
#endif

679
src/Navaids/route.cxx Normal file
View file

@ -0,0 +1,679 @@
// 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>
// FlightGear
#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 {
Waypt::Waypt(Route* aOwner) :
_altitudeFt(0.0),
_speedKts(0.0),
_altRestrict(RESTRICT_NONE),
_speedRestrict(RESTRICT_NONE),
_owner(aOwner),
_flags(0)
{
}
std::string Waypt::ident() const
{
return "";
}
bool Waypt::flag(WayptFlag aFlag) const
{
return (_flags & aFlag);
}
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)
{
_speedKts = aSpeed;
_speedRestrict = aRestrict;
}
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);
}
///////////////////////////////////////////////////////////////////////////
// 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.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";
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"));
_speedKts = aProp->getDoubleValue("speed-kts");
}
}
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-kts", _speedKts);
}
}
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

208
src/Navaids/route.hxx Normal file
View file

@ -0,0 +1,208 @@
/**
* route.hxx - defines basic route and route-element classes. Route elements
* are specialised into waypoints and related things. Routes are any class tha
* owns a collection (list, tree, graph) of route elements - such as airways,
* procedures or a flight plan.
*/
// 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.
#ifndef FG_ROUTE_HXX
#define FG_ROUTE_HXX
// std
#include <vector>
#include <map>
#include <iosfwd>
// Simgear
#include <simgear/math/SGMath.hxx>
#include <simgear/structure/SGReferenced.hxx>
#include <simgear/structure/SGSharedPtr.hxx>
#include <simgear/props/props.hxx>
// forward decls
class FGPositioned;
class SGPath;
class FGAirport;
namespace flightgear
{
// forward decls
class Route;
class Waypt;
class NavdataVisitor;
typedef SGSharedPtr<Waypt> WayptRef;
typedef enum {
WPT_MAP = 1 << 0, ///< missed approach point
WPT_IAF = 1 << 1, ///< initial approach fix
WPT_FAF = 1 << 2, ///< final approach fix
WPT_OVERFLIGHT = 1 << 3, ///< must overfly the point directly
WPT_TRANSITION = 1 << 4, ///< transition to/from enroute structure
WPT_MISS = 1 << 5, ///< segment is part of missed approach
/// waypoint position is dynamic, i.e moves based on other criteria,
/// such as altitude, inbound course, or so on.
WPT_DYNAMIC = 1 << 6,
/// waypoint was created automatically (not manually entered/loaded)
/// for example waypoints from airway routing or a procedure
WPT_GENERATED = 1 << 7,
WPT_DEPARTURE = 1 << 8,
WPT_ARRIVAL = 1 << 9
} WayptFlag;
typedef enum {
RESTRICT_NONE,
RESTRICT_AT,
RESTRICT_ABOVE,
RESTRICT_BELOW
} RouteRestriction;
/**
* Abstract base class for waypoints (and things that are treated similarly
* by navigation systems)
*/
class Waypt : public SGReferenced
{
public:
Route* 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;
/**
* The Positioned associated with this element, if one exists
*/
virtual FGPositioned* source() const
{ return NULL; }
virtual double altitudeFt() const
{ return _altitudeFt; }
virtual double speedKts() const
{ return _speedKts; }
virtual RouteRestriction altitudeRestriction() const
{ return _altRestrict; }
virtual RouteRestriction speedRestriction() const
{ return _speedRestrict; }
void setAltitude(double aAlt, RouteRestriction aRestrict);
void setSpeed(double aSpeed, RouteRestriction aRestrict);
/**
* Identifier assoicated with the waypoint. Human-readable, but
* possibly quite terse, and definitiely not unique.
*/
virtual std::string ident() const;
/**
* Test if the specified flag is set for this element
*/
virtual bool flag(WayptFlag aFlag) const;
void setFlag(WayptFlag aFlag, bool aV = true);
/**
* Factory method
*/
static WayptRef createFromProperties(Route* aOwner, SGPropertyNode_ptr aProp);
void saveAsNode(SGPropertyNode* node) const;
/**
* Test if this element and another are 'the same', i.e matching
* ident and lat/lon are approximately equal
*/
bool matches(Waypt* aOther) const;
/**
* Test if this element and a position 'the same'
* this can be defined by either position, ident or both
*/
bool matches(const SGGeod& aPos) const;
virtual std::string type() const = 0;
protected:
friend class NavdataVisitor;
Waypt(Route* aOwner);
/**
* Persistence helper - read node properties from a file
*/
virtual void initFromProperties(SGPropertyNode_ptr aProp);
/**
* Persistence helper - save this element to a node
*/
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
typedef Waypt* (FactoryFunction)(Route* aOwner) ;
static void registerFactory(const std::string aNodeType, FactoryFunction* aFactory);
double _altitudeFt;
double _speedKts;
RouteRestriction _altRestrict;
RouteRestriction _speedRestrict;
private:
/**
* Create an instance of a concrete subclass, or throw an exception
*/
static Waypt* createInstance(Route* aOwner, const std::string& aTypeName);
Route* _owner;
unsigned short _flags;
};
typedef std::vector<WayptRef> WayptVec;
class Route
{
public:
/**
*
*/
virtual std::string ident() const = 0;
static void loadAirportProcedures(const SGPath& aPath, FGAirport* aApt);
static void dumpRouteToFile(const WayptVec& aRoute, const std::string& aName);
static void dumpRouteToLineString(const std::string& aIdent,
const WayptVec& aRoute, std::ostream& aStream);
private:
};
} // of namespace flightgear
#endif // of FG_ROUTE_HXX

346
src/Navaids/routePath.cxx Normal file
View file

@ -0,0 +1,346 @@
#include <Navaids/routePath.hxx>
#include <simgear/structure/exception.hxx>
#include <simgear/magvar/magvar.hxx>
#include <simgear/timing/sg_time.hxx>
#include <Main/globals.hxx>
#include <Airports/runways.hxx>
#include <Navaids/waypoint.hxx>
#include <Navaids/positioned.hxx>
namespace flightgear {
bool geocRadialIntersection(const SGGeoc& a, double r1, const SGGeoc& b, double r2, SGGeoc& result);
}
using namespace flightgear;
// implement Point(s) known distance from a great circle
static double sqr(const double x)
{
return x * x;
}
double pointsKnownDistanceFromGC(const SGGeoc& a, const SGGeoc&b, const SGGeoc& d, double dist)
{
double A = SGGeodesy::courseRad(a, d) - SGGeodesy::courseRad(a, b);
double bDist = SGGeodesy::distanceRad(a, d);
// r=(cos(b)^2+sin(b)^2*cos(A)^2)^(1/2)
double r = pow(sqr(cos(bDist)) + sqr(sin(bDist)) * sqr(cos(A)), 0.5);
double p = atan2(sin(bDist)*cos(A), cos(bDist));
if (sqr(cos(dist)) > sqr(r)) {
SG_LOG(SG_GENERAL, SG_INFO, "pointsKnownDistanceFromGC, no points exist");
return -1.0;
}
double dp1 = p + acos(cos(dist)/r);
double dp2 = p - acos(cos(dist)/r);
double dp1Nm = fabs(dp1 * SG_RAD_TO_NM);
double dp2Nm = fabs(dp2 * SG_RAD_TO_NM);
return SGMiscd::min(dp1Nm, dp2Nm);
}
RoutePath::RoutePath(const flightgear::WayptVec& wpts) :
_waypts(wpts)
{
_pathClimbFPM = 1200;
_pathDescentFPM = 800;
_pathIAS = 190;
_pathTurnRate = 3.0; // 3 deg/sec = 180def/min = standard rate turn
}
SGGeodVec RoutePath::pathForIndex(int index) const
{
if (index == 0) {
return SGGeodVec(); // no path for first waypoint
}
if (_waypts[index]->type() == "vectors") {
return SGGeodVec(); // empty
}
if (_waypts[index]->type() == "hold") {
return pathForHold((Hold*) _waypts[index].get());
}
SGGeodVec r;
SGGeod pos;
if (!computedPositionForIndex(index-1, pos)) {
return SGGeodVec();
}
r.push_back(pos);
if (!computedPositionForIndex(index, pos)) {
return SGGeodVec();
}
r.push_back(pos);
if (_waypts[index]->type() == "runway") {
// runways get an extra point, at the end. this is particularly
// important so missed approach segments draw correctly
FGRunway* rwy = static_cast<RunwayWaypt*>(_waypts[index].get())->runway();
r.push_back(rwy->end());
}
return r;
}
SGGeod RoutePath::positionForIndex(int index) const
{
SGGeod r;
bool ok = computedPositionForIndex(index, r);
if (!ok) {
return SGGeod();
}
return r;
}
SGGeodVec RoutePath::pathForHold(Hold* hold) const
{
int turnSteps = 16;
double hdg = hold->inboundRadial();
double turnDelta = 180.0 / turnSteps;
SGGeodVec r;
double az2;
double stepTime = turnDelta / _pathTurnRate; // in seconds
double stepDist = _pathIAS * (stepTime / 3600.0) * SG_NM_TO_METER;
double legDist = hold->isDistance() ?
hold->timeOrDistance()
: _pathIAS * (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
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;
}
/**
* 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())) {
throw sg_range_exception("waypt index out of range",
"RoutePath::computedPositionForIndex");
}
WayptRef w = _waypts[index];
if (!w->flag(WPT_DYNAMIC)) {
r = w->position();
return true;
}
if (w->type() == "radialIntercept") {
// radial intersection along track
SGGeod prev;
if (!computedPositionForIndex(index - 1, prev)) {
return false;
}
SGGeoc prevGc = SGGeoc::fromGeod(prev);
SGGeoc navid = SGGeoc::fromGeod(w->position());
SGGeoc rGc;
double magVar = magVarFor(prev);
RadialIntercept* i = (RadialIntercept*) w.get();
double radial = i->radialDegMagnetic() + magVar;
double track = i->courseDegMagnetic() + magVar;
bool ok = geocRadialIntersection(prevGc, track, navid, radial, rGc);
if (!ok) {
return false;
}
r = SGGeod::fromGeoc(rGc);
return true;
} else if (w->type() == "dmeIntercept") {
// find the point along the DME track, from prev, that is the correct distance
// from the DME
SGGeod prev;
if (!computedPositionForIndex(index - 1, prev)) {
return false;
}
DMEIntercept* di = (DMEIntercept*) w.get();
SGGeoc prevGc = SGGeoc::fromGeod(prev);
SGGeoc navid = SGGeoc::fromGeod(w->position());
double distRad = di->dmeDistanceNm() * SG_NM_TO_RAD;
SGGeoc rGc;
SGGeoc bPt;
double crs = di->courseDegMagnetic() + magVarFor(prev);
SGGeodesy::advanceRadM(prevGc, crs, 100 * SG_NM_TO_RAD, bPt);
double dNm = pointsKnownDistanceFromGC(prevGc, bPt, navid, distRad);
if (dNm < 0.0) {
return false;
}
double az2;
SGGeodesy::direct(prev, crs, dNm * SG_NM_TO_METER, r, az2);
return true;
} else if (w->type() == "hdgToAlt") {
HeadingToAltitude* h = (HeadingToAltitude*) w.get();
double climb = h->altitudeFt() - computeAltitudeForIndex(index - 1);
double d = distanceForClimb(climb);
SGGeod prevPos;
if (!computedPositionForIndex(index - 1, prevPos)) {
return false;
}
double hdg = h->headingDegMagnetic() + magVarFor(prevPos);
double az2;
SGGeodesy::direct(prevPos, hdg, d * SG_NM_TO_METER, r, az2);
return true;
} else if (w->type() == "vectors"){
return false;
} else if (w->type() == "hold") {
r = w->position();
return true;
}
SG_LOG(SG_GENERAL, SG_INFO, "RoutePath::computedPositionForIndex: unhandled type:" << w->type());
return false;
}
double RoutePath::computeAltitudeForIndex(int index) const
{
if ((index < 0) || (index >= (int) _waypts.size())) {
throw sg_range_exception("waypt index out of range",
"RoutePath::computeAltitudeForIndex");
}
WayptRef w = _waypts[index];
if (w->altitudeRestriction() != RESTRICT_NONE) {
return w->altitudeFt(); // easy!
}
if (w->type() == "runway") {
FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
return rwy->threshold().getElevationFt();
} else if ((w->type() == "hold") || (w->type() == "vectors")) {
// pretend we don't change altitude in holds/vectoring
return computeAltitudeForIndex(index - 1);
}
double prevAlt = computeAltitudeForIndex(index - 1);
// find distance to previous, and hence climb/descent
SGGeod pos, prevPos;
if (!computedPositionForIndex(index, pos) ||
!computedPositionForIndex(index - 1, prevPos))
{
SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
throw sg_range_exception("unable to compute position for waypoints");
}
double d = SGGeodesy::distanceNm(prevPos, pos);
double tMinutes = (d / _pathIAS) * 60.0; // (nm / knots) * 60 = time in minutes
double deltaFt; // change in altitude in feet
if (w->flag(WPT_ARRIVAL) && !w->flag(WPT_MISS)) {
deltaFt = -_pathDescentFPM * tMinutes;
} else {
deltaFt = _pathClimbFPM * tMinutes;
}
return prevAlt + deltaFt;
}
double RoutePath::computeTrackForIndex(int index) const
{
if ((index < 0) || (index >= (int) _waypts.size())) {
throw sg_range_exception("waypt index out of range",
"RoutePath::computeTrackForIndex");
}
WayptRef w = _waypts[index];
if (w->type() == "radialIntercept") {
RadialIntercept* r = (RadialIntercept*) w.get();
return r->courseDegMagnetic();
} else if (w->type() == "dmeIntercept") {
DMEIntercept* d = (DMEIntercept*) w.get();
return d->courseDegMagnetic();
} else if (w->type() == "hdgToAlt") {
HeadingToAltitude* h = (HeadingToAltitude*) w.get();
return h->headingDegMagnetic();
} else if (w->type() == "hold") {
Hold* h = (Hold*) w.get();
return h->inboundRadial();
} else if (w->type() == "vectors") {
SG_LOG(SG_GENERAL, SG_WARN, "asked for track from VECTORS");
throw sg_range_exception("asked for track from vectors waypt");
} else if (w->type() == "runway") {
FGRunway* rwy = static_cast<RunwayWaypt*>(w.get())->runway();
return rwy->headingDeg();
}
// course based upon previous and current pos
SGGeod pos, prevPos;
if (!computedPositionForIndex(index, pos) ||
!computedPositionForIndex(index - 1, prevPos))
{
SG_LOG(SG_GENERAL, SG_WARN, "unable to compute position for waypoints");
throw sg_range_exception("unable to compute position for waypoints");
}
return SGGeodesy::courseDeg(prevPos, pos);
}
double RoutePath::distanceForClimb(double climbFt) const
{
double t = 0.0; // in seconds
if (climbFt > 0.0) {
t = (climbFt / _pathClimbFPM) * 60.0;
} else if (climbFt < 0.0) {
t = (climbFt / _pathDescentFPM) * 60.0;
}
return _pathIAS * (t / 3600.0);
}
double RoutePath::magVarFor(const SGGeod& geod) const
{
double jd = globals->get_time_params()->getJD();
return sgGetMagVar(geod, jd);
}

69
src/Navaids/routePath.hxx Normal file
View file

@ -0,0 +1,69 @@
/**
* routePath.hxx - convert a route to straight line segments, for graphical
* output or display.
*/
// Written by James Turner, started 2010.
//
// Copyright (C) 2010 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.
#ifndef FG_ROUTE_PATH_HXX
#define FG_ROUTE_PATH_HXX
#include <Navaids/route.hxx>
namespace flightgear
{
class Hold;
}
typedef std::vector<SGGeod> SGGeodVec;
class RoutePath
{
public:
RoutePath(const flightgear::WayptVec& wpts);
SGGeodVec pathForIndex(int index) const;
SGGeod positionForIndex(int index) const;
private:
class PathCtx;
SGGeodVec pathForHold(flightgear::Hold* hold) const;
bool computedPositionForIndex(int index, SGGeod& pos) const;
double computeAltitudeForIndex(int index) const;
double computeTrackForIndex(int index) const;
/**
* Find the distance (in Nm) to climb/descend a height in feet
*/
double distanceForClimb(double climbFt) const;
double magVarFor(const SGGeod& gd) const;
flightgear::WayptVec _waypts;
int _pathClimbFPM; ///< climb-rate to use for pathing
int _pathDescentFPM; ///< descent rate to use (feet-per-minute)
int _pathIAS; ///< IAS (knots) to use for pathing
double _pathTurnRate; ///< degrees-per-second, defaults to 3, i.e 180 in a minute
};
#endif

18
src/Navaids/testnavs.cxx
View file

@ -1,3 +1,21 @@
// 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 <simgear/misc/sg_path.hxx>
#include "fixlist.hxx"

471
src/Navaids/waypoint.cxx Normal file
View file

@ -0,0 +1,471 @@
// waypoint.cxx - waypoints that can occur in routes/procedures
// 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 "waypoint.hxx"
#include <simgear/structure/exception.hxx>
#include <simgear/route/waypoint.hxx>
#include <Airports/simple.hxx>
#include <Airports/runways.hxx>
using std::string;
namespace flightgear
{
BasicWaypt::BasicWaypt(const SGGeod& aPos, const string& aIdent, Route* aOwner) :
Waypt(aOwner),
_pos(aPos),
_ident(aIdent)
{
}
BasicWaypt::BasicWaypt(const SGWayPoint& aWP, Route* aOwner) :
Waypt(aOwner),
_pos(aWP.get_target()),
_ident(aWP.get_id())
{
}
BasicWaypt::BasicWaypt(Route* aOwner) :
Waypt(aOwner)
{
}
void BasicWaypt::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
throw sg_io_exception("missing lon/lat properties",
"BasicWaypt::initFromProperties");
}
Waypt::initFromProperties(aProp);
_pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"),
aProp->getDoubleValue("lat"));
_ident = aProp->getStringValue("ident");
}
void BasicWaypt::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _ident);
aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
}
//////////////////////////////////////////////////////////////////////////////
NavaidWaypoint::NavaidWaypoint(FGPositioned* aPos, Route* aOwner) :
Waypt(aOwner),
_navaid(aPos)
{
if (aPos->type() == FGPositioned::RUNWAY) {
SG_LOG(SG_GENERAL, SG_WARN, "sure you don't want to be building a runway waypt here?");
}
}
NavaidWaypoint::NavaidWaypoint(Route* aOwner) :
Waypt(aOwner)
{
}
SGGeod NavaidWaypoint::position() const
{
return SGGeod::fromGeodFt(_navaid->geod(), _altitudeFt);
}
std::string NavaidWaypoint::ident() const
{
return _navaid->ident();
}
void NavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("ident")) {
throw sg_io_exception("missing navaid value",
"NavaidWaypoint::initFromProperties");
}
Waypt::initFromProperties(aProp);
std::string idn(aProp->getStringValue("ident"));
SGGeod p;
if (aProp->hasChild("lon")) {
p = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
}
// FIXME - resolve co-located DME, etc
// is it sufficent just to ignore DMEs, actually?
FGPositionedRef nav = FGPositioned::findClosestWithIdent(idn, p, NULL);
if (!nav) {
throw sg_io_exception("unknown navaid ident:" + idn,
"NavaidWaypoint::initFromProperties");
}
_navaid = nav;
}
void NavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _navaid->ident());
// write lon/lat to disambiguate
aProp->setDoubleValue("lon", _navaid->geod().getLongitudeDeg());
aProp->setDoubleValue("lat", _navaid->geod().getLatitudeDeg());
}
OffsetNavaidWaypoint::OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner,
double aRadial, double aDistNm) :
NavaidWaypoint(aPos, aOwner),
_radial(aRadial),
_distanceNm(aDistNm)
{
init();
}
OffsetNavaidWaypoint::OffsetNavaidWaypoint(Route* aOwner) :
NavaidWaypoint(aOwner)
{
}
void OffsetNavaidWaypoint::init()
{
SGGeod offset;
double az2;
SGGeodesy::direct(_navaid->geod(), _radial, _distanceNm * SG_NM_TO_METER, offset, az2);
_geod = SGGeod::fromGeodFt(offset, _altitudeFt);
}
void OffsetNavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("radial-deg") || !aProp->hasChild("distance-nm")) {
throw sg_io_exception("missing radial/offset distance",
"OffsetNavaidWaypoint::initFromProperties");
}
NavaidWaypoint::initFromProperties(aProp);
_radial = aProp->getDoubleValue("radial-deg");
_distanceNm = aProp->getDoubleValue("distance-nm");
init();
}
void OffsetNavaidWaypoint::writeToProperties(SGPropertyNode_ptr aProp) const
{
NavaidWaypoint::writeToProperties(aProp);
aProp->setDoubleValue("radial-deg", _radial);
aProp->setDoubleValue("distance-nm", _distanceNm);
}
/////////////////////////////////////////////////////////////////////////////
RunwayWaypt::RunwayWaypt(FGRunway* aPos, Route* aOwner) :
Waypt(aOwner),
_runway(aPos)
{
}
RunwayWaypt::RunwayWaypt(Route* aOwner) :
Waypt(aOwner)
{
}
SGGeod RunwayWaypt::position() const
{
return _runway->threshold();
}
std::string RunwayWaypt::ident() const
{
return _runway->airport()->ident() + "-" + _runway->ident();
}
FGPositioned* RunwayWaypt::source() const
{
return _runway;
}
void RunwayWaypt::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("icao") || !aProp->hasChild("ident")) {
throw sg_io_exception("missing values: icao or ident",
"RunwayWaypoint::initFromProperties");
}
Waypt::initFromProperties(aProp);
std::string idn(aProp->getStringValue("ident"));
const FGAirport* apt = FGAirport::getByIdent(aProp->getStringValue("icao"));
_runway = apt->getRunwayByIdent(aProp->getStringValue("ident"));
}
void RunwayWaypt::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _runway->ident());
aProp->setStringValue("icao", _runway->airport()->ident());
}
/////////////////////////////////////////////////////////////////////////////
Hold::Hold(const SGGeod& aPos, const string& aIdent, Route* aOwner) :
BasicWaypt(aPos, aIdent, aOwner),
_righthanded(true),
_isDistance(false)
{
setFlag(WPT_DYNAMIC);
}
Hold::Hold(Route* aOwner) :
BasicWaypt(aOwner),
_righthanded(true),
_isDistance(false)
{
}
void Hold::setHoldRadial(double aInboundRadial)
{
_bearing = aInboundRadial;
}
void Hold::setHoldDistance(double aDistanceNm)
{
_isDistance = true;
_holdTD = aDistanceNm;
}
void Hold::setHoldTime(double aTimeSec)
{
_isDistance = false;
_holdTD = aTimeSec;
}
void Hold::setRightHanded()
{
_righthanded = true;
}
void Hold::setLeftHanded()
{
_righthanded = false;
}
void Hold::initFromProperties(SGPropertyNode_ptr aProp)
{
BasicWaypt::initFromProperties(aProp);
if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
throw sg_io_exception("missing lon/lat properties",
"Hold::initFromProperties");
}
_righthanded = aProp->getBoolValue("right-handed");
_isDistance = aProp->getBoolValue("is-distance");
_bearing = aProp->getDoubleValue("inbound-radial-deg");
_holdTD = aProp->getDoubleValue("td");
}
void Hold::writeToProperties(SGPropertyNode_ptr aProp) const
{
BasicWaypt::writeToProperties(aProp);
aProp->setBoolValue("right-handed", _righthanded);
aProp->setBoolValue("is-distance", _isDistance);
aProp->setDoubleValue("inbound-radial-deg", _bearing);
aProp->setDoubleValue("td", _holdTD);
}
/////////////////////////////////////////////////////////////////////////////
HeadingToAltitude::HeadingToAltitude(Route* aOwner, const string& aIdent,
double aMagHdg) :
Waypt(aOwner),
_ident(aIdent),
_magHeading(aMagHdg)
{
setFlag(WPT_DYNAMIC);
}
HeadingToAltitude::HeadingToAltitude(Route* aOwner) :
Waypt(aOwner)
{
}
void HeadingToAltitude::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("heading-deg")) {
throw sg_io_exception("missing heading/alt properties",
"HeadingToAltitude::initFromProperties");
}
Waypt::initFromProperties(aProp);
_magHeading = aProp->getDoubleValue("heading-deg");
_ident = aProp->getStringValue("ident");
}
void HeadingToAltitude::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _ident);
aProp->setDoubleValue("heading-deg", _magHeading);
}
/////////////////////////////////////////////////////////////////////////////
DMEIntercept::DMEIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos,
double aCourseDeg, double aDistanceNm) :
Waypt(aOwner),
_ident(aIdent),
_pos(aPos),
_magCourse(aCourseDeg),
_dmeDistanceNm(aDistanceNm)
{
setFlag(WPT_DYNAMIC);
}
DMEIntercept::DMEIntercept(Route* aOwner) :
Waypt(aOwner)
{
}
void DMEIntercept::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
throw sg_io_exception("missing lon/lat properties",
"DMEIntercept::initFromProperties");
}
Waypt::initFromProperties(aProp);
_pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
_ident = aProp->getStringValue("ident");
// check it's a real DME?
_magCourse = aProp->getDoubleValue("course-deg");
_dmeDistanceNm = aProp->getDoubleValue("dme-distance-nm");
}
void DMEIntercept::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _ident);
aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
aProp->setDoubleValue("course-deg", _magCourse);
aProp->setDoubleValue("dme-distance-nm", _dmeDistanceNm);
}
/////////////////////////////////////////////////////////////////////////////
RadialIntercept::RadialIntercept(Route* aOwner, const string& aIdent, const SGGeod& aPos,
double aCourseDeg, double aRadial) :
Waypt(aOwner),
_ident(aIdent),
_pos(aPos),
_magCourse(aCourseDeg),
_radial(aRadial)
{
setFlag(WPT_DYNAMIC);
}
RadialIntercept::RadialIntercept(Route* aOwner) :
Waypt(aOwner)
{
}
void RadialIntercept::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("lon") || !aProp->hasChild("lat")) {
throw sg_io_exception("missing lon/lat properties",
"RadialIntercept::initFromProperties");
}
Waypt::initFromProperties(aProp);
_pos = SGGeod::fromDeg(aProp->getDoubleValue("lon"), aProp->getDoubleValue("lat"));
_ident = aProp->getStringValue("ident");
// check it's a real VOR?
_magCourse = aProp->getDoubleValue("course-deg");
_radial = aProp->getDoubleValue("radial-deg");
}
void RadialIntercept::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("ident", _ident);
aProp->setDoubleValue("lon", _pos.getLongitudeDeg());
aProp->setDoubleValue("lat", _pos.getLatitudeDeg());
aProp->setDoubleValue("course-deg", _magCourse);
aProp->setDoubleValue("radial-deg", _radial);
}
/////////////////////////////////////////////////////////////////////////////
ATCVectors::ATCVectors(Route* aOwner, FGAirport* aFacility) :
Waypt(aOwner),
_facility(aFacility)
{
setFlag(WPT_DYNAMIC);
}
ATCVectors::~ATCVectors()
{
}
ATCVectors::ATCVectors(Route* aOwner) :
Waypt(aOwner)
{
}
SGGeod ATCVectors::position() const
{
return _facility->geod();
}
string ATCVectors::ident() const
{
return "VECTORS-" + _facility->ident();
}
void ATCVectors::initFromProperties(SGPropertyNode_ptr aProp)
{
if (!aProp->hasChild("icao")) {
throw sg_io_exception("missing icao propertie",
"ATCVectors::initFromProperties");
}
Waypt::initFromProperties(aProp);
_facility = FGAirport::getByIdent(aProp->getStringValue("icao"));
}
void ATCVectors::writeToProperties(SGPropertyNode_ptr aProp) const
{
Waypt::writeToProperties(aProp);
aProp->setStringValue("icao", _facility->ident());
}
} // of namespace

314
src/Navaids/waypoint.hxx Normal file
View file

@ -0,0 +1,314 @@
// waypoint.hxx - waypoints that can occur in routes/procedures
// 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.
#ifndef FG_WAYPOINT_HXX
#define FG_WAYPOINT_HXX
#include <Navaids/route.hxx>
#include <Navaids/positioned.hxx>
class FGAirport;
typedef SGSharedPtr<FGAirport> FGAirportRef;
class SGWayPoint;
class FGRunway;
namespace flightgear
{
class BasicWaypt : public Waypt
{
public:
BasicWaypt(const SGGeod& aPos, const std::string& aIdent, Route* aOwner);
BasicWaypt(const SGWayPoint& aWP, Route* aOwner);
BasicWaypt(Route* aOwner);
virtual SGGeod position() const
{ return _pos; }
virtual std::string ident() const
{ return _ident; }
protected:
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "basic"; }
SGGeod _pos;
std::string _ident;
};
/**
* Waypoint based upon a navaid. In practice this means any Positioned
* element, excluding runways (see below)
*/
class NavaidWaypoint : public Waypt
{
public:
NavaidWaypoint(FGPositioned* aPos, Route* aOwner);
NavaidWaypoint(Route* aOwner);
virtual SGGeod position() const;
virtual FGPositioned* source() const
{ return _navaid; }
virtual std::string ident() const;
protected:
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "navaid"; }
FGPositionedRef _navaid;
};
class OffsetNavaidWaypoint : public NavaidWaypoint
{
public:
OffsetNavaidWaypoint(FGPositioned* aPos, Route* aOwner, double aRadial, double aDistNm);
OffsetNavaidWaypoint(Route* aOwner);
virtual SGGeod position() const
{ return _geod; }
protected:
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "offset-navaid"; }
private:
void init();
SGGeod _geod;
double _radial; // true, degrees
double _distanceNm;
};
/**
* Waypoint based upon a runway.
* Runways are handled specially in various places, so it's cleaner
* to be able to distuinguish them from other navaid waypoints
*/
class RunwayWaypt : public Waypt
{
public:
RunwayWaypt(FGRunway* aPos, Route* aOwner);
RunwayWaypt(Route* aOwner);
virtual SGGeod position() const;
virtual FGPositioned* source() const;
virtual std::string ident() const;
FGRunway* runway() const
{ return _runway; }
protected:
virtual std::string type() const
{ return "runway"; }
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
private:
FGRunway* _runway;
};
class Hold : public BasicWaypt
{
public:
Hold(const SGGeod& aPos, const std::string& aIdent, Route* aOwner);
Hold(Route* aOwner);
void setHoldRadial(double aInboundRadial);
void setHoldDistance(double aDistanceNm);
void setHoldTime(double aTimeSec);
void setRightHanded();
void setLeftHanded();
double inboundRadial() const
{ return _bearing; }
bool isLeftHanded() const
{ return !_righthanded; }
bool isDistance() const
{ return _isDistance; }
double timeOrDistance() const
{ return _holdTD;}
protected:
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "hold"; }
private:
double _bearing;
bool _righthanded;
bool _isDistance;
double _holdTD;
};
class HeadingToAltitude : public Waypt
{
public:
HeadingToAltitude(Route* aOwner, const std::string& aIdent, double aMagHdg);
HeadingToAltitude(Route* aOwner);
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "hdgToAlt"; }
virtual SGGeod position() const
{ return SGGeod(); }
virtual std::string ident() const
{ return _ident; }
double headingDegMagnetic() const
{ return _magHeading; }
private:
std::string _ident;
double _magHeading;
};
class DMEIntercept : public Waypt
{
public:
DMEIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos,
double aCourseDeg, double aDistanceNm);
DMEIntercept(Route* aOwner);
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "dmeIntercept"; }
virtual SGGeod position() const
{ return _pos; }
virtual std::string ident() const
{ return _ident; }
double courseDegMagnetic() const
{ return _magCourse; }
double dmeDistanceNm() const
{ return _dmeDistanceNm; }
private:
std::string _ident;
SGGeod _pos;
double _magCourse;
double _dmeDistanceNm;
};
class RadialIntercept : public Waypt
{
public:
RadialIntercept(Route* aOwner, const std::string& aIdent, const SGGeod& aPos,
double aCourseDeg, double aRadialDeg);
RadialIntercept(Route* aOwner);
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "radialIntercept"; }
virtual SGGeod position() const
{ return _pos; }
virtual std::string ident() const
{ return _ident; }
double courseDegMagnetic() const
{ return _magCourse; }
double radialDegMagnetic() const
{ return _radial; }
private:
std::string _ident;
SGGeod _pos;
double _magCourse;
double _radial;
};
/**
* Represent ATC radar vectored segment. Common at the end of published
* missed approach procedures, and from STAR arrival points to final approach
*/
class ATCVectors : public Waypt
{
public:
ATCVectors(Route* aOwner, FGAirport* aFacility);
virtual ~ATCVectors();
ATCVectors(Route* aOwner);
virtual void initFromProperties(SGPropertyNode_ptr aProp);
virtual void writeToProperties(SGPropertyNode_ptr aProp) const;
virtual std::string type() const
{ return "vectors"; }
virtual SGGeod position() const;
virtual std::string ident() const;
private:
/**
* ATC facility. Using an airport here is incorrect, since often arrivals
* facilities will be shared between several nearby airports, but it
* suffices until we have a proper facility representation
*/
FGAirportRef _facility;
};
} // of namespace flighgear
#endif // of FG_WAYPOINT_HXX

23
src/Scripting/NasalSys.cxx
View file

@ -33,6 +33,7 @@
#include <Main/util.hxx>
#include <Scenery/scenery.hxx>
#include <Navaids/navrecord.hxx>
#include <Navaids/procedure.hxx>
#include "NasalSys.hxx"
@ -613,6 +614,28 @@ static naRef f_airportinfo(naContext c, naRef me, int argc, naRef* args)
HASHSET("ils_frequency_mhz", 17, naNum(rwy->ILS()->get_freq() / 100.0));
}
std::vector<flightgear::SID*> sids(rwy->getSIDs());
naRef sidVec = naNewVector(c);
for (unsigned int s=0; s < sids.size(); ++s) {
naRef procId = naStr_fromdata(naNewString(c),
const_cast<char *>(sids[s]->ident().c_str()),
sids[s]->ident().length());
naVec_append(sidVec, procId);
}
HASHSET("sids", 4, sidVec);
std::vector<flightgear::STAR*> stars(rwy->getSTARs());
naRef starVec = naNewVector(c);
for (unsigned int s=0; s < stars.size(); ++s) {
naRef procId = naStr_fromdata(naNewString(c),
const_cast<char *>(stars[s]->ident().c_str()),
stars[s]->ident().length());
naVec_append(starVec, procId);
}
HASHSET("stars", 5, starVec);
#undef HASHSET
naHash_set(rwys, rwyid, rwydata);
}