//
// 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.
//
///////////////////////////////////////////////////////////////////////////////
#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;
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
-#include <AIModel/submodel.hxx>
#include "instrument_mgr.hxx"
#include "tcas.hxx"
//#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
// TCAS::Annunciator ////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
-TCAS::Annunciator::Annunciator(TCAS* tcas) :
+TCAS::Annunciator::Annunciator(TCAS* _tcas) :
+ tcas(_tcas),
pLastVoice(NULL),
- voicePlayer(tcas)
+ voicePlayer(_tcas)
{
clear();
}
void
TCAS::Annunciator::bind(SGPropertyNode* node)
{
- voicePlayer.bind(node, "Sounds/tcas/");
+ voicePlayer.bind(node, "Sounds/tcas/female/");
}
void
return;
}
- if (previous.RA == AdvisoryClear)
+ if ((previous.RA == AdvisoryClear)||
+ (tcas->tracker.newTraffic()))
{
voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW);
}
previous = current;
- if (pLastVoice == pVoice)
+ if ((pLastVoice == pVoice)&&
+ (!tcas->tracker.newTraffic()))
{
// don't repeat annunciation
return;
void
TCAS::AdvisoryCoordinator::init(void)
+{
+ reinit();
+}
+
+void
+TCAS::AdvisoryCoordinator::reinit(void)
{
clear();
previous = current;
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. */
}
/* [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
TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
tcas(_tcas),
+ checkCount(0),
pAlarmThresholds(&sensitivityLevels[0])
{
+ self.radarAltFt = 0.0;
unitTest();
}
{
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);
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);
return false;
}
+ if ((name == "multiplayer")&&
+ (pModel->getBoolValue("controls/invisible")))
+ {
+ // ignored MP plane: pretend transponder is switched off
+ return false;
+ }
+
return true;
}
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))
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)
if ((distanceNm > 10)||(distanceNm < 0))
return threatLevel;
- // first stage: vertical movement
currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps");
/* Detect proximity targets
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"
(tau >= -5))
{
currentThreat.verticalTA = true;
- currentThreat.verticalRA = (tau<pAlarmThresholds->RA.Tau);
+ currentThreat.verticalRA = (tau < pAlarmThresholds->RA.Tau);
}
}
currentThreat.verticalTau = tau;
}
void
-TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat)
+TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, ThreatInfo* _pCurrentThreat)
{
pCurrentThreat = _pCurrentThreat;
pSelf = _pSelf;
}
// 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,
/** 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;
/* [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)
/* [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
newAdvisory.threatLevel = threatLevel;
tcas->advisoryCoordinator.add(newAdvisory);
- return RA;
+ return threatLevel;
}
///////////////////////////////////////////////////////////////////////////////
selfTestStep(0),
properties_handler(this),
threatDetector(this),
+ tracker(this),
advisoryCoordinator(this),
advisoryGenerator(this),
annunciator(this)
threatDetector.init();
}
+void
+TCAS::reinit(void)
+{
+ nextUpdateTime = 0;
+ advisoryCoordinator.reinit();
+}
+
void
TCAS::bind(void)
{
{
nextUpdateTime = 1.0;
+ // remove obsolete targets
+ tracker.update();
+
// get aircrafts current position/speed/heading
threatDetector.update();
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);
}
}
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)
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;
+}
projects / flightgear.git / blobdiff