1 // tcas.cxx -- Traffic Alert and Collision Avoidance System (TCAS) Emulation
3 // Written by Thorsten Brehm, started December 2010.
5 // Copyright (C) 2010 Thorsten Brehm - brehmt (at) gmail com
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
21 ///////////////////////////////////////////////////////////////////////////////
25 * [TCASII] Introduction to TCAS II Version 7, Federal Aviation Administration, November 2000
26 * http://www.arinc.com/downloads/tcas/tcas.pdf
28 * [EUROACAS] Eurocontrol Airborne Collision Avoidance System (ACAS),
29 * http://www.eurocontrol.int/msa/public/standard_page/ACAS_Startpage.html
33 * ALIM: Altitude Limit
35 * CPA: Closest point of approach as computed from a threat's range and range rate.
37 * DMOD: Distance MODification
39 * Intruder: A target that has satisfied the traffic detection criteria.
41 * Proximity target: Any target that is less than 6 nmi in range and within +/-1200ft
42 * vertically, but that does not meet the intruder or threat criteria.
44 * RA: Resolution advisory. An indication given by TCAS II to a flight crew that a
45 * vertical maneuver should, or in some cases should not, be performed to attain or
46 * maintain safe separation from a threat.
48 * SL: Sensitivity Level. A value used in defining the size of the protected volume
49 * around the own aircraft.
51 * TA: Traffic Advisory. An indication given by TCAS to the pilot when an aircraft has
52 * entered, or is projected to enter, the protected volume around the own aircraft.
54 * Tau: Approximation of the time, in seconds, to CPA or to the aircraft being at the
57 * TCAS: Traffic alert and Collision Avoidance System
62 * serviceable enable/disable TCAS processing
64 * voice/file-prefix path (and optional prefix) for sound sample files
65 * (only evaluated at start-up)
67 * inputs/mode TCAS mode selection: 0=off,1=standby,2=TA only,3=auto(TA/RA)
68 * inputs/self-test trigger self-test sequence
70 * outputs/traffic-alert intruder detected (true=TA-threat is active, includes RA-threats)
71 * outputs/advisory-alert resolution advisory is issued (true=advisory is valid)
72 * outputs/vertical-speed vertical speed required by advisory (+/-2000/1500/500/0)
74 * speaker/max-dist Max. distance where speaker is heard
75 * speaker/reference-dist Distance to pilot
76 * speaker/volume Volume at reference distance
78 * debug/threat-trigger trigger debugging test (in debug mode only)
79 * debug/threat-RA debugging RA value (in debug mode only)
80 * debug/threat-level debugging threat level (in debug mode only)
84 # pragma warning( disable: 4355 )
99 #include <simgear/constants.h>
100 #include <simgear/sg_inlines.h>
101 #include <simgear/debug/logstream.hxx>
102 #include <simgear/math/sg_geodesy.hxx>
103 #include <simgear/sound/soundmgr_openal.hxx>
104 #include <simgear/sound/sample_group.hxx>
105 #include <simgear/structure/exception.hxx>
109 #if defined( HAVE_VERSION_H ) && HAVE_VERSION_H
110 # include <Include/version.h>
112 # include <Include/no_version.h>
115 ///////////////////////////////////////////////////////////////////////////////
116 // debug switches /////////////////////////////////////////////////////////////
117 ///////////////////////////////////////////////////////////////////////////////
118 //#define FEATURE_TCAS_DEBUG_ANNUNCIATOR
119 //#define FEATURE_TCAS_DEBUG_COORDINATOR
120 //#define FEATURE_TCAS_DEBUG_THREAT_DETECTOR
121 //#define FEATURE_TCAS_DEBUG_TRACKER
122 //#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
123 //#define FEATURE_TCAS_DEBUG_PROPERTIES
125 #include <Main/fg_props.hxx>
126 #include <Main/globals.hxx>
127 #include "instrument_mgr.hxx"
130 ///////////////////////////////////////////////////////////////////////////////
131 // constants //////////////////////////////////////////////////////////////////
132 ///////////////////////////////////////////////////////////////////////////////
134 /** Sensitivity Level Definition and Alarm Thresholds (TCASII, Version 7)
135 * Max Own | |TA-level |RA-level
136 * Altitude(ft) | SL |Tau(s),DMOD(nm),ALIM(ft)|Tau(s),DMOD(nm),ALIM(ft) */
137 const TCAS::SensitivityLevel
138 TCAS::ThreatDetector::sensitivityLevels[] = {
139 { 1000, 2, {20, 0.30, 850}, {0, 0, 0 }},
140 { 2350, 3, {25, 0.33, 850}, {15, 0.20, 300}},
141 { 5000, 4, {30, 0.48, 850}, {20, 0.35, 300}},
142 {10000, 5, {40, 0.75, 850}, {25, 0.55, 350}},
143 {20000, 6, {45, 1.00, 850}, {30, 0.80, 400}},
144 {42000, 7, {48, 1.30, 850}, {35, 1.10, 600}},
145 {0, 8, {48, 1.30, 1200}, {35, 1.10, 700}}
148 ///////////////////////////////////////////////////////////////////////////////
149 // helpers ////////////////////////////////////////////////////////////////////
150 ///////////////////////////////////////////////////////////////////////////////
152 #define ADD_VOICE(Var, Sample, SayTwice) \
153 { make_voice(&Var); \
154 append(Var, Sample); \
155 if (SayTwice) append(Var, Sample); }
157 #define AVAILABLE_RA(Options, Advisory) (Advisory == (Advisory & Options))
159 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
160 /** calculate relative angle in between two headings */
162 relAngle(float Heading1, float Heading2)
164 Heading1 -= Heading2;
166 while (Heading1 >= 360.0)
169 while (Heading1 < 0.0)
176 /** calculate range and bearing of lat2/lon2 relative to lat1/lon1 */
178 calcRangeBearing(double lat1, double lon1, double lat2, double lon2,
179 double &rangeNm, double &bearing)
181 // calculate the bearing and range of the second pos from the first
182 double az2, distanceM;
183 geo_inverse_wgs_84(lat1, lon1, lat2, lon2, &bearing, &az2, &distanceM);
184 rangeNm = distanceM * SG_METER_TO_NM;
187 ///////////////////////////////////////////////////////////////////////////////
188 // VoicePlayer ////////////////////////////////////////////////////////////////
189 ///////////////////////////////////////////////////////////////////////////////
192 TCAS::VoicePlayer::init(void)
194 FGVoicePlayer::init();
196 ADD_VOICE(Voices.pTrafficTraffic, "traffic", true);
197 ADD_VOICE(Voices.pClear, "clear", false);
198 ADD_VOICE(Voices.pClimb, "climb", true);
199 ADD_VOICE(Voices.pClimbNow, "climb_now", true);
200 ADD_VOICE(Voices.pClimbCrossing, "climb_crossing", true);
201 ADD_VOICE(Voices.pClimbIncrease, "increase_climb", false);
202 ADD_VOICE(Voices.pDescend, "descend", true);
203 ADD_VOICE(Voices.pDescendNow, "descend_now", true);
204 ADD_VOICE(Voices.pDescendCrossing,"descend_crossing", true);
205 ADD_VOICE(Voices.pDescendIncrease,"increase_descent", false);
206 ADD_VOICE(Voices.pAdjustVSpeed, "adjust_vertical_speed", false);
207 ADD_VOICE(Voices.pMaintVSpeed, "maintain_vertical_speed", false);
208 ADD_VOICE(Voices.pMonitorVSpeed, "monitor_vertical_speed", false);
209 ADD_VOICE(Voices.pLevelOff, "level_off", false);
210 ADD_VOICE(Voices.pTestOk, "test_ok", false);
211 ADD_VOICE(Voices.pTestFail, "test_fail", false);
213 speaker.update_configuration();
216 /////////////////////////////////////////////////////////////////////////////
217 // TCAS::Annunciator ////////////////////////////////////////////////////////
218 /////////////////////////////////////////////////////////////////////////////
220 TCAS::Annunciator::Annunciator(TCAS* _tcas) :
228 void TCAS::Annunciator::clear(void)
230 previous.threatLevel = ThreatNone;
231 previous.RA = AdvisoryClear;
232 previous.RAOption = OptionNone;
237 TCAS::Annunciator::bind(SGPropertyNode* node)
239 voicePlayer.bind(node, "Sounds/tcas/female/");
243 TCAS::Annunciator::init(void)
245 //TODO link to GPWS module/audio-on signal must be configurable
246 nodeGpwsAlertOn = fgGetNode("/instrumentation/mk-viii/outputs/discretes/audio-on", true);
251 TCAS::Annunciator::update(void)
253 voicePlayer.update();
255 /* [TCASII]: "The priority scheme gives ground proximity warning systems (GPWS)
256 * a higher annunciation priority than a TCAS alert. TCAS aural annunciation will
257 * be inhibited during the time that a GPWS alert is active." */
258 if (nodeGpwsAlertOn->getBoolValue())
261 voicePlayer.resume();
264 /** Trigger voice sample for current alert. */
266 TCAS::Annunciator::trigger(const ResolutionAdvisory& current, bool revertedRA)
269 int RAOption = current.RAOption;
271 if (RA == AdvisoryClear)
273 if (previous.RA != AdvisoryClear)
275 voicePlayer.play(voicePlayer.Voices.pClear, VoicePlayer::PLAY_NOW);
282 if ((previous.RA == AdvisoryClear)||
283 (tcas->tracker.newTraffic()))
285 voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW);
289 VoicePlayer::Voice* pVoice = NULL;
294 pVoice = voicePlayer.Voices.pClimbNow;
296 if (AVAILABLE_RA(RAOption, OptionIncreaseClimb))
297 pVoice = voicePlayer.Voices.pClimbIncrease;
299 if (AVAILABLE_RA(RAOption, OptionCrossingClimb))
300 pVoice = voicePlayer.Voices.pClimbCrossing;
302 pVoice = voicePlayer.Voices.pClimb;
305 case AdvisoryDescend:
307 pVoice = voicePlayer.Voices.pDescendNow;
309 if (AVAILABLE_RA(RAOption, OptionIncreaseDescend))
310 pVoice = voicePlayer.Voices.pDescendIncrease;
312 if (AVAILABLE_RA(RAOption, OptionCrossingDescent))
313 pVoice = voicePlayer.Voices.pDescendCrossing;
315 pVoice = voicePlayer.Voices.pDescend;
318 case AdvisoryAdjustVSpeed:
319 pVoice = voicePlayer.Voices.pAdjustVSpeed;
322 case AdvisoryMaintVSpeed:
323 pVoice = voicePlayer.Voices.pMaintVSpeed;
326 case AdvisoryMonitorVSpeed:
327 pVoice = voicePlayer.Voices.pMonitorVSpeed;
330 case AdvisoryLevelOff:
331 pVoice = voicePlayer.Voices.pLevelOff;
334 case AdvisoryIntrusion:
338 RA = AdvisoryIntrusion;
344 if ((pLastVoice == pVoice)&&
345 (!tcas->tracker.newTraffic()))
347 // don't repeat annunciation
352 voicePlayer.play(pVoice);
354 #ifdef FEATURE_TCAS_DEBUG_ANNUNCIATOR
355 cout << "Annunciating TCAS RA " << RA << endl;
360 TCAS::Annunciator::test(bool testOk)
363 voicePlayer.play(voicePlayer.Voices.pTestOk);
365 voicePlayer.play(voicePlayer.Voices.pTestFail);
368 /////////////////////////////////////////////////////////////////////////////
369 // TCAS::AdvisoryCoordinator ////////////////////////////////////////////////
370 /////////////////////////////////////////////////////////////////////////////
372 TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) :
380 TCAS::AdvisoryCoordinator::init(void)
386 TCAS::AdvisoryCoordinator::reinit(void)
393 TCAS::AdvisoryCoordinator::bind(SGPropertyNode* node)
395 nodeTAWarning = node->getNode("outputs/traffic-alert", true);
396 nodeTAWarning->setBoolValue(false);
400 TCAS::AdvisoryCoordinator::clear(void)
402 current.threatLevel = ThreatNone;
403 current.RA = AdvisoryClear;
404 current.RAOption = OptionNone;
407 /** Add all suitable resolution advisories for a single threat. */
409 TCAS::AdvisoryCoordinator::add(const ResolutionAdvisory& newAdvisory)
411 if ((newAdvisory.RA == AdvisoryClear)||
412 (newAdvisory.threatLevel < current.threatLevel))
415 if (current.threatLevel == newAdvisory.threatLevel)
417 // combine with other advisories so far
418 current.RA &= newAdvisory.RA;
419 // remember any advisory modifier
420 current.RAOption |= newAdvisory.RAOption;
424 current = newAdvisory;
428 /** Pick and trigger suitable resolution advisory. */
430 TCAS::AdvisoryCoordinator::update(int mode)
432 bool revertedRA = false; // has advisory changed?
433 double currentTime = globals->get_sim_time_sec();
435 if (current.RA == AdvisoryClear)
437 // filter: wait 5 seconds after last TA/RA before announcing TA clearance
438 if ((previous.RA != AdvisoryClear)&&
439 (currentTime - lastTATime < 5.0))
446 #ifdef FEATURE_TCAS_DEBUG_COORDINATOR
447 cout << "TCAS::Annunciator::update: previous: " << previous.RA << ", new: " << current.RA << endl;
450 lastTATime = currentTime;
451 if ((previous.RA == AdvisoryClear)||
452 (previous.RA == AdvisoryIntrusion)||
453 ((current.RA & previous.RA) != previous.RA))
455 // no RA yet, or we can't keep previous RA: pick one - in order of priority
457 if (AVAILABLE_RA(current.RA, AdvisoryMonitorVSpeed))
459 // prio 1: monitor vertical speed only
460 current.RA = AdvisoryMonitorVSpeed;
463 if (AVAILABLE_RA(current.RA, AdvisoryMaintVSpeed))
465 // prio 2: maintain vertical speed
466 current.RA = AdvisoryMaintVSpeed;
469 if (AVAILABLE_RA(current.RA, AdvisoryAdjustVSpeed))
471 // prio 3: adjust vertical speed (TCAS II 7.0 only)
472 current.RA = AdvisoryAdjustVSpeed;
475 if (AVAILABLE_RA(current.RA, AdvisoryLevelOff))
477 // prio 3: adjust vertical speed (TCAS II 7.1 only, [EUROACAS]: CP115)
478 current.RA = AdvisoryLevelOff;
481 if (AVAILABLE_RA(current.RA, AdvisoryClimb))
484 current.RA = AdvisoryClimb;
487 if (AVAILABLE_RA(current.RA, AdvisoryDescend))
490 current.RA = AdvisoryDescend;
494 // no RA, issue a TA only
495 current.RA = AdvisoryIntrusion;
498 // check if earlier advisory was reverted
499 revertedRA = ((previous.RA != current.RA)&&
501 (previous.RA != AdvisoryIntrusion));
506 current.RA = previous.RA;
510 /* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */
511 if ((tcas->threatDetector.getRadarAlt() > 500)&&
512 (mode >= SwitchTaOnly))
513 tcas->annunciator.trigger(current, revertedRA);
515 if (current.RA == AdvisoryClear)
517 /* explicitly clear traffic alert (since aural annunciation disabled) */
518 tcas->annunciator.clear();
523 /* [TCASII] "[..] also performs the function of setting flags that control the displays.
524 * The traffic display, the RA display, [..] use these flags to alert the pilot to
525 * the presence of TAs and RAs." */
526 nodeTAWarning->setBoolValue(current.RA != AdvisoryClear);
529 ///////////////////////////////////////////////////////////////////////////////
530 // TCAS::ThreatDetector ///////////////////////////////////////////////////////
531 ///////////////////////////////////////////////////////////////////////////////
533 TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
535 pAlarmThresholds(&sensitivityLevels[0])
537 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
540 self.radarAltFt = 0.0;
545 TCAS::ThreatDetector::init(void)
547 nodeLat = fgGetNode("/position/latitude-deg", true);
548 nodeLon = fgGetNode("/position/longitude-deg", true);
549 nodePressureAlt = fgGetNode("/position/altitude-ft", true);
550 nodeRadarAlt = fgGetNode("/position/altitude-agl-ft", true);
551 nodeHeading = fgGetNode("/orientation/heading-deg", true);
552 nodeVelocity = fgGetNode("/velocities/airspeed-kt", true);
553 nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps", true);
555 tcas->advisoryGenerator.init(&self,¤tThreat);
558 /** Update local position and threat sensitivity levels. */
560 TCAS::ThreatDetector::update(void)
562 // update local position
563 self.lat = nodeLat->getDoubleValue();
564 self.lon = nodeLon->getDoubleValue();
565 self.pressureAltFt = nodePressureAlt->getDoubleValue();
566 self.heading = nodeHeading->getDoubleValue();
567 self.velocityKt = nodeVelocity->getDoubleValue();
568 self.verticalFps = nodeVerticalFps->getDoubleValue();
570 /* radar altimeter provides a lot of spikes due to uneven terrain
571 * MK-VIII GPWS-spec requires smoothing the radar altitude with a
572 * 10second moving average. Likely the TCAS spec requires the same.
573 * => We use a cheap 10 second exponential average method.
575 const double SmoothingFactor = 0.3;
576 self.radarAltFt = nodeRadarAlt->getDoubleValue()*SmoothingFactor +
577 (1-SmoothingFactor)*self.radarAltFt;
579 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
580 printf("TCAS::ThreatDetector::update: radarAlt = %f\n",self.radarAltFt);
584 // determine current altitude's "Sensitivity Level Definition and Alarm Thresholds"
586 for (sl=0;((self.radarAltFt > sensitivityLevels[sl].maxAltitude)&&
587 (sensitivityLevels[sl].maxAltitude));sl++);
588 pAlarmThresholds = &sensitivityLevels[sl];
589 tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds);
592 /** Check if plane's transponder is enabled. */
594 TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float velocityKt)
596 const string name = pModel->getName();
597 if (name != "multiplayer" && name != "aircraft")
599 // assume non-MP/non-AI planes (e.g. ships) have no transponder
605 /* assume all pilots have their transponder switched off while taxiing/parking
610 if ((name == "multiplayer")&&
611 (pModel->getBoolValue("controls/invisible")))
613 // ignored MP plane: pretend transponder is switched off
620 /** Check if plane is a threat. */
622 TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
624 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
628 float velocityKt = pModel->getDoubleValue("velocities/true-airspeed-kt");
630 if (!checkTransponder(pModel, velocityKt))
631 return ThreatInvisible;
633 int threatLevel = ThreatNone;
634 float altFt = pModel->getDoubleValue("position/altitude-ft");
635 currentThreat.relativeAltitudeFt = altFt - self.pressureAltFt;
637 // save computation time: don't care when relative altitude is excessive
638 if (fabs(currentThreat.relativeAltitudeFt) > 10000)
641 // position data of current intruder
642 double lat = pModel->getDoubleValue("position/latitude-deg");
643 double lon = pModel->getDoubleValue("position/longitude-deg");
644 float heading = pModel->getDoubleValue("orientation/true-heading-deg");
646 double distanceNm, bearing;
647 calcRangeBearing(self.lat, self.lon, lat, lon, distanceNm, bearing);
649 // save computation time: don't care for excessive distances (also captures NaNs...)
650 if ((distanceNm > 10)||(distanceNm < 0))
653 currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps");
655 /* Detect proximity targets
656 * [TCASII]: "Any target that is less than 6 nmi in range and within +/-1200ft
657 * vertically, but that does not meet the intruder or threat criteria." */
658 if ((distanceNm < 6)&&
659 (fabs(currentThreat.relativeAltitudeFt) < 1200))
661 // at least a proximity target
662 threatLevel = ThreatProximity;
665 /* do not detect any threats when in standby or on ground and taxiing */
666 if ((mode <= SwitchStandby)||
667 ((self.radarAltFt < 360)&&(self.velocityKt < 40)))
672 if (tcas->tracker.active())
674 currentThreat.callsign = pModel->getStringValue("callsign");
675 currentThreat.isTracked = tcas->tracker.isTracked(currentThreat.callsign);
678 currentThreat.isTracked = false;
680 // first stage: vertical movement
681 checkVerticalThreat();
683 // stop processing when no vertical threat
684 if ((!currentThreat.verticalTA)&&
685 (!currentThreat.isTracked))
688 // second stage: horizontal movement
689 horizontalThreat(bearing, distanceNm, heading, velocityKt);
691 if (!currentThreat.isTracked)
693 // no horizontal threat?
694 if (!currentThreat.horizontalTA)
697 if ((currentThreat.horizontalTau < 0)||
698 (currentThreat.verticalTau < 0))
700 // do not trigger new alerts when Tau is negative, but keep existing alerts
701 int previousThreatLevel = pModel->getIntValue("tcas/threat-level", 0);
702 if (previousThreatLevel == 0)
707 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
708 cout << "#" << checkCount << ": " << pModel->getStringValue("callsign") << endl;
712 /* [TCASII]: "For either a TA or an RA to be issued, both the range and
713 * vertical criteria, in terms of tau or the fixed thresholds, must be
714 * satisfied only one of the criteria is satisfied, TCAS will not issue
716 if (currentThreat.horizontalTA && currentThreat.verticalTA)
717 threatLevel = ThreatTA;
718 if (currentThreat.horizontalRA && currentThreat.verticalRA)
719 threatLevel = ThreatRA;
721 if (!tcas->tracker.active())
722 currentThreat.callsign = pModel->getStringValue("callsign");
724 tcas->tracker.add(currentThreat.callsign, threatLevel);
726 // check existing threat level
727 if (currentThreat.isTracked)
729 int oldLevel = tcas->tracker.getThreatLevel(currentThreat.callsign);
730 if (oldLevel > threatLevel)
731 threatLevel = oldLevel;
734 // find all resolution options for this conflict
735 threatLevel = tcas->advisoryGenerator.resolution(mode, threatLevel, distanceNm, altFt, heading, velocityKt);
738 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
739 printf(" threat: distance: %4.1f, bearing: %4.1f, alt: %5.1f, velocity: %4.1f, heading: %4.1f, vspeed: %4.1f, "
740 "own alt: %5.1f, own heading: %4.1f, own velocity: %4.1f, vertical tau: %3.2f"
741 //", closing speed: %f"
743 distanceNm, relAngle(bearing, self.heading), altFt, velocityKt, heading, currentThreat.verticalFps,
744 self.altFt, self.heading, self.velocityKt
745 //, currentThreat.closingSpeedKt
746 ,currentThreat.verticalTau
753 /** Check if plane is a vertical threat. */
755 TCAS::ThreatDetector::checkVerticalThreat(void)
757 // calculate relative vertical speed and altitude
758 float dV = self.verticalFps - currentThreat.verticalFps;
759 float dA = currentThreat.relativeAltitudeFt;
761 currentThreat.verticalTA = false;
762 currentThreat.verticalRA = false;
763 currentThreat.verticalTau = 0;
765 /* [TCASII]: "The vertical tau is equal to the altitude separation (feet)
766 * divided by the combined vertical speed of the two aircraft (feet/minute)
772 /* [TCASII]: "When the combined vertical speed of the TCAS and the intruder aircraft
773 * is low, TCAS will use a fixed-altitude threshold to determine whether a TA or
774 * an RA should be issued." */
775 if ((fabs(dV) < 3.0)||
776 ((tau < 0) && (tau > -5)))
778 /* vertical closing speed is low (below 180fpm/3fps), check
779 * fixed altitude range. */
780 float abs_dA = fabs(dA);
781 if (abs_dA < pAlarmThresholds->RA.ALIM)
783 // continuous intrusion at RA-level
784 currentThreat.verticalTA = true;
785 currentThreat.verticalRA = true;
788 if (abs_dA < pAlarmThresholds->TA.ALIM)
790 // continuous intrusion: with TA-level, but no RA-threat
791 currentThreat.verticalTA = true;
793 // else: no RA/TA threat
797 if ((tau < pAlarmThresholds->TA.Tau)&&
800 currentThreat.verticalTA = true;
801 currentThreat.verticalRA = (tau < pAlarmThresholds->RA.Tau);
804 currentThreat.verticalTau = tau;
806 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
807 if (currentThreat.verticalTA)
808 printf(" vertical dV=%f (%f-%f), dA=%f\n", dV, self.verticalFps, currentThreat.verticalFps, dA);
812 /** Check if plane is a horizontal threat. */
814 TCAS::ThreatDetector::horizontalThreat(float bearing, float distanceNm, float heading, float velocityKt)
817 float vxKt = sin(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - sin(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
818 float vyKt = cos(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - cos(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
820 // calculate horizontal closing speed
821 float closingSpeedKt2 = vxKt*vxKt+vyKt*vyKt;
822 float closingSpeedKt = sqrt(closingSpeedKt2);
824 /* [TCASII]: "The range tau is equal to the slant range (nmi) divided by the closing speed
825 * (knots) multiplied by 3600."
826 * => calculate allowed slant range (nmi) based on known maximum tau */
827 float TA_rangeNm = (pAlarmThresholds->TA.Tau*closingSpeedKt)/3600;
828 float RA_rangeNm = (pAlarmThresholds->RA.Tau*closingSpeedKt)/3600;
830 if (closingSpeedKt < 100)
832 /* [TCASII]: "In events where the rate of closure is very low, [..]
833 * an intruder aircraft can come very close in range without crossing the
834 * range tau boundaries [..]. To provide protection in these types of
835 * advisories, the range tau boundaries are modified [..] to use
836 * a fixed-range threshold to issue TAs and RAs in these slow closure
838 TA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->TA.DMOD/100.0);
839 RA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->RA.DMOD/100.0);
841 if (TA_rangeNm < pAlarmThresholds->TA.DMOD)
842 TA_rangeNm = pAlarmThresholds->TA.DMOD;
843 if (RA_rangeNm < pAlarmThresholds->RA.DMOD)
844 RA_rangeNm = pAlarmThresholds->RA.DMOD;
846 currentThreat.horizontalTA = (distanceNm < TA_rangeNm);
847 currentThreat.horizontalRA = (distanceNm < RA_rangeNm);
848 currentThreat.horizontalTau = -1;
850 if ((currentThreat.horizontalRA)&&
851 (currentThreat.verticalRA))
853 /* an RA will be issued. Prepare extra data for the
854 * traffic resolution stage, i.e. calculate
855 * exact time tau to horizontal CPA.
858 /* relative position of intruder is
861 * horizontal distance to intruder is r(t)
862 * r(t) = sqrt( Sx(t)^2 + Sy(t)^2 )
863 * => horizontal CPA at time t=tau, where r(t) has minimum
864 * r2(t) := r^2(t) = Sx(t)^2 + Sy(t)^2
865 * since r(t)>0 for all t => minimum of r(t) is also minimum of r2(t)
866 * => (d/dt) r2(t) = r2'(t) is 0 for t=tau
867 * r2(t) = ((Sx(t)^2 + Sy(t))^2) = c + b*t + a*t^2
868 * => r2'(t) = b + a*2*t
870 * r2'(tau) = 0 = b + 2*a*tau
873 float sx = sin(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
874 float sy = cos(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
875 float vx = vxKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
876 float vy = vyKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
877 float a = vx*vx + vy*vy;
878 float b = 2*(sx*vx + sy*vy);
882 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
883 printf(" Time to horizontal CPA: %4.2f\n",tau);
885 if (tau > pAlarmThresholds->RA.Tau)
886 tau = pAlarmThresholds->RA.Tau;
888 // remember time to horizontal CPA
889 currentThreat.horizontalTau = tau;
893 /** Test threat detection logic. */
895 TCAS::ThreatDetector::unitTest(void)
897 pAlarmThresholds = &sensitivityLevels[1];
900 self.verticalFps = 0;
902 cout << "identical altitude and vspeed " << endl;
903 checkVerticalThreat(self.altFt, self.verticalFps);
904 cout << "1000ft alt offset, dV=100 " << endl;
905 checkVerticalThreat(self.altFt+1000, 100);
906 cout << "-1000ft alt offset, dV=100 " << endl;
907 checkVerticalThreat(self.altFt-1000, 100);
908 cout << "3000ft alt offset, dV=10 " << endl;
909 checkVerticalThreat(self.altFt+3000, 10);
910 cout << "500ft alt offset, dV=100 " << endl;
911 checkVerticalThreat(self.altFt+500, 100);
912 cout << "500ft alt offset, dV=-100 " << endl;
913 checkVerticalThreat(self.altFt+500, -100);
918 cout << "10nm behind, overtaking with 1Nm/s" << endl;
919 horizontalThreat(-180, 10, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
921 cout << "10nm ahead, departing with 1Nm/s" << endl;
922 horizontalThreat(0, 20, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
925 self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
926 cout << "10nm behind, overtaking with 1Nm/s at 90 degrees" << endl;
927 horizontalThreat(-90, 20, 90, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
930 self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
931 cout << "10nm behind, overtaking with 1Nm/s at 20 degrees" << endl;
932 horizontalThreat(200, 20, 20, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
936 ///////////////////////////////////////////////////////////////////////////////
937 // TCAS::AdvisoryGenerator ////////////////////////////////////////////////////
938 ///////////////////////////////////////////////////////////////////////////////
940 TCAS::AdvisoryGenerator::AdvisoryGenerator(TCAS* _tcas) :
943 pCurrentThreat(NULL),
944 pAlarmThresholds(NULL)
949 TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, ThreatInfo* _pCurrentThreat)
951 pCurrentThreat = _pCurrentThreat;
956 TCAS::AdvisoryGenerator::setAlarmThresholds(const SensitivityLevel* _pAlarmThresholds)
958 pAlarmThresholds = _pAlarmThresholds;
961 /** Calculate projected vertical separation at horizontal CPA. */
963 TCAS::AdvisoryGenerator::verticalSeparation(float newVerticalFps)
965 // calculate relative vertical speed and altitude
966 float dV = pCurrentThreat->verticalFps - newVerticalFps;
967 float tau = pCurrentThreat->horizontalTau;
968 // don't use negative tau to project future separation...
971 return pCurrentThreat->relativeAltitudeFt + tau * dV;
974 /** Determine RA sense. */
976 TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing)
978 /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
979 * geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
983 /* [TCASII]: "Based on the range and altitude tracks of the intruder, the CAS logic models the
984 * intruder's flight path from its present position to CPA. The CAS logic then models upward
985 * and downward sense RAs for own aircraft [..] to determine which sense provides the most
986 * vertical separation at CPA." */
987 float upSenseRelAltFt = verticalSeparation(+2000/60.0);
988 float downSenseRelAltFt = verticalSeparation(-2000/60.0);
989 if (fabs(upSenseRelAltFt) >= fabs(downSenseRelAltFt))
990 RASense = +1; // upward
992 RASense = -1; // downward
994 /* [TCASII]: "In encounters where either of the senses results in the TCAS aircraft crossing through
995 * the intruder's altitude, TCAS is designed to select the nonaltitude crossing sense if the
996 * noncrossing sense provides the desired vertical separation, known as ALIM, at CPA." */
997 /* [TCASII]: "If ALIM cannot be obtained in the nonaltitude crossing sense, an altitude
998 * crossing RA will be issued." */
1000 (pCurrentThreat->relativeAltitudeFt > 200))
1002 // threat is above and RA is crossing
1003 if (fabs(downSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
1005 // non-crossing descend is sufficient
1010 // keep crossing climb RA
1016 (pCurrentThreat->relativeAltitudeFt < -200))
1018 // threat is below and RA is crossing
1019 if (fabs(upSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
1021 // non-crossing climb is sufficient
1026 // keep crossing descent RA
1030 // else: threat is at same altitude, keep optimal RA sense (non-crossing)
1032 pCurrentThreat->RASense = RASense;
1034 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
1035 printf(" RASense: %i, crossing: %u, relAlt: %4.1f, upward separation: %4.1f, downward separation: %4.1f\n",
1037 pCurrentThreat->relativeAltitudeFt,
1038 upSenseRelAltFt,downSenseRelAltFt);
1042 /** Determine suitable resolution advisories. */
1044 TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, float altFt,
1045 float heading, float velocityKt)
1047 int RAOption = OptionNone;
1048 int RA = AdvisoryIntrusion;
1050 // RAs are disabled under certain conditions
1051 if (threatLevel == ThreatRA)
1053 /* [TCASII]: "... less than 360 feet, TCAS considers the reporting aircraft
1054 * to be on the ground. If TCAS determines the intruder to be on the ground, it
1055 * inhibits the generation of advisories against this aircraft."*/
1057 threatLevel = ThreatTA;
1059 /* [EUROACAS]: "Certain RAs are inhibited at altitudes based on inputs from the radio altimeter:
1060 * [..] (c)1000ft (+/- 100ft) and below, all RAs are inhibited;" */
1061 if (pSelf->radarAltFt < 1000)
1062 threatLevel = ThreatTA;
1064 // RAs only issued in mode "Auto" (= "TA/RA" mode)
1065 if (mode != SwitchAuto)
1066 threatLevel = ThreatTA;
1069 bool isCrossing = false;
1071 // determine suitable RAs
1072 if (threatLevel == ThreatRA)
1074 /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
1075 * geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
1076 determineRAsense(RASense, isCrossing);
1078 /* second step: determine required strength */
1083 if ((pSelf->verticalFps < -1000/60.0)&&
1086 // currently descending, see if reducing current descent is sufficient
1087 float relAltFt = verticalSeparation(-500/60.0);
1088 if (relAltFt > pAlarmThresholds->TA.ALIM)
1089 RA |= AdvisoryAdjustVSpeed;
1091 RA |= AdvisoryClimb;
1093 RAOption |= OptionCrossingClimb;
1100 if ((pSelf->verticalFps > 1000/60.0)&&
1103 // currently climbing, see if reducing current climb is sufficient
1104 float relAltFt = verticalSeparation(500/60.0);
1105 if (relAltFt < -pAlarmThresholds->TA.ALIM)
1106 RA |= AdvisoryAdjustVSpeed;
1108 RA |= AdvisoryDescend;
1110 RAOption |= OptionCrossingDescent;
1114 /* [TCASII]: "When two TCAS-equipped aircraft are converging vertically with opposite rates
1115 * and are currently well separated in altitude, TCAS will first issue a vertical speed
1116 * limit (Negative) RA to reinforce the pilots' likely intention to level off at adjacent
1117 * flight levels." */
1120 /* [TCASII]: "[..] if the CAS logic determines that the response to a Positive RA has provided
1121 * ALIM feet of vertical separation before CPA, the initial RA will be weakened to either a
1122 * Do Not Descend RA (after an initial Climb RA) or a Do Not Climb RA (after an initial
1126 /* [TCASII]: "TCAS is designed to inhibit Increase Descent RAs below 1450 feet AGL; */
1128 /* [TCASII]: "Descend RAs below 1100 feet AGL;" (inhibited) */
1129 if (pSelf->radarAltFt < 1100)
1131 RA &= ~AdvisoryDescend;
1132 //TODO Support "Do not descend" RA
1133 RA |= AdvisoryIntrusion;
1137 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
1138 cout << " resolution advisory: " << RA << endl;
1141 ResolutionAdvisory newAdvisory;
1142 newAdvisory.RAOption = RAOption;
1143 newAdvisory.RA = RA;
1144 newAdvisory.threatLevel = threatLevel;
1145 tcas->advisoryCoordinator.add(newAdvisory);
1150 ///////////////////////////////////////////////////////////////////////////////
1151 // TCAS ///////////////////////////////////////////////////////////////////////
1152 ///////////////////////////////////////////////////////////////////////////////
1154 TCAS::TCAS(SGPropertyNode* pNode) :
1159 properties_handler(this),
1160 threatDetector(this),
1162 advisoryCoordinator(this),
1163 advisoryGenerator(this),
1166 for (int i = 0; i < pNode->nChildren(); ++i)
1168 SGPropertyNode* pChild = pNode->getChild(i);
1169 string cname = pChild->getName();
1170 string cval = pChild->getStringValue();
1172 if (cname == "name")
1174 else if (cname == "number")
1175 num = pChild->getIntValue();
1178 SG_LOG(SG_INSTR, SG_WARN, "Error in TCAS config logic");
1180 SG_LOG(SG_INSTR, SG_WARN, "Section = " << name);
1189 advisoryCoordinator.init();
1190 threatDetector.init();
1197 advisoryCoordinator.reinit();
1203 SGPropertyNode* node = fgGetNode(("/instrumentation/" + name).c_str(), num, true);
1205 nodeServiceable = node->getNode("serviceable", true);
1207 // TCAS mode selection (0=off, 1=standby, 2=TA only, 3=auto(TA/RA) )
1208 nodeModeSwitch = node->getNode("inputs/mode", true);
1210 nodeSelfTest = node->getNode("inputs/self-test", true);
1212 nodeSelfTest->setBoolValue(false);
1214 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1215 SGPropertyNode* nodeDebug = node->getNode("debug", true);
1217 nodeDebugTrigger = nodeDebug->getNode("threat-trigger", true);
1218 nodeDebugRA = nodeDebug->getNode("threat-RA", true);
1219 nodeDebugThreat = nodeDebug->getNode("threat-level", true);
1221 nodeDebugTrigger->setBoolValue(false);
1222 nodeDebugRA->setIntValue(3);
1223 nodeDebugThreat->setIntValue(1);
1226 annunciator.bind(node);
1227 advisoryCoordinator.bind(node);
1233 properties_handler.unbind();
1236 /** Monitor traffic for safety threats. */
1238 TCAS::update(double dt)
1240 if (!nodeServiceable->getBoolValue())
1242 int mode = nodeModeSwitch->getIntValue();
1243 if (mode == SwitchOff)
1246 nextUpdateTime -= dt;
1247 if (nextUpdateTime <= 0.0 )
1249 nextUpdateTime = 1.0;
1251 // remove obsolete targets
1254 // get aircrafts current position/speed/heading
1255 threatDetector.update();
1257 // clear old threats
1258 advisoryCoordinator.clear();
1260 if (nodeSelfTest->getBoolValue())
1262 if (threatDetector.getVelocityKt() >= 40)
1264 // disable self-test when plane moves above taxiing speed
1265 nodeSelfTest->setBoolValue(false);
1271 // speed-up self test
1273 // no further TCAS processing during self-test
1278 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1279 if (nodeDebugTrigger->getBoolValue())
1282 ResolutionAdvisory debugAdvisory;
1283 debugAdvisory.RAOption = OptionNone;
1284 debugAdvisory.RA = nodeDebugRA->getIntValue();
1285 debugAdvisory.threatLevel = nodeDebugThreat->getIntValue();
1286 advisoryCoordinator.add(debugAdvisory);
1291 SGPropertyNode* pAi = fgGetNode("/ai/models", true);
1293 // check all aircraft
1294 for (int i = pAi->nChildren() - 1; i >= -1; i--)
1296 SGPropertyNode* pModel = pAi->getChild(i);
1297 if ((pModel)&&(pModel->nChildren()))
1299 int threatLevel = threatDetector.checkThreat(mode, pModel);
1300 /* expose aircraft threat-level (to be used by other instruments,
1301 * i.e. TCAS display) */
1302 if (threatLevel==ThreatRA)
1303 pModel->setIntValue("tcas/ra-sense", -threatDetector.getRASense());
1304 pModel->setIntValue("tcas/threat-level", threatLevel);
1308 advisoryCoordinator.update(mode);
1310 annunciator.update();
1313 /** Run a single self-test iteration. */
1315 TCAS::selfTest(void)
1317 annunciator.update();
1318 if (annunciator.isPlaying())
1323 ResolutionAdvisory newAdvisory;
1324 newAdvisory.threatLevel = ThreatRA;
1325 newAdvisory.RA = AdvisoryClear;
1326 newAdvisory.RAOption = OptionNone;
1327 // TCAS audio is disabled below 500ft AGL
1328 threatDetector.setRadarAlt(501);
1330 // trigger various advisories
1331 switch(selfTestStep)
1334 newAdvisory.RA = AdvisoryIntrusion;
1335 newAdvisory.threatLevel = ThreatTA;
1338 newAdvisory.RA = AdvisoryClimb;
1341 newAdvisory.RA = AdvisoryClimb;
1342 newAdvisory.RAOption = OptionIncreaseClimb;
1345 newAdvisory.RA = AdvisoryClimb;
1346 newAdvisory.RAOption = OptionCrossingClimb;
1349 newAdvisory.RA = AdvisoryDescend;
1352 newAdvisory.RA = AdvisoryDescend;
1353 newAdvisory.RAOption = OptionIncreaseDescend;
1356 newAdvisory.RA = AdvisoryDescend;
1357 newAdvisory.RAOption = OptionCrossingDescent;
1360 newAdvisory.RA = AdvisoryAdjustVSpeed;
1363 newAdvisory.RA = AdvisoryMaintVSpeed;
1366 newAdvisory.RA = AdvisoryMonitorVSpeed;
1369 newAdvisory.threatLevel = ThreatNone;
1370 newAdvisory.RA = AdvisoryClear;
1373 annunciator.test(true);
1377 nodeSelfTest->setBoolValue(false);
1382 advisoryCoordinator.add(newAdvisory);
1383 advisoryCoordinator.update(SwitchAuto);
1388 ///////////////////////////////////////////////////////////////////////////////
1389 // TCAS::Tracker //////////////////////////////////////////////////////////////
1390 ///////////////////////////////////////////////////////////////////////////////
1392 TCAS::Tracker::Tracker(TCAS*) :
1401 TCAS::Tracker::update(void)
1403 currentTime = globals->get_sim_time_sec();
1408 // remove outdated targets
1409 TrackerTargets::iterator it = targets.begin();
1410 while (it != targets.end())
1412 TrackerTarget* pTarget = it->second;
1413 if (currentTime - pTarget->TAtimestamp > 10.0)
1415 TrackerTargets::iterator temp = it;
1417 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1418 printf("target %s no longer a TA threat.\n",temp->first.c_str());
1420 targets.erase(temp->first);
1426 if ((pTarget->threatLevel == ThreatRA)&&
1427 (currentTime - pTarget->RAtimestamp > 7.0))
1429 pTarget->threatLevel = ThreatTA;
1430 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1431 printf("target %s no longer an RA threat.\n",it->first.c_str());
1437 haveTargets = !targets.empty();
1442 TCAS::Tracker::add(const string callsign, int detectedLevel)
1444 TrackerTarget* pTarget = NULL;
1447 TrackerTargets::iterator it = targets.find(callsign);
1448 if (it != targets.end())
1450 pTarget = it->second;
1456 pTarget = new TrackerTarget();
1457 pTarget->TAtimestamp = 0;
1458 pTarget->RAtimestamp = 0;
1459 pTarget->threatLevel = 0;
1461 targets[callsign] = pTarget;
1462 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1463 printf("new target: %s, level: %i\n",callsign.c_str(),detectedLevel);
1467 if (detectedLevel > pTarget->threatLevel)
1468 pTarget->threatLevel = detectedLevel;
1470 if (detectedLevel >= ThreatTA)
1471 pTarget->TAtimestamp = currentTime;
1473 if (detectedLevel >= ThreatRA)
1474 pTarget->RAtimestamp = currentTime;
1480 TCAS::Tracker::_isTracked(string callsign)
1482 return targets.find(callsign) != targets.end();
1486 TCAS::Tracker::getThreatLevel(string callsign)
1488 TrackerTargets::iterator it = targets.find(callsign);
1489 if (it != targets.end())
1490 return it->second->threatLevel;