fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/Instrumentation/gps.cxx
James Turner 2295e82a0f Fix a use-after-free found by ASan.
2020-08-17 10:00:42 +01:00

1562 lines
44 KiB
C++
Raw Blame History

// gps.cxx - distance-measuring equipment.
// Written by David Megginson, started 2003.
//
// This file is in the Public Domain and comes with no warranty.
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include "gps.hxx"
#include <memory>
#include <set>
#include <cstring>
#include <cstdio>
#include "Main/fg_props.hxx"
#include "Main/globals.hxx" // for get_subsystem
#include "Main/util.hxx" // for fgLowPass
#include "Navaids/positioned.hxx"
#include <Navaids/waypoint.hxx>
#include "Navaids/navrecord.hxx"
#include "Navaids/FlightPlan.hxx"
#include <Navaids/routePath.hxx>
#include <Instrumentation/rnav_waypt_controller.hxx>
#include "Airports/airport.hxx"
#include "Airports/runways.hxx"
#include "Autopilot/route_mgr.hxx"
#include <simgear/math/sg_random.h>
#include <simgear/sg_inlines.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/structure/exception.hxx>
using std::unique_ptr;
using std::string;
using namespace flightgear;
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;
}
////////////////////////////////////////////////////////////////////////////
// configuration helper object
GPS::Config::Config() :
_enableTurnAnticipation(false),
_turnRate(3.0), // degrees-per-second, so 180 degree turn takes 60 seconds
_overflightDistance(0.02),
_overflightArmDistance(1.0),
_overflightArmAngle(90.0),
_waypointAlertTime(30.0),
_requireHardSurface(true),
_cdiMaxDeflectionNm(3.0), // linear mode, 3nm at the peg
_driveAutopilot(true),
_courseSelectable(false),
_followLegTrackToFix(true)
{
}
void GPS::Config::bind(GPS* aOwner, SGPropertyNode* aCfg)
{
aOwner->tie(aCfg, "turn-rate-deg-sec", SGRawValuePointer<double>(&_turnRate));
aOwner->tie(aCfg, "enable-fly-by", SGRawValuePointer<bool>(&_enableTurnAnticipation));
aOwner->tie(aCfg, "wpt-alert-time", SGRawValuePointer<double>(&_waypointAlertTime));
aOwner->tie(aCfg, "hard-surface-runways-only", SGRawValuePointer<bool>(&_requireHardSurface));
aOwner->tie(aCfg, "cdi-max-deflection-nm", SGRawValuePointer<double>(&_cdiMaxDeflectionNm));
aOwner->tie(aCfg, "drive-autopilot", SGRawValuePointer<bool>(&_driveAutopilot));
aOwner->tie(aCfg, "course-selectable", SGRawValuePointer<bool>(&_courseSelectable));
aOwner->tie(aCfg, "follow-leg-track-to-fix", SGRawValuePointer<bool>(&_followLegTrackToFix));
aOwner->tie(aCfg, "over-flight-distance-nm", SGRawValuePointer<double>(&_overflightDistance));
aOwner->tie(aCfg, "over-flight-arm-distance-nm", SGRawValuePointer<double>(&_overflightArmDistance));
aOwner->tie(aCfg, "over-flight-arm-angle-deg", SGRawValuePointer<double>(&_overflightArmAngle));
aOwner->tie(aCfg, "delegate-sequencing", SGRawValuePointer<bool>(&_delegateSequencing));
}
////////////////////////////////////////////////////////////////////////////
GPS::GPS ( SGPropertyNode *node, bool defaultGPSMode) :
_selectedCourse(0.0),
_desiredCourse(0.0),
_dataValid(false),
_lastPosValid(false),
_defaultGPSMode(defaultGPSMode),
_mode("init"),
_name(node->getStringValue("name", "gps")),
_num(node->getIntValue("number", 0)),
_callbackFlightPlanChanged(SGPropertyChangeCallback<GPS>(this,&GPS::routeManagerFlightPlanChanged,
fgGetNode("/autopilot/route-manager/signals/flightplan-changed", true))),
_callbackRouteActivated(SGPropertyChangeCallback<GPS>(this,&GPS::routeActivated,
fgGetNode("/autopilot/route-manager/active", true)))
{
string branch = "/instrumentation/" + _name;
_gpsNode = fgGetNode(branch.c_str(), _num, true );
_scratchNode = _gpsNode->getChild("scratch", 0, true);
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
_currentWayptNode = wp_node->getChild("wp", 1, true);
#if FG_210_COMPAT
_searchIsRoute = false;
_searchHasNext = false;
_searchType = FGPositioned::INVALID;
#endif
}
GPS::~GPS ()
{
}
void
GPS::init ()
{
_magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
_serviceable_node = _gpsNode->getChild("serviceable", 0, true);
_serviceable_node->setBoolValue(true);
_electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
// basic GPS outputs
_raim_node = _gpsNode->getChild("raim", 0, true);
_odometer_node = _gpsNode->getChild("odometer", 0, true);
_trip_odometer_node = _gpsNode->getChild("trip-odometer", 0, true);
_true_bug_error_node = _gpsNode->getChild("true-bug-error-deg", 0, true);
_magnetic_bug_error_node = _gpsNode->getChild("magnetic-bug-error-deg", 0, true);
_eastWestVelocity = _gpsNode->getChild("ew-velocity-msec", 0, true);
_northSouthVelocity = _gpsNode->getChild("ns-velocity-msec", 0, true);
// waypoints
// for compatibility, alias selected course down to wp/wp[1]/desired-course-deg
SGPropertyNode* wp1Crs = _currentWayptNode->getChild("desired-course-deg", 0, true);
wp1Crs->alias(_gpsNode->getChild("desired-course-deg", 0, true));
_tracking_bug_node = _gpsNode->getChild("tracking-bug", 0, true);
// route properties
// should these move to the route manager?
_routeDistanceNm = _gpsNode->getChild("route-distance-nm", 0, true);
_routeETE = _gpsNode->getChild("ETE", 0, true);
// navradio slaving properties
SGPropertyNode* toFlag = _gpsNode->getChild("to-flag", 0, true);
toFlag->alias(_currentWayptNode->getChild("to-flag"));
SGPropertyNode* fromFlag = _gpsNode->getChild("from-flag", 0, true);
fromFlag->alias(_currentWayptNode->getChild("from-flag"));
// autopilot drive properties
_apDrivingFlag = fgGetNode("/autopilot/settings/gps-driving-true-heading", true);
_apTrueHeading = fgGetNode("/autopilot/settings/true-heading-deg",true);
clearScratch();
clearOutput();
}
void
GPS::reinit ()
{
clearOutput();
}
void
GPS::bind()
{
_config.bind(this, _gpsNode->getChild("config", 0, true));
// basic GPS outputs
tie(_gpsNode, "selected-course-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getSelectedCourse, &GPS::setSelectedCourse));
tie(_gpsNode, "desired-course-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getDesiredCourse, NULL));
_desiredCourseNode = _gpsNode->getChild("desired-course-deg", 0, true);
tieSGGeodReadOnly(_gpsNode, _indicated_pos, "indicated-longitude-deg",
"indicated-latitude-deg", "indicated-altitude-ft");
tie(_gpsNode, "indicated-vertical-speed", SGRawValueMethods<GPS, double>
(*this, &GPS::getVerticalSpeed, NULL));
tie(_gpsNode, "indicated-track-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getTrueTrack, NULL));
tie(_gpsNode, "indicated-track-magnetic-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getMagTrack, NULL));
tie(_gpsNode, "indicated-ground-speed-kt", SGRawValueMethods<GPS, double>
(*this, &GPS::getGroundspeedKts, NULL));
// command system
tie(_gpsNode, "mode", SGRawValueMethods<GPS, const char*>(*this, &GPS::getMode, NULL));
tie(_gpsNode, "command", SGRawValueMethods<GPS, const char*>(*this, &GPS::getCommand, &GPS::setCommand));
tieSGGeod(_scratchNode, _scratchPos, "longitude-deg", "latitude-deg", "altitude-ft");
#if FG_210_COMPAT
tie(_scratchNode, "valid", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchValid, NULL));
tie(_scratchNode, "distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchDistance, NULL));
tie(_scratchNode, "true-bearing-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchTrueBearing, NULL));
tie(_scratchNode, "mag-bearing-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getScratchMagBearing, NULL));
tie(_scratchNode, "has-next", SGRawValueMethods<GPS, bool>(*this, &GPS::getScratchHasNext, NULL));
_scratchValid = false;
_scratchNode->setStringValue("type", "");
_scratchNode->setStringValue("query", "");
#endif
SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
SGPropertyNode* wp0_node = wp_node->getChild("wp", 0, true);
tieSGGeodReadOnly(wp0_node, _wp0_position, "longitude-deg", "latitude-deg", "altitude-ft");
tie(wp0_node, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP0Ident, NULL));
tie(wp0_node, "name", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP0Name, NULL));
tie(_currentWayptNode, "valid", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1IValid, NULL));
tie(_currentWayptNode, "ID", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Ident, NULL));
tie(_currentWayptNode, "name", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1Name, NULL));
tie(_currentWayptNode, "distance-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Distance, NULL));
tie(_currentWayptNode, "bearing-true-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1Bearing, NULL));
tie(_currentWayptNode, "bearing-mag-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1MagBearing, NULL));
tie(_currentWayptNode, "TTW-sec", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1TTW, NULL));
tie(_currentWayptNode, "TTW", SGRawValueMethods<GPS, const char*>
(*this, &GPS::getWP1TTWString, NULL));
tie(_currentWayptNode, "course-deviation-deg", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseDeviation, NULL));
tie(_currentWayptNode, "course-error-nm", SGRawValueMethods<GPS, double>
(*this, &GPS::getWP1CourseErrorNm, NULL));
tie(_currentWayptNode, "to-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1ToFlag, NULL));
tie(_currentWayptNode, "from-flag", SGRawValueMethods<GPS, bool>
(*this, &GPS::getWP1FromFlag, NULL));
// leg properties (only valid in DTO/LEG modes, not OBS)
tie(wp_node, "leg-distance-nm", SGRawValueMethods<GPS, double>(*this, &GPS::getLegDistance, NULL));
tie(wp_node, "leg-true-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegCourse, NULL));
tie(wp_node, "leg-mag-course-deg", SGRawValueMethods<GPS, double>(*this, &GPS::getLegMagCourse, NULL));
// navradio slaving properties
tie(_gpsNode, "cdi-deflection", SGRawValueMethods<GPS,double>
(*this, &GPS::getCDIDeflection));
}
void
GPS::unbind()
{
_tiedProperties.Untie();
_gpsNode.clear();
}
void
GPS::clearOutput()
{
_dataValid = false;
_last_speed_kts = 0.0;
_last_pos = SGGeod();
_lastPosValid = false;
_indicated_pos = SGGeod();
_last_vertical_speed = 0.0;
_last_true_track = 0.0;
_lastEWVelocity = _lastNSVelocity = 0.0;
_currentWaypt = _prevWaypt = NULL;
_legDistanceNm = -1.0;
_raim_node->setDoubleValue(0.0);
_indicated_pos = SGGeod();
_odometer_node->setDoubleValue(0);
_trip_odometer_node->setDoubleValue(0);
_tracking_bug_node->setDoubleValue(0);
_true_bug_error_node->setDoubleValue(0);
_magnetic_bug_error_node->setDoubleValue(0);
_northSouthVelocity->setDoubleValue(0.0);
_eastWestVelocity->setDoubleValue(0.0);
}
void
GPS::update (double delta_time_sec)
{
if (!_defaultGPSMode) {
// If it's off, don't bother.
// check if value is defined, since many aircraft don't define an output
// for the GPS, but expect the default one to work.
bool elecOn = !_electrical_node->hasValue() || _electrical_node->getBoolValue();
if (!_serviceable_node->getBoolValue() || !elecOn) {
clearOutput();
return;
}
}
if (delta_time_sec <= 0.0) {
return; // paused, don't bother
}
_raim_node->setDoubleValue(1.0);
_indicated_pos = globals->get_aircraft_position();
updateBasicData(delta_time_sec);
if (_dataValid) {
if (_wayptController.get()) {
_wayptController->update(delta_time_sec);
SGGeod p(_wayptController->position());
_currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
_currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
_currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
_desiredCourse = getLegMagCourse();
_gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
if (_wayptController->isDone()) {
doSequence();
}
updateRouteData();
}
updateTrackingBug();
driveAutopilot();
}
if (_dataValid && (_mode == "init")) {
// will select LEG mode if the route is active
routeManagerFlightPlanChanged(nullptr);
FGRouteMgr* routeMgr = globals->get_subsystem<FGRouteMgr>();
if (!routeMgr || !routeMgr->isRouteActive()) {
// initialise in OBS mode, with waypt set to the nearest airport.
// keep in mind at this point, _dataValid is not set
FGAirport::HardSurfaceFilter f;
FGPositionedRef apt = FGPositioned::findClosest(_indicated_pos, 20.0, &f);
if (apt) {
selectOBSMode(new flightgear::NavaidWaypoint(apt, nullptr));
} else {
selectOBSMode(nullptr);
}
}
if (_mode != "init")
{
// allow a realistic delay in the future, here
}
} // of init mode check
_last_pos = _indicated_pos;
_lastPosValid = !(_last_pos == SGGeod());
}
void GPS::shutdown()
{
if (_route) {
_route->removeDelegate(this);
_route = nullptr;
}
}
void GPS::routeManagerFlightPlanChanged(SGPropertyNode*)
{
if (_route) {
_route->removeDelegate(this);
}
SG_LOG(SG_INSTR, SG_DEBUG, "GPS saw route-manager flight-plan replaced.");
FGRouteMgr* routeMgr = globals->get_subsystem<FGRouteMgr>();
if (!routeMgr) {
return;
}
_route = routeMgr->flightPlan();
if (_route) {
_route->addDelegate(this);
}
if (routeMgr->isRouteActive()) {
selectLegMode();
} else {
selectOBSMode(_currentWaypt); // revert to OBS on current waypoint
}
}
void GPS::routeActivated(SGPropertyNode* aNode)
{
// if the delegate is handling this, don't do anything else ourselves
if (_config.delegateDoesSequencing()) {
return;
}
bool isActive = aNode->getBoolValue();
if (isActive) {
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");
sequence();
}
} else if (_mode == "leg") {
SG_LOG(SG_INSTR, SG_INFO, "GPS::route deactivated, switching to OBS mode");
selectOBSMode(_currentWaypt);
}
}
///////////////////////////////////////////////////////////////////////////
// implement the RNAV interface
SGGeod GPS::position()
{
if (!_dataValid) {
return SGGeod();
}
return _indicated_pos;
}
double GPS::trackDeg()
{
return _last_true_track;
}
double GPS::groundSpeedKts()
{
return _last_speed_kts;
}
double GPS::vspeedFPM()
{
return _last_vertical_speed;
}
double GPS::magvarDeg()
{
return _magvar_node->getDoubleValue();
}
double GPS::overflightDistanceM()
{
return _config.overflightDistanceNm() * SG_NM_TO_METER;
}
double GPS::overflightArmDistanceM()
{
return _config.overflightArmDistanceNm() * SG_NM_TO_METER;
}
double GPS::overflightArmAngleDeg()
{
return _config.overflightArmAngleDeg();
}
double GPS::selectedMagCourse()
{
return _selectedCourse;
}
simgear::optional<double> GPS::nextLegTrack()
{
auto next = _route->nextLeg();
if (!next)
return {};
return next->courseDeg();
}
simgear::optional<RNAV::LegData> GPS::previousLegData()
{
// if the previous controller computed valid data,
// use that. This ensures fly-by turns work out, especially
if (_wp0Data.has_value())
return _wp0Data;
// if we did not get data from the previous controller (eg, waypoint
// jumped or first waypoint), just compute the position
FlightPlan::Leg* leg = _route->previousLeg();
if (!leg) {
return {}; // no data
}
LegData legData;
Waypt* waypt = leg->waypoint();
legData.position = waypt->position();
// ensure computations use runway end, not threshold
if (waypt->type() == "runway") {
RunwayWaypt* rwpt = static_cast<RunwayWaypt*>(waypt);
legData.position = rwpt->runway()->end();
}
return legData;
}
bool GPS::canFlyBy() const
{
return _config.turnAnticipationEnabled();
}
///////////////////////////////////////////////////////////////////////////
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 );
// detect repositions
// setting this value high enough hypersonic aircraft but not spaceships
if ((distance_m / dt) > 100000.0) {
SG_LOG(SG_INSTR, SG_DEBUG, "GPS detected reposition, resetting data");
_dataValid = false;
return;
}
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/5.0);
_last_speed_kts = speed_kt;
SGGeod g = _indicated_pos;
g.setLongitudeDeg(_last_pos.getLongitudeDeg());
double northSouthM = dist(SGVec3d::fromGeod(_last_pos), SGVec3d::fromGeod(g));
northSouthM = copysign(northSouthM, _indicated_pos.getLatitudeDeg() - _last_pos.getLatitudeDeg());
double nsMSec = fgGetLowPass(_lastNSVelocity, northSouthM / dt, dt/2.0);
_lastNSVelocity = nsMSec;
_northSouthVelocity->setDoubleValue(nsMSec);
g = _indicated_pos;
g.setLatitudeDeg(_last_pos.getLatitudeDeg());
double eastWestM = dist(SGVec3d::fromGeod(_last_pos), SGVec3d::fromGeod(g));
eastWestM = copysign(eastWestM, _indicated_pos.getLongitudeDeg() - _last_pos.getLongitudeDeg());
double ewMSec = fgGetLowPass(_lastEWVelocity, eastWestM / dt, dt/2.0);
_lastEWVelocity = ewMSec;
_eastWestVelocity->setDoubleValue(ewMSec);
double odometer = _odometer_node->getDoubleValue();
_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
odometer = _trip_odometer_node->getDoubleValue();
_trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
if (!_dataValid) {
_dataValid = true;
}
}
void
GPS::updateTrackingBug()
{
double tracking_bug = _tracking_bug_node->getDoubleValue();
double true_bug_error = tracking_bug - getTrueTrack();
double magnetic_bug_error = tracking_bug - getMagTrack();
// Get the errors into the (-180,180) range.
SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
_true_bug_error_node->setDoubleValue(true_bug_error);
_magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
}
void GPS::currentWaypointChanged()
{
_wp0Data.reset();
if (!_route) {
return;
}
int index = _route->currentIndex(),
count = _route->numLegs();
if ((index < 0) || (index >= count)) {
_currentWaypt=nullptr;
_prevWaypt=nullptr;
// no active leg on the route
return;
}
if (index > 0) {
_prevWaypt = _route->previousLeg()->waypoint();
if (_prevWaypt->flag(WPT_DYNAMIC)) {
_wp0_position = _indicated_pos;
} else {
_wp0_position = _prevWaypt->position();
}
}
_currentWaypt = _route->currentLeg()->waypoint();
if (_wayptController && (_wayptController->waypoint() == _prevWaypt)) {
// capture leg data form the controller, before we destroy it
_wp0Data = _wayptController->legData();
}
wp1Changed(); // rebuild the active controller
_desiredCourse = getLegMagCourse();
_desiredCourseNode->fireValueChanged();
}
void GPS::waypointsChanged()
{
if (_mode != "leg") {
return;
}
SG_LOG(SG_INSTR, SG_DEBUG, "GPS route edited while in LEG mode, updating waypoints");
currentWaypointChanged();
}
void GPS::doSequence()
{
if (!_route) {
return;
}
if (_config.delegateDoesSequencing()) {
// new style behaviour : let the delegate handle it
_route->sequence();
} else {
// backwards compatible behaviour : we do it ourselves
if (_mode == "dto") {
SG_LOG(SG_INSTR, SG_INFO, "GPS DTO reached destination point");
if (_route && _route->isActive()) {
// check if our destination point is on the active route,
// in which case resume leg mode
const int index = _route->findWayptIndex(_currentWaypt->position());
if (index >= 0) {
SG_LOG(SG_INSTR, SG_INFO, "GPS DTO resuming LEG mode at " << _route->legAtIndex(index)->waypoint()->ident());
_mode = "leg";
_route->setCurrentIndex(index);
return;
}
}
// if we didn't enter LEG mode, drop back to OBS
selectOBSMode(_currentWaypt);
} else if (_mode == "leg") {
const int nextIndex = _route->currentIndex() + 1;
if (nextIndex >= _route->numLegs()) {
SG_LOG(SG_INSTR, SG_INFO, "GPS built-in sequencing, reached end of route,");
_route->finish();
selectOBSMode(_currentWaypt);
} else {
// sequence ourselves
_route->setCurrentIndex(nextIndex);
}
}
}
}
void GPS::cleared()
{
// if the aircraft delegates handle sequencing, don't do
// anything here
if (_config.delegateDoesSequencing()) {
return;
}
// backwards compatability : select OBS mode
if (_mode == "leg") {
selectOBSMode(_currentWaypt);
}
}
void GPS::endOfFlightPlan()
{
// backwards compatability - same logic as 'cleared', revert to OBS mode
// if we're in leg mode
cleared();
}
double GPS::turnRadiusNm(double groundSpeedKts)
{
return computeTurnRadiusNm(groundSpeedKts);
}
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 = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
if (_route) {
// walk all waypoints from wp2 to route end, and sum
for (int i=_route->currentIndex()+1; i<_route->numLegs(); ++i) {
auto leg = _route->legAtIndex(i);
// omit missed-approach waypoints in distance calculation
if (leg->waypoint()->flag(WPT_MISS))
continue;
totalDistance += leg->distanceNm();
}
}
_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() || !_defaultGPSMode) {
_apDrivingFlag->setBoolValue(false);
return;
}
// compatibility feature - allow the route-manager / GPS to drive the
// generic autopilot heading hold *in leg mode only*
bool drive = _mode == "leg";
_apDrivingFlag->setBoolValue(drive);
if (drive) {
// FIXME: we want to set desired track, not heading, here
_apTrueHeading->setDoubleValue(getWP1Bearing());
}
}
void GPS::wp1Changed()
{
if (!_currentWaypt)
return;
if (_mode == "leg") {
_wayptController.reset(WayptController::createForWaypt(this, _currentWaypt));
if (_currentWaypt->type() == "hold") {
// pass the hold count through
auto leg = _route->currentLeg();
auto holdCtl = static_cast<flightgear::HoldCtl*>(_wayptController.get());
holdCtl->setHoldCount(leg->holdCount());
}
} else if (_mode == "obs") {
_wayptController.reset(new OBSController(this, _currentWaypt));
} else if (_mode == "dto") {
_wayptController.reset(new DirectToController(this, _currentWaypt, _wp0_position));
}
_wayptController->init();
_gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
if (_mode == "obs") {
_legDistanceNm = -1.0;
} else {
_wayptController->update(0.0);
_gpsNode->setStringValue("rnav-controller-status", _wayptController->status());
_legDistanceNm = _wayptController->distanceToWayptM() * SG_METER_TO_NM;
// synchronise these properties immediately
SGGeod p(_wayptController->position());
_currentWayptNode->setDoubleValue("longitude-deg", p.getLongitudeDeg());
_currentWayptNode->setDoubleValue("latitude-deg", p.getLatitudeDeg());
_currentWayptNode->setDoubleValue("altitude-ft", p.getElevationFt());
_desiredCourse = getLegMagCourse();
}
}
/////////////////////////////////////////////////////////////////////////////
// property getter/setters
void GPS::setSelectedCourse(double crs)
{
if (_selectedCourse == crs) {
return;
}
_selectedCourse = crs;
if ((_mode == "obs") || _config.courseSelectable()) {
_desiredCourse = _selectedCourse;
_desiredCourseNode->fireValueChanged();
}
}
double GPS::getLegDistance() const
{
if (!_dataValid || (_mode == "obs")) {
return -1;
}
return _legDistanceNm;
}
double GPS::getLegCourse() const
{
if (!_dataValid || !_wayptController.get()) {
return -9999.0;
}
return _wayptController->targetTrackDeg();
}
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::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") || !_prevWaypt) {
return "";
}
// work around std::string::c_str() storage lifetime with libc++
// real fix is to allow tie-ing with std::string instead of char*
static char identBuf[16];
strncpy(identBuf, _prevWaypt->ident().c_str(), 15);
return identBuf;
}
const char* GPS::getWP0Name() const
{
if (!_dataValid || !_prevWaypt || !_prevWaypt->source()) {
return "";
}
return _prevWaypt->source()->name().c_str();
}
bool GPS::getWP1IValid() const
{
return _dataValid && _currentWaypt.get();
}
const char* GPS::getWP1Ident() const
{
if (!_dataValid || !_currentWaypt) {
return "";
}
// work around std::string::c_str() storage lifetime with libc++
// real fix is to allow tie-ing with std::string instead of char*
static char identBuf[16];
strncpy(identBuf, _currentWaypt->ident().c_str(), 15);
return identBuf;
}
const char* GPS::getWP1Name() const
{
if (!_dataValid || !_currentWaypt || !_currentWaypt->source()) {
return "";
}
return _currentWaypt->source()->name().c_str();
}
double GPS::getWP1Distance() const
{
if (!_dataValid || !_wayptController.get()) {
return -1.0;
}
return _wayptController->distanceToWayptM() * SG_METER_TO_NM;
}
double GPS::getWP1TTW() const
{
if (!_dataValid || !_wayptController.get()) {
return -1.0;
}
return _wayptController->timeToWaypt();
}
const char* GPS::getWP1TTWString() const
{
double t = getWP1TTW();
if (t <= 0.0) {
return "";
}
return makeTTWString(t);
}
double GPS::getWP1Bearing() const
{
if (!_dataValid || !_wayptController.get()) {
return -9999.0;
}
return _wayptController->trueBearingDeg();
}
double GPS::getWP1MagBearing() const
{
if (!_dataValid || !_wayptController.get()) {
return -9999.0;
}
double magBearing = _wayptController->trueBearingDeg() - _magvar_node->getDoubleValue();
SG_NORMALIZE_RANGE(magBearing, 0.0, 360.0);
return magBearing;
}
double GPS::getWP1CourseDeviation() const
{
if (!_dataValid || !_wayptController.get()) {
return 0.0;
}
return _wayptController->courseDeviationDeg();
}
double GPS::getWP1CourseErrorNm() const
{
if (!_dataValid || !_wayptController.get()) {
return 0.0;
}
return _wayptController->xtrackErrorNm();
}
bool GPS::getWP1ToFlag() const
{
if (!_dataValid || !_wayptController.get()) {
return false;
}
return _wayptController->toFlag();
}
bool GPS::getWP1FromFlag() const
{
if (!_dataValid || !_wayptController.get()) {
return false;
}
return !getWP1ToFlag();
}
#if FG_210_COMPAT
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;
}
#endif
/////////////////////////////////////////////////////////////////////////////
// scratch system
void GPS::setCommand(const char* aCmd)
{
SG_LOG(SG_INSTR, SG_DEBUG, "GPS command:" << aCmd);
if (!strcmp(aCmd, "direct")) {
directTo();
} else if (!strcmp(aCmd, "obs")) {
// valid scratch data is always used, if it's not valid we will
// use the current waypoint if one exists
selectOBSMode(isScratchPositionValid() ? nullptr : _currentWaypt);
} else if (!strcmp(aCmd, "leg")) {
selectLegMode();
} else if (!strcmp(aCmd, "exit-hold")) {
commandExitHold();
#if FG_210_COMPAT
} 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 || (_route->numLegs() == 0)) {
index = _route->numLegs();
} else if (index >= _route->numLegs()) {
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 || (_route->numLegs() == 0)) {
index = _route->numLegs();
} else if (index >= _route->numLegs()) {
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 = _route->numLegs();
} else if (index >= _route->numLegs()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:route-delete, bad index:" << index);
return;
}
removeWaypointAtIndex(index);
#endif
} else {
SG_LOG(SG_INSTR, SG_WARN, "GPS:unrecognized command:" << aCmd);
}
}
void GPS::clearScratch()
{
_scratchPos = SGGeod::fromDegFt(-9999.0, -9999.0, -9999.0);
_scratchNode->setBoolValue("valid", false);
#if FG_210_COMPAT
_scratchNode->setStringValue("type", "");
_scratchNode->setStringValue("query", "");
#endif
}
bool GPS::isScratchPositionValid() const
{
if ((_scratchPos.getLongitudeDeg() < -9990.0) ||
(_scratchPos.getLatitudeDeg() < -9990.0)) {
return false;
}
return true;
}
FGPositionedRef GPS::positionedFromScratch() const
{
if (!isScratchPositionValid()) {
return NULL;
}
std::string ident(_scratchNode->getStringValue("ident"));
return FGPositioned::findClosestWithIdent(ident, _scratchPos);
}
void GPS::directTo()
{
if (!isScratchPositionValid()) {
return;
}
_prevWaypt = NULL;
_wp0_position = _indicated_pos;
FGPositionedRef pos = positionedFromScratch();
if (pos) {
_currentWaypt = new NavaidWaypoint(pos, NULL);
} else {
_currentWaypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
}
_mode = "dto";
wp1Changed();
}
void GPS::selectOBSMode(flightgear::Waypt* waypt)
{
if (!waypt && isScratchPositionValid()) {
FGPositionedRef pos = positionedFromScratch();
if (pos) {
waypt = new NavaidWaypoint(pos, NULL);
} else {
waypt = new BasicWaypt(_scratchPos, _scratchNode->getStringValue("ident"), NULL);
}
}
_mode = "obs";
_prevWaypt = NULL;
_wp0_position = _indicated_pos;
_currentWaypt = waypt;
wp1Changed();
}
void GPS::selectLegMode()
{
if (_mode == "leg") {
return;
}
if (!_route) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:selectLegMode: no active route");
return;
}
_mode = "leg";
// clear any previous leg data which might be hanging around
// note this means you can mess up fly-by by toggling into and out LEG
// mode, but this seems reasonable
_wp0Data.reset();
// 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
currentWaypointChanged();
}
#if FG_210_COMPAT
void GPS::loadRouteWaypoint()
{
_scratchValid = false;
int index = _scratchNode->getIntValue("index", -9999);
clearScratch();
if ((index < 0) || (index >= _route->numLegs())) { // no index supplied, use current wp
index = _route->currentIndex();
}
_searchIsRoute = true;
setScratchFromRouteWaypoint(index);
}
void GPS::setScratchFromRouteWaypoint(int aIndex)
{
assert(_searchIsRoute);
if ((aIndex < 0) || (aIndex >= _route->numLegs())) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromRouteWaypoint: route-index out of bounds");
return;
}
_searchResultIndex = aIndex;
WayptRef wp = _route->legAtIndex(aIndex)->waypoint();
_scratchNode->setStringValue("ident", wp->ident());
_scratchPos = wp->position();
_scratchValid = true;
_scratchNode->setIntValue("index", aIndex);
}
void GPS::loadNearest()
{
string sty(_scratchNode->getStringValue("type"));
FGPositioned::Type ty = FGPositioned::typeFromName(sty);
if (ty == FGPositioned::INVALID) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:loadNearest: request type is invalid:" << sty);
return;
}
unique_ptr<FGPositioned::Filter> f(createFilter(ty));
int limitCount = _scratchNode->getIntValue("max-results", 1);
double cutoffDistance = _scratchNode->getDoubleValue("cutoff-nm", 400.0);
SGGeod searchPos = _indicated_pos;
if (isScratchPositionValid()) {
searchPos = _scratchPos;
}
clearScratch(); // clear now, regardless of whether we find a match or not
_searchResults =
FGPositioned::findClosestN(searchPos, limitCount, cutoffDistance, f.get());
_searchResultIndex = 0;
_searchIsRoute = false;
if (_searchResults.empty()) {
return;
}
setScratchFromCachedSearchResult();
}
bool GPS::SearchFilter::pass(FGPositioned* aPos) const
{
switch (aPos->type()) {
case FGPositioned::AIRPORT:
// heliport and seaport too?
case FGPositioned::VOR:
case FGPositioned::NDB:
case FGPositioned::FIX:
case FGPositioned::TACAN:
case FGPositioned::WAYPOINT:
return true;
default:
return false;
}
}
FGPositioned::Type GPS::SearchFilter::minType() const
{
return FGPositioned::AIRPORT;
}
FGPositioned::Type GPS::SearchFilter::maxType() const
{
return FGPositioned::VOR;
}
FGPositioned::Filter* GPS::createFilter(FGPositioned::Type aTy)
{
if (aTy == FGPositioned::AIRPORT) {
return new FGAirport::HardSurfaceFilter();
}
// if we were passed INVALID, assume it means 'all types interesting to a GPS'
if (aTy == FGPositioned::INVALID) {
return new SearchFilter;
}
return new FGPositioned::TypeFilter(aTy);
}
void GPS::search()
{
// parse search terms into local members, and exec the first search
string sty(_scratchNode->getStringValue("type"));
_searchType = FGPositioned::typeFromName(sty);
_searchQuery = _scratchNode->getStringValue("query");
if (_searchQuery.empty()) {
SG_LOG(SG_INSTR, SG_WARN, "empty GPS search query");
clearScratch();
return;
}
_searchExact = _scratchNode->getBoolValue("exact", true);
_searchResultIndex = 0;
_searchIsRoute = false;
unique_ptr<FGPositioned::Filter> f(createFilter(_searchType));
if (_searchNames) {
_searchResults = FGPositioned::findAllWithName(_searchQuery, f.get(), _searchExact);
} else {
_searchResults = FGPositioned::findAllWithIdent(_searchQuery, f.get(), _searchExact);
}
bool orderByRange = _scratchNode->getBoolValue("order-by-distance", true);
if (orderByRange) {
FGPositioned::sortByRange(_searchResults, _indicated_pos);
}
if (_searchResults.empty()) {
clearScratch();
return;
}
setScratchFromCachedSearchResult();
}
bool GPS::getScratchHasNext() const
{
int lastResult;
if (_searchIsRoute) {
lastResult = _route->numLegs() - 1;
} else {
lastResult = (int) _searchResults.size() - 1;
}
if (lastResult < 0) { // search array might be empty
return false;
}
return (_searchResultIndex < lastResult);
}
void GPS::setScratchFromCachedSearchResult()
{
int index = _searchResultIndex;
if ((index < 0) || (index >= (int) _searchResults.size())) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:setScratchFromCachedSearchResult: index out of bounds:" << index);
return;
}
setScratchFromPositioned(_searchResults[index], index);
}
void GPS::setScratchFromPositioned(FGPositioned* aPos, int aIndex)
{
clearScratch();
assert(aPos);
_scratchPos = aPos->geod();
_scratchNode->setStringValue("name", aPos->name());
_scratchNode->setStringValue("ident", aPos->ident());
_scratchNode->setStringValue("type", FGPositioned::nameForType(aPos->type()));
if (aIndex >= 0) {
_scratchNode->setIntValue("index", aIndex);
}
_scratchValid = true;
_scratchNode->setIntValue("result-count", _searchResults.size());
switch (aPos->type()) {
case FGPositioned::VOR:
_scratchNode->setDoubleValue("frequency-mhz", static_cast<FGNavRecord*>(aPos)->get_freq() / 100.0);
break;
case FGPositioned::NDB:
_scratchNode->setDoubleValue("frequency-khz", static_cast<FGNavRecord*>(aPos)->get_freq() / 100.0);
break;
case FGPositioned::AIRPORT:
addAirportToScratch((FGAirport*)aPos);
break;
default:
// no-op
break;
}
// look for being on the route and set?
}
void GPS::addAirportToScratch(FGAirport* aAirport)
{
for (unsigned int r=0; r<aAirport->numRunways(); ++r) {
SGPropertyNode* rwyNd = _scratchNode->getChild("runways", r, true);
FGRunway* rwy = aAirport->getRunwayByIndex(r);
// TODO - filter out unsuitable runways in the future
// based on config again
rwyNd->setStringValue("id", rwy->ident().c_str());
rwyNd->setIntValue("length-ft", rwy->lengthFt());
rwyNd->setIntValue("width-ft", rwy->widthFt());
rwyNd->setIntValue("heading-deg", rwy->headingDeg());
// map surface code to a string
// TODO - lighting information
if (rwy->ILS()) {
rwyNd->setDoubleValue("ils-frequency-mhz", rwy->ILS()->get_freq() / 100.0);
}
} // of runways iteration
}
void GPS::nextResult()
{
if (!getScratchHasNext()) {
return;
}
clearScratch();
if (_searchIsRoute) {
setScratchFromRouteWaypoint(++_searchResultIndex);
} else {
++_searchResultIndex;
setScratchFromCachedSearchResult();
}
}
void GPS::previousResult()
{
if (_searchResultIndex <= 0) {
return;
}
clearScratch();
--_searchResultIndex;
if (_searchIsRoute) {
setScratchFromRouteWaypoint(_searchResultIndex);
} else {
setScratchFromCachedSearchResult();
}
}
void GPS::defineWaypoint()
{
if (!isScratchPositionValid()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: invalid lat/lon");
return;
}
string ident = _scratchNode->getStringValue("ident");
if (ident.size() < 2) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: waypoint identifier must be at least two characters");
return;
}
// check for duplicate idents
FGPositioned::TypeFilter f(FGPositioned::WAYPOINT);
FGPositionedList dups = FGPositioned::findAllWithIdent(ident, &f);
if (!dups.empty()) {
SG_LOG(SG_INSTR, SG_WARN, "GPS:defineWaypoint: non-unique waypoint identifier, ho-hum");
}
SG_LOG(SG_INSTR, SG_INFO, "GPS:defineWaypoint: creating waypoint:" << ident);
FGPositionedRef wpt = FGPositioned::createUserWaypoint(ident, _scratchPos);
_searchResults.clear();
_searchResults.push_back(wpt);
setScratchFromPositioned(wpt.get(), -1);
}
void GPS::insertWaypointAtIndex(int aIndex)
{
// note we do allow index = routeMgr->size(), that's an append
if ((aIndex < 0) || (aIndex > _route->numLegs())) {
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");
WayptRef wpt = new BasicWaypt(_scratchPos, ident, NULL);
_route->insertWayptAtIndex(wpt, aIndex);
}
void GPS::removeWaypointAtIndex(int aIndex)
{
if ((aIndex < 0) || (aIndex >= _route->numLegs())) {
throw sg_range_exception("GPS::removeWaypointAtIndex: index out of bounds");
}
_route->deleteIndex(aIndex);
}
#endif // of FG_210_COMPAT
void GPS::tieSGGeod(SGPropertyNode* aNode, SGGeod& aRef,
const char* lonStr, const char* latStr, const char* altStr)
{
tie(aNode, lonStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLongitudeDeg, &SGGeod::setLongitudeDeg));
tie(aNode, latStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLatitudeDeg, &SGGeod::setLatitudeDeg));
if (altStr) {
tie(aNode, altStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getElevationFt, &SGGeod::setElevationFt));
}
}
void GPS::tieSGGeodReadOnly(SGPropertyNode* aNode, SGGeod& aRef,
const char* lonStr, const char* latStr, const char* altStr)
{
tie(aNode, lonStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLongitudeDeg, NULL));
tie(aNode, latStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getLatitudeDeg, NULL));
if (altStr) {
tie(aNode, altStr, SGRawValueMethods<SGGeod, double>(aRef, &SGGeod::getElevationFt, NULL));
}
}
void GPS::commandExitHold()
{
if (_currentWaypt && (_currentWaypt->type() == "hold")) {
auto holdCtl = static_cast<flightgear::HoldCtl*>(_wayptController.get());
holdCtl->exitHold();
} else {
SG_LOG(SG_INSTR, SG_WARN, "GPS:exit hold requested, but not currently in a hold");
}
}
// Register the subsystem.
#if 0
SGSubsystemMgr::InstancedRegistrant<GPS> registrantGPS(
SGSubsystemMgr::FDM,
{{"instrumentation", SGSubsystemMgr::Dependency::HARD}});
#endif
// end of gps.cxx
View git blame Copy permalink