src/Instrumentation/gps.cxx

   1 // gps.cxx - distance-measuring equipment.
   2 // Written by David Megginson, started 2003.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #include <simgear/compiler.h>
   7 #include <simgear/constants.h>
   8 #include <simgear/math/sg_geodesy.hxx>
   9
  10 #include <Main/fg_props.hxx>
  11
  12 #include "gps.hxx"
  13
  14
  15 GPS::GPS ()
  16     : _last_valid(false),
  17       _last_longitude_deg(0),
  18       _last_latitude_deg(0),
  19       _last_altitude_m(0)
  20 {
  21 }
  22
  23 GPS::~GPS ()
  24 {
  25 }
  26
  27 void
  28 GPS::init ()
  29 {
  30     _longitude_node = fgGetNode("/position/longitude-deg", true);
  31     _latitude_node = fgGetNode("/position/latitude-deg", true);
  32     _altitude_node = fgGetNode("/position/altitude-ft", true);
  33     _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
  34     _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
  35     _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
  36
  37     _raim_node = fgGetNode("/instrumentation/gps/raim", true);
  38     _indicated_longitude_node =
  39         fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
  40     _indicated_latitude_node =
  41         fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
  42     _indicated_altitude_node =
  43         fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
  44     _true_track_node =
  45         fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
  46     _magnetic_track_node =
  47         fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
  48     _speed_node =
  49         fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
  50 }
  51
  52 void
  53 GPS::update (double delta_time_sec)
  54 {
  55                                 // If it's off, don't bother.
  56     if (!_serviceable_node->getBoolValue() ||
  57         !_electrical_node->getBoolValue()) {
  58         _last_valid = false;
  59         _last_longitude_deg = 0;
  60         _last_latitude_deg = 0;
  61         _last_altitude_m = 0;
  62         _raim_node->setDoubleValue(false);
  63         _indicated_longitude_node->setDoubleValue(0);
  64         _indicated_latitude_node->setDoubleValue(0);
  65         _indicated_altitude_node->setDoubleValue(0);
  66         _true_track_node->setDoubleValue(0);
  67         _magnetic_track_node->setDoubleValue(0);
  68         _speed_node->setDoubleValue(0);
  69         return;
  70     }
  71
  72                                 // Get the aircraft position
  73     double longitude_deg = _longitude_node->getDoubleValue();
  74     double latitude_deg = _latitude_node->getDoubleValue();
  75     double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
  76     double magvar_deg = _magvar_node->getDoubleValue();
  77
  78     _raim_node->setBoolValue(true);
  79     _indicated_longitude_node->setDoubleValue(longitude_deg);
  80     _indicated_latitude_node->setDoubleValue(latitude_deg);
  81     _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
  82
  83     if (_last_valid) {
  84         double track1_deg, track2_deg, distance_m;
  85         geo_inverse_wgs_84(altitude_m,
  86                            _last_latitude_deg, _last_longitude_deg,
  87                            latitude_deg, longitude_deg,
  88                            &track1_deg, &track2_deg, &distance_m);
  89         double distance_nm = distance_m * SG_METER_TO_NM;
  90         double speed_kt = ((distance_m * SG_METER_TO_NM) *
  91                            ((1 / delta_time_sec) * 3600.0));
  92         _true_track_node->setDoubleValue(track1_deg);
  93         _magnetic_track_node->setDoubleValue(track1_deg - magvar_deg);
  94         _speed_node->setDoubleValue(speed_kt);
  95     } else {
  96         _true_track_node->setDoubleValue(0);
  97         _magnetic_track_node->setDoubleValue(0);
  98         _speed_node->setDoubleValue(0);
  99     }
 100
 101     _last_valid = true;
 102     _last_longitude_deg = longitude_deg;
 103     _last_latitude_deg = latitude_deg;
 104     _last_altitude_m = altitude_m;
 105 }
 106
 107 // end of gps.cxx