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     _Instrument->getNode("antenna/x-offset-m", true);
  75     _Instrument->getNode("antenna/y-offset-m", true);
  76     _Instrument->getNode("antenna/z-offset-m", true);
  77
  78     _Instrument->getNode("terrain-warning/elev-limit-deg", true);
  79     _Instrument->getNode("terrain-warning/elev-step-deg", true);
  80     _Instrument->getNode("terrain-warning/az-limit-deg", true);
  81     _Instrument->getNode("terrain-warning/az-step-deg", true);
  82     _Instrument->getNode("terrain-warning/max-range-m", true);
  83     _Instrument->getNode("terrain-warning/min-range-m", true);
  84     _Instrument->getNode("terrain-warning/tilt",true);
  85
  86     _Instrument->getNode("terrain-warning/hit/brg-deg", true);
  87     _Instrument->getNode("terrain-warning/hit/range-m", true);
  88     _Instrument->getNode("terrain-warning/hit/material", true);
  89     _Instrument->getNode("terrain-warning/hit/bumpiness", true);
  90
  91     _Instrument->getNode("terrain-warning/stabilisation/roll", true);
  92     _Instrument->getNode("terrain-warning/stabilisation/pitch", true);
  93 //    cout << "init done" << endl;
  94
  95 }
  96
  97 void
  98 agRadar::update (double delta_time_sec)
  99 {
 100     if ( ! _sim_init_done ) {
 101
 102         if ( ! fgGetBool("sim/sceneryloaded", false) )
 103             return;
 104
 105         _sim_init_done = true;
 106     }
 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
 121     update_terrain();
 122 //    wxRadarBg::update(delta_time_sec);
 123 }
 124
 125 void
 126 agRadar::setUserPos()
 127 {
 128     userpos.setLatitudeDeg(_user_lat_node->getDoubleValue());
 129     userpos.setLongitudeDeg(_user_lon_node->getDoubleValue());
 130     userpos.setElevationM(_user_alt_node->getDoubleValue() * SG_FEET_TO_METER);
 131 }
 132
 133 SGVec3d
 134 agRadar::getCartUserPos() const {
 135     SGVec3d cartUserPos = SGVec3d::fromGeod(userpos);
 136     return cartUserPos;
 137 }
 138
 139 SGVec3d
 140 agRadar::getCartAntennaPos() const {
 141
 142     float yaw   = _user_hdg_deg_node->getDoubleValue();
 143     float pitch = _user_pitch_deg_node->getDoubleValue();
 144     float roll  = _user_roll_deg_node->getDoubleValue();
 145
 146     double x_offset_m =_Instrument->getDoubleValue("antenna/x-offset-m", 0);
 147     double y_offset_m =_Instrument->getDoubleValue("antenna/y-offset-m", 0);
 148     double z_offset_m =_Instrument->getDoubleValue("antenna/y-offset-m", 0);
 149
 150     // convert geodetic positions to geocentered
 151     SGVec3d cartuserPos = getCartUserPos();
 152
 153     // Transform to the right coordinate frame, configuration is done in
 154     // the x-forward, y-right, z-up coordinates (feet), computation
 155     // in the simulation usual body x-forward, y-right, z-down coordinates
 156     // (meters) )
 157     SGVec3d _off(x_offset_m, y_offset_m, -z_offset_m);
 158
 159     // Transform the user position to the horizontal local coordinate system.
 160     SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
 161
 162     // and postrotate the orientation of the user model wrt the horizontal
 163     // local frame
 164     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
 165
 166     // The offset converted to the usual body fixed coordinate system
 167     // rotated to the earth-fixed coordinates axis
 168     SGVec3d off = hlTrans.backTransform(_off);
 169
 170     // Add the position offset of the user model to get the geocentered position
 171     SGVec3d offsetPos = cartuserPos + off;
 172
 173     return offsetPos;
 174 }
 175
 176 void
 177 agRadar::setAntennaPos() {
 178     SGGeodesy::SGCartToGeod(getCartAntennaPos(), antennapos);
 179 }
 180
 181 void
 182 agRadar::setUserVec(double az, double el)
 183 {
 184     float yaw   = _user_hdg_deg_node->getDoubleValue();
 185     float pitch = _user_pitch_deg_node->getDoubleValue();
 186     float roll  = _user_roll_deg_node->getDoubleValue();
 187     int mode    = _radar_mode_control_node->getIntValue();
 188     double tilt = _Instrument->getDoubleValue("tilt");
 189     double trk  = _Instrument->getDoubleValue("trk");
 190     bool roll_stab   = _Instrument->getBoolValue("stabilisation/roll");
 191     bool pitch_stab  = _Instrument->getBoolValue("stabilisation/pitch");
 192
 193     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az + trk, el + tilt, 0);
 194
 195     // Transform the antenna position to the horizontal local coordinate system.
 196     SGQuatd hlTrans = SGQuatd::fromLonLat(antennapos);
 197
 198     // and postrotate the orientation of the radar wrt the horizontal
 199     // local frame
 200     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 201                                             pitch_stab ? 0 :pitch,
 202                                             roll_stab ? 0 : roll);
 203     hlTrans *= offset;
 204
 205     // now rotate the rotation vector back into the
 206     // earth centered frames coordinates
 207     SGVec3d angleaxis(1,0,0);
 208     uservec = hlTrans.backTransform(angleaxis);
 209
 210 }
 211
 212 bool
 213 agRadar::getMaterial(){
 214
 215     if (globals->get_scenery()->get_elevation_m(hitpos, _elevation_m, &_material)){
 216         //_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
 217         if (_material) {
 218             const vector<string>& names = _material->get_names();
 219
 220             _solid = _material->get_solid();
 221             _load_resistance = _material->get_load_resistance();
 222             _frictionFactor =_material->get_friction_factor();
 223             _bumpinessFactor = _material->get_bumpiness();
 224
 225             if (!names.empty())
 226                 _mat_name = names[0].c_str();
 227             else
 228                 _mat_name = "";
 229
 230         }
 231         /*cout << "material " << mat_name
 232         << " solid " << _solid
 233         << " load " << _load_resistance
 234         << " frictionFactor " << frictionFactor
 235         << " _bumpinessFactor " << _bumpinessFactor
 236         << endl;*/
 237         return true;
 238     } else {
 239         return false;
 240     }
 241
 242 }
 243
 244 void
 245 agRadar::update_terrain()
 246 {
 247     int mode = _radar_mode_control_node->getIntValue();
 248
 249     double el_limit = 1;
 250     double el_step = 1;
 251     double az_limit = 50;
 252     double az_step = 10;
 253     double max_range = 40000;
 254     double min_range = 250;
 255     double tilt = -2.5;
 256     bool roll_stab   = _Instrument->getBoolValue("stabilisation/roll");
 257     bool pitch_stab  = _Instrument->getBoolValue("stabilisation/pitch");
 258     //string status = "";
 259     const char* status;
 260     bool hdg_mkr = true;
 261
 262     if (mode == 5){
 263         status = "TW";
 264         hdg_mkr = false;
 265         roll_stab   = _Instrument->getBoolValue("terrain-warning/stabilisation/roll", true);
 266         pitch_stab  = _Instrument->getBoolValue("terrain-warning/stabilisation/pitch", false);
 267         tilt        = _Instrument->getDoubleValue("terrain-warning/tilt", -2);
 268         el_limit    = _Instrument->getDoubleValue("terrain-warning/elev-limit-deg", 2);
 269         el_step     = _Instrument->getDoubleValue("terrain-warning/elev-step-deg", 1);
 270         az_limit    = _Instrument->getDoubleValue("terrain-warning/az-limit-deg", 1);
 271         az_step     = _Instrument->getDoubleValue("terrain-warning/az-step-deg", 1.5);
 272         max_range   = _Instrument->getDoubleValue("terrain-warning/max-range-m", 4000);
 273         min_range   = _Instrument->getDoubleValue("terrain-warning/min-range-m", 250);
 274     }
 275
 276     _Instrument->setDoubleValue("tilt", tilt);
 277     _Instrument->setBoolValue("stabilisation/roll", roll_stab);
 278     _Instrument->setBoolValue("stabilisation/pitch", pitch_stab);
 279     _Instrument->setStringValue("status", status);
 280     _Instrument->setDoubleValue("limit-deg", az_limit);
 281     _Instrument->setBoolValue("heading-marker", hdg_mkr);
 282     setUserPos();
 283     setAntennaPos();
 284     SGVec3d cartantennapos = getCartAntennaPos();
 285
 286     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
 287         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
 288             setUserVec(brg, elev);
 289             SGVec3d nearestHit;
 290             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
 291             SGGeodesy::SGCartToGeod(nearestHit, hitpos);
 292
 293             double course1, course2, distance;
 294
 295             SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
 296
 297             if (distance >= min_range && distance <= max_range) {
 298                 _terrain_warning_node->setBoolValue(true);
 299                 getMaterial();
 300                 _Instrument->setDoubleValue("terrain-warning/hit/brg-deg", course2);
 301                 _Instrument->setDoubleValue("terrain-warning/hit/range-m", distance);
 302                 _Instrument->setStringValue("terrain-warning/hit/material", _mat_name.c_str());
 303                 _Instrument->setDoubleValue("terrain-warning/hit/bumpiness", _bumpinessFactor);
 304                 _Instrument->setDoubleValue("terrain-warning/hit/elevation-m", _elevation_m);
 305             } else {
 306                 _terrain_warning_node->setBoolValue(false);
 307                 _Instrument->setDoubleValue("terrain-warning/hit/brg-deg", 0);
 308                 _Instrument->setDoubleValue("terrain-warning/hit/range-m", 0);
 309                 _Instrument->setStringValue("terrain-warning/hit/material", "");
 310                 _Instrument->setDoubleValue("terrain-warning/hit/bumpiness", 0);
 311                 _Instrument->setDoubleValue("terrain-warning/hit/elevation-m",0);
 312             }
 313
 314             //cout  << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
 315             //    << " ant brg " << course2
 316             //    << " elev " << _Instrument->getDoubleValue("tilt")
 317             //    << " gnd rng nm " << distance * SG_METER_TO_NM
 318             //    << " ht " << hitpos.getElevationFt()
 319             //    << " mat " << _mat_name
 320             //    << " solid " << _solid
 321             //    << " bumpiness " << _bumpinessFactor
 322             //    << endl;
 323
 324         }
 325     }
 326 }
 327
 328