From 0a85e23cdea3c146afb9526d8e782692b32ecc05 Mon Sep 17 00:00:00 2001 From: James Turner Date: Mon, 28 Nov 2022 15:34:00 +0000 Subject: [PATCH] =?UTF-8?q?Extend=20FlightPlan=20to=20handle=20=E2=80=98be?= =?UTF-8?q?tween=E2=80=99=20restrictons?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit As part of this, also improve units handling in speed/altitude restrictions. --- src/Navaids/FlightPlan.cxx | 92 ++++--- src/Navaids/FlightPlan.hxx | 17 +- src/Navaids/LevelDXML.cxx | 11 +- src/Navaids/route.cxx | 260 ++++++++++++++---- src/Navaids/route.hxx | 72 ++--- src/Navaids/waypoint.cxx | 4 +- src/Scripting/NasalFlightPlan.cxx | 71 ++++- .../unit_tests/Navaids/test_flightplan.cxx | 71 +++++ .../unit_tests/Navaids/test_flightplan.hxx | 10 +- 9 files changed, 464 insertions(+), 144 deletions(-) diff --git a/src/Navaids/FlightPlan.cxx b/src/Navaids/FlightPlan.cxx index e804a74a1..37efdbc5a 100644 --- a/src/Navaids/FlightPlan.cxx +++ b/src/Navaids/FlightPlan.cxx @@ -1301,16 +1301,7 @@ bool FlightPlan::loadVersion2XMLRoute(SGPropertyNode_ptr routeData) continue; } - LegRef l = new Leg{this, wp}; - // sync leg restrictions with waypoint ones - if (wp->speedRestriction() != RESTRICT_NONE) { - l->setSpeed(wp->speedRestriction(), wp->speed()); - } - - if (wp->altitudeRestriction() != RESTRICT_NONE) { - l->setAltitude(wp->altitudeRestriction(), wp->altitudeFt()); - } - + LegRef l = new Leg{this, wp}; if (wpNode->hasChild("hold-count")) { l->setHoldCount(wpNode->getIntValue("hold-count")); } @@ -1383,7 +1374,7 @@ WayptRef FlightPlan::parseVersion1XMLWaypt(SGPropertyNode* aWP) double altFt = aWP->getDoubleValue("altitude-ft", -9999.9); if (altFt > -9990.0) { - w->setAltitude(altFt, RESTRICT_AT); + w->setAltitude(altFt, RESTRICT_AT, ALTITUDE_FEET); } return w; @@ -1564,9 +1555,12 @@ FlightPlan::Leg* FlightPlan::Leg::cloneFor(FlightPlan* owner) const // clone local data c->_speed = _speed; c->_speedRestrict = _speedRestrict; - c->_altitudeFt = _altitudeFt; + c->_speedUnits = _speedUnits; + c->_altitude = _altitude; c->_altRestrict = _altRestrict; - + c->_altitudeUnits = _altitudeUnits; + c->_holdCount = c->_holdCount; + return c; } @@ -1583,36 +1577,37 @@ unsigned int FlightPlan::Leg::index() const return _parent->findLegIndex(this); } -int FlightPlan::Leg::altitudeFt() const +double FlightPlan::Leg::altitude(RouteUnits aUnits) const { if (_altRestrict != RESTRICT_NONE) { - return _altitudeFt; + return convertAltitudeUnits(_altitudeUnits, aUnits, _altitude); } - - return _waypt->altitudeFt(); + + return _waypt->altitude(aUnits); } -int FlightPlan::Leg::speed() const +int FlightPlan::Leg::altitudeFt() const +{ + return static_cast(altitude(ALTITUDE_FEET)); +} + +double FlightPlan::Leg::speed(RouteUnits units) const { if (_speedRestrict != RESTRICT_NONE) { - return _speed; + return convertSpeedUnits(_speedUnits, units, altitudeFt(), _speed); } - - return _waypt->speed(); + + return _waypt->speed(units); } int FlightPlan::Leg::speedKts() const { - return speed(); + return static_cast(speed(SPEED_KNOTS)); } double FlightPlan::Leg::speedMach() const { - if (!isMachRestrict(_speedRestrict)) { - return 0.0; - } - - return -(_speed / 100.0); + return speed(SPEED_MACH); } RouteRestriction FlightPlan::Leg::altitudeRestriction() const @@ -1632,21 +1627,30 @@ RouteRestriction FlightPlan::Leg::speedRestriction() const return _waypt->speedRestriction(); } - -void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed) + +void FlightPlan::Leg::setSpeed(RouteRestriction ty, double speed, RouteUnits aUnit) { _speedRestrict = ty; - if (isMachRestrict(ty)) { - _speed = (speed * -100); - } else { - _speed = speed; + if (aUnit == DEFAULT_UNITS) { + if (isMachRestrict(ty)) { + aUnit = SPEED_MACH; + } else { + // TODO: check for system in metric? + aUnit = SPEED_KNOTS; + } } + _speedUnits = aUnit; + _speed = speed; } - -void FlightPlan::Leg::setAltitude(RouteRestriction ty, int altFt) + +void FlightPlan::Leg::setAltitude(RouteRestriction ty, double alt, RouteUnits aUnit) { _altRestrict = ty; - _altitudeFt = altFt; + if (aUnit == DEFAULT_UNITS) { + aUnit = ALTITUDE_FEET; + } + _altitudeUnits = aUnit; + _altitude = alt; } double FlightPlan::Leg::courseDeg() const @@ -1722,16 +1726,24 @@ void FlightPlan::Leg::writeToProperties(SGPropertyNode* aProp) const { if (_speedRestrict != RESTRICT_NONE) { aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict)); - if (_speedRestrict == SPEED_RESTRICT_MACH) { - aProp->setDoubleValue("speed", speedMach()); + if (_speedUnits == SPEED_MACH) { + aProp->setDoubleValue("speed-mach", _speed); + } else if (_speedUnits == SPEED_KPH) { + aProp->setDoubleValue("speed-kph", _speed); } else { - aProp->setDoubleValue("speed", _speed); + aProp->setDoubleValue("speed", _speed); } } if (_altRestrict != RESTRICT_NONE) { aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict)); - aProp->setDoubleValue("altitude-ft", _altitudeFt); + if (_altitudeUnits == ALTITUDE_FLIGHTLEVEL) { + aProp->setDoubleValue("flight-level", _altitude); + } else if (_altitudeUnits == ALTITUDE_METER) { + aProp->setDoubleValue("altitude-m", _altitude); + } else { + aProp->setDoubleValue("altitude-ft", _altitude); + } } if (_holdCount > 0) { diff --git a/src/Navaids/FlightPlan.hxx b/src/Navaids/FlightPlan.hxx index 78650c982..dba8b11d3 100644 --- a/src/Navaids/FlightPlan.hxx +++ b/src/Navaids/FlightPlan.hxx @@ -92,7 +92,7 @@ public: FlightPlanRef clone(const std::string& newIdent = {}, bool convertToFlightPlan = false) const; /** - is this flight-pan a route (for planning) or an active flight-plan (which can be flow?) + is this flight-pan a route (for planning) or an active flight-plan (which can be flown?) Routes can contain Via, and cannot be active: FlightPlans contain Legs for procedures and airways, i.e what the GPS/FMC actally flies. */ @@ -131,16 +131,17 @@ public: unsigned int index() const; int altitudeFt() const; - int speed() const; - + double speed(RouteUnits units = DEFAULT_UNITS) const; + double altitude(RouteUnits units = DEFAULT_UNITS) const; + int speedKts() const; double speedMach() const; RouteRestriction altitudeRestriction() const; RouteRestriction speedRestriction() const; - void setSpeed(RouteRestriction ty, double speed); - void setAltitude(RouteRestriction ty, int altFt); + void setSpeed(RouteRestriction ty, double speed, RouteUnits units = DEFAULT_UNITS); + void setAltitude(RouteRestriction ty, double alt, RouteUnits units = DEFAULT_UNITS); double courseDeg() const; double distanceNm() const; @@ -164,8 +165,10 @@ public: const FlightPlan* _parent; RouteRestriction _speedRestrict = RESTRICT_NONE, _altRestrict = RESTRICT_NONE; - int _speed = 0; - int _altitudeFt = 0; + double _speed = 0.0; + double _altitude = 0.0; + RouteUnits _speedUnits = SPEED_KNOTS; + RouteUnits _altitudeUnits = ALTITUDE_FEET; // if > 0, we will hold at the waypoint using // the published hold side/course diff --git a/src/Navaids/LevelDXML.cxx b/src/Navaids/LevelDXML.cxx index 89c58dc4e..ce43298f2 100644 --- a/src/Navaids/LevelDXML.cxx +++ b/src/Navaids/LevelDXML.cxx @@ -12,6 +12,7 @@ #include #include +#include using std::string; using std::vector; @@ -203,15 +204,7 @@ void NavdataVisitor::endElement(const char* name) } else if (tag == "Altitude") { _altitude = atof(_text.c_str()); } else if (tag == "AltitudeRestriction") { - if (_text == "at") { - _altRestrict = RESTRICT_AT; - } else if (_text == "above") { - _altRestrict = RESTRICT_ABOVE; - } else if (_text == "below") { - _altRestrict = RESTRICT_BELOW; - } else { - throw sg_format_exception("Unrecognized altitude restriction", _text); - } + _altRestrict = restrictionFromString(_text); } else if (tag == "Hld_Rad_or_Inbd") { if (_text == "Inbd") { _holdRadial = -1.0; diff --git a/src/Navaids/route.cxx b/src/Navaids/route.cxx index 41816f374..d0a2e873d 100644 --- a/src/Navaids/route.cxx +++ b/src/Navaids/route.cxx @@ -44,6 +44,7 @@ #include #include #include +#include // for Mach conversions using std::string; using std::vector; @@ -123,7 +124,67 @@ WayptRef viaFromString(const SGGeod& basePosition, const std::string& target) return new Via(nullptr, airway, nav); } -} // namespace +static double convertSpeedToKnots(RouteUnits aUnits, double aAltitudeFt, double aValue) +{ + switch (aUnits) { + case SPEED_KNOTS: return aValue; + case SPEED_KPH: return aValue * SG_KMH_TO_MPS * SG_MPS_TO_KT; + case SPEED_MACH: return FGAtmo::knotsFromMachAtAltitudeFt(aValue, aAltitudeFt); + + default: + throw sg_format_exception("Can't convert unit to Knots", "convertSpeedToKnots"); + } +} + +static double convertSpeedFromKnots(RouteUnits aUnits, double aAltitudeFt, double aValue) +{ + if (aUnits == DEFAULT_UNITS) { + // TODO : use KPH is simulator is in metric + aUnits = SPEED_KNOTS; + } + + switch (aUnits) { + case SPEED_KNOTS: return aValue; + case SPEED_KPH: return aValue * SG_KT_TO_MPS * SG_MPS_TO_KMH; + case SPEED_MACH: return FGAtmo::machFromKnotsAtAltitudeFt(aValue, aAltitudeFt); + + default: + throw sg_format_exception("Can't convert to unit", "convertSpeedFromKnots"); + } +} + +} // anonymous namespace + +double convertSpeedUnits(RouteUnits aSrc, RouteUnits aDest, double aAltitudeFt, double aValue) +{ + const double valueKnots = convertSpeedToKnots(aSrc, aAltitudeFt, aValue); + return convertSpeedFromKnots(aDest, aAltitudeFt, valueKnots); +} + +double convertAltitudeUnits(RouteUnits aSrc, RouteUnits aDest, double aValue) +{ + if (aDest == DEFAULT_UNITS) { + // TODO : use meters if sim is in metric + aDest = ALTITUDE_FEET; + } + + double altFt = 0.0; + switch (aSrc) { + case ALTITUDE_FEET: altFt = aValue; break; + case ALTITUDE_METER: altFt = aValue * SG_METER_TO_FEET; break; + case ALTITUDE_FLIGHTLEVEL: altFt = aValue * 100; break; + default: + throw sg_format_exception("Unsupported source altitude units", "convertAltitudeUnits"); + } + + switch (aDest) { + case ALTITUDE_FEET: return altFt; + case ALTITUDE_METER: return altFt * SG_FEET_TO_METER; + case ALTITUDE_FLIGHTLEVEL: return round(altFt / 100); + default: + throw sg_format_exception("Unsupported destination altitude units", "convertAltitudeUnits"); + } +} Waypt::Waypt(RouteBase* aOwner) : _owner(aOwner), @@ -184,28 +245,80 @@ bool Waypt::matches(const SGGeod& aPos) const return (d < 100.0); // 100 metres seems plenty } -void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict) +void Waypt::setAltitude(double aAlt, RouteRestriction aRestrict, RouteUnits aUnit) { - _altitudeFt = aAlt; + if (aUnit == DEFAULT_UNITS) { + aUnit = ALTITUDE_FEET; + } + + _altitude = aAlt; + _altitudeUnits = aUnit; _altRestrict = aRestrict; } -void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict) +void Waypt::setConstraintAltitude(double aAlt) { + _constraintAltitude = aAlt; +} + +void Waypt::setSpeed(double aSpeed, RouteRestriction aRestrict, RouteUnits aUnit) +{ + if (aUnit == DEFAULT_UNITS) { + if ((aRestrict == SPEED_RESTRICT_MACH) || (aRestrict == SPEED_RESTRICT_MACH)) { + aUnit = SPEED_MACH; + } else { + aUnit = SPEED_KNOTS; + } + } + _speed = aSpeed; + _speedUnits = aUnit; _speedRestrict = aRestrict; } double Waypt::speedKts() const { - assert(_speedRestrict != SPEED_RESTRICT_MACH); - return speed(); + return speed(SPEED_KNOTS); } double Waypt::speedMach() const { - assert(_speedRestrict == SPEED_RESTRICT_MACH); - return speed(); + return speed(SPEED_MACH); +} + +double Waypt::altitudeFt() const +{ + return altitude(ALTITUDE_FEET); +} + +double Waypt::speed(RouteUnits aUnits) const +{ + if (aUnits == _speedUnits) { + return _speed; + } + + return convertSpeedUnits(_speedUnits, aUnits, altitudeFt(), _speed); +} + +double Waypt::altitude(RouteUnits aUnits) const +{ + if (aUnits == _altitudeUnits) { + return _altitude; + } + + return convertAltitudeUnits(_altitudeUnits, aUnits, _altitude); +} + +double Waypt::constraintAltitude(RouteUnits aUnits) const +{ + if (!_constraintAltitude.has_value()) + return 0.0; + + if (aUnits == _altitudeUnits) { + return _constraintAltitude.value_or(0.0); + } + + return convertAltitudeUnits(_altitudeUnits, aUnits, _constraintAltitude.value_or(0.0)); } double Waypt::magvarDeg() const @@ -234,13 +347,14 @@ std::string Waypt::icaoDescription() const /////////////////////////////////////////////////////////////////////////// // persistence -static RouteRestriction restrictionFromString(const char* aStr) +RouteRestriction restrictionFromString(const std::string& aStr) { std::string l = simgear::strutils::lowercase(aStr); if (l == "at") return RESTRICT_AT; if (l == "above") return RESTRICT_ABOVE; if (l == "below") return RESTRICT_BELOW; + if (l == "between") return RESTRICT_BETWEEN; if (l == "none") return RESTRICT_NONE; if (l == "mach") return SPEED_RESTRICT_MACH; @@ -256,6 +370,7 @@ const char* restrictionToString(RouteRestriction aRestrict) case RESTRICT_BELOW: return "below"; case RESTRICT_ABOVE: return "above"; case RESTRICT_NONE: return "none"; + case RESTRICT_BETWEEN: return "between"; case SPEED_RESTRICT_MACH: return "mach"; default: @@ -382,16 +497,22 @@ WayptRef Waypt::createFromString(RouteBase* aOwner, const std::string& s, const auto target = simgear::strutils::uppercase(s); // extract altitude - double altFt = 0.0; + double alt = 0.0; RouteRestriction altSetting = RESTRICT_NONE; - + RouteUnits altitudeUnits = ALTITUDE_FEET; + size_t pos = target.find('@'); if (pos != string::npos) { - altFt = std::stof(target.substr(pos + 1)); - target = target.substr(0, pos); - if (fgGetString("/sim/startup/units") == "meter") { - altFt *= SG_METER_TO_FEET; + auto altStr = simgear::strutils::uppercase(target.substr(pos + 1)); + if (simgear::strutils::starts_with(altStr, "FL")) { + altitudeUnits = ALTITUDE_FLIGHTLEVEL; + altStr = altStr.substr(2); // trim leading 'FL' + } else if (fgGetString("/sim/startup/units") == "meter") { + altitudeUnits = ALTITUDE_METER; } + + alt = std::stof(altStr); + target = target.substr(0, pos); altSetting = RESTRICT_AT; } @@ -448,7 +569,7 @@ WayptRef Waypt::createFromString(RouteBase* aOwner, const std::string& s, const } if (altSetting != RESTRICT_NONE) { - wpt->setAltitude(altFt, altSetting); + wpt->setAltitude(alt, altSetting, altitudeUnits); } return wpt; } @@ -462,42 +583,70 @@ void Waypt::saveAsNode(SGPropertyNode* n) const bool Waypt::initFromProperties(SGPropertyNode_ptr aProp) { - if (aProp->hasChild("generated")) { - setFlag(WPT_GENERATED, aProp->getBoolValue("generated")); - } - - if (aProp->hasChild("overflight")) { - setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight")); - } - - if (aProp->hasChild("arrival")) { - setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival")); - } - - if (aProp->hasChild("approach")) { - setFlag(WPT_APPROACH, aProp->getBoolValue("approach")); - } - - if (aProp->hasChild("departure")) { - setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure")); - } - - if (aProp->hasChild("miss")) { - setFlag(WPT_MISS, aProp->getBoolValue("miss")); - } - - if (aProp->hasChild("airway")) { - setFlag(WPT_VIA, true); - } - - if (aProp->hasChild("alt-restrict")) { - _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict").c_str()); - _altitudeFt = aProp->getDoubleValue("altitude-ft"); + if (aProp->hasChild("generated")) { + setFlag(WPT_GENERATED, aProp->getBoolValue("generated")); + } + + if (aProp->hasChild("overflight")) { + setFlag(WPT_OVERFLIGHT, aProp->getBoolValue("overflight")); + } + + if (aProp->hasChild("arrival")) { + setFlag(WPT_ARRIVAL, aProp->getBoolValue("arrival")); + } + + if (aProp->hasChild("approach")) { + setFlag(WPT_APPROACH, aProp->getBoolValue("approach")); + } + + if (aProp->hasChild("departure")) { + setFlag(WPT_DEPARTURE, aProp->getBoolValue("departure")); + } + + if (aProp->hasChild("miss")) { + setFlag(WPT_MISS, aProp->getBoolValue("miss")); + } + + if (aProp->hasChild("airway")) { + setFlag(WPT_VIA, true); + } + + if (aProp->hasChild("alt-restrict")) { + _altRestrict = restrictionFromString(aProp->getStringValue("alt-restrict")); + if (aProp->hasChild("altitude-ft")) { + _altitude = aProp->getDoubleValue("altitude-ft"); + _altitudeUnits = ALTITUDE_FEET; + } else if (aProp->hasChild("altitude-m")) { + _altitude = aProp->getDoubleValue("altitude-m"); + _altitudeUnits = ALTITUDE_METER; + } else if (aProp->hasChild("flight-level")) { + _altitude = aProp->getIntValue("flight-level"); + _altitudeUnits = ALTITUDE_FLIGHTLEVEL; + } + + if (aProp->hasChild("constraint-altitude")) { + _constraintAltitude = aProp->getDoubleValue("constraint-altitude"); + } } if (aProp->hasChild("speed-restrict")) { - _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict").c_str()); - _speed = aProp->getDoubleValue("speed"); + _speedRestrict = restrictionFromString(aProp->getStringValue("speed-restrict")); + RouteUnits units = SPEED_KNOTS; + if (_speedRestrict == SPEED_RESTRICT_MACH) { + units = SPEED_MACH; + } + + if (aProp->hasChild("speed-mach")) { + units = SPEED_MACH; + _speed = aProp->getDoubleValue("speed-mach"); + } else if (aProp->hasChild("speed-kph")) { + units = SPEED_KPH; + _speed = aProp->getDoubleValue("speed-kph"); + } else { + _speed = aProp->getDoubleValue("speed"); + } + + _speedUnits = units; } return true; @@ -537,8 +686,19 @@ void Waypt::writeToProperties(SGPropertyNode_ptr aProp) const if (_altRestrict != RESTRICT_NONE) { aProp->setStringValue("alt-restrict", restrictionToString(_altRestrict)); - aProp->setDoubleValue("altitude-ft", _altitudeFt); + if (_altitudeUnits == ALTITUDE_METER) { + aProp->setDoubleValue("altitude-m", _altitude); + } else if (_altitudeUnits == ALTITUDE_FLIGHTLEVEL) { + aProp->setDoubleValue("flight-level", _altitude); + + } else { + aProp->setDoubleValue("altitude-ft", _altitude); + } } + + if (_constraintAltitude.has_value()) { + aProp->setDoubleValue("constraint-altitude", _constraintAltitude.value_or(0.0)); + } if (_speedRestrict != RESTRICT_NONE) { aProp->setStringValue("speed-restrict", restrictionToString(_speedRestrict)); diff --git a/src/Navaids/route.hxx b/src/Navaids/route.hxx index 715151d95..bbad27ff3 100644 --- a/src/Navaids/route.hxx +++ b/src/Navaids/route.hxx @@ -4,32 +4,16 @@ * owns a collection (list, tree, graph) of route elements - such as airways, * procedures or a flight plan. */ - -// Written by James Turner, started 2009. -// -// Copyright (C) 2009 Curtis L. Olson -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License as -// published by the Free Software Foundation; either version 2 of the -// License, or (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, but -// WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this program; if not, write to the Free Software -// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// Copyright (C) 2009 James Turner +// SPDX-License-Identifier: GPL-2.0-or-later -#ifndef FG_ROUTE_HXX -#define FG_ROUTE_HXX +#pragma once // std #include #include #include +#include // Simgear #include @@ -88,14 +72,30 @@ typedef enum { RESTRICT_AT, RESTRICT_ABOVE, RESTRICT_BELOW, + RESTRICT_BETWEEN, SPEED_RESTRICT_MACH, ///< encode an 'AT' restriction in Mach, not IAS RESTRICT_DELETE, ///< ignore underlying restriction (on a leg) RESTRICT_COMPUTED, ///< data is computed, not a real restriction SPEED_COMPUTED_MACH ///< variant on above to encode a Mach value } RouteRestriction; +typedef enum { + DEFAULT_UNITS = 0, + ALTITUDE_FEET, + ALTITUDE_METER, + ALTITUDE_FLIGHTLEVEL, + SPEED_KNOTS, + SPEED_MACH, + SPEED_KPH +} RouteUnits; + +RouteRestriction restrictionFromString(const std::string& t); + bool isMachRestrict(RouteRestriction rr); - + +double convertSpeedUnits(RouteUnits aSrc, RouteUnits aDest, double aAltitudeFt, double aValue); +double convertAltitudeUnits(RouteUnits aSrc, RouteUnits aDest, double aValue); + /** * Abstract base class for waypoints (and things that are treated similarly * by navigation systems). More precisely this is route path elements, @@ -117,12 +117,14 @@ public: virtual FGPositioned* source() const { return nullptr; } - virtual double altitudeFt() const - { return _altitudeFt; } + double altitudeFt() const; - virtual double speed() const - { return _speed; } + double speed(RouteUnits aUnits = DEFAULT_UNITS) const; + double altitude(RouteUnits aUnits = DEFAULT_UNITS) const; + + double constraintAltitude(RouteUnits aUnits = DEFAULT_UNITS) const; + // wrapper - asserts if restriction type is _MACH double speedKts() const; @@ -135,9 +137,11 @@ public: virtual RouteRestriction speedRestriction() const { return _speedRestrict; } - void setAltitude(double aAlt, RouteRestriction aRestrict); - void setSpeed(double aSpeed, RouteRestriction aRestrict); + void setAltitude(double aAlt, RouteRestriction aRestrict, RouteUnits aUnits = DEFAULT_UNITS); + void setSpeed(double aSpeed, RouteRestriction aRestrict, RouteUnits aUnits = DEFAULT_UNITS); + void setConstraintAltitude(double aAlt); + /** * Identifier assoicated with the waypoint. Human-readable, but * possibly quite terse, and definitiely not unique. @@ -230,8 +234,16 @@ protected: typedef Waypt*(FactoryFunction)(RouteBase* aOwner); static void registerFactory(const std::string aNodeType, FactoryFunction* aFactory); - double _altitudeFt = 0.0; - double _speed = 0.0; // knots IAS or mach + double _altitude = 0.0; + /// some restriction types specify two altitudes, in which case this is the second value, corresponding to + /// AltitudeCons in the level-D XML procedures format. + std::optional _constraintAltitude; + + RouteUnits _altitudeUnits = ALTITUDE_FEET; + + double _speed = 0.0; + RouteUnits _speedUnits = SPEED_KNOTS; + RouteRestriction _altRestrict = RESTRICT_NONE; RouteRestriction _speedRestrict = RESTRICT_NONE; private: @@ -269,5 +281,3 @@ private: }; } // of namespace flightgear - -#endif // of FG_ROUTE_HXX diff --git a/src/Navaids/waypoint.cxx b/src/Navaids/waypoint.cxx index 539ddea4e..74671a84a 100644 --- a/src/Navaids/waypoint.cxx +++ b/src/Navaids/waypoint.cxx @@ -96,7 +96,7 @@ NavaidWaypoint::NavaidWaypoint(RouteBase* aOwner) : SGGeod NavaidWaypoint::position() const { - return SGGeod::fromGeodFt(_navaid->geod(), _altitudeFt); + return SGGeod::fromGeodFt(_navaid->geod(), altitudeFt()); } std::string NavaidWaypoint::ident() const @@ -170,7 +170,7 @@ void OffsetNavaidWaypoint::init() SGGeod offset; double az2; SGGeodesy::direct(_navaid->geod(), _radial, _distanceNm * SG_NM_TO_METER, offset, az2); - _geod = SGGeod::fromGeodFt(offset, _altitudeFt); + _geod = SGGeod::fromGeodFt(offset, altitude(ALTITUDE_FEET)); } bool OffsetNavaidWaypoint::initFromProperties(SGPropertyNode_ptr aProp) diff --git a/src/Scripting/NasalFlightPlan.cxx b/src/Scripting/NasalFlightPlan.cxx index 13d69f280..349882bb5 100644 --- a/src/Scripting/NasalFlightPlan.cxx +++ b/src/Scripting/NasalFlightPlan.cxx @@ -464,9 +464,27 @@ static RouteRestriction routeRestrictionFromArg(naRef arg) if (u == "mach") return SPEED_RESTRICT_MACH; if (u == "computed-mach") return SPEED_COMPUTED_MACH; if (u == "delete") return RESTRICT_DELETE; + if (u == "between") return RESTRICT_BETWEEN; return RESTRICT_NONE; }; +static RouteUnits routeUnitsFromArg(naRef arg) +{ + if (naIsNil(arg) || !naIsString(arg)) { + return DEFAULT_UNITS; + } + + const auto u = simgear::strutils::lowercase(naStr_data(arg)); + if ((u == "knots") || (u == "kt")) return SPEED_KNOTS; + if (u == "kph") return SPEED_KPH; + if (u == "mach") return SPEED_MACH; + if ((u == "ft") || (u == "\'") || (u == "feet")) return ALTITUDE_FEET; + if ((u == "m") || (u == "meter") || (u == "meters")) return ALTITUDE_METER; + if ((u == "fl") || (u == "flight-level")) return ALTITUDE_FLIGHTLEVEL; + + throw sg_format_exception("routeUnitsFromArg: unknown units", u); +} + naRef routeRestrictionToNasal(naContext c, RouteRestriction rr) { switch (rr) { @@ -478,6 +496,7 @@ naRef routeRestrictionToNasal(naContext c, RouteRestriction rr) case RESTRICT_COMPUTED: return stringToNasal(c, "computed"); case SPEED_COMPUTED_MACH: return stringToNasal(c, "computed-mach"); case RESTRICT_DELETE: return stringToNasal(c, "delete"); + case RESTRICT_BETWEEN: return stringToNasal(c, "between"); } return naNil(); @@ -1984,6 +2003,8 @@ static naRef f_leg_setSpeed(naContext c, naRef me, int argc, naRef* args) double speed = 0.0; RouteRestriction rr = RESTRICT_AT; + RouteUnits units = DEFAULT_UNITS; + if (argc > 0) { if (naIsNil(args[0])) { // clear the restriction to NONE @@ -1994,9 +2015,13 @@ static naRef f_leg_setSpeed(naContext c, naRef me, int argc, naRef* args) } else { naRuntimeError(c, "bad arguments to setSpeed"); } + + if (argc > 2) { + units = routeUnitsFromArg(args[2]); + } } - leg->setSpeed(rr, speed); + leg->setSpeed(rr, speed, units); } else { naRuntimeError(c, "bad arguments to setSpeed"); } @@ -2013,6 +2038,8 @@ static naRef f_leg_setAltitude(naContext c, naRef me, int argc, naRef* args) double altitude = 0.0; RouteRestriction rr = RESTRICT_AT; + RouteUnits units = DEFAULT_UNITS; + if (argc > 0) { if (naIsNil(args[0])) { // clear the restriction to NONE @@ -2023,9 +2050,13 @@ static naRef f_leg_setAltitude(naContext c, naRef me, int argc, naRef* args) } else { naRuntimeError(c, "bad arguments to leg.setAltitude"); } + + if (argc > 2) { + units = routeUnitsFromArg(args[2]); + } } - leg->setAltitude(rr, altitude); + leg->setAltitude(rr, altitude, units); } else { naRuntimeError(c, "bad arguments to setleg.setAltitude"); } @@ -2033,6 +2064,40 @@ static naRef f_leg_setAltitude(naContext c, naRef me, int argc, naRef* args) return naNil(); } +static naRef f_leg_altitude(naContext c, naRef me, int argc, naRef* args) +{ + FlightPlan::Leg* leg = fpLegGhost(me); + if (!leg) { + naRuntimeError(c, "leg.altitude called on non-flightplan-leg object"); + } + + RouteUnits units = DEFAULT_UNITS; + if (argc > 0) { + units = routeUnitsFromArg(args[9]); + } + + if (leg->altitudeRestriction() == RESTRICT_BETWEEN) { + // make a 2-tuple for the between case + } + + return naNum(leg->altitude(units)); +} + +static naRef f_leg_speed(naContext c, naRef me, int argc, naRef* args) +{ + FlightPlan::Leg* leg = fpLegGhost(me); + if (!leg) { + naRuntimeError(c, "leg.speed called on non-flightplan-leg object"); + } + + RouteUnits units = DEFAULT_UNITS; + if (argc > 0) { + units = routeUnitsFromArg(args[9]); + } + + return naNum(leg->speed(units)); +} + static naRef f_leg_courseAndDistanceFrom(naContext c, naRef me, int argc, naRef* args) { FlightPlan::Leg* leg = fpLegGhost(me); @@ -2301,6 +2366,8 @@ naRef initNasalFlightPlan(naRef globals, naContext c) naSave(c, fpLegPrototype); hashset(c, fpLegPrototype, "setSpeed", naNewFunc(c, naNewCCode(c, f_leg_setSpeed))); hashset(c, fpLegPrototype, "setAltitude", naNewFunc(c, naNewCCode(c, f_leg_setAltitude))); + hashset(c, fpLegPrototype, "altitude", naNewFunc(c, naNewCCode(c, f_leg_altitude))); + hashset(c, fpLegPrototype, "speed", naNewFunc(c, naNewCCode(c, f_leg_speed))); hashset(c, fpLegPrototype, "path", naNewFunc(c, naNewCCode(c, f_leg_path))); hashset(c, fpLegPrototype, "courseAndDistanceFrom", naNewFunc(c, naNewCCode(c, f_leg_courseAndDistanceFrom))); diff --git a/test_suite/unit_tests/Navaids/test_flightplan.cxx b/test_suite/unit_tests/Navaids/test_flightplan.cxx index df94bb40a..2ec7e5555 100644 --- a/test_suite/unit_tests/Navaids/test_flightplan.cxx +++ b/test_suite/unit_tests/Navaids/test_flightplan.cxx @@ -702,6 +702,77 @@ void FlightplanTests::testLoadSaveMachRestriction() CPPUNIT_ASSERT_EQUAL(8, f2->legAtIndex(2)->holdCount()); } +void FlightplanTests::testLoadSaveBetweenRestriction() +{ + const std::string fpXML = R"( + + 2 + + SAWG + 25 + + + SUMU + + + + navaid + PUGLI + -60.552200 + -40.490000 + + + basic + between + 36000 + 32000 + SV002 + -115.50531 + 37.89523 + + + navaid + SIGUL + -60.552200 + -40.490000 + + + + )"; + + std::istringstream stream(fpXML); + FlightPlanRef f = FlightPlan::create(); + bool ok = f->load(stream); + CPPUNIT_ASSERT(ok); + + auto leg = f->legAtIndex(1); + CPPUNIT_ASSERT_EQUAL(RESTRICT_BETWEEN, leg->altitudeRestriction()); + CPPUNIT_ASSERT_DOUBLES_EQUAL(36000, leg->altitudeFt(), 0.01); + CPPUNIT_ASSERT_DOUBLES_EQUAL(32000, leg->waypoint()->constraintAltitude(ALTITUDE_FEET), 0.01); +} + +void FlightplanTests::testRestrictionUnits() +{ + FlightPlanRef fp1 = makeTestFP("LIRF"s, "34L"s, "LEBL"s, "25R"s, + "ESINO GITRI BALEN MUREN TOSNU"s); + auto leg = fp1->legAtIndex(2); + leg->setAltitude(RESTRICT_ABOVE, 120, ALTITUDE_FLIGHTLEVEL); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->altitude(), 12000.0, 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->altitude(ALTITUDE_METER), 12000.0 * SG_FEET_TO_METER, 0.1); + + leg->setSpeed(RESTRICT_AT, 300); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->speedKts(), 300, 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->speed(), 300, 0.1); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->speed(SPEED_KPH), 300 * SG_KT_TO_MPS * SG_MPS_TO_KMH, 0.1); + + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->speedMach(), 0.473, 0.1); + + // setting a mach restriction assumes Mach units by default + leg->setSpeed(SPEED_RESTRICT_MACH, 0.7); + leg->setAltitude(RESTRICT_AT, 38000); + CPPUNIT_ASSERT_DOUBLES_EQUAL(leg->speedKts(), 401, 1); +} + void FlightplanTests::testBasicDiscontinuity() { FlightPlanRef fp1 = makeTestFP("LIRF"s, "34L"s, "LEBL"s, "25R"s, diff --git a/test_suite/unit_tests/Navaids/test_flightplan.hxx b/test_suite/unit_tests/Navaids/test_flightplan.hxx index d599fd976..c0982716d 100644 --- a/test_suite/unit_tests/Navaids/test_flightplan.hxx +++ b/test_suite/unit_tests/Navaids/test_flightplan.hxx @@ -58,9 +58,11 @@ class FlightplanTests : public CppUnit::TestFixture CPPUNIT_TEST(testViaInsertIntoFP); CPPUNIT_TEST(testViaInsertIntoRoute); CPPUNIT_TEST(loadFGFPAsRoute); - - // CPPUNIT_TEST(testParseICAORoute); - // CPPUNIT_TEST(testParseICANLowLevelRoute); + CPPUNIT_TEST(testLoadSaveBetweenRestriction); + CPPUNIT_TEST(testRestrictionUnits); + + // CPPUNIT_TEST(testParseICAORoute); + // CPPUNIT_TEST(testParseICANLowLevelRoute); CPPUNIT_TEST_SUITE_END(); public: @@ -100,6 +102,8 @@ public: void testViaInsertIntoFP(); void testViaInsertIntoRoute(); void loadFGFPAsRoute(); + void testLoadSaveBetweenRestriction(); + void testRestrictionUnits(); }; #endif // FG_FLIGHTPLAN_UNIT_TESTS_HXX