From 2908bd995d439fca42acc2ff420b69cfc5545338 Mon Sep 17 00:00:00 2001 From: david Date: Mon, 10 Mar 2003 14:09:43 +0000 Subject: [PATCH] Added simple GPS support (no waypoints yet). --- src/Instrumentation/Makefile.am | 1 + src/Instrumentation/gps.cxx | 106 +++++++++++++++++++++++++ src/Instrumentation/gps.hxx | 78 ++++++++++++++++++ src/Instrumentation/instrument_mgr.cxx | 3 + 4 files changed, 188 insertions(+) create mode 100644 src/Instrumentation/gps.cxx create mode 100644 src/Instrumentation/gps.hxx diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index 796b44d56..bf4a56875 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -2,6 +2,7 @@ noinst_LIBRARIES = libInstrumentation.a libInstrumentation_a_SOURCES = instrument_mgr.cxx instrument_mgr.hxx \ dme.cxx dme.hxx \ + gps.cxx gps.hxx \ gyro.cxx gyro.hxx \ airspeed_indicator.cxx airspeed_indicator.hxx \ attitude_indicator.cxx attitude_indicator.hxx \ diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx new file mode 100644 index 000000000..cbb0d92c9 --- /dev/null +++ b/src/Instrumentation/gps.cxx @@ -0,0 +1,106 @@ +// gps.cxx - distance-measuring equipment. +// Written by David Megginson, started 2003. +// +// This file is in the Public Domain and comes with no warranty. + +#include +#include + +#include
+ +#include "gps.hxx" + + +GPS::GPS () + : _last_valid(false), + _last_longitude_deg(0), + _last_latitude_deg(0), + _last_altitude_m(0) +{ +} + +GPS::~GPS () +{ +} + +void +GPS::init () +{ + _longitude_node = fgGetNode("/position/longitude-deg", true); + _latitude_node = fgGetNode("/position/latitude-deg", true); + _altitude_node = fgGetNode("/position/altitude-ft", true); + _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true); + _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true); + _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true); + + _raim_node = fgGetNode("/instrumentation/gps/raim", true); + _indicated_longitude_node = + fgGetNode("/instrumentation/gps/indicated-longitude-deg", true); + _indicated_latitude_node = + fgGetNode("/instrumentation/gps/indicated-latitude-deg", true); + _indicated_altitude_node = + fgGetNode("/instrumentation/gps/indicated-altitude-ft", true); + _true_track_node = + fgGetNode("/instrumentation/gps/indicated-track-true-deg", true); + _magnetic_track_node = + fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true); + _speed_node = + fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true); +} + +void +GPS::update (double delta_time_sec) +{ + // If it's off, don't bother. + if (!_serviceable_node->getBoolValue() || + !_electrical_node->getBoolValue()) { + _last_valid = false; + _last_longitude_deg = 0; + _last_latitude_deg = 0; + _last_altitude_m = 0; + _raim_node->setDoubleValue(false); + _indicated_longitude_node->setDoubleValue(0); + _indicated_latitude_node->setDoubleValue(0); + _indicated_altitude_node->setDoubleValue(0); + _true_track_node->setDoubleValue(0); + _magnetic_track_node->setDoubleValue(0); + _speed_node->setDoubleValue(0); + return; + } + + // Get the aircraft position + double longitude_deg = _longitude_node->getDoubleValue(); + double latitude_deg = _latitude_node->getDoubleValue(); + double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER; + double magvar_deg = _magvar_node->getDoubleValue(); + + _raim_node->setBoolValue(true); + _indicated_longitude_node->setDoubleValue(longitude_deg); + _indicated_latitude_node->setDoubleValue(latitude_deg); + _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET); + + if (_last_valid) { + double track1_deg, track2_deg, distance_m; + geo_inverse_wgs_84(altitude_m, + _last_latitude_deg, _last_longitude_deg, + latitude_deg, longitude_deg, + &track1_deg, &track2_deg, &distance_m); + double distance_nm = distance_m * SG_METER_TO_NM; + double speed_kt = ((distance_m * SG_METER_TO_NM) * + ((1 / delta_time_sec) * 3600.0)); + _true_track_node->setDoubleValue(track1_deg); + _magnetic_track_node->setDoubleValue(track1_deg - magvar_deg); + _speed_node->setDoubleValue(speed_kt); + } else { + _true_track_node->setDoubleValue(0); + _magnetic_track_node->setDoubleValue(0); + _speed_node->setDoubleValue(0); + } + + _last_valid = true; + _last_longitude_deg = longitude_deg; + _last_latitude_deg = latitude_deg; + _last_altitude_m = altitude_m; +} + +// end of gps.cxx diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx new file mode 100644 index 000000000..61b098680 --- /dev/null +++ b/src/Instrumentation/gps.hxx @@ -0,0 +1,78 @@ +// gps.hxx - distance-measuring equipment. +// Written by David Megginson, started 2003. +// +// This file is in the Public Domain and comes with no warranty. + + +#ifndef __INSTRUMENTS_GPS_HXX +#define __INSTRUMENTS_GPS_HXX 1 + +#ifndef __cplusplus +# error This library requires C++ +#endif + +#include +#include
+ + +/** + * Model a GPS radio. + * + * Input properties: + * + * /position/longitude-deg + * /position/latitude-deg + * /position/altitude-ft + * /environment/magnetic-variation-deg + * /systems/electrical/outputs/gps + * /instrumentation/gps/serviceable + * + * Output properties: + * + * /instrumentation/gps/indicated-longitude-deg + * /instrumentation/gps/indicated-latitude-deg + * /instrumentation/gps/indicated-altitude-ft + * /instrumentation/gps/indicated-track-true-deg + * /instrumentation/gps/indicated-track-magnetic-deg + * /instrumentation/gps/indicated-ground-speed-kt + */ +class GPS : public FGSubsystem +{ + +public: + + GPS (); + virtual ~GPS (); + + virtual void init (); + virtual void update (double delta_time_sec); + +private: + + void search (double frequency, double longitude_rad, + double latitude_rad, double altitude_m); + + SGPropertyNode_ptr _longitude_node; + SGPropertyNode_ptr _latitude_node; + SGPropertyNode_ptr _altitude_node; + SGPropertyNode_ptr _magvar_node; + SGPropertyNode_ptr _serviceable_node; + SGPropertyNode_ptr _electrical_node; + + SGPropertyNode_ptr _raim_node; + SGPropertyNode_ptr _indicated_longitude_node; + SGPropertyNode_ptr _indicated_latitude_node; + SGPropertyNode_ptr _indicated_altitude_node; + SGPropertyNode_ptr _true_track_node; + SGPropertyNode_ptr _magnetic_track_node; + SGPropertyNode_ptr _speed_node; + + bool _last_valid; + double _last_longitude_deg; + double _last_latitude_deg; + double _last_altitude_m; + +}; + + +#endif // __INSTRUMENTS_GPS_HXX diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx index 228548ec8..2e927f8d8 100644 --- a/src/Instrumentation/instrument_mgr.cxx +++ b/src/Instrumentation/instrument_mgr.cxx @@ -13,7 +13,9 @@ #include "heading_indicator.hxx" #include "vertical_speed_indicator.hxx" #include "mag_compass.hxx" + #include "dme.hxx" +#include "gps.hxx" FGInstrumentMgr::FGInstrumentMgr () @@ -27,6 +29,7 @@ FGInstrumentMgr::FGInstrumentMgr () set_subsystem("vsi", new VerticalSpeedIndicator); set_subsystem("compass", new MagCompass); set_subsystem("dme", new DME, 1.0); + set_subsystem("gps", new GPS, 0.45); } FGInstrumentMgr::~FGInstrumentMgr ()