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 "agradar.hxx"
31 agRadar::agRadar(SGPropertyNode *node) : wxRadarBg(node)
34 _name = node->getStringValue("name", "air-ground-radar");
35 _num = node->getIntValue("number", 0);
46 _user_hdg_deg_node = fgGetNode("/orientation/heading-deg", true);
47 _user_pitch_deg_node = fgGetNode("/orientation/pitch-deg", true);
48 _user_roll_deg_node = fgGetNode("/orientation/roll-deg", true);
50 _terrain_warning_node = fgGetNode("/sim/alarms/terrain-warning", true);
51 _terrain_warning_node->setBoolValue(false);
55 // those properties are used by a radar instrument of a MFD
56 // input switch = OFF | TST | STBY | ON
57 // input mode = WX | WXA | MAP | TW
58 // output status = STBY | TEST | WX | WXA | MAP | blank
59 // input lightning = true | false
60 // input TRK = +/- n degrees
61 // input TILT = +/- n degree
62 // input autotilt = true | false
63 // input range = n nm (20/40/80)
64 // input display-mode = arc | rose | map | plan
66 _Instrument->setFloatValue("trk", 0.0);
67 _Instrument->setFloatValue("tilt",-2.5);
68 _Instrument->setStringValue("status","");
69 _Instrument->setIntValue("mode-control", 5);
71 _Instrument->setBoolValue("stabilisation/roll", false);
72 _Instrument->setBoolValue("stabilisation/pitch", false);
74 _xOffsetMNode = getInstrumentNode("antenna/x-offset-m", 0.0);
75 _yOffsetMNode = getInstrumentNode("antenna/y-offset-m", 0.0);
76 _zOffsetMNode = getInstrumentNode("antenna/z-offset-m", 0.0);
78 _elevLimitDegNode = getInstrumentNode("terrain-warning/elev-limit-deg", 2.0);
79 _elevStepDegNode = getInstrumentNode("terrain-warning/elev-step-deg", 1.0);
80 _azLimitDegNode = getInstrumentNode("terrain-warning/az-limit-deg", 1.0);
81 _azStepDegNode = getInstrumentNode("terrain-warning/az-step-deg", 1.5);
82 _maxRangeMNode = getInstrumentNode("terrain-warning/max-range-m", 4000.0);
83 _minRangeMNode = getInstrumentNode("terrain-warning/min-range-m", 250.0);
84 _tiltNode = getInstrumentNode("terrain-warning/tilt", -2.0);
86 _brgDegNode = getInstrumentNode("terrain-warning/hit/brg-deg", 0.0);
87 _rangeMNode = getInstrumentNode("terrain-warning/hit/range-m", 0.0);
88 _elevationMNode = getInstrumentNode("terrain-warning/hit/elevation-m", 0.0);
89 _materialNode = getInstrumentNode("terrain-warning/hit/material", "");
90 _bumpinessNode = getInstrumentNode("terrain-warning/hit/bumpiness", 0.0);
92 _rollStabNode = getInstrumentNode("terrain-warning/stabilisation/roll",
94 _pitchStabNode = getInstrumentNode("terrain-warning/stabilisation/pitch",
96 // cout << "init done" << endl;
101 agRadar::update (double delta_time_sec)
103 if ( ! _sim_init_done ) {
105 if ( ! fgGetBool("sim/sceneryloaded", false) )
108 _sim_init_done = true;
111 if ( !_odg || ! _serviceable_node->getBoolValue() ) {
112 _Instrument->setStringValue("status","");
116 _time += delta_time_sec;
118 if (_time < _interval)
125 // wxRadarBg::update(delta_time_sec);
129 agRadar::setUserPos()
131 userpos.setLatitudeDeg(_user_lat_node->getDoubleValue());
132 userpos.setLongitudeDeg(_user_lon_node->getDoubleValue());
133 userpos.setElevationM(_user_alt_node->getDoubleValue() * SG_FEET_TO_METER);
137 agRadar::getCartUserPos() const {
138 SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
143 agRadar::getCartAntennaPos() const {
145 float yaw = _user_hdg_deg_node->getDoubleValue();
146 float pitch = _user_pitch_deg_node->getDoubleValue();
147 float roll = _user_roll_deg_node->getDoubleValue();
149 double x_offset_m =_xOffsetMNode->getDoubleValue();
150 double y_offset_m =_yOffsetMNode->getDoubleValue();
151 double z_offset_m =_zOffsetMNode->getDoubleValue();
153 // convert geodetic positions to geocentered
154 SGVec3d cartuserPos = getCartUserPos();
156 // Transform to the right coordinate frame, configuration is done in
157 // the x-forward, y-right, z-up coordinates (feet), computation
158 // in the simulation usual body x-forward, y-right, z-down coordinates
160 SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
162 // Transform the user position to the horizontal local coordinate system.
163 SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
165 // and postrotate the orientation of the user model wrt the horizontal
167 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
169 // The offset converted to the usual body fixed coordinate system
170 // rotated to the earth-fixed coordinates axis
171 SGVec3d off = hlTrans.backTransform(_off);
173 // Add the position offset of the user model to get the geocentered position
174 SGVec3d offsetPos = cartuserPos + off;
180 agRadar::setAntennaPos() {
181 SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
185 agRadar::setUserVec(double az, double el)
187 float yaw = _user_hdg_deg_node->getDoubleValue();
188 float pitch = _user_pitch_deg_node->getDoubleValue();
189 float roll = _user_roll_deg_node->getDoubleValue();
190 int mode = _radar_mode_control_node->getIntValue();
191 double tilt = _Instrument->getDoubleValue("tilt");
192 double trk = _Instrument->getDoubleValue("trk");
193 bool roll_stab = _Instrument->getBoolValue("stabilisation/roll");
194 bool pitch_stab = _Instrument->getBoolValue("stabilisation/pitch");
196 SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
198 // Transform the antenna position to the horizontal local coordinate system.
199 SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
201 // and postrotate the orientation of the radar wrt the horizontal
203 hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
204 pitch_stab ? 0 :pitch,
205 roll_stab ? 0 : roll);
208 // now rotate the rotation vector back into the
209 // earth centered frames coordinates
210 SGVec3d angleaxis(1,0,0);
211 uservec = hlTrans.backTransform(angleaxis);
216 agRadar::getMaterial(){
218 if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
219 //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
221 const vector<string>& names = _material->get_names();
223 _solid = _material->get_solid();
224 _load_resistance = _material->get_load_resistance();
225 _frictionFactor =_material->get_friction_factor();
226 _bumpinessFactor = _material->get_bumpiness();
229 _mat_name = names[0].c_str();
234 /*cout << "material " << mat_name
235 << " solid " << _solid
236 << " load " << _load_resistance
237 << " frictionFactor " << frictionFactor
238 << " _bumpinessFactor " << _bumpinessFactor
248 agRadar::update_terrain()
250 int mode = _radar_mode_control_node->getIntValue();
254 double az_limit = 50;
256 double max_range = 40000;
257 double min_range = 250;
259 bool roll_stab = _rollStabNode->getBoolValue();
260 bool pitch_stab = _pitchStabNode->getBoolValue();
261 //string status = "";
268 tilt = _tiltNode->getDoubleValue();
269 el_limit = _elevLimitDegNode->getDoubleValue();
270 el_step = _elevStepDegNode->getDoubleValue();
271 az_limit = _azLimitDegNode->getDoubleValue();
272 az_step = _azStepDegNode->getDoubleValue();
273 max_range = _maxRangeMNode->getDoubleValue();
274 min_range = _minRangeMNode->getDoubleValue();
277 _Instrument->setDoubleValue("tilt", tilt);
278 _Instrument->setBoolValue("stabilisation/roll", roll_stab);
279 _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
280 _Instrument->setStringValue("status", status);
281 _Instrument->setDoubleValue("limit-deg", az_limit);
282 _Instrument->setBoolValue("heading-marker", hdg_mkr);
285 SGVec3d cartantennapos = getCartAntennaPos();
287 for(double brg = -az_limit; brg <= az_limit; brg += az_step){
288 for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
289 setUserVec(brg, elev);
291 globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
292 SGGeodesy::SGCartToGeod(nearestHit, hitpos);
294 double course1, course2, distance;
296 SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
298 if (distance >= min_range && distance <= max_range) {
299 _terrain_warning_node->setBoolValue(true);
301 _brgDegNode->setDoubleValue(course2);
302 _rangeMNode->setDoubleValue(distance);
303 _materialNode->setStringValue(_mat_name.c_str());
304 _bumpinessNode->setDoubleValue(_bumpinessFactor);
305 _elevationMNode->setDoubleValue(_elevation_m);
307 _terrain_warning_node->setBoolValue(false);
308 _brgDegNode->setDoubleValue(0);
309 _rangeMNode->setDoubleValue(0);
310 _materialNode->setStringValue("");
311 _bumpinessNode->setDoubleValue(0);
312 _elevationMNode->setDoubleValue(0);
315 //cout << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
316 // << " ant brg " << course2
317 // << " elev " << _Instrument->getDoubleValue("tilt")
318 // << " gnd rng nm " << distance * SG_METER_TO_NM
319 // << " ht " << hitpos.getElevationFt()
320 // << " mat " << _mat_name
321 // << " solid " << _solid
322 // << " bumpiness " << _bumpinessFactor