X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Ftcas.cxx;h=12047c65efbc4ffc8a15cb860626de49d9260f66;hb=7c8ad455a327276e09687b380fb4147ae90b6c8c;hp=6598dc0119093455a25e56f0e74bf3de9bc7e4f9;hpb=bdd931aed9c9ef68d24e7c8c4412716294c12fa9;p=flightgear.git

diff --git a/src/Instrumentation/tcas.cxx b/src/Instrumentation/tcas.cxx
index 6598dc011..12047c65e 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;
@@ -115,7 +114,6 @@ using std::string;
 
 #include <Main/fg_props.hxx>
 #include <Main/globals.hxx>
-#include <AIModel/submodel.hxx>
 #include "instrument_mgr.hxx"
 #include "tcas.hxx"
 
@@ -125,6 +123,7 @@ using std::string;
 //#define FEATURE_TCAS_DEBUG_ANNUNCIATOR
 //#define FEATURE_TCAS_DEBUG_COORDINATOR
 //#define FEATURE_TCAS_DEBUG_THREAT_DETECTOR
+//#define FEATURE_TCAS_DEBUG_TRACKER
 //#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
 //#define FEATURE_TCAS_DEBUG_PROPERTIES
 
@@ -218,9 +217,10 @@ TCAS::VoicePlayer::init(void)
 // TCAS::Annunciator ////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////////////////////////
 
-TCAS::Annunciator::Annunciator(TCAS* tcas) :
+TCAS::Annunciator::Annunciator(TCAS* _tcas) :
+    tcas(_tcas),
     pLastVoice(NULL),
-    voicePlayer(tcas)
+    voicePlayer(_tcas)
 {
     clear();
 }
@@ -236,7 +236,7 @@ void TCAS::Annunciator::clear(void)
 void
 TCAS::Annunciator::bind(SGPropertyNode* node)
 {
-    voicePlayer.bind(node, "Sounds/tcas/");
+    voicePlayer.bind(node, "Sounds/tcas/female/");
 }
 
 void
@@ -279,7 +279,8 @@ TCAS::Annunciator::trigger(const ResolutionAdvisory& current, bool revertedRA)
         return;
     }
 
-    if (previous.RA == AdvisoryClear)
+    if ((previous.RA == AdvisoryClear)||
+        (tcas->tracker.newTraffic()))
     {
         voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW);
     }
@@ -340,7 +341,8 @@ TCAS::Annunciator::trigger(const ResolutionAdvisory& current, bool revertedRA)
 
     previous = current;
 
-    if (pLastVoice == pVoice)
+    if ((pLastVoice == pVoice)&&
+        (!tcas->tracker.newTraffic()))
     {
         // don't repeat annunciation
         return;
@@ -376,6 +378,12 @@ TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) :
 
 void
 TCAS::AdvisoryCoordinator::init(void)
+{
+    reinit();
+}
+
+void
+TCAS::AdvisoryCoordinator::reinit(void)
 {
     clear();
     previous = current;
@@ -400,21 +408,21 @@ TCAS::AdvisoryCoordinator::clear(void)
 void
 TCAS::AdvisoryCoordinator::add(const ResolutionAdvisory& newAdvisory)
 {
-    if (newAdvisory.RA == AdvisoryClear)
+    if ((newAdvisory.RA == AdvisoryClear)||
+        (newAdvisory.threatLevel < current.threatLevel))
         return;
 
-    if ((current.RA == AdvisoryClear)||
-        (current.threatLevel == ThreatTA))
-    {
-        current = newAdvisory;
-    }
-    else
+    if (current.threatLevel == newAdvisory.threatLevel)
     {
         // combine with other advisories so far
         current.RA &= newAdvisory.RA;
         // remember any advisory modifier
         current.RAOption |= newAdvisory.RAOption;
     }
+    else
+    {
+        current = newAdvisory;
+    }
 }
 
 /** Pick and trigger suitable resolution advisory. */
@@ -500,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
@@ -524,8 +532,10 @@ TCAS::AdvisoryCoordinator::update(int mode)
 
 TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
     tcas(_tcas),
+    checkCount(0),
     pAlarmThresholds(&sensitivityLevels[0])
 {
+    self.radarAltFt = 0.0;
     unitTest();
 }
 
@@ -534,7 +544,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);
@@ -547,18 +558,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);
@@ -582,6 +605,13 @@ TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float veloc
         return false;
     }
 
+    if ((name == "multiplayer")&&
+        (pModel->getBoolValue("controls/invisible")))
+    {
+        // ignored MP plane: pretend transponder is switched off
+        return false;
+    }
+
     return true;
 }
 
@@ -589,8 +619,10 @@ 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");
 
     if (!checkTransponder(pModel, velocityKt))
@@ -598,7 +630,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)
@@ -616,7 +648,6 @@ TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
     if ((distanceNm > 10)||(distanceNm < 0))
         return threatLevel;
 
-    // first stage: vertical movement
     currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps");
     
     /* Detect proximity targets
@@ -629,44 +660,79 @@ TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
         threatLevel = ThreatProximity;
     }
 
+    /* do not detect any threats when in standby or on ground and taxiing */
+    if ((mode <= SwitchStandby)||
+        ((self.radarAltFt < 360)&&(self.velocityKt < 40)))
+    {
+        return threatLevel;
+    }
+
+    if (tcas->tracker.active())
+    {
+        currentThreat.callsign = pModel->getStringValue("callsign");
+        currentThreat.isTracked = tcas->tracker.isTracked(currentThreat.callsign);
+    }
+    else
+        currentThreat.isTracked = false;
+    
+    // first stage: vertical movement
     checkVerticalThreat();
 
     // stop processing when no vertical threat
-    if (!currentThreat.verticalTA)
+    if ((!currentThreat.verticalTA)&&
+        (!currentThreat.isTracked))
         return threatLevel;
 
     // second stage: horizontal movement
     horizontalThreat(bearing, distanceNm, heading, velocityKt);
 
-    // no horizontal threat?
-    if (!currentThreat.horizontalTA)
-        return threatLevel;
-
-    if ((currentThreat.horizontalTau < 0)||
-        (currentThreat.verticalTau < 0))
+    if (!currentThreat.isTracked)
     {
-        // do not trigger new alerts when Tau is negative, but keep existing alerts
-        int previousThreatLevel = pModel->getIntValue("tcas/threat-level", 0);
-        if (previousThreatLevel == 0)
+        // no horizontal threat?
+        if (!currentThreat.horizontalTA)
             return threatLevel;
+
+        if ((currentThreat.horizontalTau < 0)||
+            (currentThreat.verticalTau < 0))
+        {
+            // do not trigger new alerts when Tau is negative, but keep existing alerts
+            int previousThreatLevel = pModel->getIntValue("tcas/threat-level", 0);
+            if (previousThreatLevel == 0)
+                return threatLevel;
+        }
     }
 
 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
     cout << "#" << checkCount << ": " << pModel->getStringValue("callsign") << endl;
 #endif
 
-    threatLevel = ThreatTA;
-    /* at least a TA-level threat, may also have a RA-level threat...
-     * [TCASII]: "For either a TA or an RA to be issued, both the range and 
+    
+    /* [TCASII]: "For either a TA or an RA to be issued, both the range and 
      *    vertical criteria, in terms of tau or the fixed thresholds, must be
      *    satisfied only one of the criteria is satisfied, TCAS will not issue
      *    an advisory." */
+    if (currentThreat.horizontalTA && currentThreat.verticalTA)
+        threatLevel = ThreatTA;
     if (currentThreat.horizontalRA && currentThreat.verticalRA)
         threatLevel = ThreatRA;
 
-    // find all resolution options for this conflict
-    tcas->advisoryGenerator.resolution(mode, threatLevel, distanceNm, altFt, heading, velocityKt);
+    if (!tcas->tracker.active())
+        currentThreat.callsign = pModel->getStringValue("callsign");
 
+    tcas->tracker.add(currentThreat.callsign, threatLevel);
+    
+    // check existing threat level
+    if (currentThreat.isTracked)
+    {
+        int oldLevel = tcas->tracker.getThreatLevel(currentThreat.callsign);
+        if (oldLevel > threatLevel)
+            threatLevel = oldLevel;
+    }
+
+    // find all resolution options for this conflict
+    threatLevel = tcas->advisoryGenerator.resolution(mode, threatLevel, distanceNm, altFt, heading, velocityKt);
+   
+    
 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
     printf("  threat: distance: %4.1f, bearing: %4.1f, alt: %5.1f, velocity: %4.1f, heading: %4.1f, vspeed: %4.1f, "
            "own alt: %5.1f, own heading: %4.1f, own velocity: %4.1f, vertical tau: %3.2f"
@@ -730,7 +796,7 @@ TCAS::ThreatDetector::checkVerticalThreat(void)
             (tau >= -5))
         {
             currentThreat.verticalTA = true;
-            currentThreat.verticalRA = (tau<pAlarmThresholds->RA.Tau);
+            currentThreat.verticalRA = (tau < pAlarmThresholds->RA.Tau);
         }
     }
     currentThreat.verticalTau = tau;
@@ -878,7 +944,7 @@ TCAS::AdvisoryGenerator::AdvisoryGenerator(TCAS* _tcas) :
 }
 
 void
-TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat)
+TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, ThreatInfo* _pCurrentThreat)
 {
     pCurrentThreat = _pCurrentThreat;
     pSelf = _pSelf;
@@ -961,6 +1027,8 @@ TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing)
     }
     // else: threat is at same altitude, keep optimal RA sense (non-crossing)
 
+    pCurrentThreat->RASense = RASense;
+
 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
         printf("  RASense: %i, crossing: %u, relAlt: %4.1f, upward separation: %4.1f, downward separation: %4.1f\n",
                RASense,isCrossing,
@@ -972,7 +1040,7 @@ TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing)
 /** Determine suitable resolution advisories. */
 int
 TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, float altFt,
-                                 float heading, float velocityKt)
+                                    float heading, float velocityKt)
 {
     int RAOption = OptionNone;
     int RA       = AdvisoryIntrusion;
@@ -988,7 +1056,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)
@@ -1056,7 +1124,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
@@ -1074,7 +1142,7 @@ TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, fl
     newAdvisory.threatLevel = threatLevel;
     tcas->advisoryCoordinator.add(newAdvisory);
     
-    return RA;
+    return threatLevel;
 }
 
 ///////////////////////////////////////////////////////////////////////////////
@@ -1088,6 +1156,7 @@ TCAS::TCAS(SGPropertyNode* pNode) :
     selfTestStep(0),
     properties_handler(this),
     threatDetector(this),
+    tracker(this),
     advisoryCoordinator(this),
     advisoryGenerator(this),
     annunciator(this)
@@ -1119,6 +1188,13 @@ TCAS::init(void)
     threatDetector.init();
 }
 
+void
+TCAS::reinit(void)
+{
+    nextUpdateTime = 0;
+    advisoryCoordinator.reinit();
+}
+
 void
 TCAS::bind(void)
 {
@@ -1170,6 +1246,9 @@ TCAS::update(double dt)
     {
         nextUpdateTime = 1.0;
 
+        // remove obsolete targets
+        tracker.update();
+
         // get aircrafts current position/speed/heading
         threatDetector.update();
 
@@ -1218,6 +1297,8 @@ TCAS::update(double dt)
                     int threatLevel = threatDetector.checkThreat(mode, pModel);
                     /* expose aircraft threat-level (to be used by other instruments,
                      * i.e. TCAS display) */
+                    if (threatLevel==ThreatRA)
+                        pModel->setIntValue("tcas/ra-sense", -threatDetector.getRASense());
                     pModel->setIntValue("tcas/threat-level", threatLevel);
                 }
             }
@@ -1241,8 +1322,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)
@@ -1301,3 +1382,111 @@ TCAS::selfTest(void)
 
     selfTestStep++;
 }
+
+///////////////////////////////////////////////////////////////////////////////
+// TCAS::Tracker //////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+TCAS::Tracker::Tracker(TCAS* _tcas) :
+    tcas(_tcas),
+    currentTime(0),
+    haveTargets(false),
+    newTargets(false)
+{
+    targets.clear();
+}
+
+void
+TCAS::Tracker::update(void)
+{
+    currentTime = globals->get_sim_time_sec();
+    newTargets = false;
+
+    if (haveTargets)
+    {
+        // remove outdated targets
+        TrackerTargets::iterator it = targets.begin();
+        while (it != targets.end())
+        {
+            TrackerTarget* pTarget = it->second;
+            if (currentTime - pTarget->TAtimestamp > 10.0)
+            {
+                TrackerTargets::iterator temp = it;
+                ++it;
+#ifdef FEATURE_TCAS_DEBUG_TRACKER
+                printf("target %s no longer a TA threat.\n",temp->first.c_str());
+#endif
+                targets.erase(temp->first);
+                delete pTarget;
+                pTarget = NULL;
+            }
+            else
+            {
+                if ((pTarget->threatLevel == ThreatRA)&&
+                    (currentTime - pTarget->RAtimestamp > 7.0))
+                {
+                    pTarget->threatLevel = ThreatTA;
+#ifdef FEATURE_TCAS_DEBUG_TRACKER
+                    printf("target %s no longer an RA threat.\n",it->first.c_str());
+#endif
+                }
+                ++it;
+            }
+        }
+        haveTargets = !targets.empty();
+    }
+}
+
+void
+TCAS::Tracker::add(const string callsign, int detectedLevel)
+{
+    TrackerTarget* pTarget = NULL;
+    if (haveTargets)
+    {
+        TrackerTargets::iterator it = targets.find(callsign);
+        if (it != targets.end())
+        {
+            pTarget = it->second;
+        }
+    }
+
+    if (!pTarget)
+    {
+        pTarget = new TrackerTarget();
+        pTarget->TAtimestamp = 0;
+        pTarget->RAtimestamp = 0;
+        pTarget->threatLevel = 0;
+        newTargets = true;
+        targets[callsign] = pTarget;
+#ifdef FEATURE_TCAS_DEBUG_TRACKER
+        printf("new target: %s, level: %i\n",callsign.c_str(),detectedLevel);
+#endif
+    }
+
+    if (detectedLevel > pTarget->threatLevel)
+        pTarget->threatLevel = detectedLevel;
+
+    if (detectedLevel >= ThreatTA)
+        pTarget->TAtimestamp = currentTime;
+
+    if (detectedLevel >= ThreatRA)
+        pTarget->RAtimestamp = currentTime;
+
+    haveTargets = true;
+}
+
+bool
+TCAS::Tracker::_isTracked(string callsign)
+{
+    return targets.find(callsign) != targets.end();
+}
+
+int
+TCAS::Tracker::getThreatLevel(string callsign)
+{
+    TrackerTargets::iterator it = targets.find(callsign);
+    if (it != targets.end())
+        return it->second->threatLevel;
+    else
+        return 0;
+}