src/Instrumentation/rad_alt.cxx

   1 // Radar Altimeter
   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 "rad_alt.hxx"
  31
  32
  33 radAlt::radAlt(SGPropertyNode *node) : agRadar(node)
  34 {
  35
  36     _name = node->getStringValue("name", "radar-altimeter");
  37     _num = node->getIntValue("number", 0);
  38
  39 }
  40
  41 radAlt::~radAlt()
  42 {
  43 }
  44
  45 void
  46 radAlt::init ()
  47 {
  48     agRadar::init();
  49
  50     _user_alt_agl_node     = fgGetNode("/position/altitude-agl-ft", true);
  51     _rad_alt_warning_node   = fgGetNode("/sim/alarms/rad-alt-warning", true);
  52
  53     //// those properties are used by a radar instrument of a MFD
  54     //// input switch = OFF | TST | STBY | ON
  55     //// input mode = WX | WXA | MAP | TW
  56     //// output status = STBY | TEST | WX | WXA | MAP | blank
  57     //// input lightning = true | false
  58     //// input TRK = +/- n degrees
  59     //// input TILT = +/- n degree
  60     //// input autotilt = true | false
  61     //// input range = n nm (20/40/80)
  62     //// input display-mode = arc | rose | map | plan
  63
  64     _Instrument->setFloatValue("tilt",-85);
  65     _Instrument->setStringValue("status","RA");
  66
  67     //_Instrument->setIntValue("mode-control", 10);
  68
  69     _Instrument->getDoubleValue("elev-limit", true);
  70     _Instrument->getDoubleValue("elev-step-deg", true);
  71     _Instrument->getDoubleValue("az-limit-deg", true);
  72     _Instrument->getDoubleValue("az-step-deg", true);
  73     _Instrument->getDoubleValue("max-range-m", true);
  74     _Instrument->getDoubleValue("min-range-m", true);
  75     _Instrument->getDoubleValue("tilt", true);
  76     _Instrument->getDoubleValue("set-height-ft", true);
  77     _Instrument->getDoubleValue("set-excursion-percent", true);
  78
  79     _Instrument->setDoubleValue("hit/brg-deg", 0);
  80     _Instrument->setDoubleValue("hit/range-m", 0);
  81     _Instrument->setStringValue("hit/material", "");
  82     _Instrument->setDoubleValue("hit/bumpiness", 0);
  83
  84
  85
  86     _Instrument->removeChild("terrain-warning");
  87     _Instrument->removeChild("mode-control");
  88     _Instrument->removeChild("limit-deg");
  89     _Instrument->removeChild("reference-range-nm");
  90     _Instrument->removeChild("heading-marker");
  91     _Instrument->removeChild("display-controls");
  92     _Instrument->removeChild("font");
  93
  94     //cout << " radar alt init done" << endl;
  95 }
  96
  97 void
  98 radAlt::update (double delta_time_sec)
  99 {
 100     if (!_sceneryLoaded->getBoolValue())
 101         return;
 102
 103     if ( !_odg || ! _serviceable_node->getBoolValue() ) {
 104         _Instrument->setStringValue("status","");
 105         return;
 106     }
 107
 108     _time += delta_time_sec;
 109     if (_time < _interval)
 110         return;
 111
 112     _time = 0.0;
 113
 114     update_altitude();
 115 }
 116
 117 double
 118 radAlt::getDistanceAntennaToHit(SGVec3d nearestHit) const
 119 {
 120     //calculate the distance antenna to hit
 121
 122     SGVec3d cartantennapos = getCartAntennaPos();;
 123
 124     SGVec3d diff = nearestHit - cartantennapos;
 125     double distance = norm(diff);
 126     return distance ;
 127 }
 128
 129 void
 130 radAlt::update_altitude()
 131 {
 132 //    int mode           = _radar_mode_control_node->getIntValue();
 133 //    double tilt        = _Instrument->getDoubleValue("tilt", -85);
 134     double el_limit    = _Instrument->getDoubleValue("elev-limit", 15);
 135     double el_step     = _Instrument->getDoubleValue("elev-step-deg", 15);
 136     double az_limit    = _Instrument->getDoubleValue("az-limit-deg", 15);
 137     double az_step     = _Instrument->getDoubleValue("az-step-deg", 15);
 138     double max_range   = _Instrument->getDoubleValue("max-range-m", 1500);
 139     double min_range   = _Instrument->getDoubleValue("min-range-m", 0.001);
 140     double set_ht_ft   = _Instrument->getDoubleValue("set-height-ft", 9999);
 141     double set_excur   = _Instrument->getDoubleValue("set-excursion-percent", 0);
 142
 143     _min_radalt = max_range;
 144
 145     setUserPos();
 146     setAntennaPos();
 147     SGVec3d cartantennapos = getCartAntennaPos();
 148
 149     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
 150         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
 151             setUserVec(brg, elev);
 152
 153             SGVec3d nearestHit;
 154             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
 155             SGGeodesy::SGCartToGeod(nearestHit, hitpos);
 156
 157             double radalt = getDistanceAntennaToHit(nearestHit);
 158             double course1, course2, distance;
 159
 160             SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
 161             _Instrument->setDoubleValue("hit/altitude-agl-ft",
 162                      _user_alt_agl_node->getDoubleValue());
 163
 164             if (radalt >= min_range && radalt <= max_range) {
 165                 getMaterial();
 166
 167                 if (radalt < _min_radalt)
 168                     _min_radalt = radalt;
 169
 170                 _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
 171                 _Instrument->setDoubleValue("hit/radar-altitude-ft", radalt * SG_METER_TO_FEET);
 172                 _Instrument->setDoubleValue("hit/brg-deg", course2);
 173                 _Instrument->setDoubleValue("hit/range-m", distance);
 174                 _Instrument->setStringValue("hit/material", _mat_name.c_str());
 175                 _Instrument->setDoubleValue("hit/bumpiness", _bumpinessFactor);
 176
 177                 if (set_ht_ft!= 9999){
 178
 179                     if (_min_radalt * SG_METER_TO_FEET < set_ht_ft * (100 - set_excur)/100)
 180                         _rad_alt_warning_node->setIntValue(-1);
 181                     else if (_min_radalt * SG_METER_TO_FEET > set_ht_ft * (100 + set_excur)/100)
 182                         _rad_alt_warning_node->setIntValue(1);
 183                     else
 184                         _rad_alt_warning_node->setIntValue(0);
 185
 186                 } else
 187                     _rad_alt_warning_node->setIntValue(9999);
 188
 189             } else {
 190                 _rad_alt_warning_node->setIntValue(9999);
 191                 _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
 192                 _Instrument->setDoubleValue("hit/radar-altitude-ft",0);
 193                 _Instrument->setDoubleValue("hit/brg-deg", 0);
 194                 _Instrument->setDoubleValue("hit/range-m", 0);
 195                 _Instrument->setStringValue("hit/material", "");
 196                 _Instrument->setDoubleValue("hit/bumpiness", 0);
 197             }
 198
 199             //cout  << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
 200             //    << " ant brg " << course2
 201             //    << " elev " << _Instrument->getDoubleValue("tilt")
 202             //    << " gnd rng nm " << distance * SG_METER_TO_NM
 203             //    << " ht " << hitpos.getElevationFt()
 204             //    << " mat " << _mat_name
 205             //    << " solid " << _solid
 206             //    << " bumpiness " << _bumpinessFactor
 207             //    << endl;
 208
 209         }
 210     }
 211 }
 212
 213