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     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");
 196
 197     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
 198
 199     // Transform the antenna position to the horizontal local coordinate system.
 200     SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
 201
 202     // and postrotate the orientation of the radar wrt the horizontal
 203     // local frame
 204     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 205                                             pitch_stab ? 0 :pitch,
 206                                             roll_stab ? 0 : roll);
 207     hlTrans *= offset;
 208
 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);
 213
 214 }
 215
 216 bool
 217 agRadar::getMaterial(){
 218
 219     if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
 220         //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 221         if (_material) {
 222             const vector<string>& names = _material->get_names();
 223
 224             _solid = _material->get_solid();
 225             _load_resistance = _material->get_load_resistance();
 226             _frictionFactor =_material->get_friction_factor();
 227             _bumpinessFactor = _material->get_bumpiness();
 228
 229             if (!names.empty())
 230                 _mat_name = names[0].c_str();
 231             else
 232                 _mat_name = "";
 233
 234         }
 235         /*cout << "material " << mat_name
 236         << " solid " << _solid
 237         << " load " << _load_resistance
 238         << " frictionFactor " << frictionFactor
 239         << " _bumpinessFactor " << _bumpinessFactor
 240         << endl;*/
 241         return true;
 242     } else {
 243         return false;
 244     }
 245
 246 }
 247
 248 void
 249 agRadar::update_terrain()
 250 {
 251     int mode = _radar_mode_control_node->getIntValue();
 252
 253     double el_limit = 1;
 254     double el_step = 1;
 255     double az_limit = 50;
 256     double az_step = 10;
 257     double max_range = 40000;
 258     double min_range = 250;
 259     double tilt = -2.5;
 260     bool roll_stab   = _rollStabNode->getBoolValue();
 261     bool pitch_stab  = _pitchStabNode->getBoolValue();
 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