X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fgps.cxx;h=4477dd4eddaf23e5ce1e77b7ad6a0ddfda24694a;hb=5cad5aa7da2476ca8323a61f81dea59676dca085;hp=b626e4dc1b42a525be0e511eee0669704fbe2d02;hpb=afb1e7ffe99c2655b8c28a65b58530801e37275c;p=flightgear.git diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index b626e4dc1..4477dd4ed 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -24,6 +24,7 @@ #include #include #include +#include using std::auto_ptr; using std::string; @@ -280,7 +281,10 @@ GPS::Config::setExternalCourse(double aCourseDeg) //////////////////////////////////////////////////////////////////////////// GPS::GPS ( SGPropertyNode *node) : - _last_valid(false), + _selectedCourse(0.0), + _dataValid(false), + _lastPosValid(false), + _mode("init"), _name(node->getStringValue("name", "gps")), _num(node->getIntValue("number", 0)), _computeTurnData(false), @@ -334,7 +338,6 @@ GPS::init () _magnetic_bug_error_node = node->getChild("magnetic-bug-error-deg", 0, true); // command system - _mode = "obs"; node->tie("mode", SGRawValueMethods(*this, &GPS::getMode, NULL)); node->tie("command", SGRawValueMethods(*this, &GPS::getCommand, &GPS::setCommand)); @@ -454,23 +457,16 @@ GPS::init () // last thing, add the deprecated prop watcher new DeprecatedPropListener(node); - // initialise in OBS mode, with waypt set to the nearest airport. - // keep in mind at this point, _last_valid 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(); - } + clearOutput(); } void GPS::clearOutput() { - _last_valid = false; + _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; @@ -543,50 +539,58 @@ GPS::update (double delta_time_sec) printf("%f %f \n", error_length, error_angle); */ - _raim_node->setDoubleValue(1.0); - _indicated_pos = _position.get(); + _raim_node->setDoubleValue(1.0); + _indicated_pos = _position.get(); + updateBasicData(delta_time_sec); - if (_last_valid) { - updateWithValid(delta_time_sec); + if (_dataValid) { + if (_mode == "obs") { + _selectedCourse = _config.getExternalCourse(); } else { - _last_valid = true; - - if (_route_active_node->getBoolValue()) { - // GPS init with active route - SG_LOG(SG_INSTR, SG_INFO, "GPS init with active route"); - _listener->setGuard(true); - routeActivated(); - routeManagerSequenced(); - _listener->setGuard(false); - } + updateTurn(); } - - _last_pos = _indicated_pos; -} - -void -GPS::updateWithValid(double dt) -{ - assert(_last_valid); - - updateBasicData(dt); + + updateWaypoints(); + updateTrackingBug(); + updateReferenceNavaid(delta_time_sec); + updateRouteData(); + driveAutopilot(); + } - if (_mode == "obs") { + if (_dataValid && (_mode == "init")) { + // allow a realistic delay in the future, here + SG_LOG(SG_INSTR, SG_INFO, "GPS initialisation complete"); + _selectedCourse = _config.getExternalCourse(); - } else { - updateTurn(); - } - updateWaypoints(); - updateTrackingBug(); - updateReferenceNavaid(dt); - updateRouteData(); - driveAutopilot(); + 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 ); @@ -602,6 +606,11 @@ GPS::updateBasicData(double dt) _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 @@ -718,6 +727,12 @@ 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(); @@ -733,19 +748,22 @@ void GPS::routeManagerSequenced() int index = _routeMgr->currentWaypoint(), count = _routeMgr->size(); - if ((index < 1) || (index >= count)) { + 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); - SGWayPoint wp0(_routeMgr->get_waypoint(index - 1)); - SGWayPoint wp1(_routeMgr->get_waypoint(index)); - - _wp0Ident = wp0.get_id(); - _wp0Name = wp0.get_name(); - _wp0_position = wp0.get_target(); + + 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(); @@ -1016,7 +1034,7 @@ void GPS::wp1Changed() double GPS::getLegDistance() const { - if (!_last_valid || (_mode == "obs")) { + if (!_dataValid || (_mode == "obs")) { return -1; } @@ -1025,7 +1043,7 @@ double GPS::getLegDistance() const double GPS::getLegCourse() const { - if (!_last_valid || (_mode == "obs")) { + if (!_dataValid) { return -9999.0; } @@ -1034,7 +1052,7 @@ double GPS::getLegCourse() const double GPS::getLegMagCourse() const { - if (!_last_valid || (_mode == "obs")) { + if (!_dataValid) { return 0.0; } @@ -1045,7 +1063,7 @@ double GPS::getLegMagCourse() const double GPS::getAltDistanceRatio() const { - if (!_last_valid || (_mode == "obs")) { + if (!_dataValid || (_mode == "obs")) { return 0.0; } @@ -1060,7 +1078,7 @@ double GPS::getAltDistanceRatio() const double GPS::getMagTrack() const { - if (!_last_valid) { + if (!_dataValid) { return 0.0; } @@ -1071,7 +1089,7 @@ double GPS::getMagTrack() const double GPS::getCDIDeflection() const { - if (!_last_valid) { + if (!_dataValid) { return 0.0; } @@ -1091,7 +1109,7 @@ double GPS::getCDIDeflection() const const char* GPS::getWP0Ident() const { - if (!_last_valid || (_mode != "leg")) { + if (!_dataValid || (_mode != "leg")) { return ""; } @@ -1100,7 +1118,7 @@ const char* GPS::getWP0Ident() const const char* GPS::getWP0Name() const { - if (!_last_valid || (_mode != "leg")) { + if (!_dataValid || (_mode != "leg")) { return ""; } @@ -1109,7 +1127,7 @@ const char* GPS::getWP0Name() const const char* GPS::getWP1Ident() const { - if (!_last_valid) { + if (!_dataValid) { return ""; } @@ -1118,7 +1136,7 @@ const char* GPS::getWP1Ident() const const char* GPS::getWP1Name() const { - if (!_last_valid) { + if (!_dataValid) { return ""; } @@ -1127,7 +1145,7 @@ const char* GPS::getWP1Name() const double GPS::getWP1Distance() const { - if (!_last_valid) { + if (!_dataValid) { return -1.0; } @@ -1136,7 +1154,7 @@ double GPS::getWP1Distance() const double GPS::getWP1TTW() const { - if (!_last_valid) { + if (!_dataValid) { return -1.0; } @@ -1149,7 +1167,7 @@ double GPS::getWP1TTW() const const char* GPS::getWP1TTWString() const { - if (!_last_valid) { + if (!_dataValid) { return ""; } @@ -1158,7 +1176,7 @@ const char* GPS::getWP1TTWString() const double GPS::getWP1Bearing() const { - if (!_last_valid) { + if (!_dataValid) { return -9999.0; } @@ -1167,7 +1185,7 @@ double GPS::getWP1Bearing() const double GPS::getWP1MagBearing() const { - if (!_last_valid) { + if (!_dataValid) { return -9999.0; } @@ -1176,7 +1194,7 @@ double GPS::getWP1MagBearing() const double GPS::getWP1CourseDeviation() const { - if (!_last_valid) { + if (!_dataValid) { return 0.0; } @@ -1195,7 +1213,7 @@ double GPS::getWP1CourseDeviation() const double GPS::getWP1CourseErrorNm() const { - if (!_last_valid) { + if (!_dataValid) { return 0.0; } @@ -1206,7 +1224,7 @@ double GPS::getWP1CourseErrorNm() const bool GPS::getWP1ToFlag() const { - if (!_last_valid) { + if (!_dataValid) { return false; } @@ -1218,7 +1236,7 @@ bool GPS::getWP1ToFlag() const bool GPS::getWP1FromFlag() const { - if (!_last_valid) { + if (!_dataValid) { return false; } @@ -1283,8 +1301,40 @@ void GPS::setCommand(const char* aCmd) 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:unrecognzied command:" << aCmd); + SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognized command:" << aCmd); } } @@ -1442,6 +1492,12 @@ void GPS::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; @@ -1609,6 +1665,11 @@ void GPS::selectLegMode() 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(); } @@ -1675,4 +1736,31 @@ void GPS::defineWaypoint() 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