3 // Written by Vivian MEAZZA, started Feb 2008.
6 // Copyright (C) 2008 Vivian Meazza
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.
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.
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.
28 #include "rad_alt.hxx"
30 #include <simgear/scene/material/mat.hxx>
33 #include <Main/fg_props.hxx>
34 #include <Main/globals.hxx>
35 #include <Scenery/scenery.hxx>
38 RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
40 _interval(node->getDoubleValue("update-interval-sec", 1.0))
42 _name = node->getStringValue("name", "radar-altimeter");
43 _num = node->getIntValue("number", 0);
46 RadarAltimeter::~RadarAltimeter()
51 RadarAltimeter::init ()
54 std::string branch = "/instrumentation/" + _name;
55 _Instrument = fgGetNode(branch.c_str(), _num, true);
57 _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
58 _serviceable_node = _Instrument->getNode("serviceable", true);
60 _user_alt_agl_node = fgGetNode("/position/altitude-agl-ft", true);
61 _rad_alt_warning_node = fgGetNode("/sim/alarms/rad-alt-warning", true);
63 _Instrument->setFloatValue("tilt",-85);
64 _Instrument->setStringValue("status","RA");
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);
76 _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
77 _Instrument->getDoubleValue("antenna/y-offset-m"),
78 _Instrument->getDoubleValue("antenna/z-offset-m"));
82 RadarAltimeter::update (double delta_time_sec)
84 if (!_sceneryLoaded->getBoolValue())
87 if ( ! _serviceable_node->getBoolValue() ) {
88 _Instrument->setStringValue("status","");
92 _time += delta_time_sec;
93 if (_time < _interval)
103 RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
105 return norm(nearestHit - getCartAntennaPos());
109 RadarAltimeter::updateSetHeight()
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);
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);
124 _rad_alt_warning_node->setIntValue(0);
128 RadarAltimeter::update_altitude()
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);
138 _min_radalt = max_range;
139 bool haveHit = false;
140 SGVec3d cartantennapos = getCartAntennaPos();
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);
147 globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
148 double measuredDistance = dist(cartantennapos, nearestHit);
150 if (measuredDistance >= min_range && measuredDistance <= max_range) {
151 if (measuredDistance < _min_radalt) {
152 _min_radalt = measuredDistance;
155 } // of hit within permissible range
156 } // of elevation step
159 _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
161 _rad_alt_warning_node->setIntValue(9999);
166 RadarAltimeter::getCartAntennaPos() const
168 double yaw, pitch, roll;
169 globals->get_aircraft_orientation(yaw, pitch, roll);
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
176 // Transform the user position to the horizontal local coordinate system.
177 SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
179 // and postrotate the orientation of the user model wrt the horizontal
181 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
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);
187 // Add the position offset of the user model to get the geocentered position
188 return globals->get_aircraft_position_cart() + ecfOffset;
191 SGVec3d RadarAltimeter::rayVector(double az, double el) const
193 double yaw, pitch, roll;
194 globals->get_aircraft_orientation(yaw, pitch, roll);
196 double tilt = _Instrument->getDoubleValue("tilt");
197 bool roll_stab = false,
200 SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);
202 // Transform the antenna position to the horizontal local coordinate system.
203 SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
205 // and postrotate the orientation of the radar wrt the horizontal
207 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
208 pitch_stab ? 0 :pitch,
209 roll_stab ? 0 : roll);
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);