From bdd931aed9c9ef68d24e7c8c4412716294c12fa9 Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Wed, 29 Dec 2010 22:04:19 +0100 Subject: [PATCH] Introduce new TCAS instrument - implements a TCAS II v7.0 --- src/Instrumentation/CMakeLists.txt | 1 + src/Instrumentation/Makefile.am | 3 +- src/Instrumentation/instrument_mgr.cxx | 4 + src/Instrumentation/tcas.cxx | 1303 ++++++++++++++++++++++++ src/Instrumentation/tcas.hxx | 377 +++++++ 5 files changed, 1687 insertions(+), 1 deletion(-) create mode 100644 src/Instrumentation/tcas.cxx create mode 100644 src/Instrumentation/tcas.hxx diff --git a/src/Instrumentation/CMakeLists.txt b/src/Instrumentation/CMakeLists.txt index d1cc59e3a..60d9457d3 100644 --- a/src/Instrumentation/CMakeLists.txt +++ b/src/Instrumentation/CMakeLists.txt @@ -31,6 +31,7 @@ set(SOURCES rnav_waypt_controller.cxx slip_skid_ball.cxx tacan.cxx + tcas.cxx transponder.cxx turn_indicator.cxx vertical_speed_indicator.cxx diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index f519f99a0..0edb85433 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -33,6 +33,7 @@ libInstrumentation_a_SOURCES = \ render_area_2d.cxx render_area_2d.hxx \ groundradar.cxx groundradar.hxx \ agradar.cxx agradar.hxx rad_alt.cxx rad_alt.hxx \ - rnav_waypt_controller.cxx rnav_waypt_controller.hxx + rnav_waypt_controller.cxx rnav_waypt_controller.hxx \ + tcas.cxx tcas.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src -I$(top_builddir)/src diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx index 19fcaeb06..58fe89db1 100644 --- a/src/Instrumentation/instrument_mgr.cxx +++ b/src/Instrumentation/instrument_mgr.cxx @@ -50,6 +50,7 @@ #include "groundradar.hxx" #include "agradar.hxx" #include "rad_alt.hxx" +#include "tcas.hxx" FGInstrumentMgr::FGInstrumentMgr () : _explicitGps(false) @@ -223,6 +224,9 @@ bool FGInstrumentMgr::build (SGPropertyNode* config_props) } else if ( name == "radar-altimeter" ) { set_subsystem( id, new radAlt( node ),1); + } else if ( name == "tcas" ) { + set_subsystem( id, new TCAS( node ) ); + } else { SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " << name ); diff --git a/src/Instrumentation/tcas.cxx b/src/Instrumentation/tcas.cxx new file mode 100644 index 000000000..6598dc011 --- /dev/null +++ b/src/Instrumentation/tcas.cxx @@ -0,0 +1,1303 @@ +// tcas.cxx -- Traffic Alert and Collision Avoidance System (TCAS) Emulation +// +// Written by Thorsten Brehm, started December 2010. +// +// Copyright (C) 2010 Thorsten Brehm - brehmt (at) gmail com +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// 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. +// +/////////////////////////////////////////////////////////////////////////////// + +/* References: + * + * [TCASII] Introduction to TCAS II Version 7, Federal Aviation Administration, November 2000 + * http://www.arinc.com/downloads/tcas/tcas.pdf + * + * [EUROACAS] Eurocontrol Airborne Collision Avoidance System (ACAS), + * http://www.eurocontrol.int/msa/public/standard_page/ACAS_Startpage.html + * + * Glossary: + * + * ALIM: Altitude Limit + * + * CPA: Closest point of approach as computed from a threat's range and range rate. + * + * DMOD: Distance MODification + * + * Intruder: A target that has satisfied the traffic detection criteria. + * + * Proximity target: Any target that is less than 6 nmi in range and within +/-1200ft + * vertically, but that does not meet the intruder or threat criteria. + * + * RA: Resolution advisory. An indication given by TCAS II to a flight crew that a + * vertical maneuver should, or in some cases should not, be performed to attain or + * maintain safe separation from a threat. + * + * SL: Sensitivity Level. A value used in defining the size of the protected volume + * around the own aircraft. + * + * TA: Traffic Advisory. An indication given by TCAS to the pilot when an aircraft has + * entered, or is projected to enter, the protected volume around the own aircraft. + * + * Tau: Approximation of the time, in seconds, to CPA or to the aircraft being at the + * same altitude. + * + * TCAS: Traffic alert and Collision Avoidance System + */ + +/* Module properties: + * + * serviceable enable/disable TCAS processing + * + * voice/file-prefix path (and optional prefix) for sound sample files + * (only evaluated at start-up) + * + * inputs/mode TCAS mode selection: 0=off,1=standby,2=TA only,3=auto(TA/RA) + * inputs/self-test trigger self-test sequence + * + * outputs/traffic-alert intruder detected (true=TA-threat is active, includes RA-threats) + * outputs/advisory-alert resolution advisory is issued (true=advisory is valid) + * outputs/vertical-speed vertical speed required by advisory (+/-2000/1500/500/0) + * + * speaker/max-dist Max. distance where speaker is heard + * speaker/reference-dist Distance to pilot + * speaker/volume Volume at reference distance + * + * debug/threat-trigger trigger debugging test (in debug mode only) + * debug/threat-RA debugging RA value (in debug mode only) + * debug/threat-level debugging threat level (in debug mode only) + */ + +#ifdef _MSC_VER +# pragma warning( disable: 4355 ) +#endif + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using std::string; + +#if defined( HAVE_VERSION_H ) && HAVE_VERSION_H +# include +#else +# include +#endif + +#include
+#include
+#include +#include "instrument_mgr.hxx" +#include "tcas.hxx" + +/////////////////////////////////////////////////////////////////////////////// +// debug switches ///////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// +//#define FEATURE_TCAS_DEBUG_ANNUNCIATOR +//#define FEATURE_TCAS_DEBUG_COORDINATOR +//#define FEATURE_TCAS_DEBUG_THREAT_DETECTOR +//#define FEATURE_TCAS_DEBUG_ADV_GENERATOR +//#define FEATURE_TCAS_DEBUG_PROPERTIES + +/////////////////////////////////////////////////////////////////////////////// +// constants ////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +/** Sensitivity Level Definition and Alarm Thresholds (TCASII, Version 7) + * Max Own | |TA-level |RA-level + * Altitude(ft) | SL |Tau(s),DMOD(nm),ALIM(ft)|Tau(s),DMOD(nm),ALIM(ft) */ +const TCAS::SensitivityLevel +TCAS::ThreatDetector::sensitivityLevels[] = { + { 1000, 2, {20, 0.30, 850}, {0, 0, 0 }}, + { 2350, 3, {25, 0.33, 850}, {15, 0.20, 300}}, + { 5000, 4, {30, 0.48, 850}, {20, 0.35, 300}}, + {10000, 5, {40, 0.75, 850}, {25, 0.55, 350}}, + {20000, 6, {45, 1.00, 850}, {30, 0.80, 400}}, + {42000, 7, {48, 1.30, 850}, {35, 1.10, 600}}, + {0, 8, {48, 1.30, 1200}, {35, 1.10, 700}} +}; + +/////////////////////////////////////////////////////////////////////////////// +// helpers //////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +#define ADD_VOICE(Var, Sample, SayTwice) \ + { make_voice(&Var); \ + append(Var, Sample); \ + if (SayTwice) append(Var, Sample); } + +#define AVAILABLE_RA(Options, Advisory) (Advisory == (Advisory & Options)) + +#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR +/** calculate relative angle in between two headings */ +static float +relAngle(float Heading1, float Heading2) +{ + Heading1 -= Heading2; + + while (Heading1 >= 360.0) + Heading1 -= 360.0; + + while (Heading1 < 0.0) + Heading1 += 360; + + return Heading1; +} +#endif + +/** calculate range and bearing of lat2/lon2 relative to lat1/lon1 */ +static void +calcRangeBearing(double lat1, double lon1, double lat2, double lon2, + double &rangeNm, double &bearing) +{ + // calculate the bearing and range of the second pos from the first + double az2, distanceM; + geo_inverse_wgs_84(lat1, lon1, lat2, lon2, &bearing, &az2, &distanceM); + rangeNm = distanceM * SG_METER_TO_NM; +} + +/////////////////////////////////////////////////////////////////////////////// +// VoicePlayer //////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +void +TCAS::VoicePlayer::init(void) +{ + FGVoicePlayer::init(); + + ADD_VOICE(Voices.pTrafficTraffic, "traffic", true); + ADD_VOICE(Voices.pClear, "clear", false); + ADD_VOICE(Voices.pClimb, "climb", true); + ADD_VOICE(Voices.pClimbNow, "climb_now", true); + ADD_VOICE(Voices.pClimbCrossing, "climb_crossing", true); + ADD_VOICE(Voices.pClimbIncrease, "increase_climb", false); + ADD_VOICE(Voices.pDescend, "descend", true); + ADD_VOICE(Voices.pDescendNow, "descend_now", true); + ADD_VOICE(Voices.pDescendCrossing,"descend_crossing", true); + ADD_VOICE(Voices.pDescendIncrease,"increase_descent", false); + ADD_VOICE(Voices.pAdjustVSpeed, "adjust_vertical_speed", false); + ADD_VOICE(Voices.pMaintVSpeed, "maintain_vertical_speed", false); + ADD_VOICE(Voices.pMonitorVSpeed, "monitor_vertical_speed", false); + ADD_VOICE(Voices.pLevelOff, "level_off", false); + ADD_VOICE(Voices.pTestOk, "test_ok", false); + ADD_VOICE(Voices.pTestFail, "test_fail", false); + + speaker.update_configuration(); +} + +///////////////////////////////////////////////////////////////////////////// +// TCAS::Annunciator //////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////// + +TCAS::Annunciator::Annunciator(TCAS* tcas) : + pLastVoice(NULL), + voicePlayer(tcas) +{ + clear(); +} + +void TCAS::Annunciator::clear(void) +{ + previous.threatLevel = ThreatNone; + previous.RA = AdvisoryClear; + previous.RAOption = OptionNone; + pLastVoice = NULL; +} + +void +TCAS::Annunciator::bind(SGPropertyNode* node) +{ + voicePlayer.bind(node, "Sounds/tcas/"); +} + +void +TCAS::Annunciator::init(void) +{ + //TODO link to GPWS module/audio-on signal must be configurable + nodeGpwsAlertOn = fgGetNode("/instrumentation/mk-viii/outputs/discretes/audio-on", true); + voicePlayer.init(); +} + +void +TCAS::Annunciator::update(void) +{ + voicePlayer.update(); + + /* [TCASII]: "The priority scheme gives ground proximity warning systems (GPWS) + * a higher annunciation priority than a TCAS alert. TCAS aural annunciation will + * be inhibited during the time that a GPWS alert is active." */ + if (nodeGpwsAlertOn->getBoolValue()) + voicePlayer.pause(); + else + voicePlayer.resume(); +} + +/** Trigger voice sample for current alert. */ +void +TCAS::Annunciator::trigger(const ResolutionAdvisory& current, bool revertedRA) +{ + int RA = current.RA; + int RAOption = current.RAOption; + + if (RA == AdvisoryClear) + { + if (previous.RA != AdvisoryClear) + { + voicePlayer.play(voicePlayer.Voices.pClear, VoicePlayer::PLAY_NOW); + previous = current; + } + pLastVoice = NULL; + return; + } + + if (previous.RA == AdvisoryClear) + { + voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW); + } + + // pick voice sample + VoicePlayer::Voice* pVoice = NULL; + switch(RA) + { + case AdvisoryClimb: + if (revertedRA) + pVoice = voicePlayer.Voices.pClimbNow; + else + if (AVAILABLE_RA(RAOption, OptionIncreaseClimb)) + pVoice = voicePlayer.Voices.pClimbIncrease; + else + if (AVAILABLE_RA(RAOption, OptionCrossingClimb)) + pVoice = voicePlayer.Voices.pClimbCrossing; + else + pVoice = voicePlayer.Voices.pClimb; + break; + + case AdvisoryDescend: + if (revertedRA) + pVoice = voicePlayer.Voices.pDescendNow; + else + if (AVAILABLE_RA(RAOption, OptionIncreaseDescend)) + pVoice = voicePlayer.Voices.pDescendIncrease; + else + if (AVAILABLE_RA(RAOption, OptionCrossingDescent)) + pVoice = voicePlayer.Voices.pDescendCrossing; + else + pVoice = voicePlayer.Voices.pDescend; + break; + + case AdvisoryAdjustVSpeed: + pVoice = voicePlayer.Voices.pAdjustVSpeed; + break; + + case AdvisoryMaintVSpeed: + pVoice = voicePlayer.Voices.pMaintVSpeed; + break; + + case AdvisoryMonitorVSpeed: + pVoice = voicePlayer.Voices.pMonitorVSpeed; + break; + + case AdvisoryLevelOff: + pVoice = voicePlayer.Voices.pLevelOff; + break; + + case AdvisoryIntrusion: + break; + + default: + RA = AdvisoryIntrusion; + break; + } + + previous = current; + + if (pLastVoice == pVoice) + { + // don't repeat annunciation + return; + } + pLastVoice = pVoice; + if (pVoice) + voicePlayer.play(pVoice); + +#ifdef FEATURE_TCAS_DEBUG_ANNUNCIATOR + cout << "Annunciating TCAS RA " << RA << endl; +#endif +} + +void +TCAS::Annunciator::test(bool testOk) +{ + if (testOk) + voicePlayer.play(voicePlayer.Voices.pTestOk); + else + voicePlayer.play(voicePlayer.Voices.pTestFail); +} + +///////////////////////////////////////////////////////////////////////////// +// TCAS::AdvisoryCoordinator //////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////// + +TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) : + tcas(_tcas), + lastTATime(0) +{ + init(); +} + +void +TCAS::AdvisoryCoordinator::init(void) +{ + clear(); + previous = current; +} + +void +TCAS::AdvisoryCoordinator::bind(SGPropertyNode* node) +{ + nodeTAWarning = node->getNode("outputs/traffic-alert", true); + nodeTAWarning->setBoolValue(false); +} + +void +TCAS::AdvisoryCoordinator::clear(void) +{ + current.threatLevel = ThreatNone; + current.RA = AdvisoryClear; + current.RAOption = OptionNone; +} + +/** Add all suitable resolution advisories for a single threat. */ +void +TCAS::AdvisoryCoordinator::add(const ResolutionAdvisory& newAdvisory) +{ + if (newAdvisory.RA == AdvisoryClear) + return; + + if ((current.RA == AdvisoryClear)|| + (current.threatLevel == ThreatTA)) + { + current = newAdvisory; + } + else + { + // combine with other advisories so far + current.RA &= newAdvisory.RA; + // remember any advisory modifier + current.RAOption |= newAdvisory.RAOption; + } +} + +/** Pick and trigger suitable resolution advisory. */ +void +TCAS::AdvisoryCoordinator::update(int mode) +{ + bool revertedRA = false; // has advisory changed? + double currentTime = globals->get_sim_time_sec(); + + if (current.RA == AdvisoryClear) + { + // filter: wait 5 seconds after last TA/RA before announcing TA clearance + if ((previous.RA != AdvisoryClear)&& + (currentTime - lastTATime < 5.0)) + return; + } + else + { + // intruder detected + +#ifdef FEATURE_TCAS_DEBUG_COORDINATOR + cout << "TCAS::Annunciator::update: previous: " << previous.RA << ", new: " << current.RA << endl; +#endif + + lastTATime = currentTime; + if ((previous.RA == AdvisoryClear)|| + (previous.RA == AdvisoryIntrusion)|| + ((current.RA & previous.RA) != previous.RA)) + { + // no RA yet, or we can't keep previous RA: pick one - in order of priority + + if (AVAILABLE_RA(current.RA, AdvisoryMonitorVSpeed)) + { + // prio 1: monitor vertical speed only + current.RA = AdvisoryMonitorVSpeed; + } + else + if (AVAILABLE_RA(current.RA, AdvisoryMaintVSpeed)) + { + // prio 2: maintain vertical speed + current.RA = AdvisoryMaintVSpeed; + } + else + if (AVAILABLE_RA(current.RA, AdvisoryAdjustVSpeed)) + { + // prio 3: adjust vertical speed (TCAS II 7.0 only) + current.RA = AdvisoryAdjustVSpeed; + } + else + if (AVAILABLE_RA(current.RA, AdvisoryLevelOff)) + { + // prio 3: adjust vertical speed (TCAS II 7.1 only, [EUROACAS]: CP115) + current.RA = AdvisoryLevelOff; + } + else + if (AVAILABLE_RA(current.RA, AdvisoryClimb)) + { + // prio 4: climb + current.RA = AdvisoryClimb; + } + else + if (AVAILABLE_RA(current.RA, AdvisoryDescend)) + { + // prio 5: descend + current.RA = AdvisoryDescend; + } + else + { + // no RA, issue a TA only + current.RA = AdvisoryIntrusion; + } + + // check if earlier advisory was reverted + revertedRA = ((previous.RA != current.RA)&& + (previous.RA != 0)&& + (previous.RA != AdvisoryIntrusion)); + } + else + { + // keep earlier RA + current.RA = previous.RA; + } + } + + /* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */ + if ((tcas->threatDetector.getAlt() > 500)&& + (mode >= SwitchTaOnly)) + tcas->annunciator.trigger(current, revertedRA); + else + if (current.RA == AdvisoryClear) + { + /* explicitly clear traffic alert (since aural annunciation disabled) */ + tcas->annunciator.clear(); + } + + previous = current; + + /* [TCASII] "[..] also performs the function of setting flags that control the displays. + * The traffic display, the RA display, [..] use these flags to alert the pilot to + * the presence of TAs and RAs." */ + nodeTAWarning->setBoolValue(current.RA != AdvisoryClear); +} + +/////////////////////////////////////////////////////////////////////////////// +// TCAS::ThreatDetector /////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) : + tcas(_tcas), + pAlarmThresholds(&sensitivityLevels[0]) +{ + unitTest(); +} + +void +TCAS::ThreatDetector::init(void) +{ + nodeLat = fgGetNode("/position/latitude-deg", true); + nodeLon = fgGetNode("/position/longitude-deg", true); + nodeAlt = fgGetNode("/position/altitude-ft", true); + nodeHeading = fgGetNode("/orientation/heading-deg", true); + nodeVelocity = fgGetNode("/velocities/airspeed-kt", true); + nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps", true); + + tcas->advisoryGenerator.init(&self,¤tThreat); +} + +/** Update local position and threat sensitivity levels. */ +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(); + + checkCount = 0; + + // determine current altitude's "Sensitivity Level Definition and Alarm Thresholds" + int sl=0; + for (sl=0;((self.altFt > sensitivityLevels[sl].maxAltitude)&& + (sensitivityLevels[sl].maxAltitude));sl++); + pAlarmThresholds = &sensitivityLevels[sl]; + tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds); +} + +/** Check if plane's transponder is enabled. */ +bool +TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float velocityKt) +{ + const string name = pModel->getName(); + if (name != "multiplayer" && name != "aircraft") + { + // assume non-MP/non-AI planes (e.g. ships) have no transponder + return false; + } + + if (velocityKt < 40) + { + /* assume all pilots have their transponder switched off while taxiing/parking + * (at low speed) */ + return false; + } + + return true; +} + +/** Check if plane is a threat. */ +int +TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel) +{ + checkCount++; + + float velocityKt = pModel->getDoubleValue("velocities/true-airspeed-kt"); + + if (!checkTransponder(pModel, velocityKt)) + return ThreatInvisible; + + int threatLevel = ThreatNone; + float altFt = pModel->getDoubleValue("position/altitude-ft"); + currentThreat.relativeAltitudeFt = altFt - self.altFt; + + // save computation time: don't care when relative altitude is excessive + if (fabs(currentThreat.relativeAltitudeFt) > 10000) + return threatLevel; + + // position data of current intruder + double lat = pModel->getDoubleValue("position/latitude-deg"); + double lon = pModel->getDoubleValue("position/longitude-deg"); + float heading = pModel->getDoubleValue("orientation/true-heading-deg"); + + double distanceNm, bearing; + calcRangeBearing(self.lat, self.lon, lat, lon, distanceNm, bearing); + + // save computation time: don't care for excessive distances (also captures NaNs...) + if ((distanceNm > 10)||(distanceNm < 0)) + return threatLevel; + + // first stage: vertical movement + currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps"); + + /* Detect proximity targets + * [TCASII]: "Any target that is less than 6 nmi in range and within +/-1200ft + * vertically, but that does not meet the intruder or threat criteria." */ + if ((distanceNm < 6)&& + (fabs(currentThreat.relativeAltitudeFt) < 1200)) + { + // at least a proximity target + threatLevel = ThreatProximity; + } + + checkVerticalThreat(); + + // stop processing when no vertical threat + if (!currentThreat.verticalTA) + 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)) + { + // 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 + * 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.horizontalRA && currentThreat.verticalRA) + threatLevel = ThreatRA; + + // find all resolution options for this conflict + 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" + //", closing speed: %f" + "\n", + distanceNm, relAngle(bearing, self.heading), altFt, velocityKt, heading, currentThreat.verticalFps, + self.altFt, self.heading, self.velocityKt + //, currentThreat.closingSpeedKt + ,currentThreat.verticalTau + ); +#endif + + return threatLevel; +} + +/** Check if plane is a vertical threat. */ +void +TCAS::ThreatDetector::checkVerticalThreat(void) +{ + // calculate relative vertical speed and altitude + float dV = self.verticalFps - currentThreat.verticalFps; + float dA = currentThreat.relativeAltitudeFt; + + currentThreat.verticalTA = false; + currentThreat.verticalRA = false; + currentThreat.verticalTau = 0; + + /* [TCASII]: "The vertical tau is equal to the altitude separation (feet) + * divided by the combined vertical speed of the two aircraft (feet/minute) + * times 60." */ + float tau = 0; + if (fabs(dV) > 0.1) + tau = dA/dV; + + /* [TCASII]: "When the combined vertical speed of the TCAS and the intruder aircraft + * is low, TCAS will use a fixed-altitude threshold to determine whether a TA or + * an RA should be issued." */ + if ((fabs(dV) < 3.0)|| + ((tau < 0) && (tau > -5))) + { + /* vertical closing speed is low (below 180fpm/3fps), check + * fixed altitude range. */ + float abs_dA = fabs(dA); + if (abs_dA < pAlarmThresholds->RA.ALIM) + { + // continuous intrusion at RA-level + currentThreat.verticalTA = true; + currentThreat.verticalRA = true; + } + else + if (abs_dA < pAlarmThresholds->TA.ALIM) + { + // continuous intrusion: with TA-level, but no RA-threat + currentThreat.verticalTA = true; + } + // else: no RA/TA threat + } + else + { + if ((tau < pAlarmThresholds->TA.Tau)&& + (tau >= -5)) + { + currentThreat.verticalTA = true; + currentThreat.verticalRA = (tauRA.Tau); + } + } + currentThreat.verticalTau = tau; + +#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR + if (currentThreat.verticalTA) + printf(" vertical dV=%f (%f-%f), dA=%f\n", dV, self.verticalFps, currentThreat.verticalFps, dA); +#endif +} + +/** Check if plane is a horizontal threat. */ +void +TCAS::ThreatDetector::horizontalThreat(float bearing, float distanceNm, float heading, float velocityKt) +{ + // calculate speed + float vxKt = sin(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - sin(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt; + float vyKt = cos(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - cos(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt; + + // calculate horizontal closing speed + float closingSpeedKt2 = vxKt*vxKt+vyKt*vyKt; + float closingSpeedKt = sqrt(closingSpeedKt2); + + /* [TCASII]: "The range tau is equal to the slant range (nmi) divided by the closing speed + * (knots) multiplied by 3600." + * => calculate allowed slant range (nmi) based on known maximum tau */ + float TA_rangeNm = (pAlarmThresholds->TA.Tau*closingSpeedKt)/3600; + float RA_rangeNm = (pAlarmThresholds->RA.Tau*closingSpeedKt)/3600; + + if (closingSpeedKt < 100) + { + /* [TCASII]: "In events where the rate of closure is very low, [..] + * an intruder aircraft can come very close in range without crossing the + * range tau boundaries [..]. To provide protection in these types of + * advisories, the range tau boundaries are modified [..] to use + * a fixed-range threshold to issue TAs and RAs in these slow closure + * encounters." */ + TA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->TA.DMOD/100.0); + RA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->RA.DMOD/100.0); + } + if (TA_rangeNm < pAlarmThresholds->TA.DMOD) + TA_rangeNm = pAlarmThresholds->TA.DMOD; + if (RA_rangeNm < pAlarmThresholds->RA.DMOD) + RA_rangeNm = pAlarmThresholds->RA.DMOD; + + currentThreat.horizontalTA = (distanceNm < TA_rangeNm); + currentThreat.horizontalRA = (distanceNm < RA_rangeNm); + currentThreat.horizontalTau = -1; + + if ((currentThreat.horizontalRA)&& + (currentThreat.verticalRA)) + { + /* an RA will be issued. Prepare extra data for the + * traffic resolution stage, i.e. calculate + * exact time tau to horizontal CPA. + */ + + /* relative position of intruder is + * Sx(t) = sx + vx*t + * Sy(t) = sy + vy*t + * horizontal distance to intruder is r(t) + * r(t) = sqrt( Sx(t)^2 + Sy(t)^2 ) + * => horizontal CPA at time t=tau, where r(t) has minimum + * r2(t) := r^2(t) = Sx(t)^2 + Sy(t)^2 + * since r(t)>0 for all t => minimum of r(t) is also minimum of r2(t) + * => (d/dt) r2(t) = r2'(t) is 0 for t=tau + * r2(t) = ((Sx(t)^2 + Sy(t))^2) = c + b*t + a*t^2 + * => r2'(t) = b + a*2*t + * at t=tau: + * r2'(tau) = 0 = b + 2*a*tau + * => tau = -b/(2*a) + */ + float sx = sin(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm; + float sy = cos(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm; + float vx = vxKt * (SG_KT_TO_MPS*SG_METER_TO_NM); + float vy = vyKt * (SG_KT_TO_MPS*SG_METER_TO_NM); + float a = vx*vx + vy*vy; + float b = 2*(sx*vx + sy*vy); + float tau = 0; + if (a > 0.0001) + tau = -b/(2*a); +#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR + printf(" Time to horizontal CPA: %4.2f\n",tau); +#endif + if (tau > pAlarmThresholds->RA.Tau) + tau = pAlarmThresholds->RA.Tau; + + // remember time to horizontal CPA + currentThreat.horizontalTau = tau; + } +} + +/** Test threat detection logic. */ +void +TCAS::ThreatDetector::unitTest(void) +{ + pAlarmThresholds = &sensitivityLevels[1]; +#if 0 + // vertical tests + self.verticalFps = 0; + self.altFt = 1000; + cout << "identical altitude and vspeed " << endl; + checkVerticalThreat(self.altFt, self.verticalFps); + cout << "1000ft alt offset, dV=100 " << endl; + checkVerticalThreat(self.altFt+1000, 100); + cout << "-1000ft alt offset, dV=100 " << endl; + checkVerticalThreat(self.altFt-1000, 100); + cout << "3000ft alt offset, dV=10 " << endl; + checkVerticalThreat(self.altFt+3000, 10); + cout << "500ft alt offset, dV=100 " << endl; + checkVerticalThreat(self.altFt+500, 100); + cout << "500ft alt offset, dV=-100 " << endl; + checkVerticalThreat(self.altFt+500, -100); + + // horizontal tests + self.heading = 0; + self.velocityKt = 0; + cout << "10nm behind, overtaking with 1Nm/s" << endl; + horizontalThreat(-180, 10, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM)); + + cout << "10nm ahead, departing with 1Nm/s" << endl; + horizontalThreat(0, 20, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM)); + + self.heading = 90; + self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM); + cout << "10nm behind, overtaking with 1Nm/s at 90 degrees" << endl; + horizontalThreat(-90, 20, 90, 2/(SG_KT_TO_MPS*SG_METER_TO_NM)); + + self.heading = 20; + self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM); + cout << "10nm behind, overtaking with 1Nm/s at 20 degrees" << endl; + horizontalThreat(200, 20, 20, 2/(SG_KT_TO_MPS*SG_METER_TO_NM)); +#endif +} + +/////////////////////////////////////////////////////////////////////////////// +// TCAS::AdvisoryGenerator //////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +TCAS::AdvisoryGenerator::AdvisoryGenerator(TCAS* _tcas) : + tcas(_tcas), + pSelf(NULL), + pCurrentThreat(NULL), + pAlarmThresholds(NULL) +{ +} + +void +TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat) +{ + pCurrentThreat = _pCurrentThreat; + pSelf = _pSelf; +} + +void +TCAS::AdvisoryGenerator::setAlarmThresholds(const SensitivityLevel* _pAlarmThresholds) +{ + pAlarmThresholds = _pAlarmThresholds; +} + +/** Calculate projected vertical separation at horizontal CPA. */ +float +TCAS::AdvisoryGenerator::verticalSeparation(float newVerticalFps) +{ + // calculate relative vertical speed and altitude + float dV = pCurrentThreat->verticalFps - newVerticalFps; + float tau = pCurrentThreat->horizontalTau; + // don't use negative tau to project future separation... + if (tau < 0.5) + tau = 0.5; + return pCurrentThreat->relativeAltitudeFt + tau * dV; +} + +/** Determine RA sense. */ +void +TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing) +{ + /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter + * geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */ + RASense = 0; + isCrossing = false; + + /* [TCASII]: "Based on the range and altitude tracks of the intruder, the CAS logic models the + * intruder's flight path from its present position to CPA. The CAS logic then models upward + * and downward sense RAs for own aircraft [..] to determine which sense provides the most + * vertical separation at CPA." */ + float upSenseRelAltFt = verticalSeparation(+2000/60.0); + float downSenseRelAltFt = verticalSeparation(-2000/60.0); + if (fabs(upSenseRelAltFt) >= fabs(downSenseRelAltFt)) + RASense = +1; // upward + else + RASense = -1; // downward + + /* [TCASII]: "In encounters where either of the senses results in the TCAS aircraft crossing through + * the intruder's altitude, TCAS is designed to select the nonaltitude crossing sense if the + * noncrossing sense provides the desired vertical separation, known as ALIM, at CPA." */ + /* [TCASII]: "If ALIM cannot be obtained in the nonaltitude crossing sense, an altitude + * crossing RA will be issued." */ + if ((RASense > 0)&& + (pCurrentThreat->relativeAltitudeFt > 200)) + { + // threat is above and RA is crossing + if (fabs(downSenseRelAltFt) > pAlarmThresholds->TA.ALIM) + { + // non-crossing descend is sufficient + RASense = -1; + } + else + { + // keep crossing climb RA + isCrossing = true; + } + } + else + if ((RASense < 0)&& + (pCurrentThreat->relativeAltitudeFt < -200)) + { + // threat is below and RA is crossing + if (fabs(upSenseRelAltFt) > pAlarmThresholds->TA.ALIM) + { + // non-crossing climb is sufficient + RASense = 1; + } + else + { + // keep crossing descent RA + isCrossing = true; + } + } + // else: threat is at same altitude, keep optimal RA sense (non-crossing) + +#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, + pCurrentThreat->relativeAltitudeFt, + upSenseRelAltFt,downSenseRelAltFt); +#endif +} + +/** Determine suitable resolution advisories. */ +int +TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, float altFt, + float heading, float velocityKt) +{ + int RAOption = OptionNone; + int RA = AdvisoryIntrusion; + + // RAs are disabled under certain conditions + if (threatLevel == ThreatRA) + { + /* [TCASII]: "... less than 360 feet, TCAS considers the reporting aircraft + * to be on the ground. If TCAS determines the intruder to be on the ground, it + * inhibits the generation of advisories against this aircraft."*/ + if (altFt < 360) + threatLevel = ThreatTA; + + /* [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) + threatLevel = ThreatTA; + + // RAs only issued in mode "Auto" (= "TA/RA" mode) + if (mode != SwitchAuto) + threatLevel = ThreatTA; + } + + bool isCrossing = false; + int RASense = 0; + // determine suitable RAs + if (threatLevel == ThreatRA) + { + /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter + * geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */ + determineRAsense(RASense, isCrossing); + + /* second step: determine required strength */ + if (RASense > 0) + { + // upward + + if ((pSelf->verticalFps < -1000/60.0)&& + (!isCrossing)) + { + // currently descending, see if reducing current descent is sufficient + float relAltFt = verticalSeparation(-500/60.0); + if (relAltFt > pAlarmThresholds->TA.ALIM) + RA |= AdvisoryAdjustVSpeed; + } + RA |= AdvisoryClimb; + if (isCrossing) + RAOption |= OptionCrossingClimb; + } + + if (RASense < 0) + { + // downward + + if ((pSelf->verticalFps > 1000/60.0)&& + (!isCrossing)) + { + // currently climbing, see if reducing current climb is sufficient + float relAltFt = verticalSeparation(500/60.0); + if (relAltFt < -pAlarmThresholds->TA.ALIM) + RA |= AdvisoryAdjustVSpeed; + } + RA |= AdvisoryDescend; + if (isCrossing) + RAOption |= OptionCrossingDescent; + } + + //TODO + /* [TCASII]: "When two TCAS-equipped aircraft are converging vertically with opposite rates + * and are currently well separated in altitude, TCAS will first issue a vertical speed + * limit (Negative) RA to reinforce the pilots' likely intention to level off at adjacent + * flight levels." */ + + //TODO + /* [TCASII]: "[..] if the CAS logic determines that the response to a Positive RA has provided + * ALIM feet of vertical separation before CPA, the initial RA will be weakened to either a + * Do Not Descend RA (after an initial Climb RA) or a Do Not Climb RA (after an initial + * Descend RA)." */ + + //TODO + /* [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) + { + RA &= ~AdvisoryDescend; + //TODO Support "Do not descend" RA + RA |= AdvisoryIntrusion; + } + } + +#ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR + cout << " resolution advisory: " << RA << endl; +#endif + + ResolutionAdvisory newAdvisory; + newAdvisory.RAOption = RAOption; + newAdvisory.RA = RA; + newAdvisory.threatLevel = threatLevel; + tcas->advisoryCoordinator.add(newAdvisory); + + return RA; +} + +/////////////////////////////////////////////////////////////////////////////// +// TCAS /////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +TCAS::TCAS(SGPropertyNode* pNode) : + name("tcas"), + num(0), + nextUpdateTime(0), + selfTestStep(0), + properties_handler(this), + threatDetector(this), + advisoryCoordinator(this), + advisoryGenerator(this), + annunciator(this) +{ + for (int i = 0; i < pNode->nChildren(); ++i) + { + SGPropertyNode* pChild = pNode->getChild(i); + string cname = pChild->getName(); + string cval = pChild->getStringValue(); + + if (cname == "name") + name = cval; + else if (cname == "number") + num = pChild->getIntValue(); + else + { + SG_LOG(SG_INSTR, SG_WARN, "Error in TCAS config logic"); + if (name.length()) + SG_LOG(SG_INSTR, SG_WARN, "Section = " << name); + } + } +} + +void +TCAS::init(void) +{ + annunciator.init(); + advisoryCoordinator.init(); + threatDetector.init(); +} + +void +TCAS::bind(void) +{ + SGPropertyNode* node = fgGetNode(("/instrumentation/" + name).c_str(), num, true); + + nodeServiceable = node->getNode("serviceable", true); + + // TCAS mode selection (0=off, 1=standby, 2=TA only, 3=auto(TA/RA) ) + nodeModeSwitch = node->getNode("inputs/mode", true); + // self-test button + nodeSelfTest = node->getNode("inputs/self-test", true); + // default value + nodeSelfTest->setBoolValue(false); + +#ifdef FEATURE_TCAS_DEBUG_PROPERTIES + SGPropertyNode* nodeDebug = node->getNode("debug", true); + // debug triggers + nodeDebugTrigger = nodeDebug->getNode("threat-trigger", true); + nodeDebugRA = nodeDebug->getNode("threat-RA", true); + nodeDebugThreat = nodeDebug->getNode("threat-level", true); + // default values + nodeDebugTrigger->setBoolValue(false); + nodeDebugRA->setIntValue(3); + nodeDebugThreat->setIntValue(1); +#endif + + annunciator.bind(node); + advisoryCoordinator.bind(node); +} + +void +TCAS::unbind(void) +{ + properties_handler.unbind(); +} + +/** Monitor traffic for safety threats. */ +void +TCAS::update(double dt) +{ + if (!nodeServiceable->getBoolValue()) + return; + int mode = nodeModeSwitch->getIntValue(); + if (mode == SwitchOff) + return; + + nextUpdateTime -= dt; + if (nextUpdateTime <= 0.0 ) + { + nextUpdateTime = 1.0; + + // get aircrafts current position/speed/heading + threatDetector.update(); + + // clear old threats + advisoryCoordinator.clear(); + + if (nodeSelfTest->getBoolValue()) + { + if (threatDetector.getVelocityKt() >= 40) + { + // disable self-test when plane moves above taxiing speed + nodeSelfTest->setBoolValue(false); + selfTestStep = 0; + } + else + { + selfTest(); + // speed-up self test + nextUpdateTime = 0; + // no further TCAS processing during self-test + return; + } + } + +#ifdef FEATURE_TCAS_DEBUG_PROPERTIES + if (nodeDebugTrigger->getBoolValue()) + { + // debugging test + ResolutionAdvisory debugAdvisory; + debugAdvisory.RAOption = OptionNone; + debugAdvisory.RA = nodeDebugRA->getIntValue(); + debugAdvisory.threatLevel = nodeDebugThreat->getIntValue(); + advisoryCoordinator.add(debugAdvisory); + } + else +#endif + { + SGPropertyNode* pAi = fgGetNode("/ai/models", true); + + // check all aircraft + for (int i = pAi->nChildren() - 1; i >= -1; i--) + { + SGPropertyNode* pModel = pAi->getChild(i); + if ((pModel)&&(pModel->nChildren())) + { + int threatLevel = threatDetector.checkThreat(mode, pModel); + /* expose aircraft threat-level (to be used by other instruments, + * i.e. TCAS display) */ + pModel->setIntValue("tcas/threat-level", threatLevel); + } + } + } + advisoryCoordinator.update(mode); + } + annunciator.update(); +} + +/** Run a single self-test iteration. */ +void +TCAS::selfTest(void) +{ + annunciator.update(); + if (annunciator.isPlaying()) + { + return; + } + + ResolutionAdvisory newAdvisory; + newAdvisory.threatLevel = ThreatRA; + newAdvisory.RA = AdvisoryClear; + newAdvisory.RAOption = OptionNone; + // TCAS audio is disabled below 500ft + threatDetector.setAlt(501); + + // trigger various advisories + switch(selfTestStep) + { + case 0: + newAdvisory.RA = AdvisoryIntrusion; + newAdvisory.threatLevel = ThreatTA; + break; + case 1: + newAdvisory.RA = AdvisoryClimb; + break; + case 2: + newAdvisory.RA = AdvisoryClimb; + newAdvisory.RAOption = OptionIncreaseClimb; + break; + case 3: + newAdvisory.RA = AdvisoryClimb; + newAdvisory.RAOption = OptionCrossingClimb; + break; + case 4: + newAdvisory.RA = AdvisoryDescend; + break; + case 5: + newAdvisory.RA = AdvisoryDescend; + newAdvisory.RAOption = OptionIncreaseDescend; + break; + case 6: + newAdvisory.RA = AdvisoryDescend; + newAdvisory.RAOption = OptionCrossingDescent; + break; + case 7: + newAdvisory.RA = AdvisoryAdjustVSpeed; + break; + case 8: + newAdvisory.RA = AdvisoryMaintVSpeed; + break; + case 9: + newAdvisory.RA = AdvisoryMonitorVSpeed; + break; + case 10: + newAdvisory.threatLevel = ThreatNone; + newAdvisory.RA = AdvisoryClear; + break; + case 11: + annunciator.test(true); + selfTestStep+=2; + return; + default: + nodeSelfTest->setBoolValue(false); + selfTestStep = 0; + return; + } + + advisoryCoordinator.add(newAdvisory); + advisoryCoordinator.update(SwitchAuto); + + selfTestStep++; +} diff --git a/src/Instrumentation/tcas.hxx b/src/Instrumentation/tcas.hxx new file mode 100644 index 000000000..6882c8317 --- /dev/null +++ b/src/Instrumentation/tcas.hxx @@ -0,0 +1,377 @@ +// tcas.hxx -- Traffic Alert and Collision Avoidance System (TCAS) +// +// Written by Thorsten Brehm, started December 2010. +// +// Copyright (C) 2010 Thorsten Brehm - brehmt (at) gmail com +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// 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. + +#ifndef __INSTRUMENTS_TCAS_HXX +#define __INSTRUMENTS_TCAS_HXX + +#include + +#include +#include +#include + +#include +#include +#include +#include "mk_viii.hxx" // voiceplayer only + +using std::vector; +using std::deque; +using std::map; + +class SGSampleGroup; + +#include
+ +#ifdef _MSC_VER +# pragma warning( push ) +# pragma warning( disable: 4355 ) +#endif + +/////////////////////////////////////////////////////////////////////////////// +// TCAS ////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////// + +class TCAS : public SGSubsystem +{ + + typedef enum + { + AdvisoryClear = 0, /*< Clear of traffic */ + AdvisoryIntrusion = 1, /*< Intrusion flag */ + AdvisoryClimb = AdvisoryIntrusion|(1 << 1), /*< RA climb */ + AdvisoryDescend = AdvisoryIntrusion|(1 << 2), /*< RA descend */ + AdvisoryAdjustVSpeed = AdvisoryIntrusion|(1 << 3), /*< RA adjust vertical speed (TCAS II 7.0 only) */ + AdvisoryMaintVSpeed = AdvisoryIntrusion|(1 << 4), /*< RA maintain vertical speed */ + AdvisoryMonitorVSpeed = AdvisoryIntrusion|(1 << 5), /*< RA monitor vertical speed */ + AdvisoryLevelOff = AdvisoryIntrusion|(1 << 6) /*< RA level off (TCAS II 7.1 only) */ + } EnumAdvisory; + + typedef enum + { + OptionNone = 0, /*< no option modifier */ + OptionIncreaseClimb = (1 << 0), /*< increase climb */ + OptionIncreaseDescend = (1 << 1), /*< increase descend */ + OptionCrossingClimb = (1 << 2), /*< crossing climb */ + OptionCrossingDescent = (1 << 3) /*< crossing descent */ + } EnumAdvisoryOption; + + typedef enum + { + SwitchOff = 0, /*< TCAS switched off */ + SwitchStandby = 1, /*< TCAS standby (no TA/RA) */ + SwitchTaOnly = 2, /*< TCAS in TA-only mode (no RA) */ + SwitchAuto = 3 /*< TCAS in TA/RA mode */ + } EnumModeSwitch; + + typedef enum + { + ThreatInvisible = -1,/*< Traffic is invisible to TCAS (i.e. no transponder) */ + ThreatNone = 0, /*< Traffic is visible but no threat. */ + ThreatProximity = 1, /*< Proximity intruder traffic (no threat). */ + ThreatTA = 2, /*< TA-level threat traffic. */ + ThreatRA = 3 /*< RA-level threat traffic. */ + } EnumThreatLevel; + + typedef struct + { + int threatLevel; /*< intruder threat level: 0=clear, 1=proximity, + 2=intruder, 3=proximity intruder */ + int RA; /*< resolution advisory */ + int RAOption; /*< option flags for advisory */ + } ResolutionAdvisory; + + typedef struct + { + float Tau; /*< vertical/horizontal protection range in seconds */ + float DMOD; /*< horizontal protection range in nm */ + float ALIM; /*< vertical protection range in ft */ + } Thresholds; + + typedef struct + { + double maxAltitude; /*< max altitude for this sensitivity level */ + int sl; /*< sensitivity level */ + Thresholds TA; /*< thresholds for TA-level threats */ + Thresholds RA; /*< thresholds for RA-level threats */ + } SensitivityLevel; + + typedef struct + { + bool verticalTA; + bool verticalRA; + bool horizontalTA; + bool horizontalRA; + float horizontalTau; + float verticalTau; + float relativeAltitudeFt; + float verticalFps; + } ThreatInfo; + + typedef struct + { + double lat; + double lon; + float altFt; + float heading; + float velocityKt; + float verticalFps; + } LocalInfo; /*< info structure for local aircraft */ + +// ///////////////////////////////////////////////////////////////////////////// +// // TCAS::Timer ////////////////////////////////////////////////////////////// +// ///////////////////////////////////////////////////////////////////////////// +// +// class Timer +// { +// double start_time; +// +// public: +// bool running; +// +// inline Timer () +// : running(false) {} +// +// inline void start () { running = true; start_time = globals->get_sim_time_sec(); } +// inline void stop () { running = false; } +// inline double elapsed () const { assert(running); return globals->get_sim_time_sec() - start_time; } +// inline double start_or_elapsed () +// { +// if (running) +// return elapsed(); +// else +// { +// start(); +// return 0; +// } +// } +// }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::PropertiesHandler /////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class PropertiesHandler : public FGVoicePlayer::PropertiesHandler + { + TCAS *tcas; + + public: + PropertiesHandler (TCAS *device) : + FGVoicePlayer::PropertiesHandler(), tcas(device) {} + + PropertiesHandler (void) : FGVoicePlayer::PropertiesHandler() {} + }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::VoicePlayer //////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class VoicePlayer : + public FGVoicePlayer + { + public: + VoicePlayer (TCAS* tcas) : + FGVoicePlayer(&tcas->properties_handler, "tcas") {} + + ~VoicePlayer (void) {} + + void init (void); + + struct + { + Voice* pTrafficTraffic; + Voice* pClimb; + Voice* pClimbNow; + Voice* pClimbCrossing; + Voice* pClimbIncrease; + Voice* pDescend; + Voice* pDescendNow; + Voice* pDescendCrossing; + Voice* pDescendIncrease; + Voice* pClear; + Voice* pAdjustVSpeed; + Voice* pMaintVSpeed; + Voice* pMonitorVSpeed; + Voice* pLevelOff; + Voice* pTestOk; + Voice* pTestFail; + } Voices; + private: + SGPropertyNode_ptr nodeSoundFilePrefix; + }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::Annunciator //////////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class Annunciator + { + public: + Annunciator (TCAS* tcas); + ~Annunciator (void) {} + void bind (SGPropertyNode* node); + void init (void); + void update (void); + + void trigger (const ResolutionAdvisory& newAdvisory, bool revertedRA); + void test (bool testOk); + void clear (void); + bool isPlaying (void) { return voicePlayer.is_playing();} + + private: + ResolutionAdvisory previous; + FGVoicePlayer::Voice* pLastVoice; + VoicePlayer voicePlayer; + SGPropertyNode_ptr nodeGpwsAlertOn; + }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::AdvisoryCoordinator //////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class AdvisoryCoordinator + { + public: + AdvisoryCoordinator (TCAS* _tcas); + ~AdvisoryCoordinator (void) {} + + void bind (SGPropertyNode* node); + void init (void); + void update (int mode); + + void clear (void); + void add (const ResolutionAdvisory& newAdvisory); + + private: + TCAS* tcas; + double lastTATime; + ResolutionAdvisory current; + ResolutionAdvisory previous; + SGPropertyNode_ptr nodeTAWarning; + }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::AdvisoryGenerator ////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class AdvisoryGenerator + { + public: + AdvisoryGenerator (TCAS* _tcas); + ~AdvisoryGenerator (void) {} + + void init (const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat); + + void setAlarmThresholds (const SensitivityLevel* _pAlarmThresholds); + + int resolution (int mode, int threatLevel, float distanceNm, + float altFt, float heading, float velocityKt); + + private: + float verticalSeparation (float newVerticalFps); + void determineRAsense (int& RASense, bool& isCrossing); + + private: + TCAS* tcas; + const LocalInfo* pSelf; /*< info structure for local aircraft */ + const ThreatInfo* pCurrentThreat; /*< info structure on current intruder/threat */ + const SensitivityLevel* pAlarmThresholds; + }; + + ///////////////////////////////////////////////////////////////////////////// + // TCAS::ThreatDetector ///////////////////////////////////////////////////// + ///////////////////////////////////////////////////////////////////////////// + + class ThreatDetector + { + public: + ThreatDetector (TCAS* _tcas); + ~ThreatDetector (void) {} + + void init (void); + void update (void); + + bool checkTransponder (const SGPropertyNode* pModel, float velocityKt); + int checkThreat (int mode, const SGPropertyNode* pModel); + void checkVerticalThreat (void); + void horizontalThreat (float bearing, float distanceNm, float heading, + float velocityKt); + + void setAlt (float altFt) { self.altFt = altFt;} + float getAlt (void) { return self.altFt;} + float getVelocityKt (void) { return self.velocityKt;} + + private: + void unitTest (void); + + private: + static const SensitivityLevel sensitivityLevels[]; + + TCAS* tcas; + int checkCount; + + SGPropertyNode_ptr nodeLat; + SGPropertyNode_ptr nodeLon; + SGPropertyNode_ptr nodeAlt; + SGPropertyNode_ptr nodeHeading; + SGPropertyNode_ptr nodeVelocity; + SGPropertyNode_ptr nodeVerticalFps; + + LocalInfo self; /*< info structure for local aircraft */ + ThreatInfo currentThreat; /*< info structure on current intruder/threat */ + const SensitivityLevel* pAlarmThresholds; + }; + +private: + string name; + int num; + double nextUpdateTime; + int selfTestStep; + + SGPropertyNode_ptr nodeModeSwitch; + SGPropertyNode_ptr nodeServiceable; + SGPropertyNode_ptr nodeSelfTest; + SGPropertyNode_ptr nodeDebugTrigger; + SGPropertyNode_ptr nodeDebugRA; + SGPropertyNode_ptr nodeDebugThreat; + + PropertiesHandler properties_handler; + ThreatDetector threatDetector; + AdvisoryCoordinator advisoryCoordinator; + AdvisoryGenerator advisoryGenerator; + Annunciator annunciator; + +private: + void selfTest (void); + +public: + TCAS (SGPropertyNode* node); + + virtual void bind (void); + virtual void unbind (void); + virtual void init (void); + virtual void update (double dt); +}; + +#ifdef _MSC_VER +# pragma warning( pop ) +#endif + +#endif // __INSTRUMENTS_TCAS_HXX -- 2.39.5