fly/flightgear
1
0
Fork 0
Code Activity

Merge branch 'jmt/gps'

Browse source 
Conflicts:
	src/Instrumentation/gps.cxx
This commit is contained in:
Tim Moore 2009-11-27 06:40:47 +01:00
commit e503591af4
20 changed files with 2997 additions and 892 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

10
src/AIModel/AIFlightPlanCreateCruise.cxx
View file

@ -76,18 +76,14 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
}
//cerr << "1"<< endl;
SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
double midlat = geoc.getLatitudeDeg();
double midlon = geoc.getLongitudeDeg();
SGGeod geod = SGGeod::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
prevNode = tmpNode;
tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
tmpNode = globals->get_airwaynet()->findNearestNode(geod);
FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition()));
if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) {
if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
nodes.push_back(tmpNode);
}
}

4
src/Airports/simple.cxx
View file

@ -294,7 +294,7 @@ bool FGAirport::HardSurfaceFilter::passAirport(FGAirport* aApt) const
FGAirport* FGAirport::findByIdent(const std::string& aIdent)
{
FGPositionedRef r;
AirportFilter filter;
PortsFilter filter;
r = FGPositioned::findNextWithPartialId(r, aIdent, &filter);
if (!r) {
return NULL; // we don't warn here, let the caller do that
@ -305,7 +305,7 @@ FGAirport* FGAirport::findByIdent(const std::string& aIdent)
FGAirport* FGAirport::getByIdent(const std::string& aIdent)
{
FGPositionedRef r;
AirportFilter filter;
PortsFilter filter;
r = FGPositioned::findNextWithPartialId(r, aIdent, &filter);
if (!r) {
throw sg_range_exception("No such airport with ident: " + aIdent);

18
src/Airports/simple.hxx
View file

@ -113,7 +113,7 @@ public:
}
virtual Type maxType() const {
return SEAPORT;
return AIRPORT;
}
virtual bool passAirport(FGAirport* aApt) const {
@ -121,6 +121,17 @@ public:
}
};
/**
* Filter which passes heliports and seaports in addition to airports
*/
class PortsFilter : public AirportFilter
{
public:
virtual Type maxType() const {
return SEAPORT;
}
};
class HardSurfaceFilter : public AirportFilter
{
public:
@ -128,9 +139,6 @@ public:
virtual bool passAirport(FGAirport* aApt) const;
virtual Type maxType() const {
return AIRPORT;
}
private:
double mMinLengthFt;
};
@ -138,7 +146,7 @@ public:
/**
* Syntactic wrapper around FGPositioned::findClosest - find the closest
* match for filter, and return it cast to FGAirport. The default filter
* passes all airports, including seaports and heliports.
* passes airports, but not seaports or heliports
*/
static FGAirport* findClosest(const SGGeod& aPos, double aCuttofNm, Filter* filter = NULL);

818
src/Autopilot/route_mgr.cxx
View file

@ -27,39 +27,49 @@
# include <config.h>
#endif
#include <simgear/compiler.h>
#ifdef HAVE_WINDOWS_H
#include <time.h>
#endif
#include <FDM/flight.hxx>
#include <Main/fg_props.hxx>
#include <Navaids/positioned.hxx>
#include <simgear/compiler.h>
#include "route_mgr.hxx"
#include <boost/algorithm/string/case_conv.hpp>
#include <simgear/misc/strutils.hxx>
#include <simgear/structure/exception.hxx>
#include <simgear/props/props_io.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/route/route.hxx>
#include <simgear/sg_inlines.h>
#include "Main/fg_props.hxx"
#include "Navaids/positioned.hxx"
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
#include "FDM/flight.hxx" // for getting ground speed
#define RM "/autopilot/route-manager/"
static double get_ground_speed() {
// starts in ft/s so we convert to kts
static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
double ft_s = cur_fdm_state->get_V_ground_speed()
* speedup_node->getIntValue();
double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
return kts;
}
FGRouteMgr::FGRouteMgr() :
route( new SGRoute ),
lon( NULL ),
lat( NULL ),
alt( NULL ),
true_hdg_deg( NULL ),
target_altitude_ft( NULL ),
altitude_lock( NULL ),
wp0_id( NULL ),
wp0_dist( NULL ),
wp0_eta( NULL ),
wp1_id( NULL ),
wp1_dist( NULL ),
wp1_eta( NULL ),
wpn_id( NULL ),
wpn_dist( NULL ),
wpn_eta( NULL ),
_route( new SGRoute ),
input(fgGetNode( RM "input", true )),
listener(new Listener(this)),
mirror(fgGetNode( RM "route", true )),
altitude_set( false )
mirror(fgGetNode( RM "route", true ))
{
listener = new InputListener(this);
input->setStringValue("");
input->addChangeListener(listener);
}
@ -67,141 +77,198 @@ FGRouteMgr::FGRouteMgr() :
FGRouteMgr::~FGRouteMgr() {
input->removeChangeListener(listener);
delete listener;
delete route;
delete _route;
}
void FGRouteMgr::init() {
lon = fgGetNode( "/position/longitude-deg", true );
lat = fgGetNode( "/position/latitude-deg", true );
alt = fgGetNode( "/position/altitude-ft", true );
SGPropertyNode_ptr rm(fgGetNode(RM));
lon = fgGetNode( "/position/longitude-deg", true );
lat = fgGetNode( "/position/latitude-deg", true );
alt = fgGetNode( "/position/altitude-ft", true );
magvar = fgGetNode("/environment/magnetic-variation-deg", true);
departure = fgGetNode(RM "departure", true);
departure->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDepartureICAO, &FGRouteMgr::setDepartureICAO));
departure->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDepartureName, NULL));
// init departure information from current location
SGGeod pos = SGGeod::fromDegFt(lon->getDoubleValue(), lat->getDoubleValue(), alt->getDoubleValue());
_departure = FGAirport::findClosest(pos, 20.0);
if (_departure) {
FGRunway* active = _departure->getActiveRunwayForUsage();
departure->setStringValue("runway", active->ident().c_str());
} else {
departure->setStringValue("runway", "");
}
departure->getChild("etd", 0, true);
departure->getChild("takeoff-time", 0, true);
true_hdg_deg = fgGetNode( "/autopilot/settings/true-heading-deg", true );
target_altitude_ft = fgGetNode( "/autopilot/settings/target-altitude-ft", true );
altitude_lock = fgGetNode( "/autopilot/locks/altitude", true );
wp0_id = fgGetNode( RM "wp[0]/id", true );
wp0_dist = fgGetNode( RM "wp[0]/dist", true );
wp0_eta = fgGetNode( RM "wp[0]/eta", true );
wp1_id = fgGetNode( RM "wp[1]/id", true );
wp1_dist = fgGetNode( RM "wp[1]/dist", true );
wp1_eta = fgGetNode( RM "wp[1]/eta", true );
wpn_id = fgGetNode( RM "wp-last/id", true );
wpn_dist = fgGetNode( RM "wp-last/dist", true );
wpn_eta = fgGetNode( RM "wp-last/eta", true );
route->clear();
update_mirror();
destination = fgGetNode(RM "destination", true);
destination->getChild("airport", 0, true);
destination->tie("airport", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDestinationICAO, &FGRouteMgr::setDestinationICAO));
destination->tie("name", SGRawValueMethods<FGRouteMgr, const char*>(*this,
&FGRouteMgr::getDestinationName, NULL));
destination->getChild("runway", 0, true);
destination->getChild("eta", 0, true);
destination->getChild("touchdown-time", 0, true);
alternate = fgGetNode(RM "alternate", true);
alternate->getChild("airport", 0, true);
alternate->getChild("runway", 0, true);
cruise = fgGetNode(RM "cruise", true);
cruise->getChild("altitude-ft", 0, true);
cruise->setDoubleValue("altitude-ft", 10000.0);
cruise->getChild("flight-level", 0, true);
cruise->getChild("speed-kts", 0, true);
cruise->setDoubleValue("speed-kts", 160.0);
totalDistance = fgGetNode(RM "total-distance", true);
totalDistance->setDoubleValue(0.0);
ete = fgGetNode(RM "ete", true);
ete->setDoubleValue(0.0);
elapsedFlightTime = fgGetNode(RM "flight-time", true);
elapsedFlightTime->setDoubleValue(0.0);
active = fgGetNode(RM "active", true);
active->setBoolValue(false);
airborne = fgGetNode(RM "airborne", true);
airborne->setBoolValue(false);
_edited = fgGetNode(RM "signals/edited", true);
_finished = fgGetNode(RM "signals/finished", true);
_currentWpt = fgGetNode(RM "current-wp", true);
_currentWpt->tie(SGRawValueMethods<FGRouteMgr, int>
(*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex));
// temporary distance / eta calculations, for backward-compatability
wp0 = fgGetNode(RM "wp", 0, true);
wp0->getChild("id", 0, true);
wp0->getChild("dist", 0, true);
wp0->getChild("eta", 0, true);
wp0->getChild("bearing-deg", 0, true);
wp1 = fgGetNode(RM "wp", 1, true);
wp1->getChild("id", 0, true);
wp1->getChild("dist", 0, true);
wp1->getChild("eta", 0, true);
wpn = fgGetNode(RM "wp-last", 0, true);
wpn->getChild("dist", 0, true);
wpn->getChild("eta", 0, true);
_route->clear();
update_mirror();
_pathNode = fgGetNode(RM "file-path", 0, true);
}
void FGRouteMgr::postinit() {
string_list *waypoints = globals->get_initial_waypoints();
if (!waypoints)
return;
vector<string>::iterator it;
for (it = waypoints->begin(); it != waypoints->end(); ++it)
if (waypoints) {
vector<string>::iterator it;
for (it = waypoints->begin(); it != waypoints->end(); ++it)
new_waypoint(*it);
}
weightOnWheels = fgGetNode("/gear/gear[0]/wow", false);
// check airbone flag agrees with presets
}
void FGRouteMgr::bind() { }
void FGRouteMgr::unbind() { }
static double get_ground_speed() {
// starts in ft/s so we convert to kts
static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
double ft_s = cur_fdm_state->get_V_ground_speed()
* speedup_node->getIntValue();
double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
return kts;
}
void FGRouteMgr::updateTargetAltitude() {
if (route->size() == 0) {
altitude_set = false;
return;
}
SGWayPoint wp = route->get_waypoint( 0 );
if (wp.get_target_alt() < -9990.0) {
altitude_set = false;
return;
}
altitude_set = true;
target_altitude_ft->setDoubleValue( wp.get_target_alt() * SG_METER_TO_FEET );
if ( !near_ground() ) {
// James Turner [zakalawe]: there's no explanation for this logic,
// it feels like the autopilot should pull the target altitude out of
// wp0 instead of us pushing it through here. Hmmm.
altitude_lock->setStringValue( "altitude-hold" );
}
bool FGRouteMgr::isRouteActive() const
{
return active->getBoolValue();
}
void FGRouteMgr::update( double dt ) {
if (route->size() == 0) {
return; // no route set, early return
if (dt <= 0.0) {
// paused, nothing to do here
return;
}
if (!active->getBoolValue()) {
return;
}
double groundSpeed = get_ground_speed();
if (airborne->getBoolValue()) {
time_t now = time(NULL);
elapsedFlightTime->setDoubleValue(difftime(now, _takeoffTime));
} else { // not airborne
if (weightOnWheels->getBoolValue() || (groundSpeed < 40)) {
return;
}
airborne->setBoolValue(true);
_takeoffTime = time(NULL); // start the clock
departure->setIntValue("takeoff-time", _takeoffTime);
}
// basic course/distance information
double wp_course, wp_distance;
// first way point
SGWayPoint wp = route->get_waypoint( 0 );
SGWayPoint wp = _route->get_current();
wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(),
alt->getDoubleValue(), &wp_course, &wp_distance );
true_hdg_deg->setDoubleValue( wp_course );
if ( wp_distance < 200.0 ) {
pop_waypoint();
if (route->size() == 0) {
return; // end of route, we're done for the time being
}
wp = route->get_waypoint( 0 );
// update wp0 / wp1 / wp-last for legacy users
wp0->setDoubleValue("dist", wp_distance * SG_METER_TO_NM);
wp_course -= magvar->getDoubleValue(); // expose magnetic bearing
wp0->setDoubleValue("bearing-deg", wp_course);
setETAPropertyFromDistance(wp0->getChild("eta"), wp_distance);
if ((_route->current_index() + 1) < _route->size()) {
wp = _route->get_waypoint(_route->current_index() + 1);
double wp1_course, wp1_distance;
wp.CourseAndDistance(lon->getDoubleValue(), lat->getDoubleValue(),
alt->getDoubleValue(), &wp1_course, &wp1_distance);
wp1->setDoubleValue("dist", wp1_distance * SG_METER_TO_NM);
setETAPropertyFromDistance(wp1->getChild("eta"), wp1_distance);
}
// update the property tree info for WP0
wp0_id->setStringValue( wp.get_id().c_str() );
double accum = wp_distance;
wp0_dist->setDoubleValue( accum * SG_METER_TO_NM );
setETAPropertyFromDistance(wp0_eta, accum);
// next way point
if ( route->size() > 1 ) {
SGWayPoint wp = route->get_waypoint( 1 );
// update the property tree info
wp1_id->setStringValue( wp.get_id().c_str() );
accum += wp.get_distance();
wp1_dist->setDoubleValue( accum * SG_METER_TO_NM );
setETAPropertyFromDistance(wp1_eta, accum);
}
// summarize remaining way points
if ( route->size() > 2 ) {
SGWayPoint wp;
for ( int i = 2; i < route->size(); ++i ) {
wp = route->get_waypoint( i );
accum += wp.get_distance();
}
// update the property tree info
wpn_id->setStringValue( wp.get_id().c_str() );
wpn_dist->setDoubleValue( accum * SG_METER_TO_NM );
setETAPropertyFromDistance(wpn_eta, accum);
double totalDistanceRemaining = wp_distance; // distance to current waypoint
for (int i=_route->current_index() + 1; i<_route->size(); ++i) {
totalDistanceRemaining += _route->get_waypoint(i).get_distance();
}
wpn->setDoubleValue("dist", totalDistanceRemaining * SG_METER_TO_NM);
ete->setDoubleValue(totalDistanceRemaining * SG_METER_TO_NM / groundSpeed * 3600.0);
setETAPropertyFromDistance(wpn->getChild("eta"), totalDistanceRemaining);
// get time now at destination tz as tm struct
// add ete seconds
// convert to string ... and stash in property
//destination->setDoubleValue("eta", eta);
}
void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance) {
double speed = get_ground_speed();
if (speed < 1.0) {
aProp->setStringValue("--:--");
return;
}
char eta_str[64];
double eta = aDistance * SG_METER_TO_NM / get_ground_speed();
if ( eta >= 100.0 ) {
@ -209,8 +276,7 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi
}
if ( eta < (1.0/6.0) ) {
// within 10 minutes, bump up to min/secs
eta *= 60.0;
eta *= 60.0; // within 10 minutes, bump up to min/secs
}
int major = (int)eta,
@ -220,45 +286,38 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi
}
void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) {
route->add_waypoint( wp, n );
update_mirror();
if ((n==0) || (route->size() == 1)) {
updateTargetAltitude();
}
_route->add_waypoint( wp, n );
if (_route->current_index() > n) {
_route->set_current(_route->current_index() + 1);
}
update_mirror();
_edited->fireValueChanged();
}
SGWayPoint FGRouteMgr::pop_waypoint( int n ) {
SGWayPoint wp;
if ( _route->size() <= 0 ) {
return SGWayPoint();
}
if ( n < 0 ) {
n = _route->size() - 1;
}
if (_route->current_index() > n) {
_route->set_current(_route->current_index() - 1);
}
if ( route->size() > 0 ) {
if ( n < 0 )
n = route->size() - 1;
wp = route->get_waypoint(n);
route->delete_waypoint(n);
}
if ( route->size() <= 2 ) {
wpn_id->setStringValue( "" );
wpn_dist->setDoubleValue( 0.0 );
wpn_eta->setStringValue( "" );
}
if ( route->size() <= 1 ) {
wp1_id->setStringValue( "" );
wp1_dist->setDoubleValue( 0.0 );
wp1_eta->setStringValue( "" );
}
if ( route->size() <= 0 ) {
wp0_id->setStringValue( "" );
wp0_dist->setDoubleValue( 0.0 );
wp0_eta->setStringValue( "" );
}
updateTargetAltitude();
update_mirror();
return wp;
SGWayPoint wp = _route->get_waypoint(n);
_route->delete_waypoint(n);
update_mirror();
_edited->fireValueChanged();
checkFinished();
return wp;
}
@ -275,23 +334,15 @@ void FGRouteMgr::new_waypoint( const string& target, int n ) {
add_waypoint( *wp, n );
delete wp;
if ( !near_ground() ) {
fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
}
}
SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
string target = tgt;
string target(boost::to_upper_copy(tgt));
// make upper case
for (unsigned int i = 0; i < target.size(); i++)
if (target[i] >= 'a' && target[i] <= 'z')
target[i] -= 'a' - 'A';
// extract altitude
double alt = -9999.0;
double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER;
size_t pos = target.find( '@' );
if ( pos != string::npos ) {
alt = atof( target.c_str() + pos + 1 );
@ -305,65 +356,94 @@ SGWayPoint* FGRouteMgr::make_waypoint(const string& tgt ) {
if ( pos != string::npos ) {
double lon = atof( target.substr(0, pos).c_str());
double lat = atof( target.c_str() + pos + 1);
SG_LOG( SG_GENERAL, SG_INFO, "Adding waypoint lon = " << lon << ", lat = " << lat );
return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, target );
char buf[32];
char ew = (lon < 0.0) ? 'W' : 'E';
char ns = (lat < 0.0) ? 'S' : 'N';
snprintf(buf, 32, "%c%03d%c%03d", ew, (int) fabs(lon), ns, (int)fabs(lat));
return new SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, buf);
}
SGGeod basePosition;
if (route->size() > 0) {
SGWayPoint wp = get_waypoint(route->size()-1);
basePosition = SGGeod::fromDeg(wp.get_target_lon(), wp.get_target_lat());
if (_route->size() > 0) {
SGWayPoint wp = get_waypoint(_route->size()-1);
basePosition = wp.get_target();
} else {
// route is empty, use current position
basePosition = SGGeod::fromDeg(
fgGetNode("/position/longitude-deg")->getDoubleValue(),
fgGetNode("/position/latitude-deg")->getDoubleValue());
}
vector<string> pieces(simgear::strutils::split(target, "/"));
FGPositionedRef p = FGPositioned::findClosestWithIdent(target, basePosition);
FGPositionedRef p = FGPositioned::findClosestWithIdent(pieces.front(), basePosition);
if (!p) {
SG_LOG( SG_GENERAL, SG_INFO, "Unable to find FGPositioned with ident:" << target);
return NULL;
SG_LOG( SG_AUTOPILOT, SG_INFO, "Unable to find FGPositioned with ident:" << pieces.front());
return NULL;
}
return new SGWayPoint(p->longitude(), p->latitude(), alt, SGWayPoint::WGS84, target);
SGGeod geod = SGGeod::fromGeodM(p->geod(), alt);
if (pieces.size() == 1) {
// simple case
return new SGWayPoint(geod, target, p->name());
}
if (pieces.size() == 3) {
// navaid/radial/distance-nm notation
double radial = atof(pieces[1].c_str()),
distanceNm = atof(pieces[2].c_str()),
az2;
radial += magvar->getDoubleValue(); // convert to true bearing
SGGeod offsetPos;
SGGeodesy::direct(geod, radial, distanceNm * SG_NM_TO_METER, offsetPos, az2);
offsetPos.setElevationM(alt);
SG_LOG(SG_AUTOPILOT, SG_INFO, "final offset radial is " << radial);
return new SGWayPoint(offsetPos, p->ident() + pieces[2], target);
}
if (pieces.size() == 2) {
FGAirport* apt = dynamic_cast<FGAirport*>(p.ptr());
if (!apt) {
SG_LOG(SG_AUTOPILOT, SG_INFO, "Waypoint is not an airport:" << pieces.front());
return NULL;
}
if (!apt->hasRunwayWithIdent(pieces[1])) {
SG_LOG(SG_AUTOPILOT, SG_INFO, "No runway: " << pieces[1] << " at " << pieces[0]);
return NULL;
}
FGRunway* runway = apt->getRunwayByIdent(pieces[1]);
SGGeod t = runway->threshold();
return new SGWayPoint(t.getLongitudeDeg(), t.getLatitudeDeg(), alt, SGWayPoint::WGS84, pieces[1]);
}
SG_LOG(SG_AUTOPILOT, SG_INFO, "Unable to parse waypoint:" << target);
return NULL;
}
// mirror internal route to the property system for inspection by other subsystems
void FGRouteMgr::update_mirror() {
mirror->removeChildren("wp");
for (int i = 0; i < route->size(); i++) {
SGWayPoint wp = route->get_waypoint(i);
for (int i = 0; i < _route->size(); i++) {
SGWayPoint wp = _route->get_waypoint(i);
SGPropertyNode *prop = mirror->getChild("wp", i, 1);
const SGGeod& pos(wp.get_target());
prop->setStringValue("id", wp.get_id().c_str());
prop->setStringValue("name", wp.get_name().c_str());
prop->setDoubleValue("longitude-deg", wp.get_target_lon());
prop->setDoubleValue("latitude-deg", wp.get_target_lat());
prop->setDoubleValue("altitude-m", wp.get_target_alt());
prop->setDoubleValue("altitude-ft", wp.get_target_alt() * SG_METER_TO_FEET);
prop->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
prop->setDoubleValue("latitude-deg",pos.getLatitudeDeg());
prop->setDoubleValue("altitude-m", pos.getElevationM());
prop->setDoubleValue("altitude-ft", pos.getElevationFt());
}
// set number as listener attachment point
mirror->setIntValue("num", route->size());
mirror->setIntValue("num", _route->size());
}
bool FGRouteMgr::near_ground() {
SGPropertyNode *gear = fgGetNode( "/gear/gear/wow", false );
if ( !gear || gear->getType() == simgear::props::NONE )
return fgGetBool( "/sim/presets/onground", true );
if ( fgGetDouble("/position/altitude-agl-ft", 300.0)
< fgGetDouble("/autopilot/route-manager/min-lock-altitude-agl-ft") )
return true;
return gear->getBoolValue();
}
// command interface /autopilot/route-manager/input:
//
// @CLEAR ... clear route
@ -372,12 +452,18 @@ bool FGRouteMgr::near_ground() {
// @INSERT2:KSFO@900 ... insert "KSFO@900" as 3rd entry
// KSFO@900 ... append "KSFO@900"
//
void FGRouteMgr::Listener::valueChanged(SGPropertyNode *prop)
void FGRouteMgr::InputListener::valueChanged(SGPropertyNode *prop)
{
const char *s = prop->getStringValue();
if (!strcmp(s, "@CLEAR"))
mgr->init();
else if (!strcmp(s, "@POP"))
else if (!strcmp(s, "@ACTIVATE"))
mgr->activate();
else if (!strcmp(s, "@LOAD")) {
mgr->loadRoute();
} else if (!strcmp(s, "@SAVE")) {
mgr->saveRoute();
} else if (!strcmp(s, "@POP"))
mgr->pop_waypoint(0);
else if (!strncmp(s, "@DELETE", 7))
mgr->pop_waypoint(atoi(s + 7));
@ -394,4 +480,324 @@ void FGRouteMgr::Listener::valueChanged(SGPropertyNode *prop)
mgr->new_waypoint(s);
}
// SGWayPoint( const double lon = 0.0, const double lat = 0.0,
// const double alt = 0.0, const modetype m = WGS84,
// const string& s = "", const string& n = "" );
bool FGRouteMgr::activate()
{
if (_departure) {
string runwayId(departure->getStringValue("runway"));
FGRunway* runway = NULL;
if (_departure->hasRunwayWithIdent(runwayId)) {
runway = _departure->getRunwayByIdent(runwayId);
} else {
SG_LOG(SG_AUTOPILOT, SG_INFO,
"route-manager, departure runway not found:" << runwayId);
runway = _departure->getActiveRunwayForUsage();
}
SGWayPoint swp(runway->threshold(),
_departure->ident() + "-" + runway->ident(), runway->name());
add_waypoint(swp, 0);
}
if (_destination) {
string runwayId = (destination->getStringValue("runway"));
if (_destination->hasRunwayWithIdent(runwayId)) {
FGRunway* runway = _destination->getRunwayByIdent(runwayId);
SGWayPoint swp(runway->end(),
_destination->ident() + "-" + runway->ident(), runway->name());
add_waypoint(swp);
} else {
// quite likely, since destination runway may not be known until enroute
// probably want a listener on the 'destination' node to allow an enroute
// update
add_waypoint(SGWayPoint(_destination->geod(), _destination->ident(), _destination->name()));
}
}
_route->set_current(0);
double routeDistanceNm = _route->total_distance() * SG_METER_TO_NM;
totalDistance->setDoubleValue(routeDistanceNm);
double cruiseSpeedKts = cruise->getDoubleValue("speed", 0.0);
if (cruiseSpeedKts > 1.0) {
// very very crude approximation, doesn't allow for climb / descent
// performance or anything else at all
ete->setDoubleValue(routeDistanceNm / cruiseSpeedKts * (60.0 * 60.0));
}
active->setBoolValue(true);
SG_LOG(SG_AUTOPILOT, SG_INFO, "route-manager, activate route ok");
return true;
}
void FGRouteMgr::sequence()
{
if (!active->getBoolValue()) {
SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
return;
}
if (checkFinished()) {
return;
}
_route->increment_current();
currentWaypointChanged();
_currentWpt->fireValueChanged();
}
bool FGRouteMgr::checkFinished()
{
int lastWayptIndex = _route->size() - 1;
if (_route->current_index() < lastWayptIndex) {
return false;
}
SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
_finished->fireValueChanged();
active->setBoolValue(false);
return true;
}
void FGRouteMgr::jumpToIndex(int index)
{
if (!active->getBoolValue()) {
SG_LOG(SG_AUTOPILOT, SG_ALERT, "trying to sequence waypoints with no active route");
return;
}
if ((index < 0) || (index >= _route->size())) {
SG_LOG(SG_AUTOPILOT, SG_ALERT, "passed invalid index (" <<
index << ") to FGRouteMgr::jumpToIndex");
return;
}
if (_route->current_index() == index) {
return; // no-op
}
_route->set_current(index);
currentWaypointChanged();
}
void FGRouteMgr::currentWaypointChanged()
{
SGWayPoint previous = _route->get_previous();
SGWayPoint cur = _route->get_current();
wp0->getChild("id")->setStringValue(cur.get_id());
if ((_route->current_index() + 1) < _route->size()) {
wp1->getChild("id")->setStringValue(_route->get_next().get_id());
} else {
wp1->getChild("id")->setStringValue("");
}
SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index());
}
int FGRouteMgr::findWaypoint(const SGGeod& aPos) const
{
for (int i=0; i<_route->size(); ++i) {
double d = SGGeodesy::distanceM(aPos, _route->get_waypoint(i).get_target());
if (d < 200.0) { // 200 metres seems close enough
return i;
}
}
return -1;
}
SGWayPoint FGRouteMgr::get_waypoint( int i ) const
{
return _route->get_waypoint(i);
}
int FGRouteMgr::size() const
{
return _route->size();
}
int FGRouteMgr::currentWaypoint() const
{
return _route->current_index();
}
void FGRouteMgr::saveRoute()
{
SGPath path(_pathNode->getStringValue());
SG_LOG(SG_IO, SG_INFO, "Saving route to " << path.str());
try {
writeProperties(path.str(), mirror, false, SGPropertyNode::ARCHIVE);
} catch (const sg_exception &e) {
SG_LOG(SG_IO, SG_WARN, "Error saving route:" << e.getMessage());
//guiErrorMessage("Error writing autosave.xml: ", e);
}
}
void FGRouteMgr::loadRoute()
{
try {
// deactivate route first
active->setBoolValue(false);
SGPropertyNode_ptr routeData(new SGPropertyNode);
SGPath path(_pathNode->getStringValue());
SG_LOG(SG_IO, SG_INFO, "going to read flight-plan from:" << path.str());
readProperties(path.str(), routeData);
// departure nodes
SGPropertyNode* dep = routeData->getChild("departure");
if (!dep) {
throw sg_io_exception("malformed route file, no departure node");
}
string depIdent = dep->getStringValue("airport");
_departure = (FGAirport*) fgFindAirportID(depIdent);
// destination
SGPropertyNode* dst = routeData->getChild("destination");
if (!dst) {
throw sg_io_exception("malformed route file, no destination node");
}
_destination = (FGAirport*) fgFindAirportID(dst->getStringValue("airport"));
destination->setStringValue("runway", dst->getStringValue("runway"));
// alternate
SGPropertyNode* alt = routeData->getChild("alternate");
if (alt) {
alternate->setStringValue(alt->getStringValue("airport"));
} // of cruise data loading
// cruise
SGPropertyNode* crs = routeData->getChild("cruise");
if (crs) {
cruise->setDoubleValue(crs->getDoubleValue("speed"));
} // of cruise data loading
// route nodes
_route->clear();
SGPropertyNode_ptr _route = routeData->getChild("route", 0);
SGGeod lastPos = (_departure ? _departure->geod() : SGGeod());
for (int i=0; i<_route->nChildren(); ++i) {
SGPropertyNode_ptr wp = _route->getChild("wp", i);
parseRouteWaypoint(wp);
} // of route iteration
} catch (sg_exception& e) {
SG_LOG(SG_IO, SG_WARN, "failed to load flight-plan (from '" << e.getOrigin()
<< "'):" << e.getMessage());
}
}
void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
{
SGGeod lastPos;
if (_route->size() > 0) {
lastPos = get_waypoint(_route->size()-1).get_target();
} else {
// route is empty, use departure airport position
const FGAirport* apt = fgFindAirportID(departure->getStringValue("airport"));
assert(apt); // shouldn't have got this far with an invalid airport
lastPos = apt->geod();
}
SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft");
double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER;
if (altProp) {
alt = altProp->getDoubleValue();
}
string ident(aWP->getStringValue("ident"));
if (aWP->hasChild("longitude-deg")) {
// explicit longitude/latitude
SGWayPoint swp(aWP->getDoubleValue("longitude-deg"),
aWP->getDoubleValue("latitude-deg"), alt,
SGWayPoint::WGS84, ident, aWP->getStringValue("name"));
add_waypoint(swp);
} else if (aWP->hasChild("navid")) {
// lookup by navid (possibly with offset)
string nid(aWP->getStringValue("navid"));
FGPositionedRef p = FGPositioned::findClosestWithIdent(nid, lastPos);
if (!p) {
throw sg_io_exception("bad route file, unknown navid:" + nid);
}
SGGeod pos(p->geod());
if (aWP->hasChild("offset-nm") && aWP->hasChild("offset-radial")) {
double radialDeg = aWP->getDoubleValue("offset-radial");
// convert magnetic radial to a true radial!
radialDeg += magvar->getDoubleValue();
double offsetNm = aWP->getDoubleValue("offset-nm");
double az2;
SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
}
SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), alt,
SGWayPoint::WGS84, ident, "");
add_waypoint(swp);
} else {
// lookup by ident (symbolic waypoint)
FGPositionedRef p = FGPositioned::findClosestWithIdent(ident, lastPos);
if (!p) {
throw sg_io_exception("bad route file, unknown waypoint:" + ident);
}
SGWayPoint swp(p->longitude(), p->latitude(), alt,
SGWayPoint::WGS84, p->ident(), p->name());
add_waypoint(swp);
}
}
const char* FGRouteMgr::getDepartureICAO() const
{
if (!_departure) {
return "";
}
return _departure->ident().c_str();
}
const char* FGRouteMgr::getDepartureName() const
{
if (!_departure) {
return "";
}
return _departure->name().c_str();
}
void FGRouteMgr::setDepartureICAO(const char* aIdent)
{
_departure = FGAirport::findByIdent(aIdent);
}
const char* FGRouteMgr::getDestinationICAO() const
{
if (!_destination) {
return "";
}
return _destination->ident().c_str();
}
const char* FGRouteMgr::getDestinationName() const
{
if (!_destination) {
return "";
}
return _destination->name().c_str();
}
void FGRouteMgr::setDestinationICAO(const char* aIdent)
{
_destination = FGAirport::findByIdent(aIdent);
}

162
src/Autopilot/route_mgr.hxx
View file

@ -25,9 +25,15 @@
#define _ROUTE_MGR_HXX 1
#include <simgear/props/props.hxx>
#include <simgear/route/route.hxx>
#include <simgear/route/waypoint.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
// forward decls
class SGRoute;
class SGPath;
class FGAirport;
typedef SGSharedPtr<FGAirport> FGAirportRef;
/**
* Top level route manager class
@ -38,61 +44,100 @@ class FGRouteMgr : public SGSubsystem
{
private:
SGRoute *route;
SGRoute* _route;
time_t _takeoffTime;
time_t _touchdownTime;
FGAirportRef _departure;
FGAirportRef _destination;
// automatic inputs
SGPropertyNode_ptr lon;
SGPropertyNode_ptr lat;
SGPropertyNode_ptr alt;
// automatic outputs
SGPropertyNode_ptr true_hdg_deg;
SGPropertyNode_ptr target_altitude_ft;
SGPropertyNode_ptr altitude_lock;
SGPropertyNode_ptr wp0_id;
SGPropertyNode_ptr wp0_dist;
SGPropertyNode_ptr wp0_eta;
SGPropertyNode_ptr wp1_id;
SGPropertyNode_ptr wp1_dist;
SGPropertyNode_ptr wp1_eta;
SGPropertyNode_ptr wpn_id;
SGPropertyNode_ptr wpn_dist;
SGPropertyNode_ptr wpn_eta;
class Listener : public SGPropertyChangeListener {
SGPropertyNode_ptr magvar;
// automatic outputs
SGPropertyNode_ptr departure; ///< departure airport information
SGPropertyNode_ptr destination; ///< destination airport information
SGPropertyNode_ptr alternate; ///< alternate airport information
SGPropertyNode_ptr cruise; ///< cruise information
SGPropertyNode_ptr totalDistance;
SGPropertyNode_ptr ete;
SGPropertyNode_ptr elapsedFlightTime;
SGPropertyNode_ptr active;
SGPropertyNode_ptr airborne;
SGPropertyNode_ptr wp0;
SGPropertyNode_ptr wp1;
SGPropertyNode_ptr wpn;
SGPropertyNode_ptr _pathNode;
SGPropertyNode_ptr _currentWpt;
/**
* Signal property to notify people that the route was edited
*/
SGPropertyNode_ptr _edited;
/**
* Signal property to notify when the last waypoint is reached
*/
SGPropertyNode_ptr _finished;
void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance);
class InputListener : public SGPropertyChangeListener {
public:
Listener(FGRouteMgr *m) : mgr(m) {}
InputListener(FGRouteMgr *m) : mgr(m) {}
virtual void valueChanged (SGPropertyNode * prop);
private:
FGRouteMgr *mgr;
};
SGPropertyNode_ptr input;
Listener *listener;
SGPropertyNode_ptr weightOnWheels;
InputListener *listener;
SGPropertyNode_ptr mirror;
bool altitude_set;
SGWayPoint* make_waypoint(const string& target);
void update_mirror();
bool near_ground();
/**
* Helper to set a string property to the estimated arrival time (ETA),
* formatted as either hours:minutes or minutes:seconds, based on a distance
* and the current groundspeed.
* Create a SGWayPoint from a string in the following format:
* - simple identifier
* - decimal-lon,decimal-lat
* - airport-id/runway-id
* - navaid/radial-deg/offset-nm
*/
void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance);
SGWayPoint* make_waypoint(const string& target);
void update_mirror();
void currentWaypointChanged();
/**
* Helper to update the target_altitude_ft and altitude_set flag when wp0
* changes
* Parse a route/wp node (from a saved, property-lsit formatted route)
*/
void updateTargetAltitude();
void parseRouteWaypoint(SGPropertyNode* aWP);
/**
* Check if we've reached the final waypoint.
* Returns true if we have.
*/
bool checkFinished();
// tied getters and setters
const char* getDepartureICAO() const;
const char* getDepartureName() const;
void setDepartureICAO(const char* aIdent);
const char* getDestinationICAO() const;
const char* getDestinationName() const;
void setDestinationICAO(const char* aIdent);
public:
FGRouteMgr();
@ -110,14 +155,41 @@ public:
void add_waypoint( const SGWayPoint& wp, int n = -1 );
SGWayPoint pop_waypoint( int i = 0 );
SGWayPoint get_waypoint( int i ) const {
return route->get_waypoint(i);
}
int size() const {
return route->size();
}
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 saveRoute();
void loadRoute();
};

66
src/Cockpit/hud.cxx
View file

@ -429,9 +429,9 @@ void drawHUD(osg::State* state)
= fgGetNode("/autopilot/locks/altitude", true);
static char hud_hdg_text[256];
static char hud_wp0_text[256];
static char hud_wp1_text[256];
static char hud_wp2_text[256];
static char hud_gps_text0[256];
static char hud_gps_text1[256];
static char hud_gps_text2[256];
static char hud_alt_text[256];
glEnable(GL_BLEND);
@ -468,32 +468,42 @@ void drawHUD(osg::State* state)
fgGetDouble("/autopilot/settings/true-heading-deg") );
HUD_TextList.add( fgText( 40, apY, hud_hdg_text ) );
apY -= 15;
string wp0_id = fgGetString( "/autopilot/route-manager/wp[0]/id" );
if ( wp0_id.length() > 0 ) {
snprintf( hud_wp0_text, 256, "%5s %6.1fnm %s", wp0_id.c_str(),
fgGetDouble( "/autopilot/route-manager/wp[0]/dist" ),
fgGetString( "/autopilot/route-manager/wp[0]/eta" ) );
HUD_TextList.add( fgText( 40, apY, hud_wp0_text ) );
apY -= 15;
}
string wp1_id = fgGetString( "/autopilot/route-manager/wp[1]/id" );
if ( wp1_id.length() > 0 ) {
snprintf( hud_wp1_text, 256, "%5s %6.1fnm %s", wp1_id.c_str(),
fgGetDouble( "/autopilot/route-manager/wp[1]/dist" ),
fgGetString( "/autopilot/route-manager/wp[1]/eta" ) );
HUD_TextList.add( fgText( 40, apY, hud_wp1_text ) );
apY -= 15;
}
string wp2_id = fgGetString( "/autopilot/route-manager/wp-last/id" );
if ( wp2_id.length() > 0 ) {
snprintf( hud_wp2_text, 256, "%5s %6.1fnm %s", wp2_id.c_str(),
fgGetDouble( "/autopilot/route-manager/wp-last/dist" ),
fgGetString( "/autopilot/route-manager/wp-last/eta" ) );
HUD_TextList.add( fgText( 40, apY, hud_wp2_text ) );
apY -= 15;
}
}
// GPS current waypoint information
SGPropertyNode_ptr gps = fgGetNode("/instrumentation/gps", true);
SGPropertyNode_ptr curWp = gps->getChild("wp")->getChild("wp",1);
if ((gps->getDoubleValue("raim") > 0.5) && curWp) {
// GPS is receiving a valid signal
snprintf(hud_gps_text0, 256, "WPT:%5s BRG:%03.0f %5.1fnm",
curWp->getStringValue("ID"),
curWp->getDoubleValue("bearing-mag-deg"),
curWp->getDoubleValue("distance-nm"));
HUD_TextList.add( fgText( 40, apY, hud_gps_text0 ) );
apY -= 15;
// curWp->getStringValue("TTW")
snprintf(hud_gps_text2, 256, "ETA %s", curWp->getStringValue("TTW"));
HUD_TextList.add( fgText( 40, apY, hud_gps_text2 ) );
apY -= 15;
double courseError = curWp->getDoubleValue("course-error-nm");
if (fabs(courseError) >= 0.01) {
// generate an arrow indicatinng if the pilot should turn left or right
char dir = (courseError < 0.0) ? '<' : '>';
snprintf(hud_gps_text1, 256, "GPS TRK:%03.0f XTRK:%c%4.2fnm",
gps->getDoubleValue("indicated-track-magnetic-deg"), dir, fabs(courseError));
} else { // on course, don't bother showing the XTRK error
snprintf(hud_gps_text1, 256, "GPS TRK:%03.0f",
gps->getDoubleValue("indicated-track-magnetic-deg"));
}
HUD_TextList.add( fgText( 40, apY, hud_gps_text1) );
apY -= 15;
} // of valid GPS output
////////////////////
if ( strcmp( altitude_enabled->getStringValue(), "altitude-hold" ) == 0 ) {
snprintf( hud_alt_text, 256, "alt = %.0f\n",

9
src/Cockpit/panel_io.cxx
View file

@ -260,7 +260,7 @@ readTransformation (const SGPropertyNode * node, float w_scale, float h_scale)
SGPropertyNode * target = 0;
if (type.empty()) {
SG_LOG( SG_COCKPIT, SG_INFO,
SG_LOG( SG_COCKPIT, SG_BULK,
"No type supplied for transformation " << name
<< " assuming \"rotation\"" );
type = "rotation";
@ -282,7 +282,7 @@ readTransformation (const SGPropertyNode * node, float w_scale, float h_scale)
// Check for an interpolation table
const SGPropertyNode * trans_table = node->getNode("interpolation");
if (trans_table != 0) {
SG_LOG( SG_COCKPIT, SG_INFO, "Found interpolation table with "
SG_LOG( SG_COCKPIT, SG_DEBUG, "Found interpolation table with "
<< trans_table->nChildren() << " children" );
t->table = new SGInterpTable(trans_table);
} else {
@ -430,7 +430,7 @@ readLayer (const SGPropertyNode * node, float w_scale, float h_scale)
if (type.empty()) {
SG_LOG( SG_COCKPIT, SG_INFO,
SG_LOG( SG_COCKPIT, SG_BULK,
"No type supplied for layer " << name
<< " assuming \"texture\"" );
type = "texture";
@ -641,7 +641,6 @@ readInstrument (const SGPropertyNode * node)
}
readConditions(instrument, node);
SG_LOG( SG_COCKPIT, SG_DEBUG, "Done reading instrument " << name );
return instrument;
}
@ -784,7 +783,7 @@ readPanel (const SGPropertyNode * root)
h = real_h;
}
SG_LOG( SG_COCKPIT, SG_DEBUG, "Reading instrument " << name );
SG_LOG( SG_COCKPIT, SG_BULK, "Reading instrument " << name );
// Warning - hardwired size!!!
RenderArea2D* instrument = new RenderArea2D(158, 40, 158, 40, x, y);

4
src/Environment/environment_ctrl.cxx
View file

@ -45,6 +45,10 @@ public:
virtual bool passAirport(FGAirport* aApt) const {
return aApt->getMetar();
}
// permit heliports and seaports too
virtual FGPositioned::Type maxType() const
{ return FGPositioned::SEAPORT; }
};
static AirportWithMetar airportWithMetarFilter;

2044
src/Instrumentation/gps.cxx
View file

File diff suppressed because it is too large Load diff

407
src/Instrumentation/gps.hxx
View file

@ -11,8 +11,13 @@
#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/math/SGMath.hxx>
#include "Navaids/positioned.hxx"
// forward decls
class SGRoute;
class FGRouteMgr;
class FGAirport;
class GPSListener;
class SGGeodProperty
{
@ -42,15 +47,6 @@ private:
* /systems/electrical/outputs/gps
* /instrumentation/gps/serviceable
*
* /instrumentation/gps/wp-longitude-deg
* /instrumentation/gps/wp-latitude-deg
* /instrumentation/gps/wp-altitude-ft
* /instrumentation/gps/wp-ID
* /instrumentation/gps/wp-name
* /instrumentation/gps/desired-course-deg
* /instrumentation/gps/get-nearest-airport
* /instrumentation/gps/waypoint-type
* /instrumentation/gps/tracking-bug
*
* Output properties:
*
@ -73,8 +69,7 @@ private:
* /instrumentation/gps/trip-odometer
* /instrumentation/gps/true-bug-error-deg
* /instrumentation/gps/magnetic-bug-error-deg
* /instrumentation/gps/true-bearing-error-deg
* /instrumentation/gps/magnetic-bearing-error-deg
*/
class GPS : public SGSubsystem
{
@ -89,117 +84,319 @@ public:
virtual void update (double delta_time_sec);
private:
typedef struct {
double dt;
SGGeod pos;
SGGeod wp0_pos;
SGGeod wp1_pos;
bool waypoint_changed;
double speed_kt;
double track1_deg;
double track2_deg;
double magvar_deg;
double wp0_distance;
double wp0_course_deg;
double wp0_bearing_deg;
double wp1_distance;
double wp1_course_deg;
double wp1_bearing_deg;
} UpdateContext;
friend class GPSListener;
friend class SearchFilter;
/**
* Configuration manager, track data relating to aircraft installation
*/
class Config
{
public:
Config();
void init(SGPropertyNode*);
bool turnAnticipationEnabled() const
{ return _enableTurnAnticipation; }
/**
* Desired turn rate in degrees/second. From this we derive the turn
* radius and hence how early we need to anticipate it.
*/
double turnRateDegSec() const
{ return _turnRate; }
/**
* Distance at which we arm overflight sequencing. Once inside this
* distance, a change of the wp1 'TO' flag to false will be considered
* overlight of the wp.
*/
double overflightArmDistanceNm() const
{ return _overflightArmDistance; }
/**
* Time before the next WP to activate an external annunciator
*/
double waypointAlertTime() const
{ return _waypointAlertTime; }
bool tuneNavRadioToRefVor() const
{ return _tuneRadio1ToRefVor; }
bool requireHardSurface() const
{ return _requireHardSurface; }
double minRunwayLengthFt() const
{ return _minRunwayLengthFt; }
double getExternalCourse() const;
void setExternalCourse(double aCourseDeg);
bool cdiDeflectionIsAngular() const
{ return (_cdiMaxDeflectionNm <= 0.0); }
double cdiDeflectionLinearPeg() const
{
assert(_cdiMaxDeflectionNm > 0.0);
return _cdiMaxDeflectionNm;
}
bool driveAutopilot() const
{ return _driveAutopilot; }
private:
bool _enableTurnAnticipation;
// desired turn rate in degrees per second
double _turnRate;
// distance from waypoint to arm overflight sequencing (in nm)
double _overflightArmDistance;
// time before reaching a waypoint to trigger annunicator light/sound
// (in seconds)
double _waypointAlertTime;
// should GPS automatically tune NAV1 to the reference VOR?
bool _tuneRadio1ToRefVor;
// minimum runway length to require when filtering
double _minRunwayLengthFt;
// should we require a hard-surfaced runway when filtering?
bool _requireHardSurface;
// helpers to tie course-source property
const char* getCourseSource() const;
void setCourseSource(const char* aPropPath);
// property to retrieve the external course from
SGPropertyNode_ptr _extCourseSource;
double _cdiMaxDeflectionNm;
bool _driveAutopilot;
};
class SearchFilter : public FGPositioned::Filter
{
public:
virtual bool pass(FGPositioned* aPos) const;
virtual FGPositioned::Type minType() const;
virtual FGPositioned::Type maxType() const;
};
void search (double frequency, const SGGeod& pos);
/**
* reset all output properties to default / non-service values
*/
void clearOutput();
void updateWithValid(UpdateContext& ctx);
void updateBasicData(double dt);
void updateWaypoints();
void updateTrackingBug();
void updateReferenceNavaid(double dt);
void referenceNavaidSet(const std::string& aNavaid);
void tuneNavRadios();
void updateRouteData();
void driveAutopilot();
void updateNearestAirport(UpdateContext& ctx);
void updateWaypoint0(UpdateContext& ctx);
void updateWaypoint1(UpdateContext& ctx);
void updateLegCourse(UpdateContext& ctx);
void updateWaypoint0Course(UpdateContext& ctx);
void updateWaypoint1Course(UpdateContext& ctx);
void waypointChanged(UpdateContext& ctx);
void updateTTWNode(UpdateContext& ctx, double distance_m, SGPropertyNode_ptr node);
void updateTrackingBug(UpdateContext& ctx);
void routeActivated();
void routeManagerSequenced();
void routeEdited();
void routeFinished();
SGPropertyNode_ptr _magvar_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _electrical_node;
SGPropertyNode_ptr _wp0_ID_node;
SGPropertyNode_ptr _wp0_name_node;
SGPropertyNode_ptr _wp0_course_node;
SGPropertyNode_ptr _get_nearest_airport_node;
SGPropertyNode_ptr _wp1_ID_node;
SGPropertyNode_ptr _wp1_name_node;
SGPropertyNode_ptr _wp1_course_node;
SGPropertyNode_ptr _tracking_bug_node;
void updateTurn();
void updateOverflight();
void beginTurn();
void endTurn();
double computeTurnProgress(double aBearing) const;
void computeTurnData();
void updateTurnData();
double computeTurnRadiusNm(double aGroundSpeedKts) const;
/**
* Update one-shot things when WP1 / leg data change
*/
void wp1Changed();
// scratch maintenence utilities
void setScratchFromPositioned(FGPositioned* aPos, int aIndex);
void setScratchFromCachedSearchResult();
void setScratchFromRouteWaypoint(int aIndex);
/**
* Add airport-specific information to a scratch result
*/
void addAirportToScratch(FGAirport* aAirport);
void clearScratch();
/**
* Predicate, determine if the lon/lat position in the scratch is
* valid or not.
*/
bool isScratchPositionValid() const;
FGPositioned::Filter* createFilter(FGPositioned::Type aTy);
/**
* Search kernel - called each time we step through a result
*/
void performSearch();
// command handlers
void selectLegMode();
void selectOBSMode();
void directTo();
void loadRouteWaypoint();
void loadNearest();
void search();
void nextResult();
void previousResult();
void defineWaypoint();
void insertWaypointAtIndex(int aIndex);
void removeWaypointAtIndex(int aIndex);
// tied-property getter/setters
void setCommand(const char* aCmd);
const char* getCommand() const { return ""; }
const char* getMode() const { return _mode.c_str(); }
bool getScratchValid() const { return _scratchValid; }
double getScratchDistance() const;
double getScratchMagBearing() const;
double getScratchTrueBearing() const;
bool getScratchHasNext() const { return _searchHasNext; }
double getSelectedCourse() const { return _selectedCourse; }
double getCDIDeflection() const;
double getLegDistance() const;
double getLegCourse() const;
double getLegMagCourse() const;
double getAltDistanceRatio() const;
double getTrueTrack() const { return _last_true_track; }
double getMagTrack() const;
double getGroundspeedKts() const { return _last_speed_kts; }
double getVerticalSpeed() const { return _last_vertical_speed; }
//bool getLegMode() const { return _mode == "leg"; }
//bool getObsMode() const { return _mode == "obs"; }
const char* getWP0Ident() const;
const char* getWP0Name() const;
const char* getWP1Ident() const;
const char* getWP1Name() const;
double getWP1Distance() const;
double getWP1TTW() const;
const char* getWP1TTWString() const;
double getWP1Bearing() const;
double getWP1MagBearing() const;
double getWP1CourseDeviation() const;
double getWP1CourseErrorNm() const;
bool getWP1ToFlag() const;
bool getWP1FromFlag() const;
// true-bearing-error and mag-bearing-error
// members
SGPropertyNode_ptr _magvar_node;
SGPropertyNode_ptr _serviceable_node;
SGPropertyNode_ptr _electrical_node;
SGPropertyNode_ptr _tracking_bug_node;
SGPropertyNode_ptr _raim_node;
SGPropertyNode_ptr _raim_node;
SGPropertyNode_ptr _indicated_vertical_speed_node;
SGPropertyNode_ptr _true_track_node;
SGPropertyNode_ptr _magnetic_track_node;
SGPropertyNode_ptr _speed_node;
SGPropertyNode_ptr _wp0_distance_node;
SGPropertyNode_ptr _wp0_ttw_node;
SGPropertyNode_ptr _wp0_bearing_node;
SGPropertyNode_ptr _wp0_mag_bearing_node;
SGPropertyNode_ptr _wp0_course_deviation_node;
SGPropertyNode_ptr _wp0_course_error_nm_node;
SGPropertyNode_ptr _wp0_to_flag_node;
SGPropertyNode_ptr _wp1_distance_node;
SGPropertyNode_ptr _wp1_ttw_node;
SGPropertyNode_ptr _wp1_bearing_node;
SGPropertyNode_ptr _wp1_mag_bearing_node;
SGPropertyNode_ptr _wp1_course_deviation_node;
SGPropertyNode_ptr _wp1_course_error_nm_node;
SGPropertyNode_ptr _wp1_to_flag_node;
SGPropertyNode_ptr _odometer_node;
SGPropertyNode_ptr _odometer_node;
SGPropertyNode_ptr _trip_odometer_node;
SGPropertyNode_ptr _true_bug_error_node;
SGPropertyNode_ptr _magnetic_bug_error_node;
SGPropertyNode_ptr _true_wp0_bearing_error_node;
SGPropertyNode_ptr _magnetic_wp0_bearing_error_node;
SGPropertyNode_ptr _true_wp1_bearing_error_node;
SGPropertyNode_ptr _magnetic_wp1_bearing_error_node;
SGPropertyNode_ptr _leg_distance_node;
SGPropertyNode_ptr _leg_course_node;
SGPropertyNode_ptr _leg_magnetic_course_node;
SGPropertyNode_ptr _alt_dist_ratio_node;
SGPropertyNode_ptr _leg_course_deviation_node;
SGPropertyNode_ptr _leg_course_error_nm_node;
SGPropertyNode_ptr _leg_to_flag_node;
SGPropertyNode_ptr _alt_deviation_node;
SGPropertyNode_ptr _ref_navaid_id_node;
SGPropertyNode_ptr _ref_navaid_bearing_node;
SGPropertyNode_ptr _ref_navaid_distance_node;
SGPropertyNode_ptr _ref_navaid_mag_bearing_node;
SGPropertyNode_ptr _ref_navaid_frequency_node;
SGPropertyNode_ptr _ref_navaid_name_node;
SGPropertyNode_ptr _route_active_node;
SGPropertyNode_ptr _route_current_wp_node;
SGPropertyNode_ptr _routeDistanceNm;
SGPropertyNode_ptr _routeETE;
SGPropertyNode_ptr _routeEditedSignal;
SGPropertyNode_ptr _routeFinishedSignal;
bool _last_valid;
double _selectedCourse;
bool _dataValid;
SGGeod _last_pos;
bool _lastPosValid;
double _last_speed_kts;
std::string _last_wp0_ID;
std::string _last_wp1_ID;
double _alt_dist_ratio;
double _distance_m;
double _course_deg;
double _bias_length;
double _bias_angle;
double _azimuth_error;
double _range_error;
double _elapsed_time;
double _last_true_track;
double _last_vertical_speed;
std::string _mode;
GPSListener* _listener;
Config _config;
FGRouteMgr* _routeMgr;
bool _ref_navaid_set;
double _ref_navaid_elapsed;
FGPositionedRef _ref_navaid;
std::string _name;
int _num;
SGGeodProperty _position;
SGGeodProperty _wp0_position;
SGGeodProperty _wp1_position;
SGGeodProperty _indicated_pos;
SGGeodProperty _position;
SGGeod _wp0_position;
SGGeod _wp1_position;
SGGeod _indicated_pos;
std::string _wp0Ident, _wp0Name, _wp1Ident, _wp1Name;
double _wp1DistanceM, _wp1TrueBearing;
// scratch data
SGGeod _scratchPos;
SGPropertyNode_ptr _scratchNode;
bool _scratchValid;
// search data
int _searchResultIndex;
std::string _searchQuery;
FGPositioned::Type _searchType;
bool _searchExact;
bool _searchOrderByRange;
bool _searchResultsCached;
FGPositioned::List _searchResults;
bool _searchIsRoute; ///< set if 'search' is actually the current route
bool _searchHasNext; ///< is there a result after this one?
bool _searchNames; ///< set if we're searching names instead of idents
// turn data
bool _computeTurnData; ///< do we need to update the turn data?
bool _anticipateTurn; ///< are we anticipating the next turn or not?
bool _inTurn; // is a turn in progress?
bool _turnSequenced; // have we sequenced the new leg?
double _turnAngle; // angle to turn through, in degrees
double _turnStartBearing; // bearing of inbound leg
double _turnRadius; // radius of turn in nm
SGGeod _turnPt;
SGGeod _turnCentre;
SGPropertyNode_ptr _realismSimpleGps; ///< should the GPS be simple or realistic?
// autopilot drive properties
SGPropertyNode_ptr _apTrueHeading;
SGPropertyNode_ptr _apTargetAltitudeFt;
SGPropertyNode_ptr _apAltitudeLock;
};

15
src/Instrumentation/instrument_mgr.cxx
View file

@ -51,7 +51,8 @@
#include "agradar.hxx"
#include "rad_alt.hxx"
FGInstrumentMgr::FGInstrumentMgr ()
FGInstrumentMgr::FGInstrumentMgr () :
_explicitGps(false)
{
set_subsystem("od_gauge", new FGODGauge);
set_subsystem("hud", new HUD);
@ -85,6 +86,14 @@ FGInstrumentMgr::FGInstrumentMgr ()
}
delete config_props;
if (!_explicitGps) {
SG_LOG(SG_INSTR, SG_INFO, "creating default GPS instrument");
SGPropertyNode_ptr nd(new SGPropertyNode);
nd->setStringValue("name", "gps");
nd->setIntValue("number", 0);
set_subsystem("gps[0]", new GPS(nd));
}
}
FGInstrumentMgr::~FGInstrumentMgr ()
@ -128,8 +137,8 @@ bool FGInstrumentMgr::build ()
set_subsystem( id, new Altimeter( node ) );
} else if ( name == "gps" ) {
set_subsystem( id, new GPS( node ), 0.45 );
set_subsystem( id, new GPS( node ) );
_explicitGps = true;
} else if ( name == "gsdi" ) {
set_subsystem( id, new GSDI( node ) );

1
src/Instrumentation/instrument_mgr.hxx
View file

@ -35,6 +35,7 @@ public:
private:
SGPropertyNode *config_props;
bool _explicitGps;
};
#endif // __INSTRUMENT_MGR_HXX

4
src/Instrumentation/mk_viii.hxx
View file

@ -1594,10 +1594,6 @@ private:
: mk(device) {}
virtual bool passAirport(FGAirport *a) const;
virtual FGPositioned::Type maxType() const {
return FGPositioned::AIRPORT;
}
private:
MK_VIII* mk;
};

24
src/Instrumentation/navradio.cxx
View file

@ -143,6 +143,7 @@ FGNavRadio::FGNavRadio(SGPropertyNode *node) :
gps_to_flag_node(NULL),
gps_from_flag_node(NULL),
gps_has_gs_node(NULL),
gps_xtrack_error_nm_node(NULL),
play_count(0),
last_time(0),
target_radial(0.0),
@ -265,6 +266,9 @@ FGNavRadio::init ()
gps_to_flag_node = fgGetNode("/instrumentation/gps/to-flag", true);
gps_from_flag_node = fgGetNode("/instrumentation/gps/from-flag", true);
gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true);
gps_course_node = fgGetNode("/instrumentation/gps/selected-course-deg", true);
gps_xtrack_error_nm_node = fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
_magvarNode = fgGetNode("/environment/magnetic-variation-deg", true);
std::ostringstream temp;
temp << _name << "nav-ident" << _num;
@ -652,14 +656,30 @@ void FGNavRadio::updateGPSSlaved()
_toFlag = gps_to_flag_node->getBoolValue();
_fromFlag = gps_from_flag_node->getBoolValue();
inrange_node->setBoolValue(_toFlag | _fromFlag);
bool gpsValid = (_toFlag | _fromFlag);
inrange_node->setBoolValue(gpsValid);
if (!gpsValid) {
signal_quality_norm_node->setDoubleValue(0.0);
_cdiDeflection = 0.0;
_cdiCrossTrackErrorM = 0.0;
_gsNeedleDeflection = 0.0;
return;
}
// this is unfortunate, but panel instruments use this value to decide
// if the navradio output is valid.
signal_quality_norm_node->setDoubleValue(1.0);
_cdiDeflection = gps_cdi_deflection_node->getDoubleValue();
// clmap to some range (+/- 10 degrees) as the regular deflection
SG_CLAMP_RANGE(_cdiDeflection, -10.0, 10.0 );
_cdiCrossTrackErrorM = 0.0; // FIXME, supply this
_cdiCrossTrackErrorM = gps_xtrack_error_nm_node->getDoubleValue() * SG_NM_TO_METER;
_gsNeedleDeflection = 0.0; // FIXME, supply this
double trtrue = gps_course_node->getDoubleValue() + _magvarNode->getDoubleValue();
SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0);
target_radial_true_node->setDoubleValue( trtrue );
}
void FGNavRadio::updateCDI(double dt)

5
src/Instrumentation/navradio.hxx
View file

@ -115,7 +115,10 @@ class FGNavRadio : public SGSubsystem
SGPropertyNode_ptr gps_to_flag_node;
SGPropertyNode_ptr gps_from_flag_node;
SGPropertyNode_ptr gps_has_gs_node;
SGPropertyNode_ptr gps_course_node;
SGPropertyNode_ptr gps_xtrack_error_nm_node;
SGPropertyNode_ptr _magvarNode;
// internal (private) values
int play_count;

212
src/Instrumentation/testgps.cxx Normal file
View file

@ -0,0 +1,212 @@
#include <Main/fg_init.hxx>
#include <Main/globals.hxx>
#include <Main/fg_props.hxx>
#include <Instrumentation/gps.hxx>
#include <Autopilot/route_mgr.hxx>
#include <Environment/environment_mgr.hxx>
using std::string;
char *homedir = ::getenv( "HOME" );
char *hostname = ::getenv( "HOSTNAME" );
bool free_hostname = false;
void testSetPosition(const SGGeod& aPos)
{
fgSetDouble("/position/longitude-deg", aPos.getLongitudeDeg());
fgSetDouble("/position/latitude-deg", aPos.getLatitudeDeg());
fgSetDouble("/position/altitude-ft", aPos.getElevationFt());
}
void printScratch(SGPropertyNode* scratch)
{
if (!scratch->getBoolValue("valid", false)) {
SG_LOG(SG_GENERAL, SG_ALERT, "Scratch is invalid.");
return;
}
SG_LOG(SG_GENERAL, SG_ALERT, "Scratch:" <<
scratch->getStringValue("ident") << "/" << scratch->getStringValue("name"));
SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("longitude-deg")
<< " " << scratch->getDoubleValue("latitude-deg") << " @ " << scratch->getDoubleValue("altitude-ft"));
SG_LOG(SG_GENERAL, SG_ALERT, "\t" << scratch->getDoubleValue("true-bearing-deg") <<
" (" << scratch->getDoubleValue("mag-bearing-deg") << " magnetic) " << scratch->getDoubleValue("distance-nm"));
if (scratch->hasChild("result-index")) {
SG_LOG(SG_GENERAL, SG_ALERT, "\tresult-index:" << scratch->getIntValue("result-index"));
}
if (scratch->hasChild("route-index")) {
SG_LOG(SG_GENERAL, SG_ALERT, "\troute-index:" << scratch->getIntValue("route-index"));
}
}
void createDummyRoute(FGRouteMgr* rm)
{
SGPropertyNode* rmInput = fgGetNode("/autopilot/route-manager/input", true);
rmInput->setStringValue("UW");
rmInput->setStringValue("TLA/347/13");
rmInput->setStringValue("TLA");
rmInput->setStringValue("HAVEN");
rmInput->setStringValue("NEW/305/29");
rmInput->setStringValue("NEW");
rmInput->setStringValue("OTR");
}
int main(int argc, char* argv[])
{
globals = new FGGlobals;
fgInitFGRoot(argc, argv);
if (!fgInitConfig(argc, argv) ) {
SG_LOG( SG_GENERAL, SG_ALERT, "Config option parsing failed" );
exit(-1);
}
fgInitNav();
SG_LOG(SG_GENERAL, SG_ALERT, "hello world!");
FGRouteMgr* rm = new FGRouteMgr;
globals->add_subsystem( "route-manager", rm );
// FGEnvironmentMgr* envMgr = new FGEnvironmentMgr;
// globals->add_subsystem("environment", envMgr);
// envMgr->init();
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
rm->init();
nd->setBoolValue("serviceable", true);
fgSetBool("/systems/electrical/outputs/gps", true);
gps->init();
SGPropertyNode* scratch = nd->getChild("scratch", 0, true);
SGPropertyNode* wp = nd->getChild("wp", 0, true);
SGPropertyNode* wp1 = wp->getChild("wp", 1, true);
// update a few times
gps->update(0.05);
gps->update(0.05);
scratch->setStringValue("query", "TL");
scratch->setStringValue("type", "Vor");
scratch->setBoolValue("exact", false);
nd->setStringValue("command", "search");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
// alphanumeric sort, partial matching
nd->setDoubleValue("config/min-runway-length-ft", 5000.0);
scratch->setBoolValue("exact", false);
scratch->setBoolValue("order-by-distance", false);
scratch->setStringValue("query", "KS");
scratch->setStringValue("type", "apt");
nd->setStringValue("command", "search");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
// alphanumeric sort, explicit matching
scratch->setBoolValue("exact", true);
scratch->setBoolValue("order-by-distance", true);
scratch->setStringValue("type", "vor");
scratch->setStringValue("query", "DCS");
nd->setStringValue("command", "search");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
// search on totally missing
scratch->setBoolValue("exact", true);
scratch->setBoolValue("order-by-distance", true);
scratch->setStringValue("query", "FOFOFOFOF");
nd->setStringValue("command", "search");
printScratch(scratch);
// nearest
scratch->setStringValue("type", "apt");
scratch->setIntValue("max-results", 10);
nd->setStringValue("command", "nearest");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
// direct to
nd->setStringValue("command", "direct");
SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
<< wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
<< " " << wp1->getDoubleValue("latitude-deg"));
// OBS mode
scratch->setStringValue("query", "UW");
scratch->setBoolValue("order-by-distance", true);
nd->setStringValue("command", "search");
printScratch(scratch);
nd->setStringValue("command", "obs");
SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
<< wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
<< " " << wp1->getDoubleValue("latitude-deg"));
// load route waypoints
createDummyRoute(rm);
scratch->setIntValue("route-index", 5);
nd->setStringValue("command", "load-route-wpt");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
nd->setStringValue("command", "next");
printScratch(scratch);
scratch->setIntValue("route-index", 2);
nd->setStringValue("command", "load-route-wpt");
nd->setStringValue("command", "direct");
SG_LOG(SG_GENERAL, SG_ALERT, "mode:" << nd->getStringValue("mode") << "\n\t"
<< wp1->getStringValue("ID") << " " << wp1->getDoubleValue("longitude-deg")
<< " " << wp1->getDoubleValue("latitude-deg"));
// route editing
SGGeod pos = egph->geod();
scratch->setStringValue("ident", "FOOBAR");
scratch->setDoubleValue("longitude-deg", pos.getLongitudeDeg());
scratch->setDoubleValue("latitude-deg", pos.getLatitudeDeg());
nd->setStringValue("command", "define-user-wpt");
printScratch(scratch);
return EXIT_SUCCESS;
}

49
src/Navaids/awynet.cxx
View file

@ -32,6 +32,7 @@
#include <simgear/debug/logstream.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/route/waypoint.hxx>
#include <simgear/misc/sg_path.hxx>
#include "awynet.hxx"
@ -47,18 +48,19 @@ FGNode::FGNode()
{
}
FGNode::FGNode(double lt, double ln, int idx, std::string id) :
FGNode::FGNode(const SGGeod& aPos, int idx, std::string id) :
ident(id),
geod(SGGeod::fromDeg(ln, lt)),
geod(aPos),
index(idx)
{
cart = SGVec3d::fromGeod(geod);
}
bool FGNode::matches(string id, double lt, double ln)
bool FGNode::matches(std::string id, const SGGeod& aPos)
{
if ((ident == id) &&
(fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
(fabs(ln - geod.getLongitudeDeg()) < 1.0))
(fabs(aPos.getLatitudeDeg() - geod.getLatitudeDeg()) < 1.0) &&
(fabs(aPos.getLongitudeDeg() - geod.getLongitudeDeg()) < 1.0))
return true;
else
return false;
@ -195,9 +197,9 @@ void FGAirwayNetwork::init()
}
void FGAirwayNetwork::load(SGPath path)
void FGAirwayNetwork::load(const SGPath& path)
{
string identStart, identEnd, token, name;
std::string identStart, identEnd, token, name;
double latStart, lonStart, latEnd, lonEnd;
int type, base, top;
int airwayIndex = 0;
@ -272,7 +274,8 @@ void FGAirwayNetwork::load(SGPath path)
node_map_iterator itr = nodesMap.find(string(buffer));
if (itr == nodesMap.end()) {
startIndex = nodes.size();
n = new FGNode(latStart, lonStart, startIndex, identStart);
SGGeod startPos(SGGeod::fromDeg(lonStart, latStart));
n = new FGNode(startPos, startIndex, identStart);
nodesMap[string(buffer)] = n;
nodes.push_back(n);
//cout << "Adding node: " << identStart << endl;
@ -287,7 +290,8 @@ void FGAirwayNetwork::load(SGPath path)
itr = nodesMap.find(string(buffer));
if (itr == nodesMap.end()) {
endIndex = nodes.size();
n = new FGNode(latEnd, lonEnd, endIndex, identEnd);
SGGeod endPos(SGGeod::fromDeg(lonEnd, latEnd));
n = new FGNode(endPos, endIndex, identEnd);
nodesMap[string(buffer)] = n;
nodes.push_back(n);
//cout << "Adding node: " << identEnd << endl;
@ -310,32 +314,23 @@ void FGAirwayNetwork::load(SGPath path)
}
}
int FGAirwayNetwork::findNearestNode(double lat, double lon)
int FGAirwayNetwork::findNearestNode(const SGGeod& aPos)
{
double minDist = HUGE_VAL;
double distsqrt, lat2, lon2;
int index;
SGVec3d cart = SGVec3d::fromGeod(aPos);
//cerr << "Lat " << lat << " lon " << lon << endl;
for (FGNodeVectorIterator
itr = nodes.begin();
itr != nodes.end(); itr++)
{
lat2 = (*itr)->getLatitude();
lon2 = (*itr)->getLongitude();
// Note: This equation should adjust for decreasing distance per longitude
// with increasing lat.
distsqrt =
(lat-lat2)*(lat-lat2) +
(lon-lon2)*(lon-lon2);
if (distsqrt < minDist)
{
minDist = distsqrt;
//cerr << "Test" << endl;
index = (*itr)->getIndex();
//cerr << "Minimum distance of " << minDist << " for index " << index << endl;
//cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl;
}
double d2 = distSqr(cart, (*itr)->getCart());
if (d2 < minDist)
{
minDist = d2;
index = (*itr)->getIndex();
}
//cerr << (*itr)->getIndex() << endl;
}
//cerr << " returning " << index << endl;

17
src/Navaids/awynet.hxx
View file

@ -30,13 +30,14 @@
#include <map>
#include <vector>
#include <simgear/misc/sg_path.hxx>
#include <simgear/misc/sgstream.hxx>
//#include "parking.hxx"
class FGAirway; // forward reference
class SGPath;
class SGGeod;
typedef std::vector<FGAirway> FGAirwayVector;
typedef std::vector<FGAirway *> FGAirwayPointerVector;
@ -51,28 +52,26 @@ class FGNode
private:
std::string ident;
SGGeod geod;
SGVec3d cart; // cached cartesian position
int index;
FGAirwayPointerVector next; // a vector to all the segments leaving from this node
public:
FGNode();
FGNode(double lt, double ln, int idx, std::string id);
FGNode(const SGGeod& aPos, int idx, std::string id);
void setIndex(int idx) { index = idx;};
void addAirway(FGAirway *segment) { next.push_back(segment); };
double getLatitude() { return geod.getLatitudeDeg();};
double getLongitude(){ return geod.getLongitudeDeg();};
const SGGeod& getPosition() {return geod;}
const SGVec3d& getCart() { return cart; }
int getIndex() { return index; };
std::string getIdent() { return ident; };
FGNode *getAddress() { return this;};
FGAirwayPointerVectorIterator getBeginRoute() { return next.begin(); };
FGAirwayPointerVectorIterator getEndRoute() { return next.end(); };
bool matches(std::string ident, double lat, double lon);
bool matches(std::string ident, const SGGeod& aPos);
};
typedef std::vector<FGNode *> FGNodeVector;
@ -191,12 +190,12 @@ public:
void init();
bool exists() { return hasNetwork; };
int findNearestNode(double lat, double lon);
int findNearestNode(const SGGeod& aPos);
FGNode *findNode(int idx);
FGAirRoute findShortestRoute(int start, int end);
void trace(FGNode *, int, int, double dist);
void load(SGPath path);
void load(const SGPath& path);
};
#endif

16
src/Navaids/positioned.cxx
View file

@ -30,10 +30,12 @@
#include <iostream>
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/timing/timestamp.hxx>
#include <simgear/debug/logstream.hxx>
#include <simgear/structure/exception.hxx>
#include "positioned.hxx"
@ -239,6 +241,10 @@ public:
bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const
{
if (!a || !b) {
throw sg_exception("empty reference passed to DistanceOrdering");
}
double dA = distSqr(a->cart(), mPos),
dB = distSqr(b->cart(), mPos);
return dA < dB;
@ -688,14 +694,16 @@ FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm
FGPositionedRef
FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
{
std::string id(boost::to_upper_copy(aId));
// It is essential to bound our search, to avoid iterating all the way to the end of the database.
// Do this by generating a second ID with the final character incremented by 1.
// e.g., if the partial ID is "KI", we wish to search "KIxxx" but not "KJ".
std::string upperBoundId = aId;
std::string upperBoundId = id;
upperBoundId[upperBoundId.size()-1]++;
NamedPositionedIndex::const_iterator upperBound = global_identIndex.lower_bound(upperBoundId);
NamedIndexRange range = global_identIndex.equal_range(aId);
NamedIndexRange range = global_identIndex.equal_range(id);
while (range.first != upperBound) {
for (; range.first != range.second; ++range.first) {
FGPositionedRef candidate = range.first->second;
@ -758,7 +766,7 @@ public:
return false;
}
return (::strncmp(aPos->ident().c_str(), _ident.c_str(), _ident.size()) == 0);
return (boost::algorithm::starts_with(aPos->ident(), _ident));
}
virtual FGPositioned::Type minType() const
@ -817,7 +825,7 @@ public:
return false;
}
return (::strncasecmp(aPos->name().c_str(), _name.c_str(), _name.size()) == 0);
return (boost::algorithm::istarts_with(aPos->name(), _name));
}
virtual FGPositioned::Type minType() const

4
src/Scripting/NasalSys.cxx
View file

@ -555,7 +555,9 @@ static naRef f_airportinfo(naContext c, naRef me, int argc, naRef* args)
// user provided an <id>, hopefully
apt = FGAirport::findByIdent(s);
if (!apt) {
naRuntimeError(c, "airportinfo() couldn't find airport: %s", s);
// return nil here, but don't raise a runtime error; this is a
// legitamate way to validate an ICAO code, for example in a
// dialog box or similar.
return naNil();
}
}