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 <Main/fg_props.hxx>
29 #include <Main/globals.hxx>
30 #include "agradar.hxx"
33 agRadar::agRadar(SGPropertyNode *node) : wxRadarBg(node)
36 _name = node->getStringValue("name", "air-ground-radar");
37 _num = node->getIntValue("number", 0);
48 _user_hdg_deg_node = fgGetNode("/orientation/heading-deg", true);
49 _user_pitch_deg_node = fgGetNode("/orientation/pitch-deg", true);
50 _user_roll_deg_node = fgGetNode("/orientation/roll-deg", true);
52 _terrain_warning_node = fgGetNode("/sim/alarms/terrain-warning", true);
53 _terrain_warning_node->setBoolValue(false);
57 // those properties are used by a radar instrument of a MFD
58 // input switch = OFF | TST | STBY | ON
59 // input mode = WX | WXA | MAP | TW
60 // output status = STBY | TEST | WX | WXA | MAP | blank
61 // input lightning = true | false
62 // input TRK = +/- n degrees
63 // input TILT = +/- n degree
64 // input autotilt = true | false
65 // input range = n nm (20/40/80)
66 // input display-mode = arc | rose | map | plan
68 _Instrument->setFloatValue("trk", 0.0);
69 _Instrument->setFloatValue("tilt",-2.5);
70 _Instrument->setStringValue("status","");
71 _Instrument->setIntValue("mode-control", 5);
73 _Instrument->setBoolValue("stabilisation/roll", false);
74 _Instrument->setBoolValue("stabilisation/pitch", false);
76 _xOffsetMNode = getInstrumentNode("antenna/x-offset-m", 0.0);
77 _yOffsetMNode = getInstrumentNode("antenna/y-offset-m", 0.0);
78 _zOffsetMNode = getInstrumentNode("antenna/z-offset-m", 0.0);
80 _elevLimitDegNode = getInstrumentNode("terrain-warning/elev-limit-deg", 2.0);
81 _elevStepDegNode = getInstrumentNode("terrain-warning/elev-step-deg", 1.0);
82 _azLimitDegNode = getInstrumentNode("terrain-warning/az-limit-deg", 1.0);
83 _azStepDegNode = getInstrumentNode("terrain-warning/az-step-deg", 1.5);
84 _maxRangeMNode = getInstrumentNode("terrain-warning/max-range-m", 4000.0);
85 _minRangeMNode = getInstrumentNode("terrain-warning/min-range-m", 250.0);
86 _tiltNode = getInstrumentNode("terrain-warning/tilt", -2.0);
88 _brgDegNode = getInstrumentNode("terrain-warning/hit/brg-deg", 0.0);
89 _rangeMNode = getInstrumentNode("terrain-warning/hit/range-m", 0.0);
90 _elevationMNode = getInstrumentNode("terrain-warning/hit/elevation-m", 0.0);
91 _materialNode = getInstrumentNode("terrain-warning/hit/material", "");
92 _bumpinessNode = getInstrumentNode("terrain-warning/hit/bumpiness", 0.0);
94 _rollStabNode = getInstrumentNode("terrain-warning/stabilisation/roll",
96 _pitchStabNode = getInstrumentNode("terrain-warning/stabilisation/pitch",
98 // cout << "init done" << endl;
103 agRadar::update (double delta_time_sec)
105 if (!_sceneryLoaded->getBoolValue())
108 if ( !_odg || ! _serviceable_node->getBoolValue() ) {
109 _Instrument->setStringValue("status","");
113 _time += delta_time_sec;
115 if (_time < _interval)
121 // wxRadarBg::update(delta_time_sec);
125 agRadar::setUserPos()
127 userpos.setLatitudeDeg(_user_lat_node->getDoubleValue());
128 userpos.setLongitudeDeg(_user_lon_node->getDoubleValue());
129 userpos.setElevationM(_user_alt_node->getDoubleValue() * SG_FEET_TO_METER);
133 agRadar::getCartUserPos() const {
134 SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
139 agRadar::getCartAntennaPos() const {
141 float yaw = _user_hdg_deg_node->getDoubleValue();
142 float pitch = _user_pitch_deg_node->getDoubleValue();
143 float roll = _user_roll_deg_node->getDoubleValue();
145 double x_offset_m =_xOffsetMNode->getDoubleValue();
146 double y_offset_m =_yOffsetMNode->getDoubleValue();
147 double z_offset_m =_zOffsetMNode->getDoubleValue();
149 // convert geodetic positions to geocentered
150 SGVec3d cartuserPos = getCartUserPos();
152 // Transform to the right coordinate frame, configuration is done in
153 // the x-forward, y-right, z-up coordinates (feet), computation
154 // in the simulation usual body x-forward, y-right, z-down coordinates
156 SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
158 // Transform the user position to the horizontal local coordinate system.
159 SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
161 // and postrotate the orientation of the user model wrt the horizontal
163 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
165 // The offset converted to the usual body fixed coordinate system
166 // rotated to the earth-fixed coordinates axis
167 SGVec3d off = hlTrans.backTransform(_off);
169 // Add the position offset of the user model to get the geocentered position
170 SGVec3d offsetPos = cartuserPos + off;
176 agRadar::setAntennaPos() {
177 SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
181 agRadar::setUserVec(double az, double el)
183 float yaw = _user_hdg_deg_node->getDoubleValue();
184 float pitch = _user_pitch_deg_node->getDoubleValue();
185 float roll = _user_roll_deg_node->getDoubleValue();
186 double tilt = _Instrument->getDoubleValue("tilt");
187 double trk = _Instrument->getDoubleValue("trk");
188 bool roll_stab = _Instrument->getBoolValue("stabilisation/roll");
189 bool pitch_stab = _Instrument->getBoolValue("stabilisation/pitch");
191 SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
193 // Transform the antenna position to the horizontal local coordinate system.
194 SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
196 // and postrotate the orientation of the radar wrt the horizontal
198 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
199 pitch_stab ? 0 :pitch,
200 roll_stab ? 0 : roll);
203 // now rotate the rotation vector back into the
204 // earth centered frames coordinates
205 SGVec3d angleaxis(1,0,0);
206 uservec = hlTrans.backTransform(angleaxis);
211 agRadar::getMaterial(){
213 const simgear::BVHMaterial* mat = 0;
214 if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &mat)){
215 //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
216 const SGMaterial* material = dynamic_cast<const SGMaterial*>(mat);
218 const std::vector<std::string>& names = material->get_names();
220 _solid = material->get_solid();
221 _load_resistance = material->get_load_resistance();
222 _frictionFactor = material->get_friction_factor();
223 _bumpinessFactor = material->get_bumpiness();
226 _mat_name = names[0];
231 /*cout << "material " << mat_name
232 << " solid " << _solid
233 << " load " << _load_resistance
234 << " frictionFactor " << frictionFactor
235 << " _bumpinessFactor " << _bumpinessFactor
245 agRadar::update_terrain()
247 int mode = _radar_mode_control_node->getIntValue();
251 double az_limit = 50;
253 double max_range = 40000;
254 double min_range = 250;
256 bool roll_stab = _rollStabNode->getBoolValue();
257 bool pitch_stab = _pitchStabNode->getBoolValue();
258 const char* status = "";
264 tilt = _tiltNode->getDoubleValue();
265 el_limit = _elevLimitDegNode->getDoubleValue();
266 el_step = _elevStepDegNode->getDoubleValue();
267 az_limit = _azLimitDegNode->getDoubleValue();
268 az_step = _azStepDegNode->getDoubleValue();
269 max_range = _maxRangeMNode->getDoubleValue();
270 min_range = _minRangeMNode->getDoubleValue();
273 _Instrument->setDoubleValue("tilt", tilt);
274 _Instrument->setBoolValue("stabilisation/roll", roll_stab);
275 _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
276 _Instrument->setStringValue("status", status);
277 _Instrument->setDoubleValue("limit-deg", az_limit);
278 _Instrument->setBoolValue("heading-marker", hdg_mkr);
281 SGVec3d cartantennapos = getCartAntennaPos();
283 for(double brg = -az_limit; brg <= az_limit; brg += az_step){
284 for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
285 setUserVec(brg, elev);
287 globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
288 SGGeodesy::SGCartToGeod(nearestHit, hitpos);
290 double course1, course2, distance;
292 SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
294 if (distance >= min_range && distance <= max_range) {
295 _terrain_warning_node->setBoolValue(true);
297 _brgDegNode->setDoubleValue(course2);
298 _rangeMNode->setDoubleValue(distance);
299 _materialNode->setStringValue(_mat_name.c_str());
300 _bumpinessNode->setDoubleValue(_bumpinessFactor);
301 _elevationMNode->setDoubleValue(_elevation_m);
303 _terrain_warning_node->setBoolValue(false);
304 _brgDegNode->setDoubleValue(0);
305 _rangeMNode->setDoubleValue(0);
306 _materialNode->setStringValue("");
307 _bumpinessNode->setDoubleValue(0);
308 _elevationMNode->setDoubleValue(0);
311 //cout << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
312 // << " ant brg " << course2
313 // << " elev " << _Instrument->getDoubleValue("tilt")
314 // << " gnd rng nm " << distance * SG_METER_TO_NM
315 // << " ht " << hitpos.getElevationFt()
316 // << " mat " << _mat_name
317 // << " solid " << _solid
318 // << " bumpiness " << _bumpinessFactor