1 // Radar Altimeter
2 //
3 // Written by Vivian MEAZZA, started Feb 2008.
4 //
5 //
6 // Copyright (C) 2008 Vivian Meazza
7 //
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
12 //
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Public License for more details.
17 //
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
21 //
22 //
23
24 #ifdef HAVE_CONFIG_H
25 # include "config.h"
26 #endif
27
28 #include "rad_alt.hxx"
29
30 #include <simgear/scene/material/mat.hxx>
31
32
33 #include <Main/fg_props.hxx>
34 #include <Main/globals.hxx>
35 #include <Scenery/scenery.hxx>
36
37
RadarAltimeter(SGPropertyNode * node)38 RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
39 _time(0.0),
40 _interval(node->getDoubleValue("update-interval-sec", 1.0))
41 {
42 _name = node->getStringValue("name", "radar-altimeter");
43 _num = node->getIntValue("number", 0);
44 }
45
~RadarAltimeter()46 RadarAltimeter::~RadarAltimeter()
47 {
48 }
49
50 void
init()51 RadarAltimeter::init ()
52 {
53
54 std::string branch = "/instrumentation/" + _name;
55 _Instrument = fgGetNode(branch.c_str(), _num, true);
56
57 _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
58 _serviceable_node = _Instrument->getNode("serviceable", true);
59
60 _user_alt_agl_node = fgGetNode("/position/altitude-agl-ft", true);
61 _rad_alt_warning_node = fgGetNode("/sim/alarms/rad-alt-warning", true);
62
63 _Instrument->setFloatValue("tilt",-85);
64 _Instrument->setStringValue("status","RA");
65
66 _Instrument->getDoubleValue("elev-limit", true);
67 _Instrument->getDoubleValue("elev-step-deg", true);
68 _Instrument->getDoubleValue("az-limit-deg", true);
69 _Instrument->getDoubleValue("az-step-deg", true);
70 _Instrument->getDoubleValue("max-range-m", true);
71 _Instrument->getDoubleValue("min-range-m", true);
72 _Instrument->getDoubleValue("tilt", true);
73 _Instrument->getDoubleValue("set-height-ft", true);
74 _Instrument->getDoubleValue("set-excursion-percent", true);
75
76 _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
77 _Instrument->getDoubleValue("antenna/y-offset-m"),
78 _Instrument->getDoubleValue("antenna/z-offset-m"));
79 }
80
81 void
update(double delta_time_sec)82 RadarAltimeter::update (double delta_time_sec)
83 {
84 if (!_sceneryLoaded->getBoolValue())
85 return;
86
87 if ( ! _serviceable_node->getBoolValue() ) {
88 _Instrument->setStringValue("status","");
89 return;
90 }
91
92 _time += delta_time_sec;
93 if (_time < _interval)
94 return;
95
96 _time -= _interval;
97
98 update_altitude();
99 updateSetHeight();
100 }
101
102 double
getDistanceAntennaToHit(const SGVec3d & nearestHit) const103 RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
104 {
105 return norm(nearestHit - getCartAntennaPos());
106 }
107
108 void
updateSetHeight()109 RadarAltimeter::updateSetHeight()
110 {
111 double set_ht_ft = _Instrument->getDoubleValue("set-height-ft", 9999);
112 double set_excur = _Instrument->getDoubleValue("set-excursion-percent", 0);
113 if (set_ht_ft == 9999) {
114 _rad_alt_warning_node->setIntValue(9999);
115 return;
116 }
117
118 double radarAltFt = _min_radalt * SG_METER_TO_FEET;
119 if (radarAltFt < set_ht_ft * (100 - set_excur)/100)
120 _rad_alt_warning_node->setIntValue(-1);
121 else if (radarAltFt > set_ht_ft * (100 + set_excur)/100)
122 _rad_alt_warning_node->setIntValue(1);
123 else
124 _rad_alt_warning_node->setIntValue(0);
125 }
126
127 void
update_altitude()128 RadarAltimeter::update_altitude()
129 {
130 double el_limit = _Instrument->getDoubleValue("elev-limit", 15);
131 double el_step = _Instrument->getDoubleValue("elev-step-deg", 15);
132 double az_limit = _Instrument->getDoubleValue("az-limit-deg", 15);
133 double az_step = _Instrument->getDoubleValue("az-step-deg", 15);
134 double max_range = _Instrument->getDoubleValue("max-range-m", 1500);
135 double min_range = _Instrument->getDoubleValue("min-range-m", 0.001);
136
137
138 _min_radalt = max_range;
139 bool haveHit = false;
140 SGVec3d cartantennapos = getCartAntennaPos();
141
142 for(double brg = -az_limit; brg <= az_limit; brg += az_step){
143 for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
144 SGVec3d userVec = rayVector(brg, elev);
145
146 SGVec3d nearestHit;
147 globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
148 double measuredDistance = dist(cartantennapos, nearestHit);
149
150 if (measuredDistance >= min_range && measuredDistance <= max_range) {
151 if (measuredDistance < _min_radalt) {
152 _min_radalt = measuredDistance;
153 haveHit = true;
154 }
155 } // of hit within permissible range
156 } // of elevation step
157 } // of azimuth step
158
159 _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
160 if (!haveHit) {
161 _rad_alt_warning_node->setIntValue(9999);
162 }
163 }
164
165 SGVec3d
getCartAntennaPos() const166 RadarAltimeter::getCartAntennaPos() const
167 {
168 double yaw, pitch, roll;
169 globals->get_aircraft_orientation(yaw, pitch, roll);
170
171 // Transform to the right coordinate frame, configuration is done in
172 // the x-forward, y-right, z-up coordinates (feet), computation
173 // in the simulation usual body x-forward, y-right, z-down coordinates
174 // (meters) )
175
176 // Transform the user position to the horizontal local coordinate system.
177 SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
178
179 // and postrotate the orientation of the user model wrt the horizontal
180 // local frame
181 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
182
183 // The offset converted to the usual body fixed coordinate system
184 // rotated to the earth-fixed coordinates axis
185 SGVec3d ecfOffset = hlTrans.backTransform(_antennaOffset);
186
187 // Add the position offset of the user model to get the geocentered position
188 return globals->get_aircraft_position_cart() + ecfOffset;
189 }
190
rayVector(double az,double el) const191 SGVec3d RadarAltimeter::rayVector(double az, double el) const
192 {
193 double yaw, pitch, roll;
194 globals->get_aircraft_orientation(yaw, pitch, roll);
195
196 double tilt = _Instrument->getDoubleValue("tilt");
197 bool roll_stab = false,
198 pitch_stab = false;
199
200 SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);
201
202 // Transform the antenna position to the horizontal local coordinate system.
203 SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
204
205 // and postrotate the orientation of the radar wrt the horizontal
206 // local frame
207 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
208 pitch_stab ? 0 :pitch,
209 roll_stab ? 0 : roll);
210 hlTrans *= offset;
211
212 // now rotate the rotation vector back into the
213 // earth centered frames coordinates
214 SGVec3d angleaxis(1,0,0);
215 return hlTrans.backTransform(angleaxis);
216
217 }
218
219
220 // Register the subsystem.
221 #if 0
222 SGSubsystemMgr::InstancedRegistrant<RadarAltimeter> registrantRadarAltimeter(
223 SGSubsystemMgr::FDM,
224 {{"instrumentation", SGSubsystemMgr::Dependency::HARD}});
225 #endif
226