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 "rad_alt.hxx"
  29
  30 #include <simgear/scene/material/mat.hxx>
  31
  32
  33 #include <Main/fg_props.hxx>
  34 #include <Main/globals.hxx>
  35 #include <Scenery/scenery.hxx>
  36
  37
  38 RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
  39     _time(0.0),
  40     _interval(node->getDoubleValue("update-interval-sec", 1.0))
  41 {
  42     _name = node->getStringValue("name", "radar-altimeter");
  43     _num = node->getIntValue("number", 0);
  44 }
  45
  46 RadarAltimeter::~RadarAltimeter()
  47 {
  48 }
  49
  50 void
  51 RadarAltimeter::init ()
  52 {
  53
  54     std::string branch = "/instrumentation/" + _name;
  55     _Instrument = fgGetNode(branch.c_str(), _num, true);
  56
  57     _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
  58     _serviceable_node = _Instrument->getNode("serviceable", true);
  59
  60     _user_alt_agl_node     = fgGetNode("/position/altitude-agl-ft", true);
  61     _rad_alt_warning_node   = fgGetNode("/sim/alarms/rad-alt-warning", true);
  62
  63     _Instrument->setFloatValue("tilt",-85);
  64     _Instrument->setStringValue("status","RA");
  65
  66     _Instrument->getDoubleValue("elev-limit", true);
  67     _Instrument->getDoubleValue("elev-step-deg", true);
  68     _Instrument->getDoubleValue("az-limit-deg", true);
  69     _Instrument->getDoubleValue("az-step-deg", true);
  70     _Instrument->getDoubleValue("max-range-m", true);
  71     _Instrument->getDoubleValue("min-range-m", true);
  72     _Instrument->getDoubleValue("tilt", true);
  73     _Instrument->getDoubleValue("set-height-ft", true);
  74     _Instrument->getDoubleValue("set-excursion-percent", true);
  75
  76     _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
  77                              _Instrument->getDoubleValue("antenna/y-offset-m"),
  78                              _Instrument->getDoubleValue("antenna/z-offset-m"));
  79 }
  80
  81 void
  82 RadarAltimeter::update (double delta_time_sec)
  83 {
  84     if (!_sceneryLoaded->getBoolValue())
  85         return;
  86
  87     if ( ! _serviceable_node->getBoolValue() ) {
  88         _Instrument->setStringValue("status","");
  89         return;
  90     }
  91
  92     _time += delta_time_sec;
  93     if (_time < _interval)
  94         return;
  95
  96     _time -= _interval;
  97
  98     update_altitude();
  99     updateSetHeight();
 100 }
 101
 102 double
 103 RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
 104 {
 105     return norm(nearestHit - getCartAntennaPos());
 106 }
 107
 108 void
 109 RadarAltimeter::updateSetHeight()
 110 {
 111     double set_ht_ft   = _Instrument->getDoubleValue("set-height-ft", 9999);
 112     double set_excur   = _Instrument->getDoubleValue("set-excursion-percent", 0);
 113     if (set_ht_ft == 9999) {
 114         _rad_alt_warning_node->setIntValue(9999);
 115         return;
 116     }
 117
 118     double radarAltFt = _min_radalt * SG_METER_TO_FEET;
 119     if (radarAltFt < set_ht_ft * (100 - set_excur)/100)
 120         _rad_alt_warning_node->setIntValue(-1);
 121     else if (radarAltFt > set_ht_ft * (100 + set_excur)/100)
 122         _rad_alt_warning_node->setIntValue(1);
 123     else
 124         _rad_alt_warning_node->setIntValue(0);
 125 }
 126
 127 void
 128 RadarAltimeter::update_altitude()
 129 {
 130     double el_limit    = _Instrument->getDoubleValue("elev-limit", 15);
 131     double el_step     = _Instrument->getDoubleValue("elev-step-deg", 15);
 132     double az_limit    = _Instrument->getDoubleValue("az-limit-deg", 15);
 133     double az_step     = _Instrument->getDoubleValue("az-step-deg", 15);
 134     double max_range   = _Instrument->getDoubleValue("max-range-m", 1500);
 135     double min_range   = _Instrument->getDoubleValue("min-range-m", 0.001);
 136
 137
 138     _min_radalt = max_range;
 139     bool haveHit = false;
 140     SGVec3d cartantennapos = getCartAntennaPos();
 141
 142     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
 143         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
 144             SGVec3d userVec = rayVector(brg, elev);
 145
 146             SGVec3d nearestHit;
 147             globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
 148             double measuredDistance = dist(cartantennapos, nearestHit);
 149
 150             if (measuredDistance >= min_range && measuredDistance <= max_range) {
 151                 if (measuredDistance < _min_radalt) {
 152                     _min_radalt = measuredDistance;
 153                     haveHit = true;
 154                 }
 155             } // of hit within permissible range
 156         } // of elevation step
 157     } // of azimuth step
 158
 159     _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
 160     if (!haveHit) {
 161         _rad_alt_warning_node->setIntValue(9999);
 162     }
 163 }
 164
 165 SGVec3d
 166 RadarAltimeter::getCartAntennaPos() const
 167 {
 168     double yaw, pitch, roll;
 169     globals->get_aircraft_orientation(yaw, pitch, roll);
 170
 171     // Transform to the right coordinate frame, configuration is done in
 172     // the x-forward, y-right, z-up coordinates (feet), computation
 173     // in the simulation usual body x-forward, y-right, z-down coordinates
 174     // (meters) )
 175
 176     // Transform the user position to the horizontal local coordinate system.
 177     SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
 178
 179     // and postrotate the orientation of the user model wrt the horizontal
 180     // local frame
 181     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
 182
 183     // The offset converted to the usual body fixed coordinate system
 184     // rotated to the earth-fixed coordinates axis
 185     SGVec3d ecfOffset = hlTrans.backTransform(_antennaOffset);
 186
 187     // Add the position offset of the user model to get the geocentered position
 188     return globals->get_aircraft_position_cart() + ecfOffset;
 189 }
 190
 191 SGVec3d RadarAltimeter::rayVector(double az, double el) const
 192 {
 193     double yaw, pitch, roll;
 194     globals->get_aircraft_orientation(yaw, pitch, roll);
 195
 196     double tilt = _Instrument->getDoubleValue("tilt");
 197     bool roll_stab = false,
 198         pitch_stab = false;
 199
 200     SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);
 201
 202     // Transform the antenna position to the horizontal local coordinate system.
 203     SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
 204
 205     // and postrotate the orientation of the radar wrt the horizontal
 206     // local frame
 207     hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
 208                                             pitch_stab ? 0 :pitch,
 209                                             roll_stab ? 0 : roll);
 210     hlTrans *= offset;
 211
 212     // now rotate the rotation vector back into the
 213     // earth centered frames coordinates
 214     SGVec3d angleaxis(1,0,0);
 215     return hlTrans.backTransform(angleaxis);
 216
 217 }
 218