X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Ftcas.cxx;h=ef2a9a888758ac111541fb92a9dbe97e96fe87cd;hb=1867ccb4a8484b9bc856c88629e09e25bcc35d42;hp=dbab3164ddf9c97ed504a9c053539f10ef9bf505;hpb=8cf024bf5c4d86accd9e3f1605eb7e7b8d6490f8;p=flightgear.git

diff --git a/src/Instrumentation/tcas.cxx b/src/Instrumentation/tcas.cxx
index dbab3164d..ef2a9a888 100644
--- a/src/Instrumentation/tcas.cxx
+++ b/src/Instrumentation/tcas.cxx
@@ -16,7 +16,7 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA.
 //
 ///////////////////////////////////////////////////////////////////////////////
 
@@ -100,9 +100,8 @@
 #include <simgear/sg_inlines.h>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
-#include <simgear/math/sg_random.h>
-#include <simgear/misc/sg_path.hxx>
 #include <simgear/sound/soundmgr_openal.hxx>
+#include <simgear/sound/sample_group.hxx>
 #include <simgear/structure/exception.hxx>
 
 using std::string;
@@ -113,12 +112,6 @@ using std::string;
 #  include <Include/no_version.h>
 #endif
 
-#include <Main/fg_props.hxx>
-#include <Main/globals.hxx>
-#include <AIModel/submodel.hxx>
-#include "instrument_mgr.hxx"
-#include "tcas.hxx"
-
 ///////////////////////////////////////////////////////////////////////////////
 // debug switches /////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
@@ -129,6 +122,11 @@ using std::string;
 //#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
 //#define FEATURE_TCAS_DEBUG_PROPERTIES
 
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include "instrument_mgr.hxx"
+#include "tcas.hxx"
+
 ///////////////////////////////////////////////////////////////////////////////
 // constants //////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
@@ -380,6 +378,12 @@ TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) :
 
 void
 TCAS::AdvisoryCoordinator::init(void)
+{
+    reinit();
+}
+
+void
+TCAS::AdvisoryCoordinator::reinit(void)
 {
     clear();
     previous = current;
@@ -504,7 +508,7 @@ TCAS::AdvisoryCoordinator::update(int mode)
     }
 
     /* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */
-    if ((tcas->threatDetector.getAlt() > 500)&&
+    if ((tcas->threatDetector.getRadarAlt() > 500)&&
         (mode >= SwitchTaOnly))
         tcas->annunciator.trigger(current, revertedRA);
     else
@@ -530,6 +534,10 @@ TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
     tcas(_tcas),
     pAlarmThresholds(&sensitivityLevels[0])
 {
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
+    checkCount = 0;
+#endif
+    self.radarAltFt = 0.0;
     unitTest();
 }
 
@@ -538,7 +546,8 @@ TCAS::ThreatDetector::init(void)
 {
     nodeLat         = fgGetNode("/position/latitude-deg",          true);
     nodeLon         = fgGetNode("/position/longitude-deg",         true);
-    nodeAlt         = fgGetNode("/position/altitude-ft",           true);
+    nodePressureAlt = fgGetNode("/position/altitude-ft",           true);
+    nodeRadarAlt    = fgGetNode("/position/altitude-agl-ft",       true);
     nodeHeading     = fgGetNode("/orientation/heading-deg",        true);
     nodeVelocity    = fgGetNode("/velocities/airspeed-kt",         true);
     nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps",  true);
@@ -551,18 +560,30 @@ void
 TCAS::ThreatDetector::update(void)
 {
     // update local position
-    self.lat         = nodeLat->getDoubleValue();
-    self.lon         = nodeLon->getDoubleValue();
-    self.altFt       = nodeAlt->getDoubleValue();
-    self.heading     = nodeHeading->getDoubleValue();
-    self.velocityKt  = nodeVelocity->getDoubleValue();
-    self.verticalFps = nodeVerticalFps->getDoubleValue();
+    self.lat           = nodeLat->getDoubleValue();
+    self.lon           = nodeLon->getDoubleValue();
+    self.pressureAltFt = nodePressureAlt->getDoubleValue();
+    self.heading       = nodeHeading->getDoubleValue();
+    self.velocityKt    = nodeVelocity->getDoubleValue();
+    self.verticalFps   = nodeVerticalFps->getDoubleValue();
+
+    /* radar altimeter provides a lot of spikes due to uneven terrain
+     * MK-VIII GPWS-spec requires smoothing the radar altitude with a
+     * 10second moving average. Likely the TCAS spec requires the same.
+     * => We use a cheap 10 second exponential average method.
+     */
+    const double SmoothingFactor = 0.3;
+    self.radarAltFt = nodeRadarAlt->getDoubleValue()*SmoothingFactor +
+            (1-SmoothingFactor)*self.radarAltFt;
 
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
+    printf("TCAS::ThreatDetector::update: radarAlt = %f\n",self.radarAltFt);
     checkCount = 0;
+#endif
 
     // determine current altitude's "Sensitivity Level Definition and Alarm Thresholds" 
     int sl=0;
-    for (sl=0;((self.altFt > sensitivityLevels[sl].maxAltitude)&&
+    for (sl=0;((self.radarAltFt > sensitivityLevels[sl].maxAltitude)&&
                (sensitivityLevels[sl].maxAltitude));sl++);
     pAlarmThresholds = &sensitivityLevels[sl];
     tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds);
@@ -600,7 +621,9 @@ TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float veloc
 int
 TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
 {
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
     checkCount++;
+#endif
     
     float velocityKt  = pModel->getDoubleValue("velocities/true-airspeed-kt");
 
@@ -609,7 +632,7 @@ TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
 
     int threatLevel = ThreatNone;
     float altFt = pModel->getDoubleValue("position/altitude-ft");
-    currentThreat.relativeAltitudeFt = altFt - self.altFt;
+    currentThreat.relativeAltitudeFt = altFt - self.pressureAltFt;
 
     // save computation time: don't care when relative altitude is excessive
     if (fabs(currentThreat.relativeAltitudeFt) > 10000)
@@ -641,7 +664,7 @@ TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
 
     /* do not detect any threats when in standby or on ground and taxiing */
     if ((mode <= SwitchStandby)||
-        ((self.altFt < 360)&&(self.velocityKt < 40)))
+        ((self.radarAltFt < 360)&&(self.velocityKt < 40)))
     {
         return threatLevel;
     }
@@ -1035,7 +1058,7 @@ TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, fl
 
         /* [EUROACAS]: "Certain RAs are inhibited at altitudes based on inputs from the radio altimeter:
          *   [..] (c)1000ft (+/- 100ft) and below, all RAs are inhibited;" */
-        if (pSelf->altFt < 1000)
+        if (pSelf->radarAltFt < 1000)
             threatLevel = ThreatTA;
         
         // RAs only issued in mode "Auto" (= "TA/RA" mode)
@@ -1103,7 +1126,7 @@ TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, fl
         /* [TCASII]: "TCAS is designed to inhibit Increase Descent RAs below 1450 feet AGL; */
         
         /* [TCASII]: "Descend RAs below 1100 feet AGL;" (inhibited) */
-        if (pSelf->altFt < 1100)
+        if (pSelf->radarAltFt < 1100)
         {
             RA &= ~AdvisoryDescend;
             //TODO Support "Do not descend" RA
@@ -1167,6 +1190,13 @@ TCAS::init(void)
     threatDetector.init();
 }
 
+void
+TCAS::reinit(void)
+{
+    nextUpdateTime = 0;
+    advisoryCoordinator.reinit();
+}
+
 void
 TCAS::bind(void)
 {
@@ -1294,8 +1324,8 @@ TCAS::selfTest(void)
     newAdvisory.threatLevel = ThreatRA;
     newAdvisory.RA          = AdvisoryClear;
     newAdvisory.RAOption    = OptionNone;
-    // TCAS audio is disabled below 500ft
-    threatDetector.setAlt(501);
+    // TCAS audio is disabled below 500ft AGL
+    threatDetector.setRadarAlt(501);
 
     // trigger various advisories
     switch(selfTestStep)
@@ -1359,8 +1389,7 @@ TCAS::selfTest(void)
 // TCAS::Tracker //////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
 
-TCAS::Tracker::Tracker(TCAS* _tcas) :
-    tcas(_tcas),
+TCAS::Tracker::Tracker(TCAS*) :
     currentTime(0),
     haveTargets(false),
     newTargets(false)