From 0dbe9a4c38c0471fd721c76f2e850d9006da3804 Mon Sep 17 00:00:00 2001 From: daveluff Date: Wed, 30 Nov 2005 00:47:41 +0000 Subject: [PATCH] Code to extend FGs current GPS functionality to allow the simulation of a specific approach capable GPS. Eventually this file should be broken up and the code removed to other files, such as FGs current gps files --- src/Instrumentation/dclgps.cxx | 1716 ++++++++++++++++++++++++++++++++ src/Instrumentation/dclgps.hxx | 529 ++++++++++ 2 files changed, 2245 insertions(+) create mode 100644 src/Instrumentation/dclgps.cxx create mode 100644 src/Instrumentation/dclgps.hxx diff --git a/src/Instrumentation/dclgps.cxx b/src/Instrumentation/dclgps.cxx new file mode 100644 index 000000000..f5d31b7e9 --- /dev/null +++ b/src/Instrumentation/dclgps.cxx @@ -0,0 +1,1716 @@ +// dclgps.cxx - a class to extend the operation of FG's current GPS +// code, and provide support for a KLN89-specific instrument. It +// is envisioned that eventually this file and class will be split +// up between current FG code and new KLN89-specific code and removed. +// +// Written by David Luff, started 2005. +// +// Copyright (C) 2005 - David C Luff - david.luff@nottingham.ac.uk +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +#include "dclgps.hxx" + +#include +#include +SG_USING_STD(cout); + +//using namespace std; + +// Command callbacks for FlightGear + +static bool do_kln89_msg_pressed(const SGPropertyNode* arg) { + //cout << "do_kln89_msg_pressed called!\n"; + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->MsgPressed(); + return(true); +} + +static bool do_kln89_obs_pressed(const SGPropertyNode* arg) { + //cout << "do_kln89_obs_pressed called!\n"; + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->OBSPressed(); + return(true); +} + +static bool do_kln89_alt_pressed(const SGPropertyNode* arg) { + //cout << "do_kln89_alt_pressed called!\n"; + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->AltPressed(); + return(true); +} + +static bool do_kln89_nrst_pressed(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->NrstPressed(); + return(true); +} + +static bool do_kln89_dto_pressed(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->DtoPressed(); + return(true); +} + +static bool do_kln89_clr_pressed(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->ClrPressed(); + return(true); +} + +static bool do_kln89_ent_pressed(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->EntPressed(); + return(true); +} + +static bool do_kln89_crsr_pressed(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->CrsrPressed(); + return(true); +} + +static bool do_kln89_knob1left1(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->Knob1Left1(); + return(true); +} + +static bool do_kln89_knob1right1(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->Knob1Right1(); + return(true); +} + +static bool do_kln89_knob2left1(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->Knob2Left1(); + return(true); +} + +static bool do_kln89_knob2right1(const SGPropertyNode* arg) { + DCLGPS* gps = (DCLGPS*)globals->get_subsystem("kln89"); + gps->Knob2Right1(); + return(true); +} + +// End command callbacks + +GPSWaypoint::GPSWaypoint() { + appType = GPS_APP_NONE; +} + +GPSWaypoint::~GPSWaypoint() {} + +string GPSWaypoint::GetAprId() { + if(appType == GPS_IAF) return(id + 'i'); + else if(appType == GPS_FAF) return(id + 'f'); + else if(appType == GPS_MAP) return(id + 'm'); + else if(appType == GPS_MAHP) return(id + 'h'); + else return(id); +} + +ostream& operator << (ostream& os, GPSAppWpType type) { + switch(type) { + case(GPS_IAF): return(os << "IAF"); + case(GPS_IAP): return(os << "IAP"); + case(GPS_FAF): return(os << "FAF"); + case(GPS_MAP): return(os << "MAP"); + case(GPS_MAHP): return(os << "MAHP"); + case(GPS_HDR): return(os << "HEADER"); + case(GPS_FENCE): return(os << "FENCE"); + case(GPS_APP_NONE): return(os << "NONE"); + } + return(os << "ERROR - Unknown switch in GPSAppWpType operator << "); +} + +FGIAP::FGIAP() { +} + +FGIAP::~FGIAP() { +} + +FGNPIAP::FGNPIAP() { +} + +FGNPIAP::~FGNPIAP() { +} + +GPSPage::GPSPage(DCLGPS* parent) { + _parent = parent; + _subPage = 0; +} + +GPSPage::~GPSPage() { +} + +void GPSPage::Update(double dt) {} + +void GPSPage::Knob1Left1() {} +void GPSPage::Knob1Right1() {} + +void GPSPage::Knob2Left1() { + _parent->_activePage->LooseFocus(); + _subPage--; + if(_subPage < 0) _subPage = _nSubPages - 1; +} + +void GPSPage::Knob2Right1() { + _parent->_activePage->LooseFocus(); + _subPage++; + if(_subPage >= _nSubPages) _subPage = 0; +} + +void GPSPage::CrsrPressed() {} +void GPSPage::EntPressed() {} +void GPSPage::ClrPressed() {} +void GPSPage::DtoPressed() {} +void GPSPage::NrstPressed() {} +void GPSPage::AltPressed() {} +void GPSPage::OBSPressed() {} +void GPSPage::MsgPressed() {} + +string GPSPage::GPSitoa(int n) { + char buf[4]; + // TODO - sanity check n! + sprintf(buf, "%i", n); + string s = buf; + return(s); +} + +void GPSPage::CleanUp() {} +void GPSPage::LooseFocus() {} +void GPSPage::SetId(string s) {} +string GPSPage::GetId() { return(""); } + +// ------------------------------------------------------------------------------------- // + +DCLGPS::DCLGPS(RenderArea2D* instrument) { + _instrument = instrument; + _nFields = 1; + _maxFields = 2; + _pages.clear(); + + // Units - lets default to US units - FG can set them to other units from config during startup if desired. + _altUnits = GPS_ALT_UNITS_FT; + _baroUnits = GPS_PRES_UNITS_IN; + _velUnits = GPS_VEL_UNITS_KT; + _distUnits = GPS_DIST_UNITS_NM; + + _lon_node = fgGetNode("/instrumentation/gps/indicated-longitude-deg", true); + _lat_node = fgGetNode("/instrumentation/gps/indicated-latitude-deg", true); + _alt_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true); + _grnd_speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true); + _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true); + _mag_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true); + + // Use FG's position values at construction in case FG's gps has not run first update yet. + _lon = fgGetDouble("/position/longitude-deg") * SG_DEGREES_TO_RADIANS; + _lat = fgGetDouble("/position/latitude-deg") * SG_DEGREES_TO_RADIANS; + _alt = fgGetDouble("/position/altitude-ft"); + // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG + // gps code and not our own. + _gpsLon = _lon; + _gpsLat = _lat; + _checkLon = _gpsLon; + _checkLat = _gpsLat; + _groundSpeed_ms = 0.0; + _groundSpeed_kts = 0.0; + _track = 0.0; + _magTrackDeg = 0.0; + + // Sensible defaults. These can be overriden by derived classes if desired. + _cdiScales.clear(); + _cdiScales.push_back(5.0); + _cdiScales.push_back(1.0); + _cdiScales.push_back(0.3); + _currentCdiScaleIndex = 0; + _targetCdiScaleIndex = 0; + _sourceCdiScaleIndex = 0; + _cdiScaleTransition = false; + _currentCdiScale = 5.0; + + _cleanUpPage = -1; + + _activeWaypoint.id.clear(); + _dist2Act = 0.0; + _crosstrackDist = 0.0; + _headingBugTo = true; + _navFlagged = true; + _waypointAlert = false; + _departed = false; + _departureTimeString = "----"; + _elapsedTime = 0.0; + + // Configuration Initialisation + // Should this be in kln89.cxx ? + _turnAnticipationEnabled = false; + _suaAlertEnabled = false; + _altAlertEnabled = false; + + _time = new SGTime; + + _messageStack.clear(); + + _dto = false; + + _approachLoaded = false; + _approachArm = false; + _approachReallyArmed = false; + _approachActive = false; + _approachFP = new GPSFlightPlan; +} + +DCLGPS::~DCLGPS() { + delete _time; + for(gps_waypoint_map_iterator itr = _waypoints.begin(); itr != _waypoints.end(); ++itr) { + for(unsigned int i = 0; i < (*itr).second.size(); ++i) { + delete(((*itr).second)[i]); + } + } + delete _approachFP; // Don't need to delete the waypoints inside since they point to + // the waypoints in the approach database. + // TODO - may need to delete the approach database!! +} + +void DCLGPS::draw() { + //cout << "draw called!\n"; + _instrument->draw(); +} + +void DCLGPS::init() { + globals->get_commands()->addCommand("kln89_msg_pressed", do_kln89_msg_pressed); + globals->get_commands()->addCommand("kln89_obs_pressed", do_kln89_obs_pressed); + globals->get_commands()->addCommand("kln89_alt_pressed", do_kln89_alt_pressed); + globals->get_commands()->addCommand("kln89_nrst_pressed", do_kln89_nrst_pressed); + globals->get_commands()->addCommand("kln89_dto_pressed", do_kln89_dto_pressed); + globals->get_commands()->addCommand("kln89_clr_pressed", do_kln89_clr_pressed); + globals->get_commands()->addCommand("kln89_ent_pressed", do_kln89_ent_pressed); + globals->get_commands()->addCommand("kln89_crsr_pressed", do_kln89_crsr_pressed); + globals->get_commands()->addCommand("kln89_knob1left1", do_kln89_knob1left1); + globals->get_commands()->addCommand("kln89_knob1right1", do_kln89_knob1right1); + globals->get_commands()->addCommand("kln89_knob2left1", do_kln89_knob2left1); + globals->get_commands()->addCommand("kln89_knob2right1", do_kln89_knob2right1); + + // Build the GPS-specific databases. + // TODO - consider splitting into real life GPS database regions - eg Americas, Europe etc. + // Note that this needs to run after FG's airport and nav databases are up and running + _waypoints.clear(); + const airport_list* apts = globals->get_airports()->getAirportList(); + for(unsigned int i = 0; i < apts->size(); ++i) { + FGAirport* a = (*apts)[i]; + GPSWaypoint* w = new GPSWaypoint; + w->id = a->getId(); + w->lat = a->getLatitude() * SG_DEGREES_TO_RADIANS; + w->lon = a->getLongitude() * SG_DEGREES_TO_RADIANS; + w->type = GPS_WP_APT; + gps_waypoint_map_iterator wtr = _waypoints.find(a->getId()); + if(wtr == _waypoints.end()) { + gps_waypoint_array arr; + arr.push_back(w); + _waypoints[w->id] = arr; + } else { + wtr->second.push_back(w); + } + } + nav_map_type navs = globals->get_navlist()->get_navaids(); + for(nav_map_iterator itr = navs.begin(); itr != navs.end(); ++itr) { + nav_list_type nlst = itr->second; + for(unsigned int i = 0; i < nlst.size(); ++i) { + FGNavRecord* n = nlst[i]; + if(n->get_fg_type() == FG_NAV_VOR || n->get_fg_type() == FG_NAV_NDB) { // We don't bother with ILS etc. + GPSWaypoint* w = new GPSWaypoint; + w->id = n->get_ident(); + w->lat = n->get_lat() * SG_DEGREES_TO_RADIANS; + w->lon = n->get_lon() * SG_DEGREES_TO_RADIANS; + w->type = (n->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB); + gps_waypoint_map_iterator wtr = _waypoints.find(n->get_ident()); + if(wtr == _waypoints.end()) { + gps_waypoint_array arr; + arr.push_back(w); + _waypoints[w->id] = arr; + } else { + wtr->second.push_back(w); + } + } + } + } + const fix_map_type* fixes = globals->get_fixlist()->getFixList(); + for(fix_map_const_iterator itr = fixes->begin(); itr != fixes->end(); ++itr) { + FGFix f = itr->second; + GPSWaypoint* w = new GPSWaypoint; + w->id = f.get_ident(); + w->lat = f.get_lat() * SG_DEGREES_TO_RADIANS; + w->lon = f.get_lon() * SG_DEGREES_TO_RADIANS; + w->type = GPS_WP_INT; + gps_waypoint_map_iterator wtr = _waypoints.find(f.get_ident()); + if(wtr == _waypoints.end()) { + gps_waypoint_array arr; + arr.push_back(w); + _waypoints[w->id] = arr; + } else { + wtr->second.push_back(w); + } + } + // TODO - add USR waypoints as well. + + // Not sure if this should be here, but OK for now. + CreateDefaultFlightPlans(); + + // Hack - hardwire some instrument approaches for testing. + // TODO - read these from file - either all at startup or as needed. + FGNPIAP* iap = new FGNPIAP; + iap->_id = "KHWD"; + iap->_name = "VOR/DME OR GPS-B"; + iap->_abbrev = "VOR/D"; + iap->_rwyStr = "B"; + iap->_IAF.clear(); + iap->_IAP.clear(); + iap->_MAP.clear(); + // ------- + GPSWaypoint* wp = new GPSWaypoint; + wp->id = "SUNOL"; + bool multi; + // Nasty using the find any function here, but it saves converting data from FGFix etc. + const GPSWaypoint* fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "MABRY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "IMPLY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "DECOT"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_FAF; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "MAPVV"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "OAK"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAHP; + iap->_MAP.push_back(wp); + // ------- + _np_iap[iap->_id].push_back(iap); + // ----------------------- + // ----------------------- + iap = new FGNPIAP; + iap->_id = "KHWD"; + iap->_name = "VOR OR GPS-A"; + iap->_abbrev = "VOR-"; + iap->_rwyStr = "A"; + iap->_IAF.clear(); + iap->_IAP.clear(); + iap->_MAP.clear(); + // ------- + wp = new GPSWaypoint; + wp->id = "SUNOL"; + // Nasty using the find any function here, but it saves converting data from FGFix etc. + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "MABRY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "IMPLY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "DECOT"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_FAF; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "MAPVV"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "OAK"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAHP; + iap->_MAP.push_back(wp); + // ------- + _np_iap[iap->_id].push_back(iap); + // ------------------ + // ------------------ + /* + // Ugh - don't load this one - the waypoints required aren't in fix.dat.gz - result: program crash! + // TODO - make the IAP loader robust to absent waypoints. + iap = new FGNPIAP; + iap->_id = "KHWD"; + iap->_name = "GPS RWY 28L"; + iap->_abbrev = "GPS"; + iap->_rwyStr = "28L"; + iap->_IAF.clear(); + iap->_IAP.clear(); + iap->_MAP.clear(); + // ------- + wp = new GPSWaypoint; + wp->id = "SUNOL"; + // Nasty using the find any function here, but it saves converting data from FGFix etc. + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "SJC"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "JOCPI"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "SUDGE"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_FAF; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "RW28L"; + wp->appType = GPS_MAP; + if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) { + // Assume that this is a missed-approach point based on the runway number + // Get the runway threshold location etc + } else { + fp = FindFirstById(wp->id, multi, true); + if(fp == NULL) { + cout << "Failed to find waypoint " << wp->id << " in database...\n"; + } else { + *wp = *fp; + } + } + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "OAK"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAHP; + iap->_MAP.push_back(wp); + // ------- + _np_iap[iap->_id].push_back(iap); + */ + iap = new FGNPIAP; + iap->_id = "C83"; + iap->_name = "GPS RWY 30"; + iap->_abbrev = "GPS"; + iap->_rwyStr = "30"; + iap->_IAF.clear(); + iap->_IAP.clear(); + iap->_MAP.clear(); + // ------- + wp = new GPSWaypoint; + wp->id = "PATYY"; + // Nasty using the find any function here, but it saves converting data from FGFix etc. + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "TRACY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAF; + iap->_IAF.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "TRACY"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_IAP; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "BABPI"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_FAF; + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "AMOSY"; + wp->appType = GPS_MAP; + if(wp->id.substr(0, 2) == "RW" && wp->appType == GPS_MAP) { + // Assume that this is a missed-approach point based on the runway number + // TODO: Get the runway threshold location etc + cout << "TODO - implement missed-approach point based on rwy no.\n"; + } else { + fp = FindFirstById(wp->id, multi, true); + if(fp == NULL) { + cout << "Failed to find waypoint " << wp->id << " in database...\n"; + } else { + *wp = *fp; + wp->appType = GPS_MAP; + } + } + iap->_IAP.push_back(wp); + // ------- + wp = new GPSWaypoint; + wp->id = "HAIRE"; + fp = FindFirstById(wp->id, multi, true); + *wp = *fp; + wp->appType = GPS_MAHP; + iap->_MAP.push_back(wp); + // ------- + _np_iap[iap->_id].push_back(iap); +} + +void DCLGPS::bind() { + fgTie("/instrumentation/gps/waypoint-alert", this, &DCLGPS::GetWaypointAlert); + fgTie("/instrumentation/gps/leg-mode", this, &DCLGPS::GetLegMode); + fgTie("/instrumentation/gps/obs-mode", this, &DCLGPS::GetOBSMode); + fgTie("/instrumentation/gps/approach-arm", this, &DCLGPS::GetApproachArm); + fgTie("/instrumentation/gps/approach-active", this, &DCLGPS::GetApproachActive); + fgTie("/instrumentation/gps/cdi-deflection", this, &DCLGPS::GetCDIDeflection); + fgTie("/instrumentation/gps/to-flag", this, &DCLGPS::GetToFlag); +} + +void DCLGPS::unbind() { + fgUntie("/instrumentation/gps/waypoint-alert"); + fgUntie("/instrumentation/gps/leg-mode"); + fgUntie("/instrumentation/gps/obs-mode"); + fgUntie("/instrumentation/gps/approach-arm"); + fgUntie("/instrumentation/gps/approach-active"); + fgUntie("/instrumentation/gps/cdi-deflection"); +} + +void DCLGPS::update(double dt) { + //cout << "update called!\n"; + + _lon = _lon_node->getDoubleValue() * SG_DEGREES_TO_RADIANS; + _lat = _lat_node->getDoubleValue() * SG_DEGREES_TO_RADIANS; + _alt = _alt_node->getDoubleValue(); + _groundSpeed_kts = _grnd_speed_node->getDoubleValue(); + _groundSpeed_ms = _groundSpeed_kts * 0.5144444444; + _track = _true_track_node->getDoubleValue(); + _magTrackDeg = _mag_track_node->getDoubleValue(); + // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG + // gps code and not our own. + _gpsLon = _lon; + _gpsLat = _lat; + // Check for abnormal position slew + if(GetGreatCircleDistance(_gpsLat, _gpsLon, _checkLat, _checkLon) > 1.0) { + OrientateToActiveFlightPlan(); + } + _checkLon = _gpsLon; + _checkLat = _gpsLat; + + if(!_departed) { + if(_groundSpeed_kts > 30.0) { + _departed = true; + string th = fgGetString("/instrumentation/clock/indicated-hour"); + string tm = fgGetString("/instrumentation/clock/indicated-min"); + if(th.size() == 1) th = "0" + th; + if(tm.size() == 1) tm = "0" + tm; + _departureTimeString = th + tm; + } + } else { + // TODO - check - is this prone to drift error over time? + // Should we difference the departure and current times? + // What about when the user resets the time of day from the menu? + _elapsedTime += dt; + } + + _time->update(_gpsLon * SG_DEGREES_TO_RADIANS, _gpsLat * SG_DEGREES_TO_RADIANS, 0, 0); + // FIXME - currently all the below assumes leg mode and no DTO or OBS cancelled. + if(_activeFP->IsEmpty()) { + // Not sure if we need to reset these each update or only when fp altered + _activeWaypoint.id.clear(); + _navFlagged = true; + } else if(_activeFP->waypoints.size() == 1) { + _activeWaypoint.id.clear(); + } else { + _navFlagged = false; + if(_activeWaypoint.id.empty() || _fromWaypoint.id.empty()) { + //cout << "Error, in leg mode with flightplan of 2 or more waypoints, but either active or from wp is NULL!\n"; + OrientateToActiveFlightPlan(); + } + + // Approach stuff + if(_approachLoaded) { + if(!_approachReallyArmed && !_approachActive) { + // arm if within 30nm of airport. + // TODO - let user cancel approach arm using external GPS-APR switch + bool multi; + const FGAirport* ap = FindFirstAptById(_approachID, multi, true); + if(ap != NULL) { + double d = GetGreatCircleDistance(_gpsLat, _gpsLon, ap->getLatitude() * SG_DEGREES_TO_RADIANS, ap->getLongitude() * SG_DEGREES_TO_RADIANS); + if(d <= 30) { + _approachArm = true; + _approachReallyArmed = true; + _messageStack.push_back("*Press ALT To Set Baro"); + // Not sure what we do if the user has already set CDI to 0.3 nm? + _targetCdiScaleIndex = 1; + if(_currentCdiScaleIndex == 1) { + // No problem! + } else if(_currentCdiScaleIndex == 0) { + _sourceCdiScaleIndex = 0; + _cdiScaleTransition = true; + _cdiTransitionTime = 30.0; + _currentCdiScale = _cdiScales[_currentCdiScaleIndex]; + } + } + } + } else { + // Check for approach active - we can only activate approach if it is really armed. + if(_activeWaypoint.appType == GPS_FAF) { + //cout << "Active waypoint is FAF, id is " << _activeWaypoint.id << '\n'; + if(GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) <= 2.0 && !_obsMode) { + // Assume heading is OK for now + _approachArm = false; // TODO - check - maybe arm is left on when actv comes on? + _approachReallyArmed = false; + _approachActive = true; + _targetCdiScaleIndex = 2; + if(_currentCdiScaleIndex == 2) { + // No problem! + } else if(_currentCdiScaleIndex == 1) { + _sourceCdiScaleIndex = 1; + _cdiScaleTransition = true; + _cdiTransitionTime = 30.0; // TODO - compress it if time to FAF < 30sec + _currentCdiScale = _cdiScales[_currentCdiScaleIndex]; + } else { + // Abort going active? + _approachActive = false; + } + } + } + } + } + + // CDI scale transition stuff + if(_cdiScaleTransition) { + if(fabs(_currentCdiScale - _cdiScales[_targetCdiScaleIndex]) < 0.001) { + _currentCdiScale = _cdiScales[_targetCdiScaleIndex]; + _currentCdiScaleIndex = _targetCdiScaleIndex; + _cdiScaleTransition = false; + } else { + double scaleDiff = (_targetCdiScaleIndex > _sourceCdiScaleIndex + ? _cdiScales[_sourceCdiScaleIndex] - _cdiScales[_targetCdiScaleIndex] + : _cdiScales[_targetCdiScaleIndex] - _cdiScales[_sourceCdiScaleIndex]); + //cout << "ScaleDiff = " << scaleDiff << '\n'; + if(_targetCdiScaleIndex > _sourceCdiScaleIndex) { + // Scaling down eg. 5nm -> 1nm + _currentCdiScale -= (scaleDiff * dt / _cdiTransitionTime); + if(_currentCdiScale < _cdiScales[_targetCdiScaleIndex]) { + _currentCdiScale = _cdiScales[_targetCdiScaleIndex]; + _currentCdiScaleIndex = _targetCdiScaleIndex; + _cdiScaleTransition = false; + } + } else { + _currentCdiScale += (scaleDiff * dt / _cdiTransitionTime); + if(_currentCdiScale > _cdiScales[_targetCdiScaleIndex]) { + _currentCdiScale = _cdiScales[_targetCdiScaleIndex]; + _currentCdiScaleIndex = _targetCdiScaleIndex; + _cdiScaleTransition = false; + } + } + //cout << "_currentCdiScale = " << _currentCdiScale << '\n'; + } + } else { + _currentCdiScale = _cdiScales[_currentCdiScaleIndex]; + } + + + // Urgh - I've been setting the heading bug based on DTK, + // bug I think it should be based on heading re. active waypoint + // based on what the sim does after the final waypoint is passed. + // (DTK remains the same, but if track is held == DTK heading bug + // reverses to from once wp is passed). + /* + if(_fromWaypoint != NULL) { + // TODO - how do we handle the change of track with distance over long legs? + _dtkTrue = GetGreatCircleCourse(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon) * SG_RADIANS_TO_DEGREES; + _dtkMag = GetMagHeadingFromTo(_fromWaypoint->lat, _fromWaypoint->lon, _activeWaypoint->lat, _activeWaypoint->lon); + // Don't change the heading bug if speed is too low otherwise it flickers to/from at rest + if(_groundSpeed_ms > 5) { + //cout << "track = " << _track << ", dtk = " << _dtkTrue << '\n'; + double courseDev = _track - _dtkTrue; + //cout << "courseDev = " << courseDev << ", normalized = "; + SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0); + //cout << courseDev << '\n'; + _headingBugTo = (fabs(courseDev) > 90.0 ? false : true); + } + } else { + _dtkTrue = 0.0; + _dtkMag = 0.0; + // TODO - in DTO operation the position of initiation of DTO defines the "from waypoint". + } + */ + if(!_activeWaypoint.id.empty()) { + double hdgTrue = GetGreatCircleCourse(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES; + if(_groundSpeed_ms > 5) { + //cout << "track = " << _track << ", hdgTrue = " << hdgTrue << '\n'; + double courseDev = _track - hdgTrue; + //cout << "courseDev = " << courseDev << ", normalized = "; + SG_NORMALIZE_RANGE(courseDev, -180.0, 180.0); + //cout << courseDev << '\n'; + _headingBugTo = (fabs(courseDev) > 90.0 ? false : true); + } + if(!_fromWaypoint.id.empty()) { + _dtkTrue = GetGreatCircleCourse(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_RADIANS_TO_DEGREES; + _dtkMag = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon); + } else { + _dtkTrue = 0.0; + _dtkMag = 0.0; + } + } + + _dist2Act = GetGreatCircleDistance(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon) * SG_NM_TO_METER; + if(_groundSpeed_ms > 10.0) { + _eta = _dist2Act / _groundSpeed_ms; + if(_eta <= 36) { // TODO - this is slightly different if turn anticipation is enabled. + if(_headingBugTo) { + _waypointAlert = true; // TODO - not if the from flag is set. + } + } + if(_eta < 60) { + // Check if we should sequence to next leg. + // Perhaps this should be done on distance instead, but 60s time (about 1 - 2 nm) seems reasonable for now. + //double reverseHeading = GetGreatCircleCourse(_activeWaypoint->lat, _activeWaypoint->lon, _fromWaypoint->lat, _fromWaypoint->lon); + // Hack - let's cheat and do it on heading bug for now. TODO - that stops us 'cutting the corner' + // when we happen to approach the inside turn of a waypoint - we should probably sequence at the midpoint + // of the heading difference between legs in this instance. + int idx = GetActiveWaypointIndex(); + bool finalLeg = (idx == (int)(_activeFP->waypoints.size()) - 1 ? true : false); + bool finalDto = (_dto && idx == -1); // Dto operation to a waypoint not in the flightplan - we don't sequence in this instance + if(!_headingBugTo) { + if(finalLeg) { + // Do nothing - not sure if Dto should switch off when arriving at the final waypoint of a flightplan + } else if(finalDto) { + // Do nothing + } else if(_activeWaypoint.appType == GPS_MAP) { + // Don't sequence beyond the missed approach point + cout << "ACTIVE WAYPOINT is MAP - not sequencing!!!!!\n"; + } else { + cout << "Sequencing...\n"; + _fromWaypoint = _activeWaypoint; + _activeWaypoint = *_activeFP->waypoints[idx + 1]; + _dto = false; + // TODO - course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not. + // For now assume we are not. + string s; + if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) { + // TODO - avoid the hardwiring on nav[0] + s = "Adj Nav Crs to "; + } else { + string s = "GPS Course is "; + } + double d = GetMagHeadingFromTo(_fromWaypoint.lat, _fromWaypoint.lon, _activeWaypoint.lat, _activeWaypoint.lon); + while(d < 0.0) d += 360.0; + while(d >= 360.0) d -= 360.0; + char buf[4]; + snprintf(buf, 4, "%03i", (int)(d + 0.5)); + s += buf; + _messageStack.push_back(s); + } + _waypointAlert = false; + } + } + } else { + _eta = 0.0; + } + + /* + // First attempt at a sensible cross-track correction calculation + // Uh? - I think this is implemented further down the file! + if(_fromWaypoint != NULL) { + + } else { + _crosstrackDist = 0.0; + } + */ + } +} + +double DCLGPS::GetCDIDeflection() const { + double xtd = CalcCrossTrackDeviation(); //nm + return((xtd / _currentCdiScale) * 5.0 * 2.5 * -1.0); +} + +void DCLGPS::DtoInitiate(string s) { + cout << "DtoInitiate, s = " << s << '\n'; + bool multi; + const GPSWaypoint* wp = FindFirstById(s, multi, true); + if(wp) { + cout << "Waypoint found, starting dto operation!\n"; + _dto = true; + _activeWaypoint = *wp; + _fromWaypoint.lat = _gpsLat; + _fromWaypoint.lon = _gpsLon; + _fromWaypoint.type = GPS_WP_VIRT; + _fromWaypoint.id = "DTOWP"; + } else { + cout << "Waypoint not found, ignoring dto request\n"; + // Should bring up the user waypoint page, but we're not implementing that yet. + _dto = false; // TODO - implement this some day. + } +} + +void DCLGPS::DtoCancel() { + if(_dto) { + // i.e. don't bother reorientating if we're just cancelling a DTO button press + // without having previously initiated DTO. + OrientateToActiveFlightPlan(); + } + _dto = false; +} + +void DCLGPS::Knob1Left1() {} +void DCLGPS::Knob1Right1() {} +void DCLGPS::Knob2Left1() {} +void DCLGPS::Knob2Right1() {} +void DCLGPS::CrsrPressed() { _activePage->CrsrPressed(); } +void DCLGPS::EntPressed() { _activePage->EntPressed(); } +void DCLGPS::ClrPressed() { _activePage->ClrPressed(); } +void DCLGPS::DtoPressed() {} +void DCLGPS::NrstPressed() {} +void DCLGPS::AltPressed() {} + +void DCLGPS::OBSPressed() { + _obsMode = !_obsMode; + if(_obsMode) { + if(!_activeWaypoint.id.empty()) { + _obsHeading = _dtkMag; + } + // TODO - the _fromWaypoint location will change as the OBS heading changes. + // Might need to store the OBS initiation position somewhere in case it is needed again. + SetOBSFromWaypoint(); + } +} + +// Set the _fromWaypoint position based on the active waypoint and OBS radial. +void DCLGPS::SetOBSFromWaypoint() { + if(!_obsMode) return; + if(_activeWaypoint.id.empty()) return; + + // TODO - base the 180 deg correction on the to/from flag. + _fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0); + _fromWaypoint.id = "OBSWP"; +} + +void DCLGPS::MsgPressed() {} + +void DCLGPS::CDIFSDIncrease() { + if(_currentCdiScaleIndex == 0) { + _currentCdiScaleIndex = _cdiScales.size() - 1; + } else { + _currentCdiScaleIndex--; + } +} + +void DCLGPS::CDIFSDDecrease() { + _currentCdiScaleIndex++; + if(_currentCdiScaleIndex == _cdiScales.size()) { + _currentCdiScaleIndex = 0; + } +} + +void DCLGPS::DrawChar(char c, int field, int px, int py, bool bold) { +} + +void DCLGPS::DrawText(const string& s, int field, int px, int py, bool bold) { +} + +void DCLGPS::SetBaroUnits(int n, bool wrap) { + if(n < 1) { + _baroUnits = (GPSPressureUnits)(wrap ? 3 : 1); + } else if(n > 3) { + _baroUnits = (GPSPressureUnits)(wrap ? 1 : 3); + } else { + _baroUnits = (GPSPressureUnits)n; + } +} + +void DCLGPS::CreateDefaultFlightPlans() {} + +// Get the time to the active waypoint in seconds. +// Returns -1 if groundspeed < 30 kts +double DCLGPS::GetTimeToActiveWaypoint() { + if(_groundSpeed_kts < 30.0) { + return(-1.0); + } else { + return(_eta); + } +} + +// Get the time to the final waypoint in seconds. +// Returns -1 if groundspeed < 30 kts +double DCLGPS::GetETE() { + if(_groundSpeed_kts < 30.0) { + return(-1.0); + } else { + // TODO - handle OBS / DTO operation appropriately + if(_activeFP->waypoints.empty()) { + return(-1.0); + } else { + return(GetTimeToWaypoint(_activeFP->waypoints[_activeFP->waypoints.size() - 1]->id)); + } + } +} + +// Get the time to a given waypoint (spec'd by ID) in seconds. +// returns -1 if groundspeed is less than 30kts. +// If the waypoint is an unreached part of the active flight plan the time will be via each leg. +// otherwise it will be a direct-to time. +double DCLGPS::GetTimeToWaypoint(string id) { + if(_groundSpeed_kts < 30.0) { + return(-1.0); + } + + double eta = 0.0; + int n1 = GetActiveWaypointIndex(); + int n2 = GetWaypointIndex(id); + if(n2 > n1) { + eta = _eta; + for(unsigned int i=n1+1; i<_activeFP->waypoints.size(); ++i) { + GPSWaypoint* wp1 = _activeFP->waypoints[i-1]; + GPSWaypoint* wp2 = _activeFP->waypoints[i]; + double distm = GetGreatCircleDistance(wp1->lat, wp1->lon, wp2->lat, wp2->lon) * SG_NM_TO_METER; + eta += (distm / _groundSpeed_ms); + } + return(eta); + } else if(id == _activeWaypoint.id) { + return(_eta); + } else { + bool multi; + const GPSWaypoint* wp = FindFirstById(id, multi, true); + if(wp == NULL) return(-1.0); + double distm = GetGreatCircleDistance(_gpsLat, _gpsLon, wp->lat, wp->lon); + return(distm / _groundSpeed_ms); + } + return(-1.0); // Hopefully we never get here! +} + +// Returns magnetic great-circle heading +// TODO - document units. +float DCLGPS::GetHeadingToActiveWaypoint() { + if(_activeWaypoint.id.empty()) { + return(-888); + } else { + double h = GetMagHeadingFromTo(_gpsLat, _gpsLon, _activeWaypoint.lat, _activeWaypoint.lon); + while(h <= 0.0) h += 360.0; + while(h > 360.0) h -= 360.0; + return((float)h); + } +} + +// Returns magnetic great-circle heading +// TODO - what units? +float DCLGPS::GetHeadingFromActiveWaypoint() { + if(_activeWaypoint.id.empty()) { + return(-888); + } else { + double h = GetMagHeadingFromTo(_activeWaypoint.lat, _activeWaypoint.lon, _gpsLat, _gpsLon); + while(h <= 0.0) h += 360.0; + while(h > 360.0) h -= 360.0; + return(h); + } +} + +void DCLGPS::ClearFlightPlan(int n) { + for(unsigned int i=0; i<_flightPlans[n]->waypoints.size(); ++i) { + delete _flightPlans[n]->waypoints[i]; + } + _flightPlans[n]->waypoints.clear(); +} + +void DCLGPS::ClearFlightPlan(GPSFlightPlan* fp) { + for(unsigned int i=0; iwaypoints.size(); ++i) { + delete fp->waypoints[i]; + } + fp->waypoints.clear(); +} + +int DCLGPS::GetActiveWaypointIndex() { + for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) { + if(_flightPlans[0]->waypoints[i]->id == _activeWaypoint.id) return((int)i); + } + return(-1); +} + +int DCLGPS::GetWaypointIndex(string id) { + for(unsigned int i=0; i<_flightPlans[0]->waypoints.size(); ++i) { + if(_flightPlans[0]->waypoints[i]->id == id) return((int)i); + } + return(-1); +} + +void DCLGPS::OrientateToFlightPlan(GPSFlightPlan* fp) { + //cout << "Orientating...\n"; + //cout << "_lat = " << _lat << ", _lon = " << _lon << ", _gpsLat = " << _gpsLat << ", gpsLon = " << _gpsLon << '\n'; + if(fp->IsEmpty()) { + _activeWaypoint.id.clear(); + _navFlagged = true; + } else { + _navFlagged = false; + if(fp->waypoints.size() == 1) { + // TODO - may need to flag nav here if not dto or obs, or possibly handle it somewhere else. + _activeWaypoint = *fp->waypoints[0]; + _fromWaypoint.id.clear(); + } else { + // FIXME FIXME FIXME + _fromWaypoint = *fp->waypoints[0]; + _activeWaypoint = *fp->waypoints[1]; + double dmin = 1000000; // nm!! + // For now we will simply start on the leg closest to our current position. + // It's possible that more fancy algorithms may take either heading or track + // into account when setting inital leg - I'm not sure. + // This method should handle most cases perfectly OK though. + for(unsigned int i = 1; i < fp->waypoints.size(); ++i) { + //cout << "Pass " << i << ", dmin = " << dmin << ", leg is " << fp->waypoints[i-1]->id << " to " << fp->waypoints[i]->id << '\n'; + // First get the cross track correction. + double d0 = fabs(CalcCrossTrackDeviation(*fp->waypoints[i-1], *fp->waypoints[i])); + // That is the shortest distance away we could be though - check for + // longer distances if we are 'off the end' of the leg. + double ht1 = GetGreatCircleCourse(fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon, + fp->waypoints[i]->lat, fp->waypoints[i]->lon) + * SG_RADIANS_TO_DEGREES; + // not simply the reverse of the above due to great circle navigation. + double ht2 = GetGreatCircleCourse(fp->waypoints[i]->lat, fp->waypoints[i]->lon, + fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon) + * SG_RADIANS_TO_DEGREES; + double hw1 = GetGreatCircleCourse(_gpsLat, _gpsLon, + fp->waypoints[i]->lat, fp->waypoints[i]->lon) + * SG_RADIANS_TO_DEGREES; + double hw2 = GetGreatCircleCourse(_gpsLat, _gpsLon, + fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon) + * SG_RADIANS_TO_DEGREES; + double h1 = ht1 - hw1; + double h2 = ht2 - hw2; + //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n'; + //cout << "Normalizing...\n"; + SG_NORMALIZE_RANGE(h1, -180.0, 180.0); + SG_NORMALIZE_RANGE(h2, -180.0, 180.0); + //cout << "d0, h1, h2 = " << d0 << ", " << h1 << ", " << h2 << '\n'; + if(fabs(h1) > 90.0) { + // We are past the end of the to waypoint + double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i]->lat, fp->waypoints[i]->lon); + if(d > d0) d0 = d; + //cout << "h1 triggered, d0 now = " << d0 << '\n'; + } else if(fabs(h2) > 90.0) { + // We are past the end (not yet at!) the from waypoint + double d = GetGreatCircleDistance(_gpsLat, _gpsLon, fp->waypoints[i-1]->lat, fp->waypoints[i-1]->lon); + if(d > d0) d0 = d; + //cout << "h2 triggered, d0 now = " << d0 << '\n'; + } + if(d0 < dmin) { + //cout << "THIS LEG NOW ACTIVE!\n"; + dmin = d0; + _fromWaypoint = *fp->waypoints[i-1]; + _activeWaypoint = *fp->waypoints[i]; + } + } + } + } +} + +void DCLGPS::OrientateToActiveFlightPlan() { + OrientateToFlightPlan(_activeFP); +} + +/***************************************/ + +// Utility function - create a flightplan from a list of waypoint ids and types +void DCLGPS::CreateFlightPlan(GPSFlightPlan* fp, vector ids, vector wps) { + if(fp == NULL) fp = new GPSFlightPlan; + unsigned int i; + if(!fp->waypoints.empty()) { + for(i=0; iwaypoints.size(); ++i) { + delete fp->waypoints[i]; + } + fp->waypoints.clear(); + } + if(ids.size() != wps.size()) { + cout << "ID and Waypoint types list size mismatch in GPS::CreateFlightPlan - no flightplan created!\n"; + return; + } + for(i=0; itype = wps[i]; + switch(wp->type) { + case GPS_WP_APT: + ap = FindFirstAptById(ids[i], multi, true); + if(ap == NULL) { + // error + delete wp; + } else { + wp->lat = ap->getLatitude() * SG_DEGREES_TO_RADIANS; + wp->lon = ap->getLongitude() * SG_DEGREES_TO_RADIANS; + wp->id = ids[i]; + fp->waypoints.push_back(wp); + } + break; + case GPS_WP_VOR: + np = FindFirstVorById(ids[i], multi, true); + if(np == NULL) { + // error + delete wp; + } else { + wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS; + wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS; + wp->id = ids[i]; + fp->waypoints.push_back(wp); + } + break; + case GPS_WP_NDB: + np = FindFirstNDBById(ids[i], multi, true); + if(np == NULL) { + // error + delete wp; + } else { + wp->lat = np->get_lat() * SG_DEGREES_TO_RADIANS; + wp->lon = np->get_lon() * SG_DEGREES_TO_RADIANS; + wp->id = ids[i]; + fp->waypoints.push_back(wp); + } + break; + case GPS_WP_INT: + // TODO TODO + break; + case GPS_WP_USR: + // TODO + break; + } + } +} + +/***************************************/ + +const GPSWaypoint* DCLGPS::ActualFindFirstById(string id, bool exact) { + gps_waypoint_map_const_iterator itr; + if(exact) { + itr = _waypoints.find(id); + } else { + itr = _waypoints.lower_bound(id); + } + if(itr == _waypoints.end()) { + return(NULL); + } else { + // TODO - don't just return the first one - either return all or the nearest one. + return((itr->second)[0]); + } +} + +const GPSWaypoint* DCLGPS::FindFirstById(string id, bool &multi, bool exact) { + multi = false; + if(exact) return(ActualFindFirstById(id, exact)); + + // OK, that was the easy case, now the fuzzy case + const GPSWaypoint* w1 = ActualFindFirstById(id); + if(w1 == NULL) return(w1); + + // The non-trivial code from here to the end of the function is all to deal with the fact that + // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters). + string id2 = id; + //string id3 = id+'0'; + string id4 = id+'A'; + // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters + //bool alfa = isalpha(id2[id2.size() - 1]); + id2[id2.size() - 1] = id2[id2.size() - 1] + 1; + const GPSWaypoint* w2 = ActualFindFirstById(id2); + //FGAirport* a3 = globals->get_airports()->findFirstById(id3); + const GPSWaypoint* w4 = ActualFindFirstById(id4); + //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n'; + //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n'; + //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n'; + + // TODO - the below handles the imediately following char OK + // eg id = "KD" returns "KDAA" instead of "KD5" + // but it doesn't handle numbers / letters further down the string, + // eg - id = "I" returns "IA01" instead of "IAN" + // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string. + // (Currenly fixed with recursion). + + if(w4 != w2) { // A-Z match - preferred + //cout << "A-Z match!\n"; + if(w4->id.size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); iid.size(); ++i) { + if(!isalpha(w4->id[i])) { + //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstById(w4->id.substr(0, i), multi, exact)); + } + } + } + return(w4); + } else if(w1 != w2) { // 0-9 match + //cout << "0-9 match!\n"; + if(w1->id.size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); iid.size(); ++i) { + if(!isalpha(w1->id[i])) { + //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstById(w1->id.substr(0, i), multi, exact)); + } + } + } + return(w1); + } else { // No match + return(NULL); + } + return NULL; +} + +// Host specific lookup functions +// TODO - add the ASCII / alphabetical stuff from the Atlas version +FGNavRecord* DCLGPS::FindFirstVorById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set. + multi = false; + //if(exact) return(_overlays->FindFirstVorById(id, exact)); + + nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, exact); + + if(nav.size() > 1) multi = true; + //return(nav.empty() ? NULL : *(nav.begin())); + + // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment + if(nav.empty()) return(NULL); + + for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) { + if((*it)->get_type() == 3) return(*it); + } + return(NULL); // Shouldn't get here! +} +#if 0 +Overlays::NAV* DCLGPS::FindFirstVorById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set. + multi = false; + if(exact) return(_overlays->FindFirstVorById(id, exact)); + + // OK, that was the easy case, now the fuzzy case + Overlays::NAV* n1 = _overlays->FindFirstVorById(id); + if(n1 == NULL) return(n1); + + string id2 = id; + string id3 = id+'0'; + string id4 = id+'A'; + // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters + bool alfa = isalpha(id2[id2.size() - 1]); + id2[id2.size() - 1] = id2[id2.size() - 1] + 1; + Overlays::NAV* n2 = _overlays->FindFirstVorById(id2); + //Overlays::NAV* n3 = _overlays->FindFirstVorById(id3); + //Overlays::NAV* n4 = _overlays->FindFirstVorById(id4); + //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n'; + + + if(alfa) { + if(n1 != n2) { // match + return(n1); + } else { + return(NULL); + } + } + + /* + if(n1 != n2) { + // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL + if(n4 != n2) { + // There's a letter match - return that + return(n4); + } else { + // By definition we must have a number match + if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n"; + return(n3); + } + } else { + // No match + return(NULL); + } + */ + return NULL; +} +#endif //0 + +// TODO - add the ASCII / alphabetical stuff from the Atlas version +FGNavRecord* DCLGPS::FindFirstNDBById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set. + multi = false; + //if(exact) return(_overlays->FindFirstVorById(id, exact)); + + nav_list_type nav = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, exact); + + if(nav.size() > 1) multi = true; + //return(nav.empty() ? NULL : *(nav.begin())); + + // The above is sort of what we want - unfortunately we can't guarantee no NDB/ILS at the moment + if(nav.empty()) return(NULL); + + for(nav_list_iterator it = nav.begin(); it != nav.end(); ++it) { + if((*it)->get_type() == 2) return(*it); + } + return(NULL); // Shouldn't get here! +} +#if 0 +Overlays::NAV* DCLGPS::FindFirstNDBById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set. + multi = false; + if(exact) return(_overlays->FindFirstNDBById(id, exact)); + + // OK, that was the easy case, now the fuzzy case + Overlays::NAV* n1 = _overlays->FindFirstNDBById(id); + if(n1 == NULL) return(n1); + + string id2 = id; + string id3 = id+'0'; + string id4 = id+'A'; + // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters + bool alfa = isalpha(id2[id2.size() - 1]); + id2[id2.size() - 1] = id2[id2.size() - 1] + 1; + Overlays::NAV* n2 = _overlays->FindFirstNDBById(id2); + //Overlays::NAV* n3 = _overlays->FindFirstNDBById(id3); + //Overlays::NAV* n4 = _overlays->FindFirstNDBById(id4); + //cout << "Strings sent were " << id << ", " << id2 << ", " << id3 << ", " << id4 << '\n'; + + + if(alfa) { + if(n1 != n2) { // match + return(n1); + } else { + return(NULL); + } + } + + /* + if(n1 != n2) { + // Something matches - the problem is the number/letter preference order is reversed between the GPS and the STL + if(n4 != n2) { + // There's a letter match - return that + return(n4); + } else { + // By definition we must have a number match + if(n3 == n2) cout << "HELP - LOGIC FLAW in find VOR!\n"; + return(n3); + } + } else { + // No match + return(NULL); + } + */ + return NULL; +} +#endif //0 + +// TODO - add the ASCII / alphabetical stuff from the Atlas version +const FGFix* DCLGPS::FindFirstIntById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set, and indeed can't be + // since FG can only map one Fix per ID at the moment. + multi = false; + if(exact) return(globals->get_fixlist()->findFirstByIdent(id, exact)); + + const FGFix* f1 = globals->get_fixlist()->findFirstByIdent(id, exact); + if(f1 == NULL) return(f1); + + // The non-trivial code from here to the end of the function is all to deal with the fact that + // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters). + // It is copied from the airport version which is definately needed, but at present I'm not actually + // sure if any fixes in FG or real-life have numbers in them! + string id2 = id; + //string id3 = id+'0'; + string id4 = id+'A'; + // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters + //bool alfa = isalpha(id2[id2.size() - 1]); + id2[id2.size() - 1] = id2[id2.size() - 1] + 1; + const FGFix* f2 = globals->get_fixlist()->findFirstByIdent(id2); + //const FGFix* a3 = globals->get_fixlist()->findFirstByIdent(id3); + const FGFix* f4 = globals->get_fixlist()->findFirstByIdent(id4); + + // TODO - the below handles the imediately following char OK + // eg id = "KD" returns "KDAA" instead of "KD5" + // but it doesn't handle numbers / letters further down the string, + // eg - id = "I" returns "IA01" instead of "IAN" + // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string. + // (Currenly fixed with recursion). + + if(f4 != f2) { // A-Z match - preferred + //cout << "A-Z match!\n"; + if(f4->get_ident().size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); iget_ident().size(); ++i) { + if(!isalpha(f4->get_ident()[i])) { + //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstIntById(f4->get_ident().substr(0, i), multi, exact)); + } + } + } + return(f4); + } else if(f1 != f2) { // 0-9 match + //cout << "0-9 match!\n"; + if(f1->get_ident().size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); iget_ident().size(); ++i) { + if(!isalpha(f1->get_ident()[i])) { + //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstIntById(f1->get_ident().substr(0, i), multi, exact)); + } + } + } + return(f1); + } else { // No match + return(NULL); + } + + return NULL; // Don't think we can ever get here. +} + +const FGAirport* DCLGPS::FindFirstAptById(string id, bool &multi, bool exact) { + // NOTE - at the moment multi is never set. + //cout << "FindFirstAptById, id = " << id << '\n'; + multi = false; + if(exact) return(globals->get_airports()->findFirstById(id, exact)); + + // OK, that was the easy case, now the fuzzy case + const FGAirport* a1 = globals->get_airports()->findFirstById(id); + if(a1 == NULL) return(a1); + + // The non-trivial code from here to the end of the function is all to deal with the fact that + // the KLN89 alphabetical order (numbers AFTER letters) differs from ASCII order (numbers BEFORE letters). + string id2 = id; + //string id3 = id+'0'; + string id4 = id+'A'; + // Increment the last char to provide the boundary. Note that 'Z' -> '[' but we also need to check '0' for all since GPS has numbers after letters + //bool alfa = isalpha(id2[id2.size() - 1]); + id2[id2.size() - 1] = id2[id2.size() - 1] + 1; + const FGAirport* a2 = globals->get_airports()->findFirstById(id2); + //FGAirport* a3 = globals->get_airports()->findFirstById(id3); + const FGAirport* a4 = globals->get_airports()->findFirstById(id4); + //cout << "Strings sent were " << id << ", " << id2 << " and " << id4 << '\n'; + //cout << "Airports returned were (a1, a2, a4): " << a1->getId() << ", " << a2->getId() << ", " << a4->getId() << '\n'; + //cout << "Pointers were " << a1 << ", " << a2 << ", " << a4 << '\n'; + + // TODO - the below handles the imediately following char OK + // eg id = "KD" returns "KDAA" instead of "KD5" + // but it doesn't handle numbers / letters further down the string, + // eg - id = "I" returns "IA01" instead of "IAN" + // We either need to provide a custom comparison operator, or recurse this function if !isalpha further down the string. + // (Currenly fixed with recursion). + + if(a4 != a2) { // A-Z match - preferred + //cout << "A-Z match!\n"; + if(a4->getId().size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); igetId().size(); ++i) { + if(!isalpha(a4->getId()[i])) { + //cout << "SUBSTR is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstAptById(a4->getId().substr(0, i), multi, exact)); + } + } + } + return(a4); + } else if(a1 != a2) { // 0-9 match + //cout << "0-9 match!\n"; + if(a1->getId().size() - id.size() > 2) { + // Check for numbers further on + for(unsigned int i=id.size(); igetId().size(); ++i) { + if(!isalpha(a1->getId()[i])) { + //cout << "SUBSTR2 is " << (a4->getId()).substr(0, i) << '\n'; + return(FindFirstAptById(a1->getId().substr(0, i), multi, exact)); + } + } + } + return(a1); + } else { // No match + return(NULL); + } + + return NULL; +} + +FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) { + return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR)); +} + +//---------------------------------------------------------------------------------------------------------- + +// Takes lat and lon in RADIANS!!!!!!! +double DCLGPS::GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB) { + double h = GetGreatCircleCourse(latA, lonA, latB, lonB); + h *= SG_RADIANS_TO_DEGREES; + // TODO - use the real altitude below instead of 0.0! + //cout << "MagVar = " << sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES << '\n'; + h -= sgGetMagVar(_gpsLon, _gpsLat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES; + while(h >= 360.0) h -= 360.0; + while(h < 0.0) h += 360.0; + return(h); +} + +// ---------------- Great Circle formulae from "The Aviation Formulary" ------------- +// Note that all of these assume that the world is spherical. + +double Rad2Nm(double theta) { + return(((180.0*60.0)/SG_PI)*theta); +} + +double Nm2Rad(double d) { + return((SG_PI/(180.0*60.0))*d); +} + +/* QUOTE: + +The great circle distance d between two points with coordinates {lat1,lon1} and {lat2,lon2} is given by: + +d=acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2)) + +A mathematically equivalent formula, which is less subject to rounding error for short distances is: + +d=2*asin(sqrt((sin((lat1-lat2)/2))^2 + + cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2)) + +*/ + +// Returns distance in nm, takes lat & lon in RADIANS +double DCLGPS::GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const { + double d = 2.0 * asin(sqrt(((sin((lat1-lat2)/2.0))*(sin((lat1-lat2)/2.0))) + + cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2.0))*(sin((lon1-lon2)/2.0)))); + return(Rad2Nm(d)); +} + +// fmod dosen't do what we want :-( +static double mod(double d1, double d2) { + return(d1 - d2*floor(d1/d2)); +} + +// Returns great circle course from point 1 to point 2 +// Input and output in RADIANS. +double DCLGPS::GetGreatCircleCourse (double lat1, double lon1, double lat2, double lon2) const { + //double h = 0.0; + /* + // Special case the poles + if(cos(lat1) < SG_EPSILON) { + if(lat1 > 0) { + // Starting from North Pole + h = SG_PI; + } else { + // Starting from South Pole + h = 2.0 * SG_PI; + } + } else { + // Urgh - the formula below is for negative lon +ve !!!??? + double d = GetGreatCircleDistance(lat1, lon1, lat2, lon2); + cout << "d = " << d; + d = Nm2Rad(d); + //cout << ", d_theta = " << d; + //cout << ", and d = " << Rad2Nm(d) << ' '; + if(sin(lon2 - lon1) < 0) { + cout << " A "; + h = acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1))); + } else { + cout << " B "; + h = 2.0 * SG_PI - acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1))); + } + } + cout << h * SG_RADIANS_TO_DEGREES << '\n'; + */ + + return( mod(atan2(sin(lon2-lon1)*cos(lat2), + cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)), + 2.0*SG_PI) ); +} + +// Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees) +// Note that d should be less that 1/4 Earth diameter! +GPSWaypoint DCLGPS::GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h) { + h += sgGetMagVar(wp1.lon, wp1.lat, 0.0, _time->getJD()) * SG_RADIANS_TO_DEGREES; + return(GetPositionOnRadial(wp1, d, h)); +} + +// Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees) +// Note that d should be less that 1/4 Earth diameter! +GPSWaypoint DCLGPS::GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h) { + while(h < 0.0) h += 360.0; + while(h > 360.0) h -= 360.0; + + h *= SG_DEGREES_TO_RADIANS; + d *= (SG_PI / (180.0 * 60.0)); + + double lat=asin(sin(wp1.lat)*cos(d)+cos(wp1.lat)*sin(d)*cos(h)); + double lon; + if(cos(lat)==0) { + lon=wp1.lon; // endpoint a pole + } else { + lon=mod(wp1.lon+asin(sin(h)*sin(d)/cos(lat))+SG_PI,2*SG_PI)-SG_PI; + } + + GPSWaypoint wp; + wp.lat = lat; + wp.lon = lon; + wp.type = GPS_WP_VIRT; + return(wp); +} + +// Returns cross-track deviation in Nm. +double DCLGPS::CalcCrossTrackDeviation() const { + return(CalcCrossTrackDeviation(_fromWaypoint, _activeWaypoint)); +} + +// Returns cross-track deviation of the current position between two arbitary waypoints in nm. +double DCLGPS::CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const { + //if(wp1 == NULL || wp2 == NULL) return(0.0); + if(wp1.id.empty() || wp2.id.empty()) return(0.0); + double xtd = asin(sin(Nm2Rad(GetGreatCircleDistance(wp1.lat, wp1.lon, _gpsLat, _gpsLon))) + * sin(GetGreatCircleCourse(wp1.lat, wp1.lon, _gpsLat, _gpsLon) - GetGreatCircleCourse(wp1.lat, wp1.lon, wp2.lat, wp2.lon))); + return(Rad2Nm(xtd)); +} diff --git a/src/Instrumentation/dclgps.hxx b/src/Instrumentation/dclgps.hxx new file mode 100644 index 000000000..00b7943b3 --- /dev/null +++ b/src/Instrumentation/dclgps.hxx @@ -0,0 +1,529 @@ +// dclgps.hxx - a class to extend the operation of FG's current GPS +// code, and provide support for a KLN89-specific instrument. It +// is envisioned that eventually this file and class will be split +// up between current FG code and new KLN89-specific code and removed. +// +// Written by David Luff, started 2005. +// +// Copyright (C) 2005 - David C Luff - david.luff@nottingham.ac.uk +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +#ifndef _DCLGPS_HXX +#define _DCLGPS_HXX + +#include "render_area_2d.hxx" +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +using namespace std; + +enum GPSDistanceUnits { + GPS_DIST_UNITS_NM = 0, + GPS_DIST_UNITS_KM +}; + +enum GPSSpeedUnits { + GPS_VEL_UNITS_KT, + GPS_VEL_UNITS_KPH +}; + +enum GPSAltitudeUnits { + GPS_ALT_UNITS_FT, + GPS_ALT_UNITS_M +}; + +enum GPSPressureUnits { + GPS_PRES_UNITS_IN = 1, + GPS_PRES_UNITS_MB, + GPS_PRES_UNITS_HP +}; + +// --------------------- Waypoint / Flightplan stuff ----------------------------- +// This should be merged with other similar stuff in FG at some point. + +// NOTE - ORDERING IS IMPORTANT HERE - it matches the Bendix-King page ordering! +enum GPSWpType { + GPS_WP_APT = 0, + GPS_WP_VOR, + GPS_WP_NDB, + GPS_WP_INT, + GPS_WP_USR, + GPS_WP_VIRT // Used for virtual waypoints, such as the start of DTO operation. +}; + +enum GPSAppWpType { + GPS_IAF, // Initial approach fix + GPS_IAP, // Waypoint on approach sequence that isn't any of the others. + GPS_FAF, // Final approach fix + GPS_MAP, // Missed approach point + GPS_MAHP, // Initial missed approach holding point. + GPS_HDR, // A virtual 'waypoint' to represent the approach header in the fpl page + GPS_FENCE, // A virtual 'waypoint' to represent the NO WPT SEQ fence. + GPS_APP_NONE // Not part of the approach sequence - the default. +}; + +ostream& operator << (ostream& os, GPSAppWpType type); + +struct GPSWaypoint { + GPSWaypoint(); + ~GPSWaypoint(); + string GetAprId(); // Returns the id with i, f, m or h added if appropriate. (Initial approach fix, final approach fix, etc) + string id; + float lat; // Radians + float lon; // Radians + GPSWpType type; + GPSAppWpType appType; // only used for waypoints that are part of an approach sequence +}; + +typedef vector < GPSWaypoint* > gps_waypoint_array; +typedef gps_waypoint_array::iterator gps_waypoint_array_iterator; +typedef map < string, gps_waypoint_array > gps_waypoint_map; +typedef gps_waypoint_map::iterator gps_waypoint_map_iterator; +typedef gps_waypoint_map::const_iterator gps_waypoint_map_const_iterator; + +class GPSFlightPlan { +public: + vector waypoints; + inline bool IsEmpty() { return(waypoints.size() == 0); } +}; + +// TODO - probably de-public the internals of the next 2 classes and add some methods! +// Instrument approach procedure base class +class FGIAP { +public: + FGIAP(); + virtual ~FGIAP() = 0; +//protected: + + string _id; // The ID of the airport this approach is for + string _name; // The approach name, eg "VOR/DME OR GPS-B" + string _abbrev; // The abbreviation the GPS unit uses - eg "VOR/D" in this instance. Possibly GPS model specific. + string _rwyStr; // The string used to specify the rwy - eg "B" in this instance. + bool _precision; // True for precision approach, false for non-precision. +}; + +// Non-precision instrument approach procedure +class FGNPIAP : public FGIAP { +public: + FGNPIAP(); + ~FGNPIAP(); +//private: +public: + vector _IAF; // The initial approach fix(es) + vector _IAP; // The compulsory waypoints of the approach procedure (may duplicate one of the above). + // _IAP includes the FAF and MAF. + vector _MAP; // The missed approach procedure (doesn't include the MAF). +}; + +typedef vector < FGIAP* > iap_list_type; +typedef map < string, iap_list_type > iap_map_type; +typedef iap_map_type::iterator iap_map_iterator; + +// ------------------------------------------------------------------------------ + +class DCLGPS; + +class GPSPage { + +public: + GPSPage(DCLGPS* parent); + virtual ~GPSPage() = 0; + virtual void Update(double dt); + virtual void Knob1Left1(); + virtual void Knob1Right1(); + virtual void Knob2Left1(); + virtual void Knob2Right1(); + virtual void CrsrPressed(); + virtual void EntPressed(); + virtual void ClrPressed(); + virtual void DtoPressed(); + virtual void NrstPressed(); + virtual void AltPressed(); + virtual void OBSPressed(); + virtual void MsgPressed(); + + // Sometimes a page needs to maintain state for some return paths, + // but change it for others. The CleanUp function can be used for + // changing state for non-ENT return paths in conjunction with + // GPS::_cleanUpPage + virtual void CleanUp(); + + // The LooseFocus function is called when a page or subpage looses focus + // and allow pages to clean up state that is maintained whilst focus is + // retained, but lost on return. + virtual void LooseFocus(); + + // Allows pages that display info for a given ID to have it set/get if they implement these functions. + virtual void SetId(string s); + virtual string GetId(); + + inline int GetSubPage() { return(_subPage); } + + inline int GetNSubPages() { return(_nSubPages); } + + inline string GetName() { return(_name); } + +protected: + DCLGPS* _parent; + string _name; // eg. "APT", "NAV" etc + int _nSubPages; + // _subpage is zero based + int _subPage; // The subpage gets remembered when other pages are displayed + string GPSitoa(int n); +}; + +/*-----------------------------------------------------------------------*/ + +typedef vector gps_page_list_type; +typedef gps_page_list_type::iterator gps_page_list_itr; + +// TODO - merge generic GPS functions instead and split out KLN specific stuff. +class DCLGPS : public SGSubsystem, public FGPanelInstrument { + + friend class GPSPage; + +public: + DCLGPS(RenderArea2D* instrument); + virtual ~DCLGPS() = 0; + + virtual void draw(); + + virtual void init(); + virtual void bind(); + virtual void unbind(); + virtual void update(double dt); + + // Render string s in display field field at position x, y + // WHERE POSITION IS IN CHARACTER UNITS! + // zero y at bottom? + virtual void DrawText(const string& s, int field, int px, int py, bool bold = false); + + // Render a char at a given position as above + virtual void DrawChar(char c, int field, int px, int py, bool bold = false); + + virtual void Knob1Right1(); + virtual void Knob1Left1(); + virtual void Knob2Right1(); + virtual void Knob2Left1(); + virtual void CrsrPressed(); + virtual void EntPressed(); + virtual void ClrPressed(); + virtual void DtoPressed(); + virtual void NrstPressed(); + virtual void AltPressed(); + virtual void OBSPressed(); + virtual void MsgPressed(); + + // Set the number of fields + inline void SetNumFields(int n) { _nFields = (n > _maxFields ? _maxFields : (n < 1 ? 1 : n)); } + + // Set Units + // m if true, ft if false + inline void SetAltUnitsSI(bool b) { _altUnits = (b ? GPS_ALT_UNITS_M : GPS_ALT_UNITS_FT); } + // Returns true if alt units are SI (m), false if ft + inline bool GetAltUnitsSI() { return(_altUnits == GPS_ALT_UNITS_M ? true : false); } + // km and k/h if true, nm and kt if false + inline void SetDistVelUnitsSI(bool b) { _distUnits = (b ? GPS_DIST_UNITS_KM : GPS_DIST_UNITS_NM); _velUnits = (b ? GPS_VEL_UNITS_KPH : GPS_VEL_UNITS_KT); } + // Returns true if dist/vel units are SI + inline bool GetDistVelUnitsSI() { return(_distUnits == GPS_DIST_UNITS_KM && _velUnits == GPS_VEL_UNITS_KPH ? true : false); } + // Set baro units - 1 = in, 2 = mB, 3 = hP Wrapping if for the convienience of the GPS setter. + void SetBaroUnits(int n, bool wrap = false); + // Get baro units: 1 = in, 2 = mB, 3 = hP + inline int GetBaroUnits() { return((int)_baroUnits); } + + // It is expected that specific GPS units will override these functions. + // Increase the CDI full-scale deflection (ie. increase the nm per dot) one (GPS unit dependent) increment. Wraps if necessary (GPS unit dependent). + virtual void CDIFSDIncrease(); + // Ditto for decrease the distance per dot + virtual void CDIFSDDecrease(); + + // Host specifc + ////inline void SetOverlays(Overlays* overlays) { _overlays = overlays; } + + virtual void CreateDefaultFlightPlans(); + + void SetOBSFromWaypoint(); + + inline GPSWaypoint* GetActiveWaypoint() { return &_activeWaypoint; } + // Get the (zero-based) position of the active waypoint in the active flightplan + // Returns -1 if no active waypoint. + int GetActiveWaypointIndex(); + // Ditto for an arbitrary waypoint id + int GetWaypointIndex(string id); + + // Returns meters + inline float GetDistToActiveWaypoint() { return _dist2Act; } + // Returns degrees (magnetic) + float GetHeadingToActiveWaypoint(); + // Returns degrees (magnetic) + float GetHeadingFromActiveWaypoint(); + // Get the time to the active waypoint in seconds. + // Returns -1 if groundspeed < 30 kts + double GetTimeToActiveWaypoint(); + // Get the time to the final waypoint in seconds. + // Returns -1 if groundspeed < 30 kts + double GetETE(); + // Get the time to a given waypoint (spec'd by ID) in seconds. + // returns -1 if groundspeed is less than 30kts. + // If the waypoint is an unreached part of the active flight plan the time will be via each leg. + // otherwise it will be a direct-to time. + double GetTimeToWaypoint(string id); + + // Return true if waypoint alerting is occuring + inline bool GetWaypointAlert() const { return(_waypointAlert); } + // Return true if in OBS mode + inline bool GetOBSMode() const { return(_obsMode); } + // Return true if in Leg mode + inline bool GetLegMode() const { return(!_obsMode); } + + // Clear a flightplan + void ClearFlightPlan(int n); + void ClearFlightPlan(GPSFlightPlan* fp); + + // Returns true if an approach is loaded/armed/active in the active flight plan + inline bool ApproachLoaded() const { return(_approachLoaded); } + inline bool GetApproachArm() const { return(_approachArm); } + inline bool GetApproachActive() const { return(_approachActive); } + double GetCDIDeflection() const; + inline bool GetToFlag() const { return(_headingBugTo); } + + // Initiate Direct To operation to the supplied ID. + void DtoInitiate(string id); + // Cancel Direct To operation + void DtoCancel(); + +protected: + // Maximum number of display fields for this device + int _maxFields; + // Current number of on-screen fields + int _nFields; + // Full x border + int _xBorder; + // Full y border + int _yBorder; + // Lower (y) border per field + int _yFieldBorder[4]; + // Left (x) border per field + int _xFieldBorder[4]; + // Field start in x dir (border is part of field since it is the normal char border - sometimes map mode etc draws in it) + int _xFieldStart[4]; + // Field start in y dir (for completeness - KLN89 only has vertical divider. + int _yFieldStart[4]; + + // The number of pages on the cyclic knob control + unsigned int _nPages; + // The current page we're on (Not sure how this ties in with extra pages such as direct or nearest). + unsigned int _curPage; + + // 2D rendering area + RenderArea2D* _instrument; + + // The actual pages + gps_page_list_type _pages; + + // The currently active page + GPSPage* _activePage; + // And a facility to save the immediately preceeding active page + GPSPage* _lastActivePage; + + // Units + GPSSpeedUnits _velUnits; + GPSDistanceUnits _distUnits; + GPSPressureUnits _baroUnits; + GPSAltitudeUnits _altUnits; + + // CDI full-scale deflection, specified either as an index into a vector of values (standard values) or as a double precision float (intermediate values). + // This will influence how an externally driven CDI will display as well as the NAV1 page. + // Hence the variables are located here, not in the nav page class. + vector _cdiScales; + unsigned int _currentCdiScaleIndex; + bool _cdiScaleTransition; // Set true when the floating CDI value is used during transitions + double _currentCdiScale; // The floating value to use. + unsigned int _targetCdiScaleIndex; // The target indexed value to attain during a transition. + unsigned int _sourceCdiScaleIndex; // The source indexed value during a transition - so we know which way we're heading! + // Timers to handle the transitions - not sure if we need these. + double _apprArmTimer; + double _apprActvTimer; + double _cdiTransitionTime; // Time for transition to occur in - normally 30sec but may be quicker if time to FAF < 30sec? + // + + // Data and lookup functions + // All waypoints mapped by id. + gps_waypoint_map _waypoints; +private: + // Worker function for the below. + const GPSWaypoint* ActualFindFirstById(string id, bool exact = false); +protected: + // Find first of any type of waypoint by id. (TODO - Possibly we should return multiple waypoints here). + const GPSWaypoint* FindFirstById(string id, bool &multi, bool exact = false); + FGNavRecord* FindFirstVorById(string id, bool &multi, bool exact = false); + FGNavRecord* FindFirstNDBById(string id, bool &multi, bool exact = false); + const FGAirport* FindFirstAptById(string id, bool &multi, bool exact = false); + const FGFix* FindFirstIntById(string id, bool &multi, bool exact = false); + // Find the closest VOR to a position in RADIANS. + FGNavRecord* FindClosestVor(double lat_rad, double lon_rad); + + // Position, orientation and velocity. + // These should be read from FG's built-in GPS logic if possible. + // Use the property node pointers below to do this. + SGPropertyNode_ptr _lon_node; + SGPropertyNode_ptr _lat_node; + SGPropertyNode_ptr _alt_node; + SGPropertyNode_ptr _grnd_speed_node; + SGPropertyNode_ptr _true_track_node; + SGPropertyNode_ptr _mag_track_node; + // Present position. (Radians) + double _lat, _lon; + // Present altitude (ft). (Yuk! but it saves converting ft->m->ft every update). + double _alt; + // Reported position as measured by GPS. For now this is the same + // as present position, but in the future we might want to model + // GPS lat and lon errors. + // Note - we can depriciate _gpsLat and _gpsLon if we implement error handling in FG + // gps code and not our own. + double _gpsLat, _gpsLon; //(Radians) + // Hack - it seems that the GPS gets initialised before FG's initial position is properly set. + // By checking for abnormal slew in the position we can force a re-initialisation of active flight + // plan leg and anything else that might be affected. + // TODO - sort FlightGear's initialisation order properly!!! + double _checkLat, _checkLon; // (Radians) + double _groundSpeed_ms; // filtered groundspeed (m/s) + double _groundSpeed_kts; // ditto in knots + double _track; // filtered true track (degrees) + double _magTrackDeg; // magnetic track in degrees calculated from true track above + + // _navFlagged is set true when GPS navigation is either not possible or not logical. + // This includes not receiving adequate signals, and not having an active flightplan entered. + bool _navFlagged; + + // Positional functions copied from ATCutils that might get replaced + // INPUT in RADIANS, returns DEGREES! + // Magnetic + double GetMagHeadingFromTo(double latA, double lonA, double latB, double lonB); + // True + //double GetHeadingFromTo(double latA, double lonA, double latB, double lonB); + + // Given two positions (lat & lon in RADIANS), get the HORIZONTAL separation (in meters) + //double GetHorizontalSeparation(double lat1, double lon1, double lat2, double lon2); + + // Proper great circle positional functions from The Aviation Formulary + // Returns distance in Nm, input in RADIANS. + double GetGreatCircleDistance(double lat1, double lon1, double lat2, double lon2) const; + + // Input in RADIANS, output in DEGREES. + // True + double GetGreatCircleCourse(double lat1, double lon1, double lat2, double lon2) const; + + // Return a position on a radial from wp1 given distance d (nm) and magnetic heading h (degrees) + // Note that d should be less that 1/4 Earth diameter! + GPSWaypoint GetPositionOnMagRadial(const GPSWaypoint& wp1, double d, double h); + + // Return a position on a radial from wp1 given distance d (nm) and TRUE heading h (degrees) + // Note that d should be less that 1/4 Earth diameter! + GPSWaypoint GetPositionOnRadial(const GPSWaypoint& wp1, double d, double h); + + // Calculate the current cross-track deviation in nm. + // Returns zero if a sensible value cannot be calculated. + double CalcCrossTrackDeviation() const; + + // Calculate the cross-track deviation between 2 arbitrary waypoints in nm. + // Returns zero if a sensible value cannot be calculated. + double CalcCrossTrackDeviation(const GPSWaypoint& wp1, const GPSWaypoint& wp2) const; + + // Flightplans + // GPS can have up to _maxFlightPlans flightplans stored, PLUS an active FP which may or my not be one of the stored ones. + // This is from KLN89, but is probably not far off the mark for most if not all GPS. + vector _flightPlans; + unsigned int _maxFlightPlans; + GPSFlightPlan* _activeFP; + + // Modes of operation. + // This is currently somewhat Bendix-King specific, but probably applies fundamentally to other units as well + // Mode defaults to leg, but is OBS if _obsMode is true. + bool _obsMode; + // _dto is set true for DTO operation + bool _dto; + // In leg mode, we need to know if we are displaying a from and to waypoint, or just the to waypoint (eg. when OBS mode is cancelled). + bool _fullLegMode; + // In OBS mode we need to know the set OBS heading + int _obsHeading; + + // Operational variables + GPSWaypoint _activeWaypoint; + GPSWaypoint _fromWaypoint; + float _dist2Act; + float _crosstrackDist; // UNITS ?????????? + double _eta; // ETA in SECONDS to active waypoint. + // Desired track for active leg, true and magnetic, in degrees + double _dtkTrue, _dtkMag; + bool _headingBugTo; // Set true when the heading bug is TO, false when FROM. + bool _waypointAlert; // Set true when waypoint alerting is happening. (This is a variable NOT a user-setting). + bool _departed; // Set when groundspeed first exceeds 30kts. + string _departureTimeString; // Ditto. + double _elapsedTime; // Elapsed time in seconds since departure + + // Configuration that affects flightplan operation + bool _turnAnticipationEnabled; + + // Configuration that affects general operation + bool _suaAlertEnabled; // Alert user to potential SUA entry + bool _altAlertEnabled; // Alert user to min safe alt violation + + // Magvar stuff. Might get some of this stuff (such as time) from FG in future. + SGTime* _time; + + list _messageStack; + + virtual void CreateFlightPlan(GPSFlightPlan* fp, vector ids, vector wps); + + // Orientate the GPS unit to a flightplan - ie. figure out from current position + // and possibly orientation which leg of the FP we are on. + virtual void OrientateToFlightPlan(GPSFlightPlan* fp); + + // Ditto for active fp. Probably all we need really! + virtual void OrientateToActiveFlightPlan(); + + int _cleanUpPage; // -1 => no cleanup required. + + // IAP stuff + iap_map_type _np_iap; // Non-precision approaches + iap_map_type _pr_iap; // Precision approaches + bool _approachLoaded; // Set true when an approach is loaded in the active flightplan + bool _approachArm; // Set true when in approach-arm mode + bool _approachReallyArmed; // Apparently, approach-arm mode can be set from an external GPS-APR switch outside 30nm from airport, + // but the CDI scale change doesn't happen until 30nm from airport. Bizarre that it can be armed without + // the scale change, but it's in the manual... + bool _approachActive; // Set true when in approach-active mode + GPSFlightPlan* _approachFP; // Current approach - not necessarily loaded. + string _approachID; // ID of the airport we have an approach loaded for - bit of a hack that can hopefully be removed in future. + // More hackery since we aren't actually storing an approach class... Doh! + string _approachAbbrev; + string _approachRwyStr; +}; + +#endif // _DCLGPS_HXX -- 2.39.5