X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Frad_alt.cxx;h=0ab143bbd847274afc92825b58be53b08f6d0c2b;hb=99dd8c08b5d9d2621d081a03037f6b2469674ad2;hp=4d06bb95fd740ace15d656c664ed8390baf1383e;hpb=c2040f30a26451a9bc445843094117f63f3998a1;p=flightgear.git

diff --git a/src/Instrumentation/rad_alt.cxx b/src/Instrumentation/rad_alt.cxx
index 4d06bb95f..0ab143bbd 100644
--- a/src/Instrumentation/rad_alt.cxx
+++ b/src/Instrumentation/rad_alt.cxx
@@ -27,43 +27,42 @@
 
 #include "rad_alt.hxx"
 
+#include <simgear/scene/material/mat.hxx>
+
+
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include <Scenery/scenery.hxx>
 
-radAlt::radAlt(SGPropertyNode *node) : agRadar(node) 
-{
 
+RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
+    _time(0.0),
+    _interval(node->getDoubleValue("update-interval-sec", 1.0))
+{
     _name = node->getStringValue("name", "radar-altimeter");
     _num = node->getIntValue("number", 0);
-
 }
 
-radAlt::~radAlt()
+RadarAltimeter::~RadarAltimeter()
 {
 }
 
 void
-radAlt::init ()
+RadarAltimeter::init ()
 {
-    agRadar::init();
+
+    std::string branch = "/instrumentation/" + _name;
+    _Instrument = fgGetNode(branch.c_str(), _num, true);
+
+    _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
+    _serviceable_node = _Instrument->getNode("serviceable", true);
 
     _user_alt_agl_node     = fgGetNode("/position/altitude-agl-ft", true);
     _rad_alt_warning_node   = fgGetNode("/sim/alarms/rad-alt-warning", true);
 
-    //// those properties are used by a radar instrument of a MFD
-    //// input switch = OFF | TST | STBY | ON
-    //// input mode = WX | WXA | MAP | TW
-    //// output status = STBY | TEST | WX | WXA | MAP | blank
-    //// input lightning = true | false
-    //// input TRK = +/- n degrees
-    //// input TILT = +/- n degree
-    //// input autotilt = true | false
-    //// input range = n nm (20/40/80)
-    //// input display-mode = arc | rose | map | plan
-
     _Instrument->setFloatValue("tilt",-85);
     _Instrument->setStringValue("status","RA");
 
-    //_Instrument->setIntValue("mode-control", 10);
-
     _Instrument->getDoubleValue("elev-limit", true);
     _Instrument->getDoubleValue("elev-step-deg", true);
     _Instrument->getDoubleValue("az-limit-deg", true);
@@ -74,145 +73,146 @@ radAlt::init ()
     _Instrument->getDoubleValue("set-height-ft", true);
     _Instrument->getDoubleValue("set-excursion-percent", true);
 
-    _Instrument->setDoubleValue("hit/brg-deg", 0);
-    _Instrument->setDoubleValue("hit/range-m", 0);
-    _Instrument->setStringValue("hit/material", "");
-    _Instrument->setDoubleValue("hit/bumpiness", 0);
-
-
-
-    _Instrument->removeChild("terrain-warning");
-    _Instrument->removeChild("mode-control");
-    _Instrument->removeChild("limit-deg");
-    _Instrument->removeChild("reference-range-nm");
-    _Instrument->removeChild("heading-marker");
-    _Instrument->removeChild("display-controls");
-    _Instrument->removeChild("font");
-
-    //cout << " radar alt init done" << endl;
+    _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
+                             _Instrument->getDoubleValue("antenna/y-offset-m"),
+                             _Instrument->getDoubleValue("antenna/z-offset-m"));
 }
 
 void
-radAlt::update (double delta_time_sec)
+RadarAltimeter::update (double delta_time_sec)
 {
-    if ( ! _sim_init_done ) {
-
-        if ( ! fgGetBool("sim/sceneryloaded", false) )
-            return;
-
-        _sim_init_done = true;
-    }
+    if (!_sceneryLoaded->getBoolValue())
+        return;
 
-    if ( !_odg || ! _serviceable_node->getBoolValue() ) {
+    if ( ! _serviceable_node->getBoolValue() ) {
         _Instrument->setStringValue("status","");
         return;
     }
 
     _time += delta_time_sec;
-
     if (_time < _interval)
         return;
 
-    _time = 0.0;
+    _time -= _interval;
+
     update_altitude();
+    updateSetHeight();
 }
 
-
-
 double
-radAlt::getDistanceAntennaToHit(SGVec3d nearestHit) const 
+RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
 {
-    //calculate the distance antenna to hit
-
-    SGVec3d cartantennapos = getCartAntennaPos();;
+    return norm(nearestHit - getCartAntennaPos());
+}
 
-    SGVec3d diff = nearestHit - cartantennapos;
-    double distance = norm(diff);
-    return distance ;
+void
+RadarAltimeter::updateSetHeight()
+{
+    double set_ht_ft   = _Instrument->getDoubleValue("set-height-ft", 9999);
+    double set_excur   = _Instrument->getDoubleValue("set-excursion-percent", 0);
+    if (set_ht_ft == 9999) {
+        _rad_alt_warning_node->setIntValue(9999);
+        return;
+    }
+    
+    double radarAltFt = _min_radalt * SG_METER_TO_FEET;
+    if (radarAltFt < set_ht_ft * (100 - set_excur)/100)
+        _rad_alt_warning_node->setIntValue(-1);
+    else if (radarAltFt > set_ht_ft * (100 + set_excur)/100)
+        _rad_alt_warning_node->setIntValue(1);
+    else
+        _rad_alt_warning_node->setIntValue(0);
 }
 
 void
-radAlt::update_altitude()
+RadarAltimeter::update_altitude()
 {
-    int mode           = _radar_mode_control_node->getIntValue();
-    double tilt        = _Instrument->getDoubleValue("tilt", -85);
     double el_limit    = _Instrument->getDoubleValue("elev-limit", 15);
     double el_step     = _Instrument->getDoubleValue("elev-step-deg", 15);
     double az_limit    = _Instrument->getDoubleValue("az-limit-deg", 15);
     double az_step     = _Instrument->getDoubleValue("az-step-deg", 15);
     double max_range   = _Instrument->getDoubleValue("max-range-m", 1500);
     double min_range   = _Instrument->getDoubleValue("min-range-m", 0.001);
-    double set_ht_ft   = _Instrument->getDoubleValue("set-height-ft", 9999);
-    double set_excur   = _Instrument->getDoubleValue("set-excursion-percent", 0);
+
 
     _min_radalt = max_range;
+    bool haveHit = false;
+    SGVec3d cartantennapos = getCartAntennaPos();
 
     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
-        setUserPos();
-        setAntennaPos();
-        SGVec3d cartantennapos = getCartAntennaPos();
-
         for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
-            setUserVec(brg, elev);
+            SGVec3d userVec = rayVector(brg, elev);
 
             SGVec3d nearestHit;
-            globals->get_scenery()->get_cart_ground_intersection(cartantennapos, uservec, nearestHit);
-            SGGeodesy::SGCartToGeod(nearestHit, hitpos);
-
-            double radalt = getDistanceAntennaToHit(nearestHit);
-            double course1, course2, distance;
-
-            SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
-            _Instrument->setDoubleValue("hit/altitude-agl-ft",
-                     _user_alt_agl_node->getDoubleValue());
-
-            if (radalt >= min_range && radalt <= max_range) {
-                getMaterial();
-                
-                if (radalt < _min_radalt)
-                    _min_radalt = radalt;
-
-                _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
-                _Instrument->setDoubleValue("hit/radar-altitude-ft", radalt * SG_METER_TO_FEET);
-                _Instrument->setDoubleValue("hit/brg-deg", course2);
-                _Instrument->setDoubleValue("hit/range-m", distance);
-                _Instrument->setStringValue("hit/material", _mat_name.c_str());
-                _Instrument->setDoubleValue("hit/bumpiness", _bumpinessFactor);
-
-                if (set_ht_ft!= 9999){
-
-                    if (_min_radalt * SG_METER_TO_FEET < set_ht_ft * (100 - set_excur)/100)
-                        _rad_alt_warning_node->setIntValue(-1);
-                    else if (_min_radalt * SG_METER_TO_FEET > set_ht_ft * (100 + set_excur)/100)
-                        _rad_alt_warning_node->setIntValue(1);
-                    else
-                        _rad_alt_warning_node->setIntValue(0);
-
-                } else
-                    _rad_alt_warning_node->setIntValue(9999);
-
-            } else {
-                _rad_alt_warning_node->setIntValue(9999);
-                _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
-                _Instrument->setDoubleValue("hit/radar-altitude-ft",0);
-                _Instrument->setDoubleValue("hit/brg-deg", 0);
-                _Instrument->setDoubleValue("hit/range-m", 0);
-                _Instrument->setStringValue("hit/material", "");
-                _Instrument->setDoubleValue("hit/bumpiness", 0);
-            }
-
-            //cout  << "usr hdg " << _user_hdg_deg_node->getDoubleValue()
-            //    << " ant brg " << course2 
-            //    << " elev " << _Instrument->getDoubleValue("tilt")
-            //    << " gnd rng nm " << distance * SG_METER_TO_NM
-            //    << " ht " << hitpos.getElevationFt()
-            //    << " mat " << _mat_name
-            //    << " solid " << _solid
-            //    << " bumpiness " << _bumpinessFactor
-            //    << endl;
-
-        }
+            globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
+            double measuredDistance = dist(cartantennapos, nearestHit);
+
+            if (measuredDistance >= min_range && measuredDistance <= max_range) {                
+                if (measuredDistance < _min_radalt) {
+                    _min_radalt = measuredDistance;
+                    haveHit = true;
+                }
+            } // of hit within permissible range
+        } // of elevation step
+    } // of azimuth step
+    
+    _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
+    if (!haveHit) {
+        _rad_alt_warning_node->setIntValue(9999);
     }
 }
 
+SGVec3d
+RadarAltimeter::getCartAntennaPos() const
+{
+    double yaw, pitch, roll;
+    globals->get_aircraft_orientation(yaw, pitch, roll);
+    
+    // Transform to the right coordinate frame, configuration is done in
+    // the x-forward, y-right, z-up coordinates (feet), computation
+    // in the simulation usual body x-forward, y-right, z-down coordinates
+    // (meters) )
+    
+    // Transform the user position to the horizontal local coordinate system.
+    SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
+    
+    // and postrotate the orientation of the user model wrt the horizontal
+    // local frame
+    hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);
+    
+    // The offset converted to the usual body fixed coordinate system
+    // rotated to the earth-fixed coordinates axis
+    SGVec3d ecfOffset = hlTrans.backTransform(_antennaOffset);
+    
+    // Add the position offset of the user model to get the geocentered position
+    return globals->get_aircraft_position_cart() + ecfOffset;
+}
+
+SGVec3d RadarAltimeter::rayVector(double az, double el) const
+{
+    double yaw, pitch, roll;
+    globals->get_aircraft_orientation(yaw, pitch, roll);
+    
+    double tilt = _Instrument->getDoubleValue("tilt");
+    bool roll_stab = false,
+        pitch_stab = false;
+    
+    SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);
+    
+    // Transform the antenna position to the horizontal local coordinate system.
+    SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());
+    
+    // and postrotate the orientation of the radar wrt the horizontal
+    // local frame
+    hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
+                                            pitch_stab ? 0 :pitch,
+                                            roll_stab ? 0 : roll);
+    hlTrans *= offset;
+    
+    // now rotate the rotation vector back into the
+    // earth centered frames coordinates
+    SGVec3d angleaxis(1,0,0);
+    return hlTrans.backTransform(angleaxis);
+    
+}