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 ( ! _sim_init_done ) {
 106
 107         if ( ! fgGetBool("sim/sceneryloaded", false) )
 108             return;
 109
 110         _sim_init_done = true;
 111     }
 112
 113     if ( !_odg || ! _serviceable_node->getBoolValue() ) {
 114         _Instrument->setStringValue("status","");
 115         return;
 116     }
 117
 118     _time += delta_time_sec;
 119
 120     if (_time < _interval)
 121         return;
 122
 123     _time = 0.0;
 124
 125
 126     update_terrain();
 127 //    wxRadarBg::update(delta_time_sec);
 128 }
 129
 130 void
 131 agRadar::setUserPos()
 132 {
 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);
 136 }
 137
 138 SGVec3d
 139 agRadar::getCartUserPos() const {
 140     SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
 141     return cartUserPos;
 142 }
 143
 144 SGVec3d
 145 agRadar::getCartAntennaPos() const {
 146
 147     float yaw   = _user_hdg_deg_node->getDoubleValue();
 148     float pitch = _user_pitch_deg_node->getDoubleValue();
 149     float roll  = _user_roll_deg_node->getDoubleValue();
 150
 151     double x_offset_m =_xOffsetMNode->getDoubleValue();
 152     double y_offset_m =_yOffsetMNode->getDoubleValue();
 153     double z_offset_m =_zOffsetMNode->getDoubleValue();
 154
 155     // convert geodetic positions to geocentered
 156     SGVec3d cartuserPos = getCartUserPos();
 157
 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
 161     // (meters) )
 162     SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
 163
 164     // Transform the user position to the horizontal local coordinate system.
 165     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
 166
 167     // and postrotate the orientation of the user model wrt the horizontal
 168     // local frame
 169     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
 170
 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);
 174
 175     // Add the position offset of the user model to get the geocentered position
 176     SGVec3d offsetPos = cartuserPos + off;
 177
 178     return offsetPos;
 179 }
 180
 181 void
 182 agRadar::setAntennaPos() {
 183     SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
 184 }
 185
 186 void
 187 agRadar::setUserVec(double az, double el)
 188 {
 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     int mode    = _radar_mode_control_node->getIntValue();
 193     double tilt = _Instrument->getDoubleValue("tilt");
 194     double trk  = _Instrument->getDoubleValue("trk");
 195     bool roll_stab   = _Instrument->getBoolValue("stabilisation/roll");
 196     bool pitch_stab  = _Instrument->getBoolValue("stabilisation/pitch");
 197
 198     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
 199
 200     // Transform the antenna position to the horizontal local coordinate system.
 201     SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
 202
 203     // and postrotate the orientation of the radar wrt the horizontal
 204     // local frame
 205     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 206                                             pitch_stab ? 0 :pitch,
 207                                             roll_stab ? 0 : roll);
 208     hlTrans *= offset;
 209
 210     // now rotate the rotation vector back into the
 211     // earth centered frames coordinates
 212     SGVec3d angleaxis(1,0,0);
 213     uservec = hlTrans.backTransform(angleaxis);
 214
 215 }
 216
 217 bool
 218 agRadar::getMaterial(){
 219
 220     if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
 221         //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 222         if (_material) {
 223             const vector<string>& names = _material->get_names();
 224
 225             _solid = _material->get_solid();
 226             _load_resistance = _material->get_load_resistance();
 227             _frictionFactor =_material->get_friction_factor();
 228             _bumpinessFactor = _material->get_bumpiness();
 229
 230             if (!names.empty())
 231                 _mat_name = names[0].c_str();
 232             else
 233                 _mat_name = "";
 234
 235         }
 236         /*cout << "material " << mat_name
 237         << " solid " << _solid
 238         << " load " << _load_resistance
 239         << " frictionFactor " << frictionFactor
 240         << " _bumpinessFactor " << _bumpinessFactor
 241         << endl;*/
 242         return true;
 243     } else {
 244         return false;
 245     }
 246
 247 }
 248
 249 void
 250 agRadar::update_terrain()
 251 {
 252     int mode = _radar_mode_control_node->getIntValue();
 253
 254     double el_limit = 1;
 255     double el_step = 1;
 256     double az_limit = 50;
 257     double az_step = 10;
 258     double max_range = 40000;
 259     double min_range = 250;
 260     double tilt = -2.5;
 261     bool roll_stab   = _rollStabNode->getBoolValue();
 262     bool pitch_stab  = _pitchStabNode->getBoolValue();
 263     //string status = "";
 264     const char* status;
 265     bool hdg_mkr = true;
 266
 267     if (mode == 5){
 268         status = "TW";
 269         hdg_mkr = false;
 270         tilt        = _tiltNode->getDoubleValue();
 271         el_limit    = _elevLimitDegNode->getDoubleValue();
 272         el_step     = _elevStepDegNode->getDoubleValue();
 273         az_limit    = _azLimitDegNode->getDoubleValue();
 274         az_step     = _azStepDegNode->getDoubleValue();
 275         max_range   = _maxRangeMNode->getDoubleValue();
 276         min_range   = _minRangeMNode->getDoubleValue();
 277     }
 278
 279     _Instrument->setDoubleValue("tilt", tilt);
 280     _Instrument->setBoolValue("stabilisation/roll", roll_stab);
 281     _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
 282     _Instrument->setStringValue("status", status);
 283     _Instrument->setDoubleValue("limit-deg", az_limit);
 284     _Instrument->setBoolValue("heading-marker", hdg_mkr);
 285     setUserPos();
 286     setAntennaPos();
 287     SGVec3d cartantennapos = getCartAntennaPos();
 288
 289     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
 290         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
 291             setUserVec(brg, elev);
 292             SGVec3d nearestHit;
 293             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
 294             SGGeodesy::SGCartToGeod(nearestHit, hitpos);
 295
 296             double course1, course2, distance;
 297
 298             SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
 299
 300             if (distance >= min_range && distance <= max_range) {
 301                 _terrain_warning_node->setBoolValue(true);
 302                 getMaterial();
 303                 _brgDegNode->setDoubleValue(course2);
 304                 _rangeMNode->setDoubleValue(distance);
 305                 _materialNode->setStringValue(_mat_name.c_str());
 306                 _bumpinessNode->setDoubleValue(_bumpinessFactor);
 307                 _elevationMNode->setDoubleValue(_elevation_m);
 308             } else {
 309                 _terrain_warning_node->setBoolValue(false);
 310                 _brgDegNode->setDoubleValue(0);
 311                 _rangeMNode->setDoubleValue(0);
 312                 _materialNode->setStringValue("");
 313                 _bumpinessNode->setDoubleValue(0);
 314                 _elevationMNode->setDoubleValue(0);
 315             }
 316
 317             //cout  << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
 318             //    << " ant brg " << course2
 319             //    << " elev " << _Instrument->getDoubleValue("tilt")
 320             //    << " gnd rng nm " << distance * SG_METER_TO_NM
 321             //    << " ht " << hitpos.getElevationFt()
 322             //    << " mat " << _mat_name
 323             //    << " solid " << _solid
 324             //    << " bumpiness " << _bumpinessFactor
 325             //    << endl;
 326
 327         }
 328     }
 329 }
 330
 331