From: ThorstenB Date: Mon, 17 Jan 2011 22:27:50 +0000 (+0100) Subject: Added tracker stage to TCAS. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=27a0ff5442ecb7a0cee4834822a5ba270df03a79;p=flightgear.git Added tracker stage to TCAS. Added relative/absolte altitude display mode to wxradar. --- diff --git a/src/Instrumentation/tcas.cxx b/src/Instrumentation/tcas.cxx index 6598dc011..47242fe5f 100644 --- a/src/Instrumentation/tcas.cxx +++ b/src/Instrumentation/tcas.cxx @@ -125,6 +125,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 +219,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(); } @@ -279,7 +281,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 +343,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; @@ -400,21 +404,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. */ @@ -582,6 +586,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; } @@ -590,7 +601,7 @@ int TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel) { checkCount++; - + float velocityKt = pModel->getDoubleValue("velocities/true-airspeed-kt"); if (!checkTransponder(pModel, velocityKt)) @@ -616,7 +627,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 +639,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.altFt < 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 +775,7 @@ TCAS::ThreatDetector::checkVerticalThreat(void) (tau >= -5)) { currentThreat.verticalTA = true; - currentThreat.verticalRA = (tauRA.Tau); + currentThreat.verticalRA = (tau < pAlarmThresholds->RA.Tau); } } currentThreat.verticalTau = tau; @@ -878,7 +923,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 +1006,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 +1019,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; @@ -1074,7 +1121,7 @@ TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, fl newAdvisory.threatLevel = threatLevel; tcas->advisoryCoordinator.add(newAdvisory); - return RA; + return threatLevel; } /////////////////////////////////////////////////////////////////////////////// @@ -1088,6 +1135,7 @@ TCAS::TCAS(SGPropertyNode* pNode) : selfTestStep(0), properties_handler(this), threatDetector(this), + tracker(this), advisoryCoordinator(this), advisoryGenerator(this), annunciator(this) @@ -1170,6 +1218,9 @@ TCAS::update(double dt) { nextUpdateTime = 1.0; + // remove obsolete targets + tracker.update(); + // get aircrafts current position/speed/heading threatDetector.update(); @@ -1218,6 +1269,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); } } @@ -1301,3 +1354,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; +} diff --git a/src/Instrumentation/tcas.hxx b/src/Instrumentation/tcas.hxx index 6882c8317..6672eb83f 100644 --- a/src/Instrumentation/tcas.hxx +++ b/src/Instrumentation/tcas.hxx @@ -30,7 +30,7 @@ #include #include #include -#include "mk_viii.hxx" // voiceplayer only +#include "mk_viii.hxx" // FGVoicePlayer only using std::vector; using std::deque; @@ -115,16 +115,28 @@ class TCAS : public SGSubsystem typedef struct { - bool verticalTA; - bool verticalRA; - bool horizontalTA; - bool horizontalRA; - float horizontalTau; - float verticalTau; - float relativeAltitudeFt; - float verticalFps; + string callsign; + bool verticalTA; + bool verticalRA; + bool horizontalTA; + bool horizontalRA; + bool isTracked; + float horizontalTau; + float verticalTau; + float relativeAltitudeFt; + float verticalFps; + int RASense; } ThreatInfo; + typedef struct + { + int threatLevel; + double TAtimestamp; + double RAtimestamp; + } TrackerTarget; + + typedef map TrackerTargets; + typedef struct { double lat; @@ -236,6 +248,7 @@ class TCAS : public SGSubsystem bool isPlaying (void) { return voicePlayer.is_playing();} private: + TCAS* tcas; ResolutionAdvisory previous; FGVoicePlayer::Voice* pLastVoice; VoicePlayer voicePlayer; @@ -267,6 +280,33 @@ class TCAS : public SGSubsystem SGPropertyNode_ptr nodeTAWarning; }; + ///////////////////////////////////////////////////////////////////////////// + // TCAS::Tracker //////////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class Tracker + { + public: + Tracker (TCAS* _tcas); + ~Tracker (void) {} + + void update (void); + + void add (const string callsign, int detectedLevel); + bool active (void) { return haveTargets;} + bool newTraffic (void) { return newTargets;} + bool isTracked (string callsign) { if (!haveTargets) return false;else return _isTracked(callsign);} + bool _isTracked (string callsign); + int getThreatLevel (string callsign); + + private: + TCAS* tcas; + double currentTime; + bool haveTargets; + bool newTargets; + TrackerTargets targets; + }; + ///////////////////////////////////////////////////////////////////////////// // TCAS::AdvisoryGenerator ////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// @@ -277,7 +317,7 @@ class TCAS : public SGSubsystem AdvisoryGenerator (TCAS* _tcas); ~AdvisoryGenerator (void) {} - void init (const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat); + void init (const LocalInfo* _pSelf, ThreatInfo* _pCurrentThreat); void setAlarmThresholds (const SensitivityLevel* _pAlarmThresholds); @@ -291,7 +331,7 @@ class TCAS : public SGSubsystem private: TCAS* tcas; const LocalInfo* pSelf; /*< info structure for local aircraft */ - const ThreatInfo* pCurrentThreat; /*< info structure on current intruder/threat */ + ThreatInfo* pCurrentThreat; /*< info structure on current intruder/threat */ const SensitivityLevel* pAlarmThresholds; }; @@ -317,6 +357,7 @@ class TCAS : public SGSubsystem void setAlt (float altFt) { self.altFt = altFt;} float getAlt (void) { return self.altFt;} float getVelocityKt (void) { return self.velocityKt;} + int getRASense (void) { return currentThreat.RASense;} private: void unitTest (void); @@ -354,6 +395,7 @@ private: PropertiesHandler properties_handler; ThreatDetector threatDetector; + Tracker tracker; AdvisoryCoordinator advisoryCoordinator; AdvisoryGenerator advisoryGenerator; Annunciator annunciator; diff --git a/src/Instrumentation/wxradar.cxx b/src/Instrumentation/wxradar.cxx index d4d6d890c..d88d0cde9 100644 --- a/src/Instrumentation/wxradar.cxx +++ b/src/Instrumentation/wxradar.cxx @@ -186,6 +186,7 @@ wxRadarBg::init () _radar_centre_node = n->getNode("centre", true); _radar_rotate_node = n->getNode("rotate", true); _radar_tcas_node = n->getNode("tcas", true); + _radar_absalt_node = n->getNode("abs-altitude", true); _radar_centre_node->setBoolValue(false); if (!_radar_coverage_node->hasValue()) @@ -670,10 +671,11 @@ wxRadarBg::update_aircraft() if (!_ai_enabled_node->getBoolValue()) return; - bool draw_tcas = _radar_tcas_node->getBoolValue(); - bool draw_echoes = _radar_position_node->getBoolValue(); - bool draw_symbols = _radar_symbol_node->getBoolValue(); - bool draw_data = _radar_data_node->getBoolValue(); + bool draw_tcas = _radar_tcas_node->getBoolValue(); + bool draw_absolute = _radar_absalt_node->getBoolValue(); + bool draw_echoes = _radar_position_node->getBoolValue(); + bool draw_symbols = _radar_symbol_node->getBoolValue(); + bool draw_data = _radar_data_node->getBoolValue(); if (!draw_echoes && !draw_symbols && !draw_data) return; @@ -761,7 +763,7 @@ wxRadarBg::update_aircraft() bool is_tcas_contact = false; if (draw_tcas) { - is_tcas_contact = update_tcas(model,range,user_alt,alt,bearing,radius); + is_tcas_contact = update_tcas(model,range,user_alt,alt,bearing,radius,draw_absolute); } // pos mode @@ -796,7 +798,7 @@ wxRadarBg::update_aircraft() * Return true when processed as TCAS contact, false otherwise. */ bool wxRadarBg::update_tcas(const SGPropertyNode *model,double range,double user_alt,double alt, - double bearing,double radius) + double bearing,double radius,bool absMode) { int threatLevel=0; { @@ -858,6 +860,13 @@ wxRadarBg::update_tcas(const SGPropertyNode *model,double range,double user_alt, dy=-30; } altStr->setPosition(osg::Vec3((int)pos.x()-30, (int)pos.y()+dy, 0)); + if (absMode) + { + // absolute altitude display + text << setprecision(0) << fixed + << setw(3) << setfill('0') << alt/100 << endl; + } + else // relative altitude display if (sign) { text << sign diff --git a/src/Instrumentation/wxradar.hxx b/src/Instrumentation/wxradar.hxx index 20ddbe8b6..f962045e1 100644 --- a/src/Instrumentation/wxradar.hxx +++ b/src/Instrumentation/wxradar.hxx @@ -129,6 +129,7 @@ private: SGPropertyNode_ptr _radar_hdg_marker_node; SGPropertyNode_ptr _radar_rotate_node; SGPropertyNode_ptr _radar_tcas_node; + SGPropertyNode_ptr _radar_absalt_node; SGPropertyNode_ptr _font_node; SGPropertyNode_ptr _ai_enabled_node; @@ -156,7 +157,7 @@ private: void update_data(const SGPropertyNode *ac, double alt, double heading, double radius, double bearing, bool selected); bool update_tcas(const SGPropertyNode *model,double range,double user_alt,double alt, - double bearing,double radius); + double bearing,double radius, bool absMode); void center_map(); void apply_map_offset(); void updateFont();