1 // gps.cxx - distance-measuring equipment.
2 // Written by David Megginson, started 2003.
4 // This file is in the Public Domain and comes with no warranty.
6 #include <simgear/compiler.h>
7 #include <simgear/math/sg_geodesy.hxx>
9 #include <Main/fg_props.hxx>
16 _last_longitude_deg(0),
17 _last_latitude_deg(0),
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);
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);
44 fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
45 _magnetic_track_node =
46 fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
48 fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
52 GPS::update (double delta_time_sec)
54 // If it's off, don't bother.
55 if (!_serviceable_node->getBoolValue() ||
56 !_electrical_node->getBoolValue()) {
58 _last_longitude_deg = 0;
59 _last_latitude_deg = 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);
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();
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);
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);
95 _true_track_node->setDoubleValue(0);
96 _magnetic_track_node->setDoubleValue(0);
97 _speed_node->setDoubleValue(0);
101 _last_longitude_deg = longitude_deg;
102 _last_latitude_deg = latitude_deg;
103 _last_altitude_m = altitude_m;