#include "dclgps.hxx"
#include <simgear/sg_inlines.h>
-#include <simgear/structure/commands.hxx>
+#include <simgear/misc/sg_path.hxx>
#include <simgear/timing/sg_time.hxx>
#include <simgear/magvar/magvar.hxx>
+#include <simgear/structure/exception.hxx>
#include <Main/fg_props.hxx>
#include <Navaids/fix.hxx>
+#include <Navaids/navrecord.hxx>
+#include <Airports/simple.hxx>
+#include <Airports/runways.hxx>
+#include <fstream>
#include <iostream>
-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
+using namespace std;
GPSWaypoint::GPSWaypoint() {
appType = GPS_APP_NONE;
else return(id);
}
-GPSWaypoint* GPSWaypoint::createFromFix(const FGFix* aFix)
+static GPSWpType
+GPSWpTypeFromFGPosType(FGPositioned::Type aType)
{
- assert(aFix);
- return new GPSWaypoint(aFix->get_ident(),
- aFix->get_lat() * SG_DEGREES_TO_RADIANS,
- aFix->get_lon() * SG_DEGREES_TO_RADIANS,
- GPS_WP_INT);
-}
-
-GPSWaypoint* GPSWaypoint::createFromNav(const FGNavRecord* aNav)
-{
- assert(aNav);
- return new GPSWaypoint(aNav->get_ident(),
- aNav->get_lat() * SG_DEGREES_TO_RADIANS,
- aNav->get_lon() * SG_DEGREES_TO_RADIANS,
- (aNav->get_fg_type() == FG_NAV_VOR ? GPS_WP_VOR : GPS_WP_NDB));
+ switch (aType) {
+ case FGPositioned::AIRPORT:
+ case FGPositioned::SEAPORT:
+ case FGPositioned::HELIPORT:
+ return GPS_WP_APT;
+
+ case FGPositioned::VOR:
+ return GPS_WP_VOR;
+
+ case FGPositioned::NDB:
+ return GPS_WP_NDB;
+
+ case FGPositioned::WAYPOINT:
+ return GPS_WP_USR;
+
+ case FGPositioned::FIX:
+ return GPS_WP_INT;
+
+ default:
+ return GPS_WP_USR;
+ }
}
-GPSWaypoint* GPSWaypoint::createFromAirport(const FGAirport* aApt)
+GPSWaypoint* GPSWaypoint::createFromPositioned(const FGPositioned* aPos)
{
- assert(aApt);
- return new GPSWaypoint(aApt->getId(),
- aApt->getLatitude() * SG_DEGREES_TO_RADIANS,
- aApt->getLongitude() * SG_DEGREES_TO_RADIANS,
- GPS_WP_APT);
+ if (!aPos) {
+ return NULL; // happens if find returns no match
+ }
+
+ return new GPSWaypoint(aPos->ident(),
+ aPos->latitude() * SG_DEGREES_TO_RADIANS,
+ aPos->longitude() * SG_DEGREES_TO_RADIANS,
+ GPSWpTypeFromFGPosType(aPos->type())
+ );
}
ostream& operator << (ostream& os, GPSAppWpType type) {
ClockTime::~ClockTime() {
}
-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(const string& s) {}
-
// ------------------------------------------------------------------------------------- //
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);
// Configuration Initialisation
// Should this be in kln89.cxx ?
_turnAnticipationEnabled = false;
- _suaAlertEnabled = false;
- _altAlertEnabled = false;
_time = new SGTime;
// TODO - may need to delete the approach database!!
}
-void DCLGPS::draw() {
- //cout << "draw called!\n";
- _instrument->draw();
+void DCLGPS::draw(osg::State& state) {
+ _instrument->Draw(state);
}
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);
// Not sure if this should be here, but OK for now.
CreateDefaultFlightPlans();
+
+ LoadApproachData();
}
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);
+ _tiedProperties.setRoot(fgGetNode("/instrumentation/gps", true));
+ _tiedProperties.Tie("waypoint-alert", this, &DCLGPS::GetWaypointAlert);
+ _tiedProperties.Tie("leg-mode", this, &DCLGPS::GetLegMode);
+ _tiedProperties.Tie("obs-mode", this, &DCLGPS::GetOBSMode);
+ _tiedProperties.Tie("approach-arm", this, &DCLGPS::GetApproachArm);
+ _tiedProperties.Tie("approach-active", this, &DCLGPS::GetApproachActive);
+ _tiedProperties.Tie("cdi-deflection", this, &DCLGPS::GetCDIDeflection);
+ _tiedProperties.Tie("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");
+ _tiedProperties.Untie();
}
void DCLGPS::update(double dt) {
_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.
+ // Course alteration message format is dependent on whether we are slaved HSI/CDI indicator or not.
string s;
if(fgGetBool("/instrumentation/nav[0]/slaved-to-gps")) {
// TODO - avoid the hardwiring on nav[0]
}
}
+/*
+ Expand a SIAP ident to the full procedure name (as shown on the approach chart).
+ NOTE: Some of this is inferred from data, some is from documentation.
+
+ Example expansions from ARINC 424-18 [and the airport they're taken from]:
+ "R10LY" <--> "RNAV (GPS) Y RWY 10 L" [KBOI]
+ "R10-Z" <--> "RNAV (GPS) Z RWY 10" [KHTO]
+ "S25" <--> "VOR or GPS RWY 25" [KHHR]
+ "P20" <--> "GPS RWY 20" [KDAN]
+ "NDB-B" <--> "NDB or GPS-B" [KDAW]
+ "NDBC" <--> "NDB or GPS-C" [KEMT]
+ "VDMA" <--> "VOR/DME or GPS-A" [KDAW]
+ "VDM-A" <--> "VOR/DME or GPS-A" [KEAG]
+ "VDMB" <--> "VOR/DME or GPS-B" [KDKX]
+ "VORA" <--> "VOR or GPS-A" [KEMT]
+
+ It seems that there are 2 basic types of expansions; those that include
+ the runway and those that don't. Of those that don't, it seems that 2
+ different positions within the string to encode the identifying letter
+ are used, i.e. with a dash and without.
+*/
+string DCLGPS::ExpandSIAPIdent(const string& ident) {
+ string name;
+ bool has_rwy = false;
+
+ switch(ident[0]) {
+ case 'N': name = "NDB or GPS"; has_rwy = false; break;
+ case 'P': name = "GPS"; has_rwy = true; break;
+ case 'R': name = "RNAV (GPS)"; has_rwy = true; break;
+ case 'S': name = "VOR or GPS"; has_rwy = true; break;
+ case 'V':
+ if(ident[1] == 'D') name = "VOR/DME or GPS";
+ else name = "VOR or GPS";
+ has_rwy = false;
+ break;
+ default: // TODO output a log message
+ break;
+ }
+
+ if(has_rwy) {
+ // Add the identifying letter if present
+ if(ident.size() == 5) {
+ name += ' ';
+ name += ident[4];
+ }
+
+ // Add the runway
+ name += " RWY ";
+ name += ident.substr(1, 2);
+
+ // Add a left/right/centre indication if present.
+ if(ident.size() > 3) {
+ if((ident[3] != '-') && (ident[3] != ' ')) { // Early versions of the spec allowed a blank instead of a dash so check for both
+ name += ' ';
+ name += ident[3];
+ }
+ }
+ } else {
+ // Add the identifying letter, which I *think* should always be present, but seems to be inconsistent as to whether a dash is used.
+ if(ident.size() == 5) {
+ name += '-';
+ name += ident[4];
+ } else if(ident.size() == 4) {
+ name += '-';
+ name += ident[3];
+ } else {
+ // No suffix letter
+ }
+ }
+
+ return(name);
+}
+
+/*
+ Load instrument approaches from an ARINC 424 file.
+ Tested on ARINC 424-18.
+ Known / current best guess at the format:
+ Col 1: Always 'S'. If it isn't, ditch it.
+ Col 2-4: "Customer area" code, eg "USA", "CAN". I think that CAN is used for Alaska.
+ Col 5: Section code. Used in conjunction with sub-section code. Definitions are with sub-section code.
+ Col 6: Always blank.
+ Col 7-10: ICAO (or FAA) airport ident. Left justified if < 4 chars.
+ Col 11-12: Based on ICAO geographical region.
+ Col 13: Sub-section code. Used in conjunction with section code.
+ "HD/E/F" => Helicopter record.
+ "HS" => Helicopter minimum safe altitude.
+ "PA" => Airport record.
+ "PF" => Approach segment.
+ "PG" => Runway record.
+ "PP" => Path point record. ???
+ "PS" => MSA record (minimum safe altitude).
+
+ ------ The following is for "PF", approach segment -------
+
+ Col 14-19: SIAP ident for this approach (left justified). This is a standardised abbreviated approach name.
+ e.g. "R10LZ" expands to "RNAV (GPS) Z RWY 10 L". See the comment block for ExpandSIAPIdent for full details.
+ Col 20: Route type. This is tricky - I don't have full documentation and am having to guess a bit.
+ 'A' => Arrival route? This seems to be used to encode arrival routes from the IAF to the approach proper.
+ Note that the final fix of the arrival route is duplicated in the approach proper.
+ 'D' => VOR/DME or GPS
+ 'N' => NDB or GPS
+ 'P' => GPS (ARINC 424-18), GPS and RNAV (GPS) (ARINC 424-15 and before).
+ 'R' => RNAV (GPS) (ARINC 424-18).
+ 'S' => VOR or GPS
+ Col 21-25: Transition identifier. AFAICT, this is the ident of the IAF for this initial approach route, and left blank for the final approach course. See col 30-34 for the actual fix ident.
+ Col 26: BLANK
+ Col 27-29: Sequence number - position within the route segment. Rule: 10-20-30 etc.
+ Col 30-34: Fix identifer. The ident of the waypoint.
+ Col 35-36: ICAO geographical region code. I think we can ignore this for now.
+ Col 37: Section code - ??? I don't know what this means
+ Col 38 Subsection code - ??? ditto - no idea!
+ Col 40: Waypoint type.
+ 'A' => Airport as waypoint
+ 'E' => Essential waypoint (e.g. change of heading at this waypoint, etc).
+ 'G' => Runway or helipad as waypoint
+ 'H' => Heliport as waypoint
+ 'N' => NDB as waypoint
+ 'P' => Phantom waypoint (not sure if this is used in rev 18?)
+ 'V' => VOR as waypoint
+ Col 41: Waypoint type.
+ 'B' => Flyover, approach transition, or final approach.
+ 'E' => end of route segment (transition waypoint). (Actually "End of terminal procedure route type" in the docs).
+ 'N' => ??? I've also seen 'N' in this column, but don't know what it indicates.
+ 'Y' => Flyover.
+ Col 43: Waypoint type. May also be blank when none of the below.
+ 'A' => Initial approach fix (IAF)
+ 'F' => Final approach fix
+ 'H' => Holding fix
+ 'I' => Final approach course fix
+ 'M' => Missed approach point
+ 'P' => ??? This is present, but I don't know what this means and it wasn't in the FAA docs that I found the above in!
+ ??? Possibly procedure turn?
+ 'C' => ??? This is also present in the data, but missing from the docs. Is at airport 00R.
+ Col 107-111 MSA center fix. We can ignore this.
+*/
+void DCLGPS::LoadApproachData() {
+ FGNPIAP* iap = NULL;
+ GPSWaypoint* wp;
+ GPSFlightPlan* fp;
+ const GPSWaypoint* cwp;
+
+ std::ifstream fin;
+ SGPath path = globals->get_fg_root();
+ path.append("Navaids/rnav.dat");
+ fin.open(path.c_str(), ios::in);
+ if(!fin) {
+ //cout << "Unable to open input file " << path.c_str() << '\n';
+ return;
+ } else {
+ //cout << "Opened " << path.c_str() << " for reading\n";
+ }
+ char tmp[256];
+ string s;
+
+ string apt_ident; // This gets set to the ICAO code of the current airport being processed.
+ string iap_ident; // The abbreviated name of the current approach being processed.
+ string wp_ident; // The ident of the waypoint of the current line
+ string last_apt_ident;
+ string last_iap_ident;
+ string last_wp_ident;
+ // There is no need to save the full name - it can be generated on the fly from the abbreviated name as and when needed.
+ bool apt_in_progress = false; // Set true whilst loading all the approaches for a given airport.
+ bool iap_in_progress = false; // Set true whilst loading a given approach.
+ bool iap_error = false; // Set true if there is an error loading a given approach.
+ bool route_in_progress = false; // Set true when we are loading a "route" segment of the approach.
+ int last_sequence_number = 0; // Position within the route, rule (rev 18): 10, 20, 30 etc.
+ int sequence_number;
+ char last_route_type = 0;
+ char route_type;
+ char waypoint_fix_type; // This is the waypoint type from col 43, i.e. the type of fix. May be blank.
+
+ int j;
+
+ // Debugging info
+ unsigned int nLoaded = 0;
+ unsigned int nErrors = 0;
+
+ //for(i=0; i<64; ++i) {
+ while(!fin.eof()) {
+ fin.getline(tmp, 256);
+ //s = Fake_rnav_dat[i];
+ s = tmp;
+ if(s.size() < 132) continue;
+ if(s[0] == 'S') { // Valid line
+ string country_code = s.substr(1, 3);
+ if(country_code == "USA") { // For now we'll stick to US procedures in case there are unknown gotchas with others
+ if(s[4] == 'P') { // Includes approaches.
+ if(s[12] == 'A') { // Airport record
+ apt_ident = s.substr(6, 4);
+ // Trim any whitespace from the ident. The ident is left justified,
+ // so any space will be at the end.
+ if(apt_ident[3] == ' ') apt_ident = apt_ident.substr(0, 3);
+ // I think that all idents are either 3 or 4 chars - could check this though!
+ if(!apt_in_progress) {
+ last_apt_ident = apt_ident;
+ apt_in_progress = 1;
+ } else {
+ if(last_apt_ident != apt_ident) {
+ if(iap_in_progress) {
+ if(iap_error) {
+ //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
+ nErrors++;
+ } else {
+ _np_iap[iap->_aptIdent].push_back(iap);
+ //cout << "** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
+ nLoaded++;
+ }
+ iap_in_progress = false;
+ }
+ }
+ last_apt_ident = apt_ident;
+ }
+ iap_in_progress = 0;
+ } else if(s[12] == 'F') { // Approach segment
+ if(apt_in_progress) {
+ iap_ident = s.substr(13, 6);
+ // Trim any whitespace from the RH end.
+ for(j=0; j<6; ++j) {
+ if(iap_ident[5-j] == ' ') {
+ iap_ident = iap_ident.substr(0, 5-j);
+ } else {
+ // It's important to break here, since earlier versions of ARINC 424 allowed spaces in the ident.
+ break;
+ }
+ }
+ if(iap_in_progress) {
+ if(iap_ident != last_iap_ident) {
+ // This is a new approach - store the last one and trigger
+ // starting afresh by setting the in progress flag to false.
+ if(iap_error) {
+ //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
+ nErrors++;
+ } else {
+ _np_iap[iap->_aptIdent].push_back(iap);
+ //cout << "Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
+ nLoaded++;
+ }
+ iap_in_progress = false;
+ }
+ }
+ if(!iap_in_progress) {
+ iap = new FGNPIAP;
+ iap->_aptIdent = apt_ident;
+ iap->_ident = iap_ident;
+ iap->_name = ExpandSIAPIdent(iap_ident); // I suspect that it's probably better to just store idents, and to expand the names as needed.
+ // Note, we haven't set iap->_rwyStr yet.
+ last_iap_ident = iap_ident;
+ iap_in_progress = true;
+ iap_error = false;
+ }
+
+ // Route type
+ route_type = s[19];
+ sequence_number = atoi(s.substr(26,3).c_str());
+ wp_ident = s.substr(29, 5);
+ waypoint_fix_type = s[42];
+ // Trim any whitespace from the RH end
+ for(j=0; j<5; ++j) {
+ if(wp_ident[4-j] == ' ') {
+ wp_ident = wp_ident.substr(0, 4-j);
+ } else {
+ break;
+ }
+ }
+
+ // Ignore lines with no waypoint ID for now - these are normally part of the
+ // missed approach procedure, and we don't use them in the KLN89.
+ if(!wp_ident.empty()) {
+ // Make a local copy of the waypoint for now, since we're not yet sure if we'll be using it
+ GPSWaypoint w;
+ w.id = wp_ident;
+ bool wp_error = false;
+ if(w.id.substr(0, 2) == "RW" && waypoint_fix_type == 'M') {
+ // Assume that this is a missed-approach point based on the runway number, which appears to be standard for most approaches.
+ // Note: Currently fgFindAirportID returns NULL on error, but getRunwayByIdent throws an exception.
+ const FGAirport* apt = fgFindAirportID(iap->_aptIdent);
+ if(apt) {
+ string rwystr;
+ try {
+ rwystr = w.id.substr(2, 2);
+ // TODO - sanity check the rwystr at this point to ensure we have a double digit number
+ if(w.id.size() > 4) {
+ if(w.id[4] == 'L' || w.id[4] == 'C' || w.id[4] == 'R') {
+ rwystr += w.id[4];
+ }
+ }
+ FGRunway* rwy = apt->getRunwayByIdent(rwystr);
+ w.lat = rwy->begin().getLatitudeRad();
+ w.lon = rwy->begin().getLongitudeRad();
+ } catch(const sg_exception&) {
+ SG_LOG(SG_INSTR, SG_WARN, "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent);
+ //cout << "Unable to find runway " << w.id.substr(2, 2) << " at airport " << iap->_aptIdent << " ( w.id = " << w.id << ", rwystr = " << rwystr << " )\n";
+ wp_error = true;
+ }
+ } else {
+ wp_error = true;
+ }
+ } else {
+ cwp = FindFirstByExactId(w.id);
+ if(cwp) {
+ w = *cwp;
+ } else {
+ wp_error = true;
+ }
+ }
+ switch(waypoint_fix_type) {
+ case 'A': w.appType = GPS_IAF; break;
+ case 'F': w.appType = GPS_FAF; break;
+ case 'H': w.appType = GPS_MAHP; break;
+ case 'I': w.appType = GPS_IAP; break;
+ case 'M': w.appType = GPS_MAP; break;
+ case ' ': w.appType = GPS_APP_NONE; break;
+ //default: cout << "Unknown waypoint_fix_type: \'" << waypoint_fix_type << "\' [" << apt_ident << ", " << iap_ident << "]\n";
+ }
+
+ if(wp_error) {
+ //cout << "Unable to find waypoint " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
+ iap_error = true;
+ }
+
+ if(!wp_error) {
+ if(route_in_progress) {
+ if(sequence_number > last_sequence_number) {
+ // TODO - add a check for runway numbers
+ // Check for the waypoint ID being the same as the previous line.
+ // This is often the case for the missed approach holding point.
+ if(wp_ident == last_wp_ident) {
+ if(waypoint_fix_type == 'H') {
+ if(!iap->_IAP.empty()) {
+ if(iap->_IAP[iap->_IAP.size() - 1]->appType == GPS_APP_NONE) {
+ iap->_IAP[iap->_IAP.size() - 1]->appType = GPS_MAHP;
+ } else {
+ //cout << "Waypoint is MAHP and another type! " << w.id << " [" << apt_ident << ", " << iap_ident << "]\n";
+ }
+ }
+ }
+ } else {
+ // Create a new waypoint on the heap, copy the local copy into it, and push it onto the approach.
+ wp = new GPSWaypoint;
+ *wp = w;
+ if(route_type == 'A') {
+ fp->waypoints.push_back(wp);
+ } else {
+ iap->_IAP.push_back(wp);
+ }
+ }
+ } else if(sequence_number == last_sequence_number) {
+ // This seems to happen once per final approach route - one of the waypoints
+ // is duplicated with the same sequence number - I'm not sure what information
+ // the second line give yet so ignore it for now.
+ // TODO - figure this out!
+ } else {
+ // Finalise the current route and start a new one
+ //
+ // Finalise the current route
+ if(last_route_type == 'A') {
+ // Push the flightplan onto the approach
+ iap->_approachRoutes.push_back(fp);
+ } else {
+ // All the waypoints get pushed individually - don't need to do it.
+ }
+ // Start a new one
+ // There are basically 2 possibilities here - either it's one of the arrival transitions,
+ // or it's the core final approach course.
+ wp = new GPSWaypoint;
+ *wp = w;
+ if(route_type == 'A') { // It's one of the arrival transition(s)
+ fp = new GPSFlightPlan;
+ fp->waypoints.push_back(wp);
+ } else {
+ iap->_IAP.push_back(wp);
+ }
+ route_in_progress = true;
+ }
+ } else {
+ // Start a new route.
+ // There are basically 2 possibilities here - either it's one of the arrival transitions,
+ // or it's the core final approach course.
+ wp = new GPSWaypoint;
+ *wp = w;
+ if(route_type == 'A') { // It's one of the arrival transition(s)
+ fp = new GPSFlightPlan;
+ fp->waypoints.push_back(wp);
+ } else {
+ iap->_IAP.push_back(wp);
+ }
+ route_in_progress = true;
+ }
+ last_route_type = route_type;
+ last_wp_ident = wp_ident;
+ last_sequence_number = sequence_number;
+ }
+ }
+ } else {
+ // ERROR - no airport record read.
+ }
+ }
+ } else {
+ // Check and finalise any approaches in progress
+ // TODO - sanity check that the approach has all the required elements
+ if(iap_in_progress) {
+ // This is a new approach - store the last one and trigger
+ // starting afresh by setting the in progress flag to false.
+ if(iap_error) {
+ //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
+ nErrors++;
+ } else {
+ _np_iap[iap->_aptIdent].push_back(iap);
+ //cout << "* Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
+ nLoaded++;
+ }
+ iap_in_progress = false;
+ }
+ }
+ }
+ }
+ }
+
+ // If we get to the end of the file, load any approach that is still in progress
+ // TODO - sanity check that the approach has all the required elements
+ if(iap_in_progress) {
+ if(iap_error) {
+ //cout << "ERROR: Unable to load approach " << iap->_ident << " at " << iap->_aptIdent << '\n';
+ nErrors++;
+ } else {
+ _np_iap[iap->_aptIdent].push_back(iap);
+ //cout << "*** Loaded " << iap->_aptIdent << "\t" << iap->_ident << '\n';
+ nLoaded++;
+ }
+ }
+
+ //cout << "Done loading approach database\n";
+ //cout << "Loaded: " << nLoaded << '\n';
+ //cout << "Failed: " << nErrors << '\n';
+
+ fin.close();
+}
+
+GPSWaypoint* DCLGPS::GetActiveWaypoint() {
+ return &_activeWaypoint;
+}
+
+// Returns meters
+float DCLGPS::GetDistToActiveWaypoint() {
+ return _dist2Act;
+}
+
// I don't yet fully understand all the gotchas about where to source time from.
// This function sets the initial timer before the clock exports properties
// and the one below uses the clock to be consistent with the rest of the code.
}
void DCLGPS::DtoInitiate(const string& s) {
- //cout << "DtoInitiate, s = " << s << '\n';
const GPSWaypoint* wp = FindFirstByExactId(s);
if(wp) {
- //cout << "Waypoint found, starting dto operation!\n";
+ // TODO - Currently we start DTO operation unconditionally, regardless of which mode we are in.
+ // In fact, the following rules apply:
+ // In LEG mode, start DTO as we currently do.
+ // In OBS mode, set the active waypoint to the requested waypoint, and then:
+ // If the KLN89 is not connected to an external HSI or CDI, set the OBS course to go direct to the waypoint.
+ // If the KLN89 *is* connected to an external HSI or CDI, it cannot set the course itself, and will display
+ // a scratchpad message with the course to set manually on the HSI/CDI.
+ // In both OBS cases, leave _dto false, since we don't need the virtual waypoint created.
_dto = true;
_activeWaypoint = *wp;
_fromWaypoint.lat = _gpsLat;
_fromWaypoint.lon = _gpsLon;
_fromWaypoint.type = GPS_WP_VIRT;
- _fromWaypoint.id = "DTOWP";
- delete wp;
+ _fromWaypoint.id = "_DTOWP_";
+ delete wp;
} 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.
+ // TODO - Should bring up the user waypoint page.
+ _dto = false;
}
}
_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() {
+void DCLGPS::ToggleOBSMode() {
_obsMode = !_obsMode;
if(_obsMode) {
- if(!_activeWaypoint.id.empty()) {
- _obsHeading = static_cast<int>(_dtkMag);
+ // We need to set the OBS heading. The rules for this are:
+ //
+ // If the KLN89 is connected to an external HSI or CDI, then the OBS heading must be set
+ // to the OBS radial on the external indicator, and cannot be changed in the KLN89.
+ //
+ // If the KLN89 is not connected to an external indicator, then:
+ // If there is an active waypoint, the OBS heading is set such that the
+ // deviation indicator remains at the same deviation (i.e. set to DTK,
+ // although there may be some small difference).
+ //
+ // If there is not an active waypoint, I am not sure what value should be
+ // set.
+ //
+ if(fgGetBool("/instrumentation/nav/slaved-to-gps")) {
+ _obsHeading = static_cast<int>(fgGetDouble("/instrumentation/nav/radials/selected-deg") + 0.5);
+ } else if(!_activeWaypoint.id.empty()) {
+ // NOTE: I am not sure if this is completely correct. There are sometimes small differences
+ // between DTK and default OBS heading in the simulator (~ 1 or 2 degrees). It might also
+ // be that OBS heading should be stored as float and only displayed as int, in order to maintain
+ // the initial bar deviation?
+ _obsHeading = static_cast<int>(_dtkMag + 0.5);
+ //cout << "_dtkMag = " << _dtkMag << ", _dtkTrue = " << _dtkTrue << ", _obsHeading = " << _obsHeading << '\n';
+ } else {
+ // TODO - what should we really do here?
+ _obsHeading = 0;
}
+
+ // Valid OBS heading values are 0 -> 359 degrees inclusive (from kln89 simulator).
+ if(_obsHeading > 359) _obsHeading -= 360;
+ if(_obsHeading < 0) _obsHeading += 360;
+
// 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();
// TODO - base the 180 deg correction on the to/from flag.
_fromWaypoint = GetPositionOnMagRadial(_activeWaypoint, 10, _obsHeading + 180.0);
- _fromWaypoint.id = "OBSWP";
+ _fromWaypoint.type = GPS_WP_VIRT;
+ _fromWaypoint.id = "_OBSWP_";
}
-void DCLGPS::MsgPressed() {}
-
void DCLGPS::CDIFSDIncrease() {
if(_currentCdiScaleIndex == 0) {
_currentCdiScaleIndex = _cdiScales.size() - 1;
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.
case GPS_WP_USR:
// TODO
break;
+ case GPS_WP_VIRT:
+ // TODO
+ break;
}
}
}
/***************************************/
-/**
- * STL functor for use with algorithms. This comapres strings according to
- * the KLN-89's notion of ordering, with digits after letters.
- * Also implements FGIdentOrdering so it can be passed into the various list
- * find helpers.
- */
-
-class stringOrderKLN89 : public FGIdentOrdering
+class DCLGPSFilter : public FGPositioned::Filter
{
public:
- bool operator()(const gps_waypoint_map::value_type& aA, const std::string& aB) const
- {
- return compare(aA.first, aB);
- }
-
- bool operator()(const std::string& aS1, const std::string& aS2) const
- {
- return compare(aS1, aS2);
- }
-
- virtual bool compare(const std::string& aS1, const std::string& aS2) const
- {
- if (aS1.empty()) return true;
- if (aS2.empty()) return false;
-
- char* a = (char*) aS1.c_str();
- char* b = (char*) aS2.c_str();
-
- for ( ; *a && *b; ++a, ++b) {
- if (*a == *b) continue;
-
- bool aDigit = isdigit(*a);
- bool bDigit = isdigit(*b);
-
- if (aDigit == bDigit) {
- return (*a < *b); // we already know they're not equal
- }
-
- // digit-ness differs
- if (aDigit) return false; // s1 = KS9 goes *after* s2 = KSA
- assert(bDigit);
- return true; // s1 = KSF, s2 = KS5, s1 is indeed < s2
+ virtual bool pass(const FGPositioned* aPos) const {
+ switch (aPos->type()) {
+ case FGPositioned::AIRPORT:
+ // how about heliports and seaports?
+ case FGPositioned::NDB:
+ case FGPositioned::VOR:
+ case FGPositioned::WAYPOINT:
+ case FGPositioned::FIX:
+ break;
+ default: return false; // reject all other types
}
-
- if (*b) return true; // *a == 0, s2 is longer
- return false; // s1 is longer, or strings are equal
+ return true;
}
};
GPSWaypoint* DCLGPS::FindFirstById(const string& id) const
{
- stringOrderKLN89 ordering;
- nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, false);
- nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, false);
- const FGFix* fix = globals->get_fixlist()->findFirstByIdent(id, &ordering);
- const FGAirport* apt = globals->get_airports()->findFirstById(id, &ordering);
- // search local gps waypoints (USR)
-
-// pick the best - ugly logic, sorry. This is a temporary fix to getting rid
-// of the huge local waypoint table, it'll die when there's a way to query
-// this stuff centrally.
-// what we're doing is using map inserts to order the result, then using
-// the first entry (begin()) as the lowest, hence best, match
- map<string, GPSWpType, stringOrderKLN89> sorter;
- if (fix) sorter[fix->get_ident()] = GPS_WP_INT;
- if (apt) sorter[apt->getId()] = GPS_WP_APT;
- if (!vors.empty()) sorter[vors.front()->get_ident()] = GPS_WP_VOR;
- if (!ndbs.empty()) sorter[ndbs.front()->get_ident()] = GPS_WP_NDB;
-
- if (sorter.empty()) return NULL; // no results at all
- GPSWpType ty = sorter.begin()->second;
-
- switch (ty) {
- case GPS_WP_INT:
- return GPSWaypoint::createFromFix(fix);
-
- case GPS_WP_APT:
- return GPSWaypoint::createFromAirport(apt);
-
- case GPS_WP_VOR:
- return GPSWaypoint::createFromNav(vors.front());
-
- case GPS_WP_NDB:
- return GPSWaypoint::createFromNav(ndbs.front());
- default:
- return NULL; // can't happen
- }
+ DCLGPSFilter filter;
+ FGPositionedRef result = FGPositioned::findNextWithPartialId(NULL, id, &filter);
+ return GPSWaypoint::createFromPositioned(result);
}
GPSWaypoint* DCLGPS::FindFirstByExactId(const string& id) const
{
- if (const FGAirport* apt = globals->get_airports()->search(id)) {
- return GPSWaypoint::createFromAirport(apt);
- }
-
- if (const FGFix* fix = globals->get_fixlist()->search(id)) {
- return GPSWaypoint::createFromFix(fix);
- }
-
- nav_list_type vors = globals->get_navlist()->findFirstByIdent(id, FG_NAV_VOR, true);
- if (!vors.empty()) {
- return GPSWaypoint::createFromNav(vors.front());
- }
+ SGGeod pos(SGGeod::fromRad(_lon, _lat));
+ FGPositionedRef result = FGPositioned::findClosestWithIdent(id, pos);
+ return GPSWaypoint::createFromPositioned(result);
+}
+
+// TODO - add the ASCII / alphabetical stuff from the Atlas version
+FGPositioned* DCLGPS::FindTypedFirstById(const string& id, FGPositioned::Type ty, bool &multi, bool exact)
+{
+ multi = false;
+ FGPositioned::TypeFilter filter(ty);
- nav_list_type ndbs = globals->get_navlist()->findFirstByIdent(id, FG_NAV_NDB, true);
- if (!ndbs.empty()) {
- return GPSWaypoint::createFromNav(ndbs.front());
+ if (exact) {
+ FGPositioned::List matches =
+ FGPositioned::findAllWithIdent(id, &filter);
+ FGPositioned::sortByRange(matches, SGGeod::fromRad(_lon, _lat));
+ multi = (matches.size() > 1);
+ return matches.empty() ? NULL : matches.front().ptr();
}
- return NULL;
+ return FGPositioned::findNextWithPartialId(NULL, id, &filter);
}
-// Host specific lookup functions
-// TODO - add the ASCII / alphabetical stuff from the Atlas version
-FGNavRecord* DCLGPS::FindFirstVorById(const 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!
+FGNavRecord* DCLGPS::FindFirstVorById(const string& id, bool &multi, bool exact)
+{
+ return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::VOR, multi, exact));
}
-// TODO - add the ASCII / alphabetical stuff from the Atlas version
-FGNavRecord* DCLGPS::FindFirstNDBById(const 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!
+FGNavRecord* DCLGPS::FindFirstNDBById(const string& id, bool &multi, bool exact)
+{
+ return dynamic_cast<FGNavRecord*>(FindTypedFirstById(id, FGPositioned::NDB, multi, exact));
}
-const FGFix* DCLGPS::FindFirstIntById(const 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()->search(id);
-
- stringOrderKLN89 ordering;
- return globals->get_fixlist()->findFirstByIdent(id, &ordering);
+const FGFix* DCLGPS::FindFirstIntById(const string& id, bool &multi, bool exact)
+{
+ return dynamic_cast<FGFix*>(FindTypedFirstById(id, FGPositioned::FIX, multi, exact));
}
-const FGAirport* DCLGPS::FindFirstAptById(const 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()->search(id));
-
- stringOrderKLN89 ordering;
- return globals->get_airports()->findFirstById(id, &ordering);
+const FGAirport* DCLGPS::FindFirstAptById(const string& id, bool &multi, bool exact)
+{
+ return dynamic_cast<FGAirport*>(FindTypedFirstById(id, FGPositioned::AIRPORT, multi, exact));
}
-FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
- return(globals->get_navlist()->findClosest(lon_rad, lat_rad, 0.0, FG_NAV_VOR));
+FGNavRecord* DCLGPS::FindClosestVor(double lat_rad, double lon_rad) {
+ FGPositioned::TypeFilter filter(FGPositioned::VOR);
+ double cutoff = 1000; // nautical miles
+ FGPositionedRef v = FGPositioned::findClosest(SGGeod::fromRad(lon_rad, lat_rad), cutoff, &filter);
+ if (!v) {
+ return NULL;
+ }
+
+ return dynamic_cast<FGNavRecord*>(v.ptr());
}
//----------------------------------------------------------------------------------------------------------