src/Instrumentation/agradar.cxx

   1 // Air Ground Radar
   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 "agradar.hxx"
  29
  30
  31 agRadar::agRadar(SGPropertyNode *node) : wxRadarBg(node)
  32 {
  33
  34     _name = node->getStringValue("name", "air-ground-radar");
  35     _num = node->getIntValue("number", 0);
  36
  37 }
  38
  39 agRadar::~agRadar ()
  40 {
  41 }
  42
  43 void
  44 agRadar::init ()
  45 {
  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);
  49
  50     _terrain_warning_node   = fgGetNode("/sim/alarms/terrain-warning", true);
  51     _terrain_warning_node->setBoolValue(false);
  52
  53     wxRadarBg::init();
  54
  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
  65
  66     _Instrument->setFloatValue("trk", 0.0);
  67     _Instrument->setFloatValue("tilt",-2.5);
  68     _Instrument->setStringValue("status","");
  69     _Instrument->setIntValue("mode-control", 5);
  70
  71     _Instrument->setBoolValue("stabilisation/roll", false);
  72     _Instrument->setBoolValue("stabilisation/pitch", false);
  73
  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);
  77
  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);
  85
  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);
  91
  92     _rollStabNode = getInstrumentNode("terrain-warning/stabilisation/roll",
  93                                       true);
  94     _pitchStabNode = getInstrumentNode("terrain-warning/stabilisation/pitch",
  95                                        false);
  96 //    cout << "init done" << endl;
  97
  98 }
  99
 100 void
 101 agRadar::update (double delta_time_sec)
 102 {
 103     if ( ! _sim_init_done ) {
 104
 105         if ( ! fgGetBool("sim/sceneryloaded", false) )
 106             return;
 107
 108         _sim_init_done = true;
 109     }
 110
 111     if ( !_odg || ! _serviceable_node->getBoolValue() ) {
 112         _Instrument->setStringValue("status","");
 113         return;
 114     }
 115
 116     _time += delta_time_sec;
 117
 118     if (_time < _interval)
 119         return;
 120
 121     _time = 0.0;
 122
 123
 124     update_terrain();
 125 //    wxRadarBg::update(delta_time_sec);
 126 }
 127
 128 void
 129 agRadar::setUserPos()
 130 {
 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);
 134 }
 135
 136 SGVec3d
 137 agRadar::getCartUserPos() const {
 138     SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
 139     return cartUserPos;
 140 }
 141
 142 SGVec3d
 143 agRadar::getCartAntennaPos() const {
 144
 145     float yaw   = _user_hdg_deg_node->getDoubleValue();
 146     float pitch = _user_pitch_deg_node->getDoubleValue();
 147     float roll  = _user_roll_deg_node->getDoubleValue();
 148
 149     double x_offset_m =_xOffsetMNode->getDoubleValue();
 150     double y_offset_m =_yOffsetMNode->getDoubleValue();
 151     double z_offset_m =_zOffsetMNode->getDoubleValue();
 152
 153     // convert geodetic positions to geocentered
 154     SGVec3d cartuserPos = getCartUserPos();
 155
 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
 159     // (meters) )
 160     SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
 161
 162     // Transform the user position to the horizontal local coordinate system.
 163     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
 164
 165     // and postrotate the orientation of the user model wrt the horizontal
 166     // local frame
 167     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
 168
 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);
 172
 173     // Add the position offset of the user model to get the geocentered position
 174     SGVec3d offsetPos = cartuserPos + off;
 175
 176     return offsetPos;
 177 }
 178
 179 void
 180 agRadar::setAntennaPos() {
 181     SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
 182 }
 183
 184 void
 185 agRadar::setUserVec(double az, double el)
 186 {
 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");
 195
 196     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
 197
 198     // Transform the antenna position to the horizontal local coordinate system.
 199     SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
 200
 201     // and postrotate the orientation of the radar wrt the horizontal
 202     // local frame
 203     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 204                                             pitch_stab ? 0 :pitch,
 205                                             roll_stab ? 0 : roll);
 206     hlTrans *= offset;
 207
 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);
 212
 213 }
 214
 215 bool
 216 agRadar::getMaterial(){
 217
 218     if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
 219         //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 220         if (_material) {
 221             const vector<string>& names = _material->get_names();
 222
 223             _solid = _material->get_solid();
 224             _load_resistance = _material->get_load_resistance();
 225             _frictionFactor =_material->get_friction_factor();
 226             _bumpinessFactor = _material->get_bumpiness();
 227
 228             if (!names.empty())
 229                 _mat_name = names[0].c_str();
 230             else
 231                 _mat_name = "";
 232
 233         }
 234         /*cout << "material " << mat_name
 235         << " solid " << _solid
 236         << " load " << _load_resistance
 237         << " frictionFactor " << frictionFactor
 238         << " _bumpinessFactor " << _bumpinessFactor
 239         << endl;*/
 240         return true;
 241     } else {
 242         return false;
 243     }
 244
 245 }
 246
 247 void
 248 agRadar::update_terrain()
 249 {
 250     int mode = _radar_mode_control_node->getIntValue();
 251
 252     double el_limit = 1;
 253     double el_step = 1;
 254     double az_limit = 50;
 255     double az_step = 10;
 256     double max_range = 40000;
 257     double min_range = 250;
 258     double tilt = -2.5;
 259     bool roll_stab   = _rollStabNode->getBoolValue();
 260     bool pitch_stab  = _pitchStabNode->getBoolValue();
 261     //string status = "";
 262     const char* status;
 263     bool hdg_mkr = true;
 264
 265     if (mode == 5){
 266         status = "TW";
 267         hdg_mkr = false;
 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();
 275     }
 276
 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);
 283     setUserPos();
 284     setAntennaPos();
 285     SGVec3d cartantennapos = getCartAntennaPos();
 286
 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);
 290             SGVec3d nearestHit;
 291             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
 292             SGGeodesy::SGCartToGeod(nearestHit, hitpos);
 293
 294             double course1, course2, distance;
 295
 296             SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
 297
 298             if (distance >= min_range && distance <= max_range) {
 299                 _terrain_warning_node->setBoolValue(true);
 300                 getMaterial();
 301                 _brgDegNode->setDoubleValue(course2);
 302                 _rangeMNode->setDoubleValue(distance);
 303                 _materialNode->setStringValue(_mat_name.c_str());
 304                 _bumpinessNode->setDoubleValue(_bumpinessFactor);
 305                 _elevationMNode->setDoubleValue(_elevation_m);
 306             } else {
 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);
 313             }
 314
 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
 323             //    << endl;
 324
 325         }
 326     }
 327 }
 328
 329