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