Make radar-altimeter indépendant of wxradar.

src/Instrumentation/instrument_mgr.cxx

@@ -206,7 +206,7 @@ bool FGInstrumentMgr::build (SGPropertyNode* config_props)
         // the instruments file
           continue;
         } else if ( name == "radar-altimeter" ) {
-            set_subsystem( id, new radAlt( node ) );
+            set_subsystem( id, new RadarAltimeter( node ) );
 
         } else if ( name == "tcas" ) {
             set_subsystem( id, new TCAS( node ), 0.2);

src/Instrumentation/rad_alt.cxx

@@ -25,47 +25,44 @@
 #  include "config.h"
 #endif
 
-#include <Main/fg_props.hxx>
-#include <Main/globals.hxx>
 #include "rad_alt.hxx"
 
+#include <simgear/scene/material/mat.hxx>
 
-radAlt::radAlt(SGPropertyNode *node) : agRadar(node) 
+
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include <Scenery/scenery.hxx>
+
+
+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();
+    _serviceable_node = _Instrument->getNode("serviceable", true);
+    _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
 
+    std::string branch = "/instrumentation/" + _name;
+    _Instrument = fgGetNode(branch.c_str(), _num, 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);
@@ -76,31 +73,18 @@ radAlt::init ()
     _Instrument->getDoubleValue("set-height-ft", true);
     _Instrument->getDoubleValue("set-excursion-percent", true);
 
-    _Instrument->setDoubleValue("hit/brg-deg", 0);
+    _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
-    _Instrument->setDoubleValue("hit/range-m", 0);
+                             _Instrument->getDoubleValue("antenna/y-offset-m"),
-    _Instrument->setStringValue("hit/material", "");
+                             _Instrument->getDoubleValue("antenna/z-offset-m"));
-    _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;
 }
 
 void
-radAlt::update (double delta_time_sec)
+RadarAltimeter::update (double delta_time_sec)
 {
     if (!_sceneryLoaded->getBoolValue())
         return;
 
-    if ( !_odg || ! _serviceable_node->getBoolValue() ) {
+    if ( ! _serviceable_node->getBoolValue() ) {
         _Instrument->setStringValue("status","");
         return;
     }
@@ -109,105 +93,126 @@ radAlt::update (double 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
+    return norm(nearestHit - getCartAntennaPos());
-
-    SGVec3d cartantennapos = getCartAntennaPos();;
-
-    SGVec3d diff = nearestHit - cartantennapos;
-    double distance = norm(diff);
-    return distance ;
 }
 
 void
-radAlt::update_altitude()
+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
+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;
-    setUserPos();
-    setAntennaPos();
     SGVec3d cartantennapos = getCartAntennaPos();
 
     for(double brg = -az_limit; brg <= az_limit; brg += az_step){
         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);
+            globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
-            SGGeodesy::SGCartToGeod(nearestHit, hitpos);
+            double measuredDistance = dist(cartantennapos, nearestHit);
 
-            double radalt = getDistanceAntennaToHit(nearestHit);
+            if (measuredDistance >= min_range && measuredDistance <= max_range) {                
-            double course1, course2, distance;
+                if (measuredDistance < _min_radalt) {
-
+                    _min_radalt = measuredDistance;
-            SGGeodesy::inverse(hitpos, antennapos, course1, course2, distance);
+                    haveHit = true;
-            _Instrument->setDoubleValue("hit/altitude-agl-ft",
+                }
-                     _user_alt_agl_node->getDoubleValue());
+            } // of hit within permissible range
-
+        } // of elevation step
-            if (radalt >= min_range && radalt <= max_range) {
+    } // of azimuth step
-                getMaterial();
+    
-                
+    _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
-                if (radalt < _min_radalt)
+    if (!haveHit) {
-                    _min_radalt = radalt;
+        _rad_alt_warning_node->setIntValue(9999);
-
-                _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;
-
-        }
     }
 }
 
+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);
+    
+}
 

src/Instrumentation/rad_alt.hxx

@@ -25,17 +25,15 @@
 #define _INST_RADALT_HXX
 
 #include <simgear/structure/subsystem_mgr.hxx>
-#include <Scenery/scenery.hxx>
+#include <simgear/props/props.hxx>
-#include <simgear/scene/material/mat.hxx>
+#include <simgear/math/SGMath.hxx>
 
-#include <Cockpit/agradar.hxx>
+class RadarAltimeter : public SGSubsystem
-
+{
-class radAlt : public agRadar{
 public:
 
-    radAlt ( SGPropertyNode *node );
+    RadarAltimeter ( SGPropertyNode *node );
-    radAlt ();
+    virtual ~RadarAltimeter ();
-    virtual ~radAlt ();
 
 private:
 
@@ -43,10 +41,26 @@ private:
     virtual void update (double dt);
 
     void update_altitude();
-    double getDistanceAntennaToHit(SGVec3d h) const;
+    void updateSetHeight();
-
+    
+    double getDistanceAntennaToHit(const SGVec3d& h) const;
+    SGVec3d getCartAntennaPos()const;
+    
+    SGVec3d rayVector(double az, double el) const;
+    
+    SGPropertyNode_ptr _Instrument;
     SGPropertyNode_ptr _user_alt_agl_node;
     SGPropertyNode_ptr _rad_alt_warning_node;
+    SGPropertyNode_ptr _serviceable_node;
+    SGPropertyNode_ptr _sceneryLoaded;
+    
+    
+    SGVec3d _antennaOffset; // in aircraft local XYZ frame
+    
+    std::string _name;
+    int _num;
+    double _time;
+    double _interval;
 
     double _min_radalt;