X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fgps.cxx;h=4477dd4eddaf23e5ce1e77b7ad6a0ddfda24694a;hb=5cad5aa7da2476ca8323a61f81dea59676dca085;hp=b8dcb6d58d58c0a76c2e3c89125ec6eed7df7864;hpb=2c72f131639d4c2c2f1de7cff65f7ceab95f61c4;p=flightgear.git diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index b8dcb6d58..4477dd4ed 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -7,57 +7,289 @@ # include #endif -#include -#include +#include "gps.hxx" + +#include +#include + +#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 #include #include -#include +#include +#include -#include
-#include
-#include
-#include
-#include -#include +using std::auto_ptr; +using std::string; -#include "gps.hxx" +/////////////////////////////////////////////////////////////////// + +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()); + } +} -SG_USING_STD(string); - - -GPS::GPS ( SGPropertyNode *node) - : _last_valid(false), - _last_longitude_deg(0), - _last_latitude_deg(0), - _last_altitude_m(0), - _last_speed_kts(0) -{ - int i; - for ( i = 0; i < node->nChildren(); ++i ) { - SGPropertyNode *child = node->getChild(i); - string cname = child->getName(); - string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "number" ) { - num = child->getIntValue(); - } else { - SG_LOG( SG_INSTR, SG_WARN, "Error in gps config logic" ); - if ( name.length() ) { - SG_LOG( SG_INSTR, SG_WARN, "Section = " << name ); - } - } +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 void tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef, + const char* lonStr, const char* latStr, const char* altStr) +{ + aNode->tie(lonStr, SGRawValueMethods(aRef, &SGGeod::getLongitudeDeg, &SGGeod::setLongitudeDeg)); + aNode->tie(latStr, SGRawValueMethods(aRef, &SGGeod::getLatitudeDeg, &SGGeod::setLatitudeDeg)); + + if (altStr) { + aNode->tie(altStr, SGRawValueMethods(aRef, &SGGeod::getElevationFt, &SGGeod::setElevationFt)); + } +} + +static void tieSGGeodReadOnly(SGPropertyNode* aNode, SGGeod& aRef, + const char* lonStr, const char* latStr, const char* altStr) +{ + aNode->tie(lonStr, SGRawValueMethods(aRef, &SGGeod::getLongitudeDeg, NULL)); + aNode->tie(latStr, SGRawValueMethods(aRef, &SGGeod::getLatitudeDeg, NULL)); + + if (altStr) { + aNode->tie(altStr, SGRawValueMethods(aRef, &SGGeod::getElevationFt, NULL)); + } +} + +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"); + _parents.insert(wp); + _parents.insert(wp->getChild("wp", 0)); + _parents.insert(wp->getChild("wp", 1)); + + std::set::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 _parents; +}; + +//////////////////////////////////////////////////////////////////////////// +// configuration helper object + +GPS::Config::Config() : + _enableTurnAnticipation(true), + _turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds + _overflightArmDistance(0.5), + _waypointAlertTime(30.0), + _tuneRadio1ToRefVor(false), + _minRunwayLengthFt(0.0), + _requireHardSurface(true), + _cdiMaxDeflectionNm(-1), // default to angular mode + _driveAutopilot(true) +{ + _enableTurnAnticipation = false; + _extCourseSource = fgGetNode("/instrumentation/nav[0]/radials/selected-deg", true); } -GPS::GPS () - : _last_valid(false), - _last_longitude_deg(0), - _last_latitude_deg(0), - _last_altitude_m(0), - _last_speed_kts(0) +void GPS::Config::init(SGPropertyNode* aCfgNode) +{ + aCfgNode->tie("turn-rate-deg-sec", SGRawValuePointer(&_turnRate)); + aCfgNode->tie("turn-anticipation", SGRawValuePointer(&_enableTurnAnticipation)); + aCfgNode->tie("wpt-alert-time", SGRawValuePointer(&_waypointAlertTime)); + aCfgNode->tie("tune-nav-radio-to-ref-vor", SGRawValuePointer(&_tuneRadio1ToRefVor)); + aCfgNode->tie("min-runway-length-ft", SGRawValuePointer(&_minRunwayLengthFt)); + aCfgNode->tie("hard-surface-runways-only", SGRawValuePointer(&_requireHardSurface)); + + aCfgNode->tie("course-source", SGRawValueMethods + (*this, &GPS::Config::getCourseSource, &GPS::Config::setCourseSource)); + + aCfgNode->tie("cdi-max-deflection-nm", SGRawValuePointer(&_cdiMaxDeflectionNm)); + aCfgNode->tie("drive-autopilot", SGRawValuePointer(&_driveAutopilot)); +} + +const char* +GPS::Config::getCourseSource() const +{ + if (!_extCourseSource) { + return ""; + } + + return _extCourseSource->getPath(true); +} + +void +GPS::Config::setCourseSource(const char* aPath) +{ + SGPropertyNode* nd = fgGetNode(aPath, false); + if (!nd) { + SG_LOG(SG_INSTR, SG_WARN, "couldn't find course source at:" << aPath); + _extCourseSource = NULL; + } + + _extCourseSource = nd; +} + +double +GPS::Config::getExternalCourse() const +{ + if (!_extCourseSource) { + return 0.0; + } + + return _extCourseSource->getDoubleValue(); +} + +void +GPS::Config::setExternalCourse(double aCourseDeg) +{ + if (!_extCourseSource) { + return; + } + + _extCourseSource->setDoubleValue(aCourseDeg); +} + +//////////////////////////////////////////////////////////////////////////// + +GPS::GPS ( SGPropertyNode *node) : + _selectedCourse(0.0), + _dataValid(false), + _lastPosValid(false), + _mode("init"), + _name(node->getStringValue("name", "gps")), + _num(node->getIntValue("number", 0)), + _computeTurnData(false), + _anticipateTurn(false), + _inTurn(false) { } @@ -68,161 +300,204 @@ GPS::~GPS () void GPS::init () { - route = new SGRoute; - route->clear(); - + _routeMgr = (FGRouteMgr*) globals->get_subsystem("route-manager"); + assert(_routeMgr); + string branch; - branch = "/instrumentation/" + name; - - SGPropertyNode *node = fgGetNode(branch.c_str(), num, true ); - - _longitude_node = fgGetNode("/position/longitude-deg", true); - _latitude_node = fgGetNode("/position/latitude-deg", true); - _altitude_node = fgGetNode("/position/altitude-ft", true); - _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true); - _serviceable_node = node->getChild("serviceable", 0, true); - _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true); - - SGPropertyNode *wp_node = node->getChild("wp", 0, true); - SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true); - SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true); - addWp = wp1_node->getChild("Add-to-route", 0, true); - - _wp0_longitude_node = wp0_node->getChild("longitude-deg", 0, true); - _wp0_latitude_node = wp0_node->getChild("latitude-deg", 0, true); - _wp0_altitude_node = wp0_node->getChild("altitude-deg", 0, true); - _wp0_ID_node = wp0_node->getChild("ID", 0, true); - _wp0_name_node = wp0_node->getChild("name", 0, true); - _wp0_course_node = wp0_node->getChild("desired-course-deg", 0, true); - _wp0_waypoint_type_node = wp0_node->getChild("waypoint-type", 0, true); - _wp0_distance_node = wp0_node->getChild("distance-nm", 0, true); - _wp0_ttw_node = wp0_node->getChild("TTW", 0, true); - _wp0_bearing_node = wp0_node->getChild("bearing-true-deg", 0, true); - _wp0_mag_bearing_node = wp0_node->getChild("bearing-mag-deg", 0, true); - _wp0_course_deviation_node = - wp0_node->getChild("course-deviation-deg", 0, true); - _wp0_course_error_nm_node = wp0_node->getChild("course-error-nm", 0, true); - _wp0_to_flag_node = wp0_node->getChild("to-flag", 0, true); - _true_wp0_bearing_error_node = - wp0_node->getChild("true-bearing-error-deg", 0, true); - _magnetic_wp0_bearing_error_node = - wp0_node->getChild("magnetic-bearing-error-deg", 0, true); - - _wp1_longitude_node = wp1_node->getChild("longitude-deg", 0, true); - _wp1_latitude_node = wp1_node->getChild("latitude-deg", 0, true); - _wp1_altitude_node = wp1_node->getChild("altitude-deg", 0, true); - _wp1_ID_node = wp1_node->getChild("ID", 0, true); - _wp1_name_node = wp1_node->getChild("name", 0, true); - _wp1_course_node = wp1_node->getChild("desired-course-deg", 0, true); - _wp1_waypoint_type_node = wp1_node->getChild("waypoint-type", 0, true); - _wp1_distance_node = wp1_node->getChild("distance-nm", 0, true); - _wp1_ttw_node = wp1_node->getChild("TTW", 0, true); - _wp1_bearing_node = wp1_node->getChild("bearing-true-deg", 0, true); - _wp1_mag_bearing_node = wp1_node->getChild("bearing-mag-deg", 0, true); - _wp1_course_deviation_node = - wp1_node->getChild("course-deviation-deg", 0, true); - _wp1_course_error_nm_node = wp1_node->getChild("course-error-nm", 0, true); - _wp1_to_flag_node = wp1_node->getChild("to-flag", 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); - _get_nearest_airport_node = - wp1_node->getChild("get-nearest-airport", 0, true); - - _tracking_bug_node = node->getChild("tracking-bug", 0, true); - _raim_node = node->getChild("raim", 0, true); - - _indicated_longitude_node = - node->getChild("indicated-longitude-deg", 0, true); - _indicated_latitude_node = - node->getChild("indicated-latitude-deg", 0, true); - _indicated_altitude_node = - node->getChild("indicated-altitude-ft", 0, true); - _indicated_vertical_speed_node = - node->getChild("indicated-vertical-speed", 0, true); - _true_track_node = - node->getChild("indicated-track-true-deg", 0, true); - _magnetic_track_node = - node->getChild("indicated-track_magnetic-deg", 0, true); - _speed_node = - node->getChild("indicated-ground-speed-kt", 0, true); - _odometer_node = - node->getChild("odometer", 0, true); - _trip_odometer_node = - node->getChild("trip-odometer", 0, true); - _true_bug_error_node = - node->getChild("true-bug-error-deg", 0, true); - _magnetic_bug_error_node = - node->getChild("magnetic-bug-error-deg", 0, true); - - _leg_distance_node = - wp_node->getChild("leg-distance-nm", 0, true); - _leg_course_node = - wp_node->getChild("leg-true-course-deg", 0, true); - _leg_magnetic_course_node = - wp_node->getChild("leg-mag-course-deg", 0, true); - _alt_dist_ratio_node = - wp_node->getChild("alt-dist-ratio", 0, true); - _leg_course_deviation_node = - wp_node->getChild("leg-course-deviation-deg", 0, true); - _leg_course_error_nm_node = - wp_node->getChild("leg-course-error-nm", 0, true); - _leg_to_flag_node = - wp_node->getChild("leg-to-flag", 0, true); - _alt_deviation_node = - wp_node->getChild("alt-deviation-ft", 0, true); - - _route = node->getChild("route", 0, true); - popWp = _route->getChild("Pop-WP", 0, true); - - addWp->setBoolValue(false); - popWp->setBoolValue(false); - - _serviceable_node->setBoolValue(true); + branch = "/instrumentation/" + _name; + + SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true ); + _config.init(node->getChild("config", 0, true)); + + _position.init("/position/longitude-deg", "/position/latitude-deg", "/position/altitude-ft"); + _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true); + _serviceable_node = node->getChild("serviceable", 0, true); + _serviceable_node->setBoolValue(true); + _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true); + +// basic GPS outputs + node->tie("selected-course-deg", SGRawValueMethods(*this, &GPS::getSelectedCourse, NULL)); + + _raim_node = node->getChild("raim", 0, true); + + tieSGGeodReadOnly(node, _indicated_pos, "indicated-longitude-deg", + "indicated-latitude-deg", "indicated-altitude-ft"); + + node->tie("indicated-vertical-speed", SGRawValueMethods + (*this, &GPS::getVerticalSpeed, NULL)); + node->tie("indicated-track-true-deg", SGRawValueMethods + (*this, &GPS::getTrueTrack, NULL)); + node->tie("indicated-track-magnetic-deg", SGRawValueMethods + (*this, &GPS::getMagTrack, NULL)); + node->tie("indicated-ground-speed-kt", SGRawValueMethods + (*this, &GPS::getGroundspeedKts, NULL)); + + _odometer_node = node->getChild("odometer", 0, true); + _trip_odometer_node = node->getChild("trip-odometer", 0, true); + _true_bug_error_node = node->getChild("true-bug-error-deg", 0, true); + _magnetic_bug_error_node = node->getChild("magnetic-bug-error-deg", 0, true); + +// command system + node->tie("mode", SGRawValueMethods(*this, &GPS::getMode, NULL)); + node->tie("command", SGRawValueMethods(*this, &GPS::getCommand, &GPS::setCommand)); + + _scratchNode = node->getChild("scratch", 0, true); + tieSGGeod(_scratchNode, _scratchPos, "longitude-deg", "latitude-deg", "altitude-ft"); + _scratchNode->tie("valid", SGRawValueMethods(*this, &GPS::getScratchValid, NULL)); + _scratchNode->tie("distance-nm", SGRawValueMethods(*this, &GPS::getScratchDistance, NULL)); + _scratchNode->tie("true-bearing-deg", SGRawValueMethods(*this, &GPS::getScratchTrueBearing, NULL)); + _scratchNode->tie("mag-bearing-deg", SGRawValueMethods(*this, &GPS::getScratchMagBearing, NULL)); + _scratchNode->tie("has-next", SGRawValueMethods(*this, &GPS::getScratchHasNext, NULL)); + _scratchValid = false; + +// waypoint data (including various historical things) + SGPropertyNode *wp_node = node->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"); + wp0_node->tie("ID", SGRawValueMethods + (*this, &GPS::getWP0Ident, NULL)); + wp0_node->tie("name", SGRawValueMethods + (*this, &GPS::getWP0Name, NULL)); + + tieSGGeodReadOnly(wp1_node, _wp1_position, "longitude-deg", "latitude-deg", "altitude-ft"); + wp1_node->tie("ID", SGRawValueMethods + (*this, &GPS::getWP1Ident, NULL)); + wp1_node->tie("name", SGRawValueMethods + (*this, &GPS::getWP1Name, NULL)); + + // 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(node->getChild("selected-course-deg")); + +// _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); + + wp1_node->tie("distance-nm", SGRawValueMethods + (*this, &GPS::getWP1Distance, NULL)); + wp1_node->tie("bearing-true-deg", SGRawValueMethods + (*this, &GPS::getWP1Bearing, NULL)); + wp1_node->tie("bearing-mag-deg", SGRawValueMethods + (*this, &GPS::getWP1MagBearing, NULL)); + wp1_node->tie("TTW-sec", SGRawValueMethods + (*this, &GPS::getWP1TTW, NULL)); + wp1_node->tie("TTW", SGRawValueMethods + (*this, &GPS::getWP1TTWString, NULL)); + + wp1_node->tie("course-deviation-deg", SGRawValueMethods + (*this, &GPS::getWP1CourseDeviation, NULL)); + wp1_node->tie("course-error-nm", SGRawValueMethods + (*this, &GPS::getWP1CourseErrorNm, NULL)); + wp1_node->tie("to-flag", SGRawValueMethods + (*this, &GPS::getWP1ToFlag, NULL)); + wp1_node->tie("from-flag", SGRawValueMethods + (*this, &GPS::getWP1FromFlag, NULL)); + + _tracking_bug_node = node->getChild("tracking-bug", 0, true); + +// leg properties (only valid in DTO/LEG modes, not OBS) + wp_node->tie("leg-distance-nm", SGRawValueMethods(*this, &GPS::getLegDistance, NULL)); + wp_node->tie("leg-true-course-deg", SGRawValueMethods(*this, &GPS::getLegCourse, NULL)); + wp_node->tie("leg-mag-course-deg", SGRawValueMethods(*this, &GPS::getLegMagCourse, NULL)); + wp_node->tie("alt-dist-ratio", SGRawValueMethods(*this, &GPS::getAltDistanceRatio, NULL)); + +// reference navid + SGPropertyNode_ptr ref_navaid = node->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 = node->getChild("route-distance-nm", 0, true); + _routeETE = node->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 + node->tie("cdi-deflection", SGRawValueMethods + (*this, &GPS::getCDIDeflection)); + + SGPropertyNode* toFlag = node->getChild("to-flag", 0, true); + toFlag->alias(wp1_node->getChild("to-flag")); + + SGPropertyNode* fromFlag = node->getChild("from-flag", 0, true); + fromFlag->alias(wp1_node->getChild("from-flag")); + + +// autopilot drive properties + _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(node); + + clearOutput(); +} + +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; + + _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); } void GPS::update (double delta_time_sec) { - // If it's off, don't bother. - if (!_serviceable_node->getBoolValue() || - !_electrical_node->getBoolValue()) { - _last_valid = false; - _last_longitude_deg = 0; - _last_latitude_deg = 0; - _last_altitude_m = 0; - _last_speed_kts = 0; - _raim_node->setDoubleValue(false); - _indicated_longitude_node->setDoubleValue(0); - _indicated_latitude_node->setDoubleValue(0); - _indicated_altitude_node->setDoubleValue(0); - _indicated_vertical_speed_node->setDoubleValue(0); - _true_track_node->setDoubleValue(0); - _magnetic_track_node->setDoubleValue(0); - _speed_node->setDoubleValue(0); - _wp1_distance_node->setDoubleValue(0); - _wp1_bearing_node->setDoubleValue(0); - _wp1_longitude_node->setDoubleValue(0); - _wp1_latitude_node->setDoubleValue(0); - _wp1_course_node->setDoubleValue(0); - _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); - _true_wp1_bearing_error_node->setDoubleValue(0); - _magnetic_wp1_bearing_error_node->setDoubleValue(0); - return; + if (!_realismSimpleGps->getBoolValue()) { + // If it's off, don't bother. + if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) { + clearOutput(); + return; } - - // Get the aircraft position + } + + if (delta_time_sec <= 0.0) { + return; // paused, don't bother + } // TODO: Add noise and other errors. - double longitude_deg = _longitude_node->getDoubleValue(); - double latitude_deg = _latitude_node->getDoubleValue(); - double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER; - double magvar_deg = _magvar_node->getDoubleValue(); - /* // Bias and random error @@ -264,447 +539,1228 @@ GPS::update (double delta_time_sec) 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") { + _selectedCourse = _config.getExternalCourse(); + } else { + 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"); + + _selectedCourse = _config.getExternalCourse(); + + 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 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/20.0); + _last_speed_kts = speed_kt; + + 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; + } +} - double speed_kt, vertical_speed_mpm; - - _raim_node->setBoolValue(true); - _indicated_longitude_node->setDoubleValue(longitude_deg); - _indicated_latitude_node->setDoubleValue(latitude_deg); - _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET); - - if (_last_valid) { - double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing; - geo_inverse_wgs_84(altitude_m, - _last_latitude_deg, _last_longitude_deg, - latitude_deg, longitude_deg, - &track1_deg, &track2_deg, &distance_m); - speed_kt = ((distance_m * SG_METER_TO_NM) * - ((1 / delta_time_sec) * 3600.0)); - vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 / - delta_time_sec); - _indicated_vertical_speed_node->setDoubleValue - (vertical_speed_mpm * SG_METER_TO_FEET); - _true_track_node->setDoubleValue(track1_deg); - mag_track_bearing = track1_deg - magvar_deg; - SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0); - _magnetic_track_node->setDoubleValue(mag_track_bearing); - speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0); - _last_speed_kts = speed_kt; - _speed_node->setDoubleValue(speed_kt); - - 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); - - // Get waypoint 0 position - double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue(); - double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue(); - double wp0_altitude_m = _wp0_altitude_node->getDoubleValue() - * SG_FEET_TO_METER; - double wp0_course_deg = _wp0_course_node->getDoubleValue(); - double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg, - wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg; - string wp0_ID = _wp0_ID_node->getStringValue(); - - // Get waypoint 1 position - double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue(); - double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue(); - double wp1_altitude_m = _wp1_altitude_node->getDoubleValue() - * SG_FEET_TO_METER; - double wp1_course_deg = _wp1_course_node->getDoubleValue(); - double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg, - wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg; - string wp1_ID = _wp1_ID_node->getStringValue(); - - // If the get-nearest-airport-node is true. - // Get the nearest airport, and set it as waypoint 1. - if (_get_nearest_airport_node->getBoolValue()) { - FGAirport a; - //cout << "Airport found" << endl; - a = globals->get_airports()->search(longitude_deg, latitude_deg, false); - _wp1_ID_node->setStringValue(a.id.c_str()); - wp1_longitude_deg = a.longitude; - wp1_latitude_deg = a.latitude; - _wp1_name_node->setStringValue(a.name.c_str()); - _get_nearest_airport_node->setBoolValue(false); - _last_wp1_ID = wp1_ID = a.id.c_str(); - } - - // If the waypoint 0 ID has changed, try to find the new ID - // in the airport-, fix-, nav-database. - if ( !(_last_wp0_ID == wp0_ID) ) { - string waypont_type = - _wp0_waypoint_type_node->getStringValue(); - if (waypont_type == "airport") { - FGAirport a; - a = globals->get_airports()->search( wp0_ID ); - if ( a.id == wp0_ID ) { - //cout << "Airport found" << endl; - wp0_longitude_deg = a.longitude; - wp0_latitude_deg = a.latitude; - _wp0_name_node->setStringValue(a.name.c_str()); - } - } - else if (waypont_type == "nav") { - FGNavRecord *n - = globals->get_navlist()->findByIdent(wp0_ID.c_str(), - longitude_deg, - latitude_deg); - if ( n != NULL ) { - //cout << "Nav found" << endl; - wp0_longitude_deg = n->get_lon(); - wp0_latitude_deg = n->get_lat(); - _wp0_name_node->setStringValue(n->get_name().c_str()); - } - } - else if (waypont_type == "fix") { - FGFix f; - if ( globals->get_fixlist()->query(wp0_ID, &f) ) { - //cout << "Fix found" << endl; - wp0_longitude_deg = f.get_lon(); - wp0_latitude_deg = f.get_lat(); - _wp0_name_node->setStringValue(wp0_ID.c_str()); - } - } - _last_wp0_ID = wp0_ID; - } - - // If the waypoint 1 ID has changed, try to find the new ID - // in the airport-, fix-, nav-database. - if ( !(_last_wp1_ID == wp1_ID) ) { - string waypont_type = - _wp1_waypoint_type_node->getStringValue(); - if (waypont_type == "airport") { - FGAirport a; - a = globals->get_airports()->search( wp1_ID ); - if ( a.id == wp1_ID ) { - //cout << "Airport found" << endl; - wp1_longitude_deg = a.longitude; - wp1_latitude_deg = a.latitude; - _wp1_name_node->setStringValue(a.name.c_str()); - } - } - else if (waypont_type == "nav") { - FGNavRecord *n - = globals->get_navlist()->findByIdent(wp1_ID.c_str(), - longitude_deg, - latitude_deg); - if ( n != NULL ) { - //cout << "Nav found" << endl; - wp1_longitude_deg = n->get_lon(); - wp1_latitude_deg = n->get_lat(); - _wp1_name_node->setStringValue(n->get_name().c_str()); - } - } - else if (waypont_type == "fix") { - FGFix f; - if ( globals->get_fixlist()->query(wp1_ID, &f) ) { - //cout << "Fix found" << endl; - wp1_longitude_deg = f.get_lon(); - wp1_latitude_deg = f.get_lat(); - _wp1_name_node->setStringValue(wp1_ID.c_str()); - } - } - _last_wp1_ID = wp1_ID; - } - - - - // If any of the two waypoints have changed - // we need to calculate a new course between them, - // and values for vertical navigation. - if ( wp0_longitude_deg != _wp0_longitude_deg || - wp0_latitude_deg != _wp0_latitude_deg || - wp0_altitude_m != _wp0_altitude_m || - wp1_longitude_deg != _wp1_longitude_deg || - wp1_latitude_deg != _wp1_latitude_deg || - wp1_altitude_m != _wp1_altitude_m ) - { - // Update the global variables - _wp0_longitude_deg = wp0_longitude_deg; - _wp0_latitude_deg = wp0_latitude_deg; - _wp0_altitude_m = wp0_altitude_m; - _wp1_longitude_deg = wp1_longitude_deg; - _wp1_latitude_deg = wp1_latitude_deg; - _wp1_altitude_m = wp1_altitude_m; - - // Get the course and distance from wp0 to wp1 - SGWayPoint wp0(wp0_longitude_deg, - wp0_latitude_deg, wp0_altitude_m); - SGWayPoint wp1(wp1_longitude_deg, - wp1_latitude_deg, wp1_altitude_m); - - wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m); - double leg_mag_course = _course_deg - magvar_deg; - SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0); - - // Get the altitude / distance ratio - if ( distance_m > 0.0 ) { - double alt_difference_m = wp0_altitude_m - wp1_altitude_m; - _alt_dist_ratio = alt_difference_m / _distance_m; - } - - _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM); - _leg_course_node->setDoubleValue(_course_deg); - _leg_magnetic_course_node->setDoubleValue(leg_mag_course); - _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio); - - _wp0_longitude_node->setDoubleValue(wp0_longitude_deg); - _wp0_latitude_node->setDoubleValue(wp0_latitude_deg); - _wp1_longitude_node->setDoubleValue(wp1_longitude_deg); - _wp1_latitude_node->setDoubleValue(wp1_latitude_deg); - } - - - // Find the bearing and distance to waypoint 0. - SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m); - wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m, - &wp0_bearing_deg, &wp0_distance); - _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM); - _wp0_bearing_node->setDoubleValue(wp0_bearing_deg); - double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg; - SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0); - _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg); - wp0_bearing_error_deg = track1_deg - wp0_bearing_deg; - SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0); - _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg); - - // Estimate time to waypoint 0. - // The estimation does not take track into consideration, - // so if you are going away from the waypoint the TTW will - // increase. Makes most sense when travelling directly towards - // the waypoint. - if (speed_kt > 0.0 && wp0_distance > 0.0) { - wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600); - } - else { - wp0_TTW = 0.0; - } - unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5); - if (wp0_TTW_seconds < 356400) { // That's 99 hours - unsigned int wp0_TTW_minutes = 0; - unsigned int wp0_TTW_hours = 0; - char wp0_TTW_str[9]; - while (wp0_TTW_seconds >= 3600) { - wp0_TTW_seconds -= 3600; - wp0_TTW_hours++; - } - while (wp0_TTW_seconds >= 60) { - wp0_TTW_seconds -= 60; - wp0_TTW_minutes++; - } - snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d", - wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds); - _wp0_ttw_node->setStringValue(wp0_TTW_str); - } - else - _wp0_ttw_node->setStringValue("--:--:--"); - - // Course deviation is the diffenrence between the bearing - // and the course. - wp0_course_deviation_deg = wp0_bearing_deg - - wp0_course_deg; - SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -180.0, 180.0); - - // If the course deviation is less than 90 degrees to either side, - // our desired course is towards the waypoint. - // It does not matter if we are actually moving - // towards or from the waypoint. - if (fabs(wp0_course_deviation_deg) < 90.0) { - _wp0_to_flag_node->setBoolValue(true); } - // If it's more than 90 degrees the desired - // course is from the waypoint. - else if (fabs(wp0_course_deviation_deg) > 90.0) { - _wp0_to_flag_node->setBoolValue(false); - // When the course is away from the waypoint, - // it makes sense to change the sign of the deviation. - wp0_course_deviation_deg *= -1.0; - SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -90.0, 90.0); - } - - _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg); - - // Cross track error. - wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0) - * (wp0_distance); - _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m - * SG_METER_TO_NM); - - - - // Find the bearing and distance to waypoint 1. - SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m); - wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m, - &wp1_bearing_deg, &wp1_distance); - _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM); - _wp1_bearing_node->setDoubleValue(wp1_bearing_deg); - double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg; - SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0); - _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg); - wp1_bearing_error_deg = track1_deg - wp1_bearing_deg; - SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0); - _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg); - - // Estimate time to waypoint 1. - // The estimation does not take track into consideration, - // so if you are going away from the waypoint the TTW will - // increase. Makes most sense when travelling directly towards - // the waypoint. - if (speed_kt > 0.0 && wp1_distance > 0.0) { - wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600); - } - else { - wp1_TTW = 0.0; - } - unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5); - if (wp1_TTW_seconds < 356400) { // That's 99 hours - unsigned int wp1_TTW_minutes = 0; - unsigned int wp1_TTW_hours = 0; - char wp1_TTW_str[9]; - while (wp1_TTW_seconds >= 3600) { - wp1_TTW_seconds -= 3600; - wp1_TTW_hours++; - } - while (wp1_TTW_seconds >= 60) { - wp1_TTW_seconds -= 60; - wp1_TTW_minutes++; - } - snprintf(wp1_TTW_str, 9, "%02d:%02d:%02d", - wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds); - _wp1_ttw_node->setStringValue(wp1_TTW_str); - } - else - _wp1_ttw_node->setStringValue("--:--:--"); - - // Course deviation is the diffenrence between the bearing - // and the course. - wp1_course_deviation_deg = wp1_bearing_deg - wp1_course_deg; - SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -180.0, 180.0); - - // If the course deviation is less than 90 degrees to either side, - // our desired course is towards the waypoint. - // It does not matter if we are actually moving - // towards or from the waypoint. - if (fabs(wp1_course_deviation_deg) < 90.0) { - _wp1_to_flag_node->setBoolValue(true); } - // If it's more than 90 degrees the desired - // course is from the waypoint. - else if (fabs(wp1_course_deviation_deg) > 90.0) { - _wp1_to_flag_node->setBoolValue(false); - // When the course is away from the waypoint, - // it makes sense to change the sign of the deviation. - wp1_course_deviation_deg *= -1.0; - SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0); - } - - _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg); - - // Cross track error. - wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0) - * (wp1_distance); - _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m - * SG_METER_TO_NM); - - - // Leg course deviation is the diffenrence between the bearing - // and the course. - double course_deviation_deg = wp1_bearing_deg - _course_deg; - SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0); - - // If the course deviation is less than 90 degrees to either side, - // our desired course is towards the waypoint. - // It does not matter if we are actually moving - // towards or from the waypoint. - if (fabs(course_deviation_deg) < 90.0) { - _leg_to_flag_node->setBoolValue(true); } - // If it's more than 90 degrees the desired - // course is from the waypoint. - else if (fabs(course_deviation_deg) > 90.0) { - _leg_to_flag_node->setBoolValue(false); - // When the course is away from the waypoint, - // it makes sense to change the sign of the deviation. - course_deviation_deg *= -1.0; - SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0); - } - - _leg_course_deviation_node->setDoubleValue(course_deviation_deg); - - // Cross track error. - double course_error_m = sin(course_deviation_deg * SG_PI / 180.0) - * (_distance_m); - _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM); - - // Altitude deviation - double desired_altitude_m = wp1_altitude_m - + wp1_distance * _alt_dist_ratio; - double altitude_deviation_m = altitude_m - desired_altitude_m; - _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET); - - - - // Tracking bug. - double tracking_bug = _tracking_bug_node->getDoubleValue(); - double true_bug_error = tracking_bug - track1_deg; - double magnetic_bug_error = tracking_bug - mag_track_bearing; - - // 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); - - - // Add WP 1 to the route. - if ( addWp->getBoolValue() ) - { - addWp->setBoolValue(false); - - SGWayPoint tempWp( _wp1_longitude_node->getDoubleValue(), - _wp1_latitude_node->getDoubleValue(), - _wp1_altitude_node->getDoubleValue(), - SGWayPoint::WGS84, - _wp1_ID_node->getStringValue(), - _wp1_name_node->getStringValue() ); - - route->add_waypoint(tempWp); - - SGPropertyNode *wp = - _route->getChild("Waypoint", route->size()-1, true); - SGPropertyNode *id = wp->getChild("ID", 0, true); - SGPropertyNode *name = wp->getChild("Name", 0, true); - SGPropertyNode *lat = wp->getChild("Latitude", 0, true); - SGPropertyNode *lon = wp->getChild("Longitude", 0, true); - SGPropertyNode *alt = wp->getChild("Altitude", 0, true); - - id->setStringValue( tempWp.get_id().c_str() ); - name->setStringValue( tempWp.get_name().c_str() ); - lat->setDoubleValue( tempWp.get_target_lat() ); - lon->setDoubleValue( tempWp.get_target_lon() ); - alt->setDoubleValue( tempWp.get_target_alt() ); - } - - if ( popWp->getBoolValue() ) - { - popWp->setBoolValue(false); - - route->delete_first(); - _route->removeChild("Waypoint", 0, false); - } +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); + tuneNavRadios(); + } 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); + tuneNavRadios(); + } else { + _ref_navaid_set = false; + _ref_navaid_elapsed = 9999.0; // update next tick + } +} + +void GPS::tuneNavRadios() +{ + if (!_ref_navaid || !_config.tuneNavRadioToRefVor()) { + return; + } + + SGPropertyNode_ptr navRadio1 = fgGetNode("/instrumentation/nav", false); + if (!navRadio1) { + return; + } + + FGNavRecord* vor = (FGNavRecord*) _ref_navaid.ptr(); + SGPropertyNode_ptr freqs = navRadio1->getChild("frequencies"); + freqs->setDoubleValue("selected-mhz", vor->get_freq() / 100.0); +} + +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(); + + _selectedCourse = getLegMagCourse(); + 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 = _selectedCourse; +// 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()) { + return; + } + + // FIXME: we want to set desired track, not heading, here + _apTrueHeading->setDoubleValue(getWP1Bearing()); +} + +void GPS::wp1Changed() +{ + // update external HSI/CDI/NavDisplay/PFD/etc + _config.setExternalCourse(getLegMagCourse()); + + if (!_config.driveAutopilot()) { + return; + } + + double altFt = _wp1_position.getElevationFt(); + if (altFt < -9990.0) { + _apTargetAltitudeFt->setDoubleValue(0.0); + } else { + _apTargetAltitudeFt->setDoubleValue(altFt); + } +} +///////////////////////////////////////////////////////////////////////////// +// property getter/setters + +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; + } + + return _wp1TrueBearing - _magvar_node->getDoubleValue(); +} + +double GPS::getWP1CourseDeviation() const +{ + if (!_dataValid) { + return 0.0; + } + + double dev = getWP1MagBearing() - _selectedCourse; + 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() - _selectedCourse; + 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() +{ + if (!isScratchPositionValid()) { + SG_LOG(SG_INSTR, SG_WARN, "invalid DTO lat/lon"); + return; + } + + _wp0_position = _indicated_pos; + _wp1Ident = _scratchNode->getStringValue("ident"); + _wp1Name = _scratchNode->getStringValue("name"); + _wp1_position = _scratchPos; + + _mode = "dto"; + _selectedCourse = getLegMagCourse(); + 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 == -9999) { // 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); + + int lastResult = _routeMgr->size() - 1; + _searchHasNext = (_searchResultIndex < lastResult); +} + +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 f(createFilter(ty)); + int limitCount = _scratchNode->getIntValue("max-results", 1); + double cutoffDistance = _scratchNode->getDoubleValue("cutoff-nm", 400.0); + + clearScratch(); // clear now, regardless of whether we find a match or not + + _searchResults = + FGPositioned::findClosestN(_indicated_pos, limitCount, cutoffDistance, f.get()); + _searchResultsCached = true; + _searchResultIndex = 0; + _searchIsRoute = false; + _searchHasNext = false; + + if (_searchResults.empty()) { + SG_LOG(SG_INSTR, SG_INFO, "GPS:loadNearest: no matches at all"); + return; + } + + _searchHasNext = (_searchResults.size() > 1); + 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); + _searchOrderByRange = _scratchNode->getBoolValue("order-by-distance", true); + _searchResultIndex = 0; + _searchIsRoute = false; + _searchHasNext = false; + + if (_searchExact && _searchOrderByRange) { + // immediate mode search, get all the results now and cache them + auto_ptr f(createFilter(_searchType)); + if (_searchNames) { + _searchResults = FGPositioned::findAllWithNameSortedByRange(_searchQuery, _indicated_pos, f.get()); + } else { + _searchResults = FGPositioned::findAllWithIdentSortedByRange(_searchQuery, _indicated_pos, f.get()); + } + + _searchResultsCached = true; + + if (_searchResults.empty()) { + clearScratch(); + return; + } + + _searchHasNext = (_searchResults.size() > 1); + setScratchFromCachedSearchResult(); + } else { + // iterative search, look up result zero + _searchResultsCached = false; + performSearch(); + } +} + +void GPS::performSearch() +{ + auto_ptr f(createFilter(_searchType)); + clearScratch(); + + FGPositionedRef r; + if (_searchNames) { + if (_searchOrderByRange) { + r = FGPositioned::findClosestWithPartialName(_indicated_pos, _searchQuery, f.get(), _searchResultIndex, _searchHasNext); + } else { + r = FGPositioned::findWithPartialName(_searchQuery, f.get(), _searchResultIndex, _searchHasNext); + } + } else { + if (_searchOrderByRange) { + r = FGPositioned::findClosestWithPartialId(_indicated_pos, _searchQuery, f.get(), _searchResultIndex, _searchHasNext); } else { - _true_track_node->setDoubleValue(0.0); - _magnetic_track_node->setDoubleValue(0.0); - _speed_node->setDoubleValue(0.0); + r = FGPositioned::findWithPartialId(_searchQuery, f.get(), _searchResultIndex, _searchHasNext); } + } + + if (!r) { + return; + } + + setScratchFromPositioned(r.get(), _searchResultIndex); +} + +void GPS::setScratchFromCachedSearchResult() +{ + assert(_searchResultsCached); + 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); + + int lastResult = (int) _searchResults.size() - 1; + _searchHasNext = (_searchResultIndex < lastResult); +} - _last_valid = true; - _last_longitude_deg = longitude_deg; - _last_latitude_deg = latitude_deg; - _last_altitude_m = altitude_m; +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; + if (_searchResultsCached) { + _scratchNode->setIntValue("result-count", _searchResults.size()); + } + + switch (aPos->type()) { + case FGPositioned::VOR: + _scratchNode->setDoubleValue("frequency-mhz", static_cast(aPos)->get_freq() / 100.0); + break; + + case FGPositioned::NDB: + _scratchNode->setDoubleValue("frequency-khz", static_cast(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; rnumRunways(); ++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()) { + SG_LOG(SG_INSTR, SG_WARN, "invalid OBS lat/lon"); + return; + } + + SG_LOG(SG_INSTR, SG_INFO, "GPS switching to OBS mode"); + _mode = "obs"; + + _wp1Ident = _scratchNode->getStringValue("ident"); + _wp1Name = _scratchNode->getStringValue("name"); + _wp1_position = _scratchPos; + _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 (!_searchHasNext) { + return; + } + + clearScratch(); + if (_searchIsRoute) { + setScratchFromRouteWaypoint(++_searchResultIndex); + } else if (_searchResultsCached) { + ++_searchResultIndex; + setScratchFromCachedSearchResult(); + } else { + ++_searchResultIndex; + performSearch(); + } // of iterative search case +} + +void GPS::previousResult() +{ + if (_searchResultIndex <= 0) { + return; + } + + clearScratch(); + --_searchResultIndex; + + if (_searchIsRoute) { + setScratchFromRouteWaypoint(_searchResultIndex); + } else if (_searchResultsCached) { + setScratchFromCachedSearchResult(); + } else { + performSearch(); + } +} + +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::findAllWithIdentSortedByRange(ident, _indicated_pos, &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); + _searchResultsCached = false; + 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); } // end of gps.cxx