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 ( ! _sim_init_done ) {
107 if ( ! fgGetBool("sim/sceneryloaded", false) )
110 _sim_init_done = true;
113 if ( !_odg || ! _serviceable_node->getBoolValue() ) {
114 _Instrument->setStringValue("status","");
118 _time += delta_time_sec;
120 if (_time < _interval)
127 // wxRadarBg::update(delta_time_sec);
131 agRadar::setUserPos()
133 userpos.setLatitudeDeg(_user_lat_node->getDoubleValue());
134 userpos.setLongitudeDeg(_user_lon_node->getDoubleValue());
135 userpos.setElevationM(_user_alt_node->getDoubleValue() * SG_FEET_TO_METER);
139 agRadar::getCartUserPos() const {
140 SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
145 agRadar::getCartAntennaPos() const {
147 float yaw = _user_hdg_deg_node->getDoubleValue();
148 float pitch = _user_pitch_deg_node->getDoubleValue();
149 float roll = _user_roll_deg_node->getDoubleValue();
151 double x_offset_m =_xOffsetMNode->getDoubleValue();
152 double y_offset_m =_yOffsetMNode->getDoubleValue();
153 double z_offset_m =_zOffsetMNode->getDoubleValue();
155 // convert geodetic positions to geocentered
156 SGVec3d cartuserPos = getCartUserPos();
158 // Transform to the right coordinate frame, configuration is done in
159 // the x-forward, y-right, z-up coordinates (feet), computation
160 // in the simulation usual body x-forward, y-right, z-down coordinates
162 SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
164 // Transform the user position to the horizontal local coordinate system.
165 SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
167 // and postrotate the orientation of the user model wrt the horizontal
169 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
171 // The offset converted to the usual body fixed coordinate system
172 // rotated to the earth-fixed coordinates axis
173 SGVec3d off = hlTrans.backTransform(_off);
175 // Add the position offset of the user model to get the geocentered position
176 SGVec3d offsetPos = cartuserPos + off;
182 agRadar::setAntennaPos() {
183 SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
187 agRadar::setUserVec(double az, double el)
189 float yaw = _user_hdg_deg_node->getDoubleValue();
190 float pitch = _user_pitch_deg_node->getDoubleValue();
191 float roll = _user_roll_deg_node->getDoubleValue();
192 double tilt = _Instrument->getDoubleValue("tilt");
193 double trk = _Instrument->getDoubleValue("trk");
194 bool roll_stab = _Instrument->getBoolValue("stabilisation/roll");
195 bool pitch_stab = _Instrument->getBoolValue("stabilisation/pitch");
197 SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
199 // Transform the antenna position to the horizontal local coordinate system.
200 SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
202 // and postrotate the orientation of the radar wrt the horizontal
204 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
205 pitch_stab ? 0 :pitch,
206 roll_stab ? 0 : roll);
209 // now rotate the rotation vector back into the
210 // earth centered frames coordinates
211 SGVec3d angleaxis(1,0,0);
212 uservec = hlTrans.backTransform(angleaxis);
217 agRadar::getMaterial(){
219 if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
220 //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
222 const vector<string>& names = _material->get_names();
224 _solid = _material->get_solid();
225 _load_resistance = _material->get_load_resistance();
226 _frictionFactor =_material->get_friction_factor();
227 _bumpinessFactor = _material->get_bumpiness();
230 _mat_name = names[0].c_str();
235 /*cout << "material " << mat_name
236 << " solid " << _solid
237 << " load " << _load_resistance
238 << " frictionFactor " << frictionFactor
239 << " _bumpinessFactor " << _bumpinessFactor
249 agRadar::update_terrain()
251 int mode = _radar_mode_control_node->getIntValue();
255 double az_limit = 50;
257 double max_range = 40000;
258 double min_range = 250;
260 bool roll_stab = _rollStabNode->getBoolValue();
261 bool pitch_stab = _pitchStabNode->getBoolValue();
262 //string status = "";
269 tilt = _tiltNode->getDoubleValue();
270 el_limit = _elevLimitDegNode->getDoubleValue();
271 el_step = _elevStepDegNode->getDoubleValue();
272 az_limit = _azLimitDegNode->getDoubleValue();
273 az_step = _azStepDegNode->getDoubleValue();
274 max_range = _maxRangeMNode->getDoubleValue();
275 min_range = _minRangeMNode->getDoubleValue();
278 _Instrument->setDoubleValue("tilt", tilt);
279 _Instrument->setBoolValue("stabilisation/roll", roll_stab);
280 _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
281 _Instrument->setStringValue("status", status);
282 _Instrument->setDoubleValue("limit-deg", az_limit);
283 _Instrument->setBoolValue("heading-marker", hdg_mkr);
286 SGVec3d cartantennapos = getCartAntennaPos();
288 for(double brg = -az_limit; brg <= az_limit; brg += az_step){
289 for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
290 setUserVec(brg, elev);
292 globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
293 SGGeodesy::SGCartToGeod(nearestHit, hitpos);
295 double course1, course2, distance;
297 SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
299 if (distance >= min_range && distance <= max_range) {
300 _terrain_warning_node->setBoolValue(true);
302 _brgDegNode->setDoubleValue(course2);
303 _rangeMNode->setDoubleValue(distance);
304 _materialNode->setStringValue(_mat_name.c_str());
305 _bumpinessNode->setDoubleValue(_bumpinessFactor);
306 _elevationMNode->setDoubleValue(_elevation_m);
308 _terrain_warning_node->setBoolValue(false);
309 _brgDegNode->setDoubleValue(0);
310 _rangeMNode->setDoubleValue(0);
311 _materialNode->setStringValue("");
312 _bumpinessNode->setDoubleValue(0);
313 _elevationMNode->setDoubleValue(0);
316 //cout << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
317 // << " ant brg " << course2
318 // << " elev " << _Instrument->getDoubleValue("tilt")
319 // << " gnd rng nm " << distance * SG_METER_TO_NM
320 // << " ht " << hitpos.getElevationFt()
321 // << " mat " << _mat_name
322 // << " solid " << _solid
323 // << " bumpiness " << _bumpinessFactor