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 <Main/fg_props.hxx>
  29 #include <Main/globals.hxx>
  30 #include "agradar.hxx"
  31
  32
  33 agRadar::agRadar(SGPropertyNode *node) : wxRadarBg(node)
  34 {
  35
  36     _name = node->getStringValue("name", "air-ground-radar");
  37     _num = node->getIntValue("number", 0);
  38
  39 }
  40
  41 agRadar::~agRadar ()
  42 {
  43 }
  44
  45 void
  46 agRadar::init ()
  47 {
  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);
  51
  52     _terrain_warning_node   = fgGetNode("/sim/alarms/terrain-warning", true);
  53     _terrain_warning_node->setBoolValue(false);
  54
  55     wxRadarBg::init();
  56
  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
  67
  68     _Instrument->setFloatValue("trk", 0.0);
  69     _Instrument->setFloatValue("tilt",-2.5);
  70     _Instrument->setStringValue("status","");
  71     _Instrument->setIntValue("mode-control", 5);
  72
  73     _Instrument->setBoolValue("stabilisation/roll", false);
  74     _Instrument->setBoolValue("stabilisation/pitch", false);
  75
  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);
  79
  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);
  87
  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);
  93
  94     _rollStabNode = getInstrumentNode("terrain-warning/stabilisation/roll",
  95                                       true);
  96     _pitchStabNode = getInstrumentNode("terrain-warning/stabilisation/pitch",
  97                                        false);
  98 //    cout << "init done" << endl;
  99
 100 }
 101
 102 void
 103 agRadar::update (double delta_time_sec)
 104 {
 105     if (!_sceneryLoaded->getBoolValue())
 106         return;
 107
 108     if ( !_odg || ! _serviceable_node->getBoolValue() ) {
 109         _Instrument->setStringValue("status","");
 110         return;
 111     }
 112
 113     _time += delta_time_sec;
 114
 115     if (_time < _interval)
 116         return;
 117
 118     _time = 0.0;
 119
 120     update_terrain();
 121 //    wxRadarBg::update(delta_time_sec);
 122 }
 123
 124 void
 125 agRadar::setUserPos()
 126 {
 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);
 130 }
 131
 132 SGVec3d
 133 agRadar::getCartUserPos() const {
 134     SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
 135     return cartUserPos;
 136 }
 137
 138 SGVec3d
 139 agRadar::getCartAntennaPos() const {
 140
 141     float yaw   = _user_hdg_deg_node->getDoubleValue();
 142     float pitch = _user_pitch_deg_node->getDoubleValue();
 143     float roll  = _user_roll_deg_node->getDoubleValue();
 144
 145     double x_offset_m =_xOffsetMNode->getDoubleValue();
 146     double y_offset_m =_yOffsetMNode->getDoubleValue();
 147     double z_offset_m =_zOffsetMNode->getDoubleValue();
 148
 149     // convert geodetic positions to geocentered
 150     SGVec3d cartuserPos = getCartUserPos();
 151
 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
 155     // (meters) )
 156     SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
 157
 158     // Transform the user position to the horizontal local coordinate system.
 159     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
 160
 161     // and postrotate the orientation of the user model wrt the horizontal
 162     // local frame
 163     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
 164
 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);
 168
 169     // Add the position offset of the user model to get the geocentered position
 170     SGVec3d offsetPos = cartuserPos + off;
 171
 172     return offsetPos;
 173 }
 174
 175 void
 176 agRadar::setAntennaPos() {
 177     SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
 178 }
 179
 180 void
 181 agRadar::setUserVec(double az, double el)
 182 {
 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");
 190
 191     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
 192
 193     // Transform the antenna position to the horizontal local coordinate system.
 194     SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
 195
 196     // and postrotate the orientation of the radar wrt the horizontal
 197     // local frame
 198     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 199                                             pitch_stab ? 0 :pitch,
 200                                             roll_stab ? 0 : roll);
 201     hlTrans *= offset;
 202
 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);
 207
 208 }
 209
 210 bool
 211 agRadar::getMaterial(){
 212
 213     if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
 214         //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 215         if (_material) {
 216             const std::vector<std::string>& names = _material->get_names();
 217
 218             _solid = _material->get_solid();
 219             _load_resistance = _material->get_load_resistance();
 220             _frictionFactor =_material->get_friction_factor();
 221             _bumpinessFactor = _material->get_bumpiness();
 222
 223             if (!names.empty())
 224                 _mat_name = names[0].c_str();
 225             else
 226                 _mat_name = "";
 227
 228         }
 229         /*cout << "material " << mat_name
 230         << " solid " << _solid
 231         << " load " << _load_resistance
 232         << " frictionFactor " << frictionFactor
 233         << " _bumpinessFactor " << _bumpinessFactor
 234         << endl;*/
 235         return true;
 236     } else {
 237         return false;
 238     }
 239
 240 }
 241
 242 void
 243 agRadar::update_terrain()
 244 {
 245     int mode = _radar_mode_control_node->getIntValue();
 246
 247     double el_limit = 1;
 248     double el_step = 1;
 249     double az_limit = 50;
 250     double az_step = 10;
 251     double max_range = 40000;
 252     double min_range = 250;
 253     double tilt = -2.5;
 254     bool roll_stab   = _rollStabNode->getBoolValue();
 255     bool pitch_stab  = _pitchStabNode->getBoolValue();
 256     const char* status = "";
 257     bool hdg_mkr = true;
 258
 259     if (mode == 5){
 260         status = "TW";
 261         hdg_mkr = false;
 262         tilt        = _tiltNode->getDoubleValue();
 263         el_limit    = _elevLimitDegNode->getDoubleValue();
 264         el_step     = _elevStepDegNode->getDoubleValue();
 265         az_limit    = _azLimitDegNode->getDoubleValue();
 266         az_step     = _azStepDegNode->getDoubleValue();
 267         max_range   = _maxRangeMNode->getDoubleValue();
 268         min_range   = _minRangeMNode->getDoubleValue();
 269     }
 270
 271     _Instrument->setDoubleValue("tilt", tilt);
 272     _Instrument->setBoolValue("stabilisation/roll", roll_stab);
 273     _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
 274     _Instrument->setStringValue("status", status);
 275     _Instrument->setDoubleValue("limit-deg", az_limit);
 276     _Instrument->setBoolValue("heading-marker", hdg_mkr);
 277     setUserPos();
 278     setAntennaPos();
 279     SGVec3d cartantennapos = getCartAntennaPos();
 280
 281     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
 282         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
 283             setUserVec(brg, elev);
 284             SGVec3d nearestHit;
 285             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
 286             SGGeodesy::SGCartToGeod(nearestHit, hitpos);
 287
 288             double course1, course2, distance;
 289
 290             SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
 291
 292             if (distance >= min_range && distance <= max_range) {
 293                 _terrain_warning_node->setBoolValue(true);
 294                 getMaterial();
 295                 _brgDegNode->setDoubleValue(course2);
 296                 _rangeMNode->setDoubleValue(distance);
 297                 _materialNode->setStringValue(_mat_name.c_str());
 298                 _bumpinessNode->setDoubleValue(_bumpinessFactor);
 299                 _elevationMNode->setDoubleValue(_elevation_m);
 300             } else {
 301                 _terrain_warning_node->setBoolValue(false);
 302                 _brgDegNode->setDoubleValue(0);
 303                 _rangeMNode->setDoubleValue(0);
 304                 _materialNode->setStringValue("");
 305                 _bumpinessNode->setDoubleValue(0);
 306                 _elevationMNode->setDoubleValue(0);
 307             }
 308
 309             //cout  << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
 310             //    << " ant brg " << course2
 311             //    << " elev " << _Instrument->getDoubleValue("tilt")
 312             //    << " gnd rng nm " << distance * SG_METER_TO_NM
 313             //    << " ht " << hitpos.getElevationFt()
 314             //    << " mat " << _mat_name
 315             //    << " solid " << _solid
 316             //    << " bumpiness " << _bumpinessFactor
 317             //    << endl;
 318
 319         }
 320     }
 321 }
 322
 323