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/constants.h>
8 #include <simgear/math/sg_geodesy.hxx>
10 #include <Main/fg_props.hxx>
17 _last_longitude_deg(0),
18 _last_latitude_deg(0),
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);
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);
45 fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
46 _magnetic_track_node =
47 fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
49 fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
53 GPS::update (double delta_time_sec)
55 // If it's off, don't bother.
56 if (!_serviceable_node->getBoolValue() ||
57 !_electrical_node->getBoolValue()) {
59 _last_longitude_deg = 0;
60 _last_latitude_deg = 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);
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();
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);
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);
96 _true_track_node->setDoubleValue(0);
97 _magnetic_track_node->setDoubleValue(0);
98 _speed_node->setDoubleValue(0);
102 _last_longitude_deg = longitude_deg;
103 _last_latitude_deg = latitude_deg;
104 _last_altitude_m = altitude_m;