fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/Instrumentation/gps.cxx
James Turner 57cb0a809b Fix bug 150 (airports not found in GPS search) 
Overhaul and simplify the GPS search logic based
on experience and testing with the API. Also fix
search-by-name, which was broken.
2010-08-14 19:16:28 +01:00

1729 lines
49 KiB
C++
Raw Blame History

// gps.cxx - distance-measuring equipment.
// Written by David Megginson, started 2003.
//
// This file is in the Public Domain and comes with no warranty.
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include "gps.hxx"
#include <memory>
#include <set>
#include <cstring>
#include "Main/fg_props.hxx"
#include "Main/globals.hxx" // for get_subsystem
#include "Main/util.hxx" // for fgLowPass
#include "Navaids/positioned.hxx"
#include "Navaids/navrecord.hxx"
#include "Airports/simple.hxx"
#include "Airports/runways.hxx"
#include "Autopilot/route_mgr.hxx"
#include <simgear/math/sg_random.h>
#include <simgear/sg_inlines.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/structure/exception.hxx>
using std::auto_ptr;
using std::string;
///////////////////////////////////////////////////////////////////
void SGGeodProperty::init(SGPropertyNode* base, const char* lonStr, const char* latStr, const char* altStr)
{
_lon = base->getChild(lonStr, 0, true);
_lat = base->getChild(latStr, 0, true);
if (altStr) {
_alt = base->getChild(altStr, 0, true);
}
}
void SGGeodProperty::init(const char* lonStr, const char* latStr, const char* altStr)
{
_lon = fgGetNode(lonStr, true);
_lat = fgGetNode(latStr, true);
if (altStr) {
_alt = fgGetNode(altStr, true);
}
}
void SGGeodProperty::clear()
{
_lon = _lat = _alt = NULL;
}
void SGGeodProperty::operator=(const SGGeod& geod)
{
_lon->setDoubleValue(geod.getLongitudeDeg());
_lat->setDoubleValue(geod.getLatitudeDeg());
if (_alt) {
_alt->setDoubleValue(geod.getElevationFt());
}
}
SGGeod SGGeodProperty::get() const
{
double lon = _lon->getDoubleValue(),
lat = _lat->getDoubleValue();
if (_alt) {
return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue());
} else {
return SGGeod::fromDeg(lon,lat);
}
}
static const char* makeTTWString(double TTW)
{
if ((TTW <= 0.0) || (TTW >= 356400.5)) { // 99 hours
return "--:--:--";
}
unsigned int TTW_seconds = (int) (TTW + 0.5);
unsigned int TTW_minutes = 0;
unsigned int TTW_hours = 0;
static char TTW_str[9];
TTW_hours = TTW_seconds / 3600;
TTW_minutes = (TTW_seconds / 60) % 60;
TTW_seconds = TTW_seconds % 60;
snprintf(TTW_str, 9, "%02d:%02d:%02d",
TTW_hours, TTW_minutes, TTW_seconds);
return TTW_str;
}
/////////////////////////////////////////////////////////////////////////////
class GPSListener : public SGPropertyChangeListener
{
public:
GPSListener(GPS *m) :
_gps(m),
_guard(false) {}
virtual void valueChanged (SGPropertyNode * prop)
{
if (_guard) {
return;
}
_guard = true;
if (prop == _gps->_route_current_wp_node) {
_gps->routeManagerSequenced();
} else if (prop == _gps->_route_active_node) {
_gps->routeActivated();
} else if (prop == _gps->_ref_navaid_id_node) {
_gps->referenceNavaidSet(prop->getStringValue(""));
} else if (prop == _gps->_routeEditedSignal) {
_gps->routeEdited();
} else if (prop == _gps->_routeFinishedSignal) {
_gps->routeFinished();
}
_guard = false;
}
void setGuard(bool g) {
_guard = g;
}
private:
GPS* _gps;
bool _guard; // re-entrancy guard
};
////////////////////////////////////////////////////////////////////////////
/**
* Helper to monitor for Nasal or other code accessing properties we haven't
* defined. For the moment we complain about all such activites, since various
* users assume all kinds of weird, wonderful and non-existent interfaces.
*/
class DeprecatedPropListener : public SGPropertyChangeListener
{
public:
DeprecatedPropListener(SGPropertyNode* gps)
{
_parents.insert(gps);
SGPropertyNode* wp = gps->getChild("wp", 0, true);
_parents.insert(wp);
_parents.insert(wp->getChild("wp", 0, true));
_parents.insert(wp->getChild("wp", 1, true));
std::set<SGPropertyNode*>::iterator it;
for (it = _parents.begin(); it != _parents.end(); ++it) {
(*it)->addChangeListener(this);
}
}
virtual void valueChanged (SGPropertyNode * prop)
{
}
virtual void childAdded (SGPropertyNode * parent, SGPropertyNode * child)
{
if (isDeprecated(parent, child)) {
SG_LOG(SG_INSTR, SG_WARN, "GPS: someone accessed a deprecated property:"
<< child->getPath(true));
}
}
private:
bool isDeprecated(SGPropertyNode * parent, SGPropertyNode * child) const
{
if (_parents.count(parent) < 1) {
return false;
}
// no child exclusions yet
return true;
}
std::set<SGPropertyNode*> _parents;
};
////////////////////////////////////////////////////////////////////////////
// configuration helper object
GPS::Config::Config() :
_enableTurnAnticipation(true),
_turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds
_overflightArmDistance(1.0),
_waypointAlertTime(30.0),
_minRunwayLengthFt(0.0),
_requireHardSurface(true),
_cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg
_driveAutopilot(true),
_courseSelectable(false)
{
_enableTurnAnticipation = false;
}
void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
{
aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer<double>(&_turnRate));
aOwner->tie(aCfg, "turn-anticipation", SGRawValuePointer<bool>(&_enableTurnAnticipation));
aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer<double>(&_waypointAlertTime));
aOwner->tie(aCfg, "min-runway-length-ft", SGRawValuePointer<double>(&_minRunwayLengthFt));
aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer<bool>(&_requireHardSurface));
aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_cdiMaxDeflectionNm));
aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer<bool>(&_driveAutopilot));
aOwner->tie(aCfg, "course-selectable", SGRawValuePointer<bool>(&_courseSelectable));
}
////////////////////////////////////////////////////////////////////////////
GPS::GPS ( SGPropertyNode *node) :
_selectedCourse(0.0),
_desiredCourse(0.0),
_dataValid(false),
_lastPosValid(false),
_mode("init"),
_name(node->getStringValue("name", "gps")),
_num(node->getIntValue("number", 0)),
_computeTurnData(false),
_anticipateTurn(false),
_inTurn(false)
{
string branch = "/instrumentation/" + _name;
_gpsNode = fgGetNode(branch.c_str(), _num, true );
_scratchNode = _gpsNode->getChild("scratch", 0, true);
}
GPS::~GPS ()
{
}
void
GPS::init ()
{
_routeMgr = (FGRouteMgr*) globals->get_subsystem("route-manager");
assert(_routeMgr);
_position.init("/position/longitude-deg", "/position/latitude-deg", "/position/altitude-ft");
_magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
_serviceable_node = _gpsNode->getChild("serviceable", 0, true);
_serviceable_node->setBoolValue(true);
_electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
// basic GPS outputs
_raim_node = _gpsNode->getChild("raim", 0, true);
_odometer_node = _gpsNode->getChild("odometer", 0, true);
_trip_odometer_node = _gpsNode->getChild("trip-odometer", 0, true);
_true_bug_error_node = _gpsNode->getChild("true-bug-error-deg", 0, true);
_magnetic_bug_error_node = _gpsNode->getChild("magnetic-bug-error-deg", 0, true);
_eastWestVelocity = _gpsNode->getChild("ew-velocity-msec", 0, true);
_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);
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);
// reference navid
SGPropertyNode_ptr ref_navaid = _gpsNode->getChild("ref-navaid", 0, true);
_ref_navaid_id_node = ref_navaid->getChild("id", 0, true);
_ref_navaid_name_node = ref_navaid->getChild("name", 0, true);
_ref_navaid_bearing_node = ref_navaid->getChild("bearing-deg", 0, true);
_ref_navaid_frequency_node = ref_navaid->getChild("frequency-mhz", 0, true);
_ref_navaid_distance_node = ref_navaid->getChild("distance-nm", 0, true);
_ref_navaid_mag_bearing_node = ref_navaid->getChild("mag-bearing-deg", 0, true);
_ref_navaid_elapsed = 0.0;
_ref_navaid_set = false;
// route properties
// should these move to the route manager?
_routeDistanceNm = _gpsNode->getChild("route-distance-nm", 0, true);
_routeETE = _gpsNode->getChild("ETE", 0, true);
_routeEditedSignal = fgGetNode("/autopilot/route-manager/signals/edited", true);
_routeFinishedSignal = fgGetNode("/autopilot/route-manager/signals/finished", true);
// add listener to various things
_listener = new GPSListener(this);
_route_current_wp_node = fgGetNode("/autopilot/route-manager/current-wp", true);
_route_current_wp_node->addChangeListener(_listener);
_route_active_node = fgGetNode("/autopilot/route-manager/active", true);
_route_active_node->addChangeListener(_listener);
_ref_navaid_id_node->addChangeListener(_listener);
_routeEditedSignal->addChangeListener(_listener);
_routeFinishedSignal->addChangeListener(_listener);
// navradio slaving properties
SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true);
toFlag->alias(wp1_node->getChild("to-flag"));
SGPropertyNode* fromFlag = _gpsNode->getChild("from-flag", 0, true);
fromFlag->alias(wp1_node->getChild("from-flag"));
// autopilot drive properties
_apDrivingFlag = fgGetNode("/autopilot/settings/gps-driving-true-heading", true);
_apTrueHeading = fgGetNode("/autopilot/settings/true-heading-deg",true);
_apTargetAltitudeFt = fgGetNode("/autopilot/settings/target-altitude-ft", true);
_apAltitudeLock = fgGetNode("/autopilot/locks/altitude", true);
// realism prop[s]
_realismSimpleGps = fgGetNode("/sim/realism/simple-gps", true);
if (!_realismSimpleGps->hasValue()) {
_realismSimpleGps->setBoolValue(true);
}
// last thing, add the deprecated prop watcher
new DeprecatedPropListener(_gpsNode);
clearOutput();
}
void
GPS::bind()
{
_config.bind(this, _gpsNode->getChild("config", 0, true));
// basic GPS outputs
tie(_gpsNode, "selected-course-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getSelectedCourse, &GPS::setSelectedCourse));
tie(_gpsNode, "desired-course-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getDesiredCourse, NULL));
_desiredCourseNode = _gpsNode->getChild("desired-course-deg");
tieSGGeodReadOnly(_gpsNode, _indicated_pos, "indicated-longitude-deg",
"indicated-latitude-deg", "indicated-altitude-ft");
tie(_gpsNode, "indicated-vertical-speed", SGRawValueMethods<GPS, double>
(*this, &GPS::getVerticalSpeed, NULL));
tie(_gpsNode, "indicated-track-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getTrueTrack, NULL));
tie(_gpsNode, "indicated-track-magnetic-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getMagTrack, NULL));
tie(_gpsNode, "indicated-ground-speed-kt", SGRawValueMethods<GPS, double>
(*this, &GPS::getGroundspeedKts, NULL));
// command system
tie(_gpsNode, "mode", SGRawValueMethods<GPS, const char*>(*this, &GPS::getMode, NULL));
tie(_gpsNode, "command", SGRawValueMethods<GPS, const char*>(*this, &GPS::getCommand, &GPS::setCommand));
tieSGGeod(_scratchNode, _scratchPos, "longitude-deg", "latitude-deg", "altitude-ft");
tie(_scratchNode, "valid", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchValid, NULL));
tie(_scratchNode, "distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchDistance, NULL));
tie(_scratchNode, "true-bearing-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchTrueBearing, NULL));
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>
(*this, &GPS::getWP1Distance, NULL));
tie(wp1_node, "bearing-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Bearing, NULL));
tie(wp1_node, "bearing-mag-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1MagBearing, NULL));
tie(wp1_node, "TTW-sec", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1TTW, NULL));
tie(wp1_node, "TTW", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1TTWString, NULL));
tie(wp1_node, "course-deviation-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseDeviation, NULL));
tie(wp1_node, "course-error-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseErrorNm, NULL));
tie(wp1_node, "to-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1ToFlag, NULL));
tie(wp1_node, "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>
(*this, &GPS::getCDIDeflection));
}
void
GPS::unbind()
{
for (unsigned int t=0; t<_tiedNodes.size(); ++t) {
_tiedNodes[t]->untie();
}
_tiedNodes.clear();
}
void
GPS::clearOutput()
{
_dataValid = false;
_last_speed_kts = 0.0;
_last_pos = SGGeod();
_lastPosValid = false;
_indicated_pos = SGGeod();
_last_vertical_speed = 0.0;
_last_true_track = 0.0;
_lastEWVelocity = _lastNSVelocity = 0.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);
_true_bug_error_node->setDoubleValue(0);
_magnetic_bug_error_node->setDoubleValue(0);
_northSouthVelocity->setDoubleValue(0.0);
_eastWestVelocity->setDoubleValue(0.0);
}
void
GPS::update (double delta_time_sec)
{
if (!_realismSimpleGps->getBoolValue()) {
// If it's off, don't bother.
if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) {
clearOutput();
return;
}
}
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();
}
updateWaypoints();
updateTrackingBug();
updateReferenceNavaid(delta_time_sec);
updateRouteData();
driveAutopilot();
}
if (_dataValid && (_mode == "init")) {
// allow a realistic delay in the future, here
SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete");
if (_route_active_node->getBoolValue()) {
// GPS init with active route
SG_LOG(SG_INSTR, SG_INFO, "GPS init with active route");
selectLegMode();
} else {
// initialise in OBS mode, with waypt set to the nearest airport.
// keep in mind at this point, _dataValid is not set
auto_ptr<FGPositioned::Filter> f(createFilter(FGPositioned::AIRPORT));
FGPositionedRef apt = FGPositioned::findClosest(_position.get(), 20.0, f.get());
if (apt) {
setScratchFromPositioned(apt, 0);
selectOBSMode();
}
}
} // of init mode check
_last_pos = _indicated_pos;
_lastPosValid = true;
}
void
GPS::updateBasicData(double dt)
{
if (!_lastPosValid) {
return;
}
double distance_m;
double track2_deg;
SGGeodesy::inverse(_last_pos, _indicated_pos, _last_true_track, track2_deg, distance_m );
double speed_kt = ((distance_m * SG_METER_TO_NM) * ((1 / dt) * 3600.0));
double vertical_speed_mpm = ((_indicated_pos.getElevationM() - _last_pos.getElevationM()) * 60 / dt);
_last_vertical_speed = vertical_speed_mpm * SG_METER_TO_FEET;
speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/10.0);
_last_speed_kts = speed_kt;
SGGeod g = _indicated_pos;
g.setLongitudeDeg(_last_pos.getLongitudeDeg());
double northSouthM = SGGeodesy::distanceM(_last_pos, g);
northSouthM = copysign(northSouthM, _indicated_pos.getLatitudeDeg() - _last_pos.getLatitudeDeg());
double nsMSec = fgGetLowPass(_lastNSVelocity, northSouthM / dt, dt/2.0);
_lastNSVelocity = nsMSec;
_northSouthVelocity->setDoubleValue(nsMSec);
g = _indicated_pos;
g.setLatitudeDeg(_last_pos.getLatitudeDeg());
double eastWestM = SGGeodesy::distanceM(_last_pos, g);
eastWestM = copysign(eastWestM, _indicated_pos.getLongitudeDeg() - _last_pos.getLongitudeDeg());
double ewMSec = fgGetLowPass(_lastEWVelocity, eastWestM / dt, dt/2.0);
_lastEWVelocity = ewMSec;
_eastWestVelocity->setDoubleValue(ewMSec);
double odometer = _odometer_node->getDoubleValue();
_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
odometer = _trip_odometer_node->getDoubleValue();
_trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
if (!_dataValid) {
SG_LOG(SG_INSTR, SG_INFO, "GPS setting data valid");
_dataValid = true;
}
}
void
GPS::updateTrackingBug()
{
double tracking_bug = _tracking_bug_node->getDoubleValue();
double true_bug_error = tracking_bug - getTrueTrack();
double magnetic_bug_error = tracking_bug - getMagTrack();
// Get the errors into the (-180,180) range.
SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
_true_bug_error_node->setDoubleValue(true_bug_error);
_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) {
_ref_navaid_elapsed += dt;
if (_ref_navaid_elapsed > 5.0) {
_ref_navaid_elapsed = 0.0;
FGPositioned::TypeFilter vorFilter(FGPositioned::VOR);
FGPositionedRef nav = FGPositioned::findClosest(_indicated_pos, 400.0, &vorFilter);
if (!nav) {
SG_LOG(SG_INSTR, SG_INFO, "GPS couldn't find a reference navid");
_ref_navaid_id_node->setStringValue("");
_ref_navaid_name_node->setStringValue("");
_ref_navaid_bearing_node->setDoubleValue(0.0);
_ref_navaid_mag_bearing_node->setDoubleValue(0.0);
_ref_navaid_distance_node->setDoubleValue(0.0);
_ref_navaid_frequency_node->setStringValue("");
} else if (nav != _ref_navaid) {
SG_LOG(SG_INSTR, SG_INFO, "GPS code selected new ref-navaid:" << nav->ident());
_listener->setGuard(true);
_ref_navaid_id_node->setStringValue(nav->ident().c_str());
_ref_navaid_name_node->setStringValue(nav->name().c_str());
FGNavRecord* vor = (FGNavRecord*) nav.ptr();
_ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0);
_listener->setGuard(false);
} else {
// SG_LOG(SG_INSTR, SG_ALERT, "matched existing");
}
_ref_navaid = nav;
}
}
if (_ref_navaid) {
double trueCourse, distanceM, az2;
SGGeodesy::inverse(_indicated_pos, _ref_navaid->geod(), trueCourse, az2, distanceM);
_ref_navaid_distance_node->setDoubleValue(distanceM * SG_METER_TO_NM);
_ref_navaid_bearing_node->setDoubleValue(trueCourse);
_ref_navaid_mag_bearing_node->setDoubleValue(trueCourse - _magvar_node->getDoubleValue());
}
}
void GPS::referenceNavaidSet(const std::string& aNavaid)
{
_ref_navaid = NULL;
// allow setting an empty string to restore normal nearest-vor selection
if (aNavaid.size() > 0) {
FGPositioned::TypeFilter vorFilter(FGPositioned::VOR);
_ref_navaid = FGPositioned::findClosestWithIdent(aNavaid,
_position.get(), &vorFilter);
if (!_ref_navaid) {
SG_LOG(SG_INSTR, SG_ALERT, "GPS: unknown ref navaid:" << aNavaid);
}
}
if (_ref_navaid) {
_ref_navaid_set = true;
SG_LOG(SG_INSTR, SG_INFO, "GPS code set explict ref-navaid:" << _ref_navaid->ident());
_ref_navaid_id_node->setStringValue(_ref_navaid->ident().c_str());
_ref_navaid_name_node->setStringValue(_ref_navaid->name().c_str());
FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr();
_ref_navaid_frequency_node->setDoubleValue(vor->get_freq() / 100.0);
} else {
_ref_navaid_set = false;
_ref_navaid_elapsed = 9999.0; // update next tick
}
}
void GPS::routeActivated()
{
if (_route_active_node->getBoolValue()) {
SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, switching to LEG mode");
selectLegMode();
// if we've already passed the current waypoint, sequence.
if (_dataValid && getWP1FromFlag()) {
SG_LOG(SG_INSTR, SG_INFO, "GPS::route activated, FROM wp1, sequencing");
_routeMgr->sequence();
}
} else if (_mode == "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode");
selectOBSMode();
}
}
void GPS::routeManagerSequenced()
{
if (_mode != "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS ignoring route sequencing, not in LEG mode");
return;
}
int index = _routeMgr->currentWaypoint(),
count = _routeMgr->size();
if ((index < 0) || (index >= count)) {
SG_LOG(SG_INSTR, SG_ALERT, "GPS: malformed route, index=" << index);
return;
}
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();
}
SGWayPoint wp1(_routeMgr->get_waypoint(index));
_wp1Ident = wp1.get_id();
_wp1Name = wp1.get_name();
_wp1_position = wp1.get_target();
_desiredCourse = getLegMagCourse();
_desiredCourseNode->fireValueChanged();
wp1Changed();
}
void GPS::routeEdited()
{
if (_mode != "leg") {
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS route edited while in LEG mode, updating waypoints");
routeManagerSequenced();
}
void GPS::routeFinished()
{
if (_mode != "leg") {
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS");
_mode = "obs";
_wp0_position = _indicated_pos;
wp1Changed();
}
void GPS::updateTurn()
{
bool printProgress = false;
if (_computeTurnData) {
if (_last_speed_kts < 60) {
// need valid leg course and sensible ground speed to compute the turn
return;
}
computeTurnData();
printProgress = true;
}
if (!_anticipateTurn) {
updateOverflight();
return;
}
updateTurnData();
// find bearing to turn centre
double bearing, az2, distanceM;
SGGeodesy::inverse(_indicated_pos, _turnCentre, bearing, az2, distanceM);
double progress = computeTurnProgress(bearing);
if (printProgress) {
SG_LOG(SG_INSTR, SG_INFO,"turn progress=" << progress);
}
if (!_inTurn && (progress > 0.0)) {
beginTurn();
}
if (_inTurn && !_turnSequenced && (progress > 0.5)) {
_turnSequenced = true;
SG_LOG(SG_INSTR, SG_INFO, "turn passed midpoint, sequencing");
_routeMgr->sequence();
}
if (_inTurn && (progress >= 1.0)) {
endTurn();
}
if (_inTurn) {
// drive deviation and desired course
double desiredCourse = bearing - copysign(90, _turnAngle);
SG_NORMALIZE_RANGE(desiredCourse, 0.0, 360.0);
double deviationNm = (distanceM * SG_METER_TO_NM) - _turnRadius;
double deviationDeg = desiredCourse - getMagTrack();
deviationNm = copysign(deviationNm, deviationDeg);
// FXIME
//_wp1_course_deviation_node->setDoubleValue(deviationDeg);
//_wp1_course_error_nm_node->setDoubleValue(deviationNm);
//_cdiDeflectionNode->setDoubleValue(deviationDeg);
}
}
void GPS::updateOverflight()
{
if ((_wp1DistanceM * SG_METER_TO_NM) > _config.overflightArmDistanceNm()) {
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);
if (index >= 0) {
SG_LOG(SG_INSTR, SG_INFO, "GPS DTO, resuming LEG mode at wp:" << index);
_mode = "leg";
_routeMgr->jumpToIndex(index);
}
}
} else if (_mode == "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS doing overflight sequencing");
_routeMgr->sequence();
} else if (_mode == "obs") {
// nothing to do here, TO/FROM will update but that's fine
}
_computeTurnData = true;
}
void GPS::beginTurn()
{
_inTurn = true;
_turnSequenced = false;
SG_LOG(SG_INSTR, SG_INFO, "begining turn");
}
void GPS::endTurn()
{
_inTurn = false;
SG_LOG(SG_INSTR, SG_INFO, "ending turn");
_computeTurnData = true;
}
double GPS::computeTurnProgress(double aBearing) const
{
double startBearing = _turnStartBearing + copysign(90, _turnAngle);
return (aBearing - startBearing) / _turnAngle;
}
void GPS::computeTurnData()
{
_computeTurnData = false;
if (_mode != "leg") { // and approach modes in the future
_anticipateTurn = false;
return;
}
int curIndex = _routeMgr->currentWaypoint();
if ((curIndex + 1) >= _routeMgr->size()) {
_anticipateTurn = false;
return;
}
if (!_config.turnAnticipationEnabled()) {
_anticipateTurn = false;
return;
}
_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);
// compute offset bearing
_turnAngle = crs - _turnStartBearing;
SG_NORMALIZE_RANGE(_turnAngle, -180.0, 180.0);
double median = _turnStartBearing + (_turnAngle * 0.5);
double offsetBearing = median + copysign(90, _turnAngle);
SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
SG_LOG(SG_INSTR, SG_INFO, "GPS computeTurnData: in=" << _turnStartBearing <<
", out=" << crs << "; turnAngle=" << _turnAngle << ", median=" << median
<< ", offset=" << offsetBearing);
SG_LOG(SG_INSTR, SG_INFO, "next leg is now:" << wp1.get_id() << "->" << wp2.get_id());
_turnPt = _wp1_position;
_anticipateTurn = true;
}
void GPS::updateTurnData()
{
// depends on ground speed, so needs to be updated per-frame
_turnRadius = computeTurnRadiusNm(_last_speed_kts);
// compute the turn centre, based on the turn radius.
// key thing is to understand that we're working a right-angle triangle,
// where the right-angle is the point we start the turn. From that point,
// one side is the inbound course to the turn pt, and the other is the
// perpendicular line, of length 'r', to the turn centre.
// the triangle's hypotenuse, which we need to find, is the distance from the
// turn pt to the turn center (in the direction of the offset bearing)
// note that d - _turnRadius tell us how much we're 'cutting' the corner.
double halfTurnAngle = fabs(_turnAngle * 0.5) * SG_DEGREES_TO_RADIANS;
double d = _turnRadius / cos(halfTurnAngle);
// SG_LOG(SG_INSTR, SG_INFO, "turnRadius=" << _turnRadius << ", d=" << d
// << " (cut distance=" << d - _turnRadius << ")");
double median = _turnStartBearing + (_turnAngle * 0.5);
double offsetBearing = median + copysign(90, _turnAngle);
SG_NORMALIZE_RANGE(offsetBearing, 0.0, 360.0);
double az2;
SGGeodesy::direct(_turnPt, offsetBearing, d * SG_NM_TO_METER, _turnCentre, az2);
}
double GPS::computeTurnRadiusNm(double aGroundSpeedKts) const
{
// turn time is seconds to execute a 360 turn.
double turnTime = 360.0 / _config.turnRateDegSec();
// c is ground distance covered in that time (circumference of the circle)
double c = turnTime * (aGroundSpeedKts / 3600.0); // convert knts to nm/sec
// divide by 2PI to go from circumference -> radius
return c / (2 * M_PI);
}
void GPS::updateRouteData()
{
double totalDistance = _wp1DistanceM * 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();
}
_routeDistanceNm->setDoubleValue(totalDistance * SG_METER_TO_NM);
if (_last_speed_kts > 1.0) {
double TTW = ((totalDistance * SG_METER_TO_NM) / _last_speed_kts) * 3600.0;
_routeETE->setStringValue(makeTTWString(TTW));
}
}
void GPS::driveAutopilot()
{
if (!_config.driveAutopilot() || !_realismSimpleGps->getBoolValue()) {
_apDrivingFlag->setBoolValue(false);
return;
}
// compatability feature - allow the route-manager / GPS to drive the
// generic autopilot heading hold *in leg mode only*
bool drive = _mode == "leg";
_apDrivingFlag->setBoolValue(drive);
if (drive) {
// FIXME: we want to set desired track, not heading, here
_apTrueHeading->setDoubleValue(getWP1Bearing());
}
}
void GPS::wp1Changed()
{
if (!_config.driveAutopilot()) {
return;
}
double altFt = _wp1_position.getElevationFt();
if (altFt > -9990.0) {
_apTargetAltitudeFt->setDoubleValue(altFt);
}
}
/////////////////////////////////////////////////////////////////////////////
// property getter/setters
void GPS::setSelectedCourse(double crs)
{
if (_selectedCourse == crs) {
return;
}
_selectedCourse = crs;
if ((_mode == "obs") || _config.courseSelectable()) {
_desiredCourse = _selectedCourse;
_desiredCourseNode->fireValueChanged();
}
}
double GPS::getLegDistance() const
{
if (!_dataValid || (_mode == "obs")) {
return -1;
}
return SGGeodesy::distanceNm(_wp0_position, _wp1_position);
}
double GPS::getLegCourse() const
{
if (!_dataValid) {
return -9999.0;
}
return SGGeodesy::courseDeg(_wp0_position, _wp1_position);
}
double GPS::getLegMagCourse() const
{
if (!_dataValid) {
return 0.0;
}
double m = getLegCourse() - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(m, 0.0, 360.0);
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) {
return 0.0;
}
double m = getTrueTrack() - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(m, 0.0, 360.0);
return m;
}
double GPS::getCDIDeflection() const
{
if (!_dataValid) {
return 0.0;
}
double defl;
if (_config.cdiDeflectionIsAngular()) {
defl = getWP1CourseDeviation();
SG_CLAMP_RANGE(defl, -10.0, 10.0); // as in navradio.cxx
} else {
double fullScale = _config.cdiDeflectionLinearPeg();
double normError = getWP1CourseErrorNm() / fullScale;
SG_CLAMP_RANGE(normError, -1.0, 1.0);
defl = normError * 10.0; // re-scale to navradio limits, i.e [-10.0 .. 10.0]
}
return defl;
}
const char* GPS::getWP0Ident() const
{
if (!_dataValid || (_mode != "leg")) {
return "";
}
return _wp0Ident.c_str();
}
const char* GPS::getWP0Name() const
{
if (!_dataValid || (_mode != "leg")) {
return "";
}
return _wp0Name.c_str();
}
const char* GPS::getWP1Ident() const
{
if (!_dataValid) {
return "";
}
return _wp1Ident.c_str();
}
const char* GPS::getWP1Name() const
{
if (!_dataValid) {
return "";
}
return _wp1Name.c_str();
}
double GPS::getWP1Distance() const
{
if (!_dataValid) {
return -1.0;
}
return _wp1DistanceM * SG_METER_TO_NM;
}
double GPS::getWP1TTW() const
{
if (!_dataValid) {
return -1.0;
}
if (_last_speed_kts < 1.0) {
return -1.0;
}
return (getWP1Distance() / _last_speed_kts) * 3600.0;
}
const char* GPS::getWP1TTWString() const
{
if (!_dataValid) {
return "";
}
return makeTTWString(getWP1TTW());
}
double GPS::getWP1Bearing() const
{
if (!_dataValid) {
return -9999.0;
}
return _wp1TrueBearing;
}
double GPS::getWP1MagBearing() const
{
if (!_dataValid) {
return -9999.0;
}
double magBearing = _wp1TrueBearing - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(magBearing, 0.0, 360.0);
return magBearing;
}
double GPS::getWP1CourseDeviation() const
{
if (!_dataValid) {
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;
}
double GPS::getWP1CourseErrorNm() const
{
if (!_dataValid) {
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;
}
bool GPS::getWP1ToFlag() const
{
if (!_dataValid) {
return false;
}
double dev = getWP1MagBearing() - _desiredCourse;
SG_NORMALIZE_RANGE(dev, -180.0, 180.0);
return (fabs(dev) < 90.0);
}
bool GPS::getWP1FromFlag() const
{
if (!_dataValid) {
return false;
}
return !getWP1ToFlag();
}
double GPS::getScratchDistance() const
{
if (!_scratchValid) {
return 0.0;
}
return SGGeodesy::distanceNm(_indicated_pos, _scratchPos);
}
double GPS::getScratchTrueBearing() const
{
if (!_scratchValid) {
return 0.0;
}
return SGGeodesy::courseDeg(_indicated_pos, _scratchPos);
}
double GPS::getScratchMagBearing() const
{
if (!_scratchValid) {
return 0.0;
}
double crs = getScratchTrueBearing() - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(crs, 0.0, 360.0);
return crs;
}
/////////////////////////////////////////////////////////////////////////////
// command / scratch / search system
void GPS::setCommand(const char* aCmd)
{
SG_LOG(SG_INSTR, SG_INFO, "GPS command:" << aCmd);
if (!strcmp(aCmd, "direct")) {
directTo();
} else if (!strcmp(aCmd, "obs")) {
selectOBSMode();
} else if (!strcmp(aCmd, "leg")) {
selectLegMode();
} else if (!strcmp(aCmd, "load-route-wpt")) {
loadRouteWaypoint();
} else if (!strcmp(aCmd, "nearest")) {
loadNearest();
} else if (!strcmp(aCmd, "search")) {
_searchNames = false;
search();
} else if (!strcmp(aCmd, "search-names")) {
_searchNames = true;
search();
} else if (!strcmp(aCmd, "next")) {
nextResult();
} else if (!strcmp(aCmd, "previous")) {
previousResult();
} else if (!strcmp(aCmd, "define-user-wpt")) {
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()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-before, bad index:" << index);
return;
}
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()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-insert-after, bad index:" << index);
return;
} else {
++index;
}
insertWaypointAtIndex(index);
} else if (!strcmp(aCmd, "route-delete")) {
int index = _scratchNode->getIntValue("index");
if (index < 0) {
index = _routeMgr->size();
} else if (index >= _routeMgr->size()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index);
return;
}
removeWaypointAtIndex(index);
} else {
SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognized command:" << aCmd);
}
}
void GPS::clearScratch()
{
_scratchPos = SGGeod::fromDegFt(-9999.0, -9999.0, -9999.0);
_scratchValid = false;
_scratchNode->setStringValue("type", "");
_scratchNode->setStringValue("query", "");
}
bool GPS::isScratchPositionValid() const
{
if ((_scratchPos.getLongitudeDeg() < -9990.0) ||
(_scratchPos.getLatitudeDeg() < -9990.0)) {
return false;
}
return true;
}
void GPS::directTo()
{
_wp0_position = _indicated_pos;
if (isScratchPositionValid()) {
_wp1Ident = _scratchNode->getStringValue("ident");
_wp1Name = _scratchNode->getStringValue("name");
_wp1_position = _scratchPos;
}
_mode = "dto";
_desiredCourse = getLegMagCourse();
_desiredCourseNode->fireValueChanged();
clearScratch();
wp1Changed();
}
void GPS::loadRouteWaypoint()
{
_scratchValid = false;
// if (!_routeMgr->isRouteActive()) {
// SG_LOG(SG_INSTR, SG_WARN, "GPS:loadWaypoint: no active route");
// return;
// }
int index = _scratchNode->getIntValue("index", -9999);
clearScratch();
if ((index < 0) || (index >= _routeMgr->size())) { // no index supplied, use current wp
index = _routeMgr->currentWaypoint();
}
_searchIsRoute = true;
setScratchFromRouteWaypoint(index);
}
void GPS::setScratchFromRouteWaypoint(int aIndex)
{
assert(_searchIsRoute);
if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
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();
_scratchValid = true;
_scratchNode->setDoubleValue("course", wp.get_track());
_scratchNode->setIntValue("index", aIndex);
}
void GPS::loadNearest()
{
string sty(_scratchNode->getStringValue("type"));
FGPositioned::Type ty = FGPositioned::typeFromName(sty);
if (ty == FGPositioned::INVALID) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:loadNearest: request type is invalid:" << sty);
return;
}
auto_ptr<FGPositioned::Filter> f(createFilter(ty));
int limitCount = _scratchNode->getIntValue("max-results", 1);
double cutoffDistance = _scratchNode->getDoubleValue("cutoff-nm", 400.0);
SGGeod searchPos = _indicated_pos;
if (isScratchPositionValid()) {
searchPos = _scratchPos;
}
clearScratch(); // clear now, regardless of whether we find a match or not
_searchResults =
FGPositioned::findClosestN(searchPos, limitCount, cutoffDistance, f.get());
_searchResultIndex = 0;
_searchIsRoute = false;
if (_searchResults.empty()) {
SG_LOG(SG_INSTR, SG_INFO, "GPS:loadNearest: no matches at all");
return;
}
setScratchFromCachedSearchResult();
}
bool GPS::SearchFilter::pass(FGPositioned* aPos) const
{
switch (aPos->type()) {
case FGPositioned::AIRPORT:
// heliport and seaport too?
case FGPositioned::VOR:
case FGPositioned::NDB:
case FGPositioned::FIX:
case FGPositioned::TACAN:
case FGPositioned::WAYPOINT:
return true;
default:
return false;
}
}
FGPositioned::Type GPS::SearchFilter::minType() const
{
return FGPositioned::AIRPORT;
}
FGPositioned::Type GPS::SearchFilter::maxType() const
{
return FGPositioned::WAYPOINT;
}
FGPositioned::Filter* GPS::createFilter(FGPositioned::Type aTy)
{
if (aTy == FGPositioned::AIRPORT) {
return new FGAirport::HardSurfaceFilter(_config.minRunwayLengthFt());
}
// if we were passed INVALID, assume it means 'all types interesting to a GPS'
if (aTy == FGPositioned::INVALID) {
return new SearchFilter;
}
return new FGPositioned::TypeFilter(aTy);
}
void GPS::search()
{
// parse search terms into local members, and exec the first search
string sty(_scratchNode->getStringValue("type"));
_searchType = FGPositioned::typeFromName(sty);
_searchQuery = _scratchNode->getStringValue("query");
if (_searchQuery.empty()) {
SG_LOG(SG_INSTR, SG_WARN, "empty GPS search query");
clearScratch();
return;
}
_searchExact = _scratchNode->getBoolValue("exact", true);
_searchResultIndex = 0;
_searchIsRoute = false;
auto_ptr<FGPositioned::Filter> f(createFilter(_searchType));
if (_searchNames) {
_searchResults = FGPositioned::findAllWithName(_searchQuery, f.get());
} else {
_searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get());
}
bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true);
if (orderByRange) {
FGPositioned::sortByRange(_searchResults, _indicated_pos);
}
if (_searchResults.empty()) {
clearScratch();
return;
}
setScratchFromCachedSearchResult();
}
bool GPS::getScratchHasNext() const
{
int lastResult;
if (_searchIsRoute) {
lastResult = _routeMgr->size() - 1;
} else {
lastResult = (int) _searchResults.size() - 1;
}
if (lastResult < 0) { // search array might be empty
return false;
}
return (_searchResultIndex < lastResult);
}
void GPS::setScratchFromCachedSearchResult()
{
int index = _searchResultIndex;
if ((index < 0) || (index >= (int) _searchResults.size())) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromCachedSearchResult: index out of bounds:" << index);
return;
}
setScratchFromPositioned(_searchResults[index], index);
}
void GPS::setScratchFromPositioned(FGPositioned* aPos, int aIndex)
{
clearScratch();
assert(aPos);
_scratchPos = aPos->geod();
_scratchNode->setStringValue("name", aPos->name());
_scratchNode->setStringValue("ident", aPos->ident());
_scratchNode->setStringValue("type", FGPositioned::nameForType(aPos->type()));
if (aIndex >= 0) {
_scratchNode->setIntValue("index", aIndex);
}
_scratchValid = true;
_scratchNode->setIntValue("result-count", _searchResults.size());
switch (aPos->type()) {
case FGPositioned::VOR:
_scratchNode->setDoubleValue("frequency-mhz", static_cast<FGNavRecord*>(aPos)->get_freq() / 100.0);
break;
case FGPositioned::NDB:
_scratchNode->setDoubleValue("frequency-khz", static_cast<FGNavRecord*>(aPos)->get_freq() / 100.0);
break;
case FGPositioned::AIRPORT:
addAirportToScratch((FGAirport*)aPos);
break;
default:
// no-op
break;
}
// look for being on the route and set?
}
void GPS::addAirportToScratch(FGAirport* aAirport)
{
for (unsigned int r=0; r<aAirport->numRunways(); ++r) {
SGPropertyNode* rwyNd = _scratchNode->getChild("runways", r, true);
FGRunway* rwy = aAirport->getRunwayByIndex(r);
// TODO - filter out unsuitable runways in the future
// based on config again
rwyNd->setStringValue("id", rwy->ident().c_str());
rwyNd->setIntValue("length-ft", rwy->lengthFt());
rwyNd->setIntValue("width-ft", rwy->widthFt());
rwyNd->setIntValue("heading-deg", rwy->headingDeg());
// map surface code to a string
// TODO - lighting information
if (rwy->ILS()) {
rwyNd->setDoubleValue("ils-frequency-mhz", rwy->ILS()->get_freq() / 100.0);
}
} // of runways iteration
}
void GPS::selectOBSMode()
{
if (isScratchPositionValid()) {
_wp1Ident = _scratchNode->getStringValue("ident");
_wp1Name = _scratchNode->getStringValue("name");
_wp1_position = _scratchPos;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode");
_mode = "obs";
_wp0_position = _indicated_pos;
wp1Changed();
}
void GPS::selectLegMode()
{
if (_mode == "leg") {
return;
}
if (!_routeMgr->isRouteActive()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:selectLegMode: no active route");
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS switching to LEG mode");
_mode = "leg";
// depending on the situation, this will either get over-written
// in routeManagerSequenced or not; either way it does no harm to
// set it here.
_wp0_position = _indicated_pos;
// not really sequenced, but does all the work of updating wp0/1
routeManagerSequenced();
}
void GPS::nextResult()
{
if (!getScratchHasNext()) {
return;
}
clearScratch();
if (_searchIsRoute) {
setScratchFromRouteWaypoint(++_searchResultIndex);
} else {
++_searchResultIndex;
setScratchFromCachedSearchResult();
}
}
void GPS::previousResult()
{
if (_searchResultIndex <= 0) {
return;
}
clearScratch();
--_searchResultIndex;
if (_searchIsRoute) {
setScratchFromRouteWaypoint(_searchResultIndex);
} else {
setScratchFromCachedSearchResult();
}
}
void GPS::defineWaypoint()
{
if (!isScratchPositionValid()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: invalid lat/lon");
return;
}
string ident = _scratchNode->getStringValue("ident");
if (ident.size() < 2) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: waypoint identifier must be at least two characters");
return;
}
// check for duplicate idents
FGPositioned::TypeFilter f(FGPositioned::WAYPOINT);
FGPositioned::List dups = FGPositioned::findAllWithIdent(ident, &f);
if (!dups.empty()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: non-unique waypoint identifier, ho-hum");
}
SG_LOG(SG_INSTR, SG_INFO, "GPS:defineWaypoint: creating waypoint:" << ident);
FGPositionedRef wpt = FGPositioned::createUserWaypoint(ident, _scratchPos);
_searchResults.clear();
_searchResults.push_back(wpt);
setScratchFromPositioned(wpt.get(), -1);
}
void GPS::insertWaypointAtIndex(int aIndex)
{
// note we do allow index = routeMgr->size(), that's an append
if ((aIndex < 0) || (aIndex > _routeMgr->size())) {
throw sg_range_exception("GPS::insertWaypointAtIndex: index out of bounds");
}
if (!isScratchPositionValid()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:insertWaypointAtIndex: invalid lat/lon");
return;
}
string ident = _scratchNode->getStringValue("ident");
string name = _scratchNode->getStringValue("name");
_routeMgr->add_waypoint(SGWayPoint(_scratchPos, ident, name), aIndex);
}
void GPS::removeWaypointAtIndex(int aIndex)
{
if ((aIndex < 0) || (aIndex >= _routeMgr->size())) {
throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
}
_routeMgr->pop_waypoint(aIndex);
}
void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef,
const char* lonStr, const char* latStr, const char* altStr)
{
tie(aNode, lonStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLongitudeDeg, &SGGeod::setLongitudeDeg));
tie(aNode, latStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLatitudeDeg, &SGGeod::setLatitudeDeg));
if (altStr) {
tie(aNode, altStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getElevationFt, &SGGeod::setElevationFt));
}
}
void GPS::tieSGGeodReadOnly(SGPropertyNode* aNode, SGGeod& aRef,
const char* lonStr, const char* latStr, const char* altStr)
{
tie(aNode, lonStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLongitudeDeg, NULL));
tie(aNode, latStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLatitudeDeg, NULL));
if (altStr) {
tie(aNode, altStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getElevationFt, NULL));
}
}
// end of gps.cxx
View git blame Copy permalink