src/Instrumentation/tcas.cxx

   1 // tcas.cxx -- Traffic Alert and Collision Avoidance System (TCAS) Emulation
   2 //
   3 // Written by Thorsten Brehm, started December 2010.
   4 //
   5 // Copyright (C) 2010 Thorsten Brehm - brehmt (at) gmail com
   6 //
   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.
  11 //
  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.
  16 //
  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., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 ///////////////////////////////////////////////////////////////////////////////
  22
  23 /* References:
  24  *
  25  *  [TCASII] Introduction to TCAS II Version 7, Federal Aviation Administration, November 2000
  26  *           http://www.arinc.com/downloads/tcas/tcas.pdf
  27  *
  28  *  [EUROACAS] Eurocontrol Airborne Collision Avoidance System (ACAS),
  29  *           http://www.eurocontrol.int/msa/public/standard_page/ACAS_Startpage.html
  30  *
  31  * Glossary:
  32  *
  33  *  ALIM: Altitude Limit
  34  *
  35  *  CPA: Closest point of approach as computed from a threat's range and range rate.
  36  *
  37  *  DMOD: Distance MODification
  38  *
  39  *  Intruder: A target that has satisfied the traffic detection criteria.
  40  *
  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.
  43  *
  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.
  47  *
  48  *  SL: Sensitivity Level. A value used in defining the size of the protected volume
  49  *      around the own aircraft.
  50  *
  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.
  53  *
  54  *  Tau: Approximation of the time, in seconds, to CPA or to the aircraft being at the
  55  *       same altitude.
  56  *
  57  *  TCAS: Traffic alert and Collision Avoidance System
  58  */
  59
  60 /* Module properties:
  61  *
  62  *   serviceable             enable/disable TCAS processing
  63  *
  64  *   voice/file-prefix       path (and optional prefix) for sound sample files
  65  *                           (only evaluated at start-up)
  66  *
  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
  69  *
  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)
  73  *
  74  *   speaker/max-dist        Max. distance where speaker is heard
  75  *   speaker/reference-dist  Distance to pilot
  76  *   speaker/volume          Volume at reference distance
  77  *
  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)
  81  */
  82
  83 #ifdef _MSC_VER
  84 #  pragma warning( disable: 4355 )
  85 #endif
  86
  87 #ifdef HAVE_CONFIG_H
  88 #  include <config.h>
  89 #endif
  90
  91 #include <stdio.h>
  92 #include <string.h>
  93 #include <assert.h>
  94 #include <math.h>
  95
  96 #include <string>
  97 #include <sstream>
  98
  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/math/sg_random.h>
 104 #include <simgear/misc/sg_path.hxx>
 105 #include <simgear/sound/soundmgr_openal.hxx>
 106 #include <simgear/structure/exception.hxx>
 107
 108 using std::string;
 109
 110 #if defined( HAVE_VERSION_H ) && HAVE_VERSION_H
 111 #  include <Include/version.h>
 112 #else
 113 #  include <Include/no_version.h>
 114 #endif
 115
 116 #include <Main/fg_props.hxx>
 117 #include <Main/globals.hxx>
 118 #include <AIModel/submodel.hxx>
 119 #include "instrument_mgr.hxx"
 120 #include "tcas.hxx"
 121
 122 ///////////////////////////////////////////////////////////////////////////////
 123 // debug switches /////////////////////////////////////////////////////////////
 124 ///////////////////////////////////////////////////////////////////////////////
 125 //#define FEATURE_TCAS_DEBUG_ANNUNCIATOR
 126 //#define FEATURE_TCAS_DEBUG_COORDINATOR
 127 //#define FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 128 //#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
 129 //#define FEATURE_TCAS_DEBUG_PROPERTIES
 130
 131 ///////////////////////////////////////////////////////////////////////////////
 132 // constants //////////////////////////////////////////////////////////////////
 133 ///////////////////////////////////////////////////////////////////////////////
 134
 135 /** Sensitivity Level Definition and Alarm Thresholds (TCASII, Version 7)
 136  *  Max Own        |    |TA-level                |RA-level
 137  *  Altitude(ft)   | SL |Tau(s),DMOD(nm),ALIM(ft)|Tau(s),DMOD(nm),ALIM(ft) */
 138 const TCAS::SensitivityLevel
 139 TCAS::ThreatDetector::sensitivityLevels[] = {
 140    { 1000,           2,  {20,   0.30,     850},  {0,    0,        0  }},
 141    { 2350,           3,  {25,   0.33,     850},  {15,   0.20,     300}},
 142    { 5000,           4,  {30,   0.48,     850},  {20,   0.35,     300}},
 143    {10000,           5,  {40,   0.75,     850},  {25,   0.55,     350}},
 144    {20000,           6,  {45,   1.00,     850},  {30,   0.80,     400}},
 145    {42000,           7,  {48,   1.30,     850},  {35,   1.10,     600}},
 146    {0,               8,  {48,   1.30,    1200},  {35,   1.10,     700}}
 147 };
 148
 149 ///////////////////////////////////////////////////////////////////////////////
 150 // helpers ////////////////////////////////////////////////////////////////////
 151 ///////////////////////////////////////////////////////////////////////////////
 152
 153 #define ADD_VOICE(Var, Sample, SayTwice)    \
 154     { make_voice(&Var);                    \
 155       append(Var, Sample);                  \
 156       if (SayTwice) append(Var, Sample); }
 157
 158 #define AVAILABLE_RA(Options, Advisory) (Advisory == (Advisory & Options))
 159
 160 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 161 /** calculate relative angle in between two headings */
 162 static float
 163 relAngle(float Heading1, float Heading2)
 164 {
 165     Heading1 -= Heading2;
 166
 167     while (Heading1 >= 360.0)
 168         Heading1 -= 360.0;
 169
 170     while (Heading1 < 0.0)
 171         Heading1 += 360;
 172
 173     return Heading1;
 174 }
 175 #endif
 176
 177 /** calculate range and bearing of lat2/lon2 relative to lat1/lon1 */
 178 static void
 179 calcRangeBearing(double lat1, double lon1, double lat2, double lon2,
 180                  double &rangeNm, double &bearing)
 181 {
 182     // calculate the bearing and range of the second pos from the first
 183     double az2, distanceM;
 184     geo_inverse_wgs_84(lat1, lon1, lat2, lon2, &bearing, &az2, &distanceM);
 185     rangeNm = distanceM * SG_METER_TO_NM;
 186 }
 187
 188 ///////////////////////////////////////////////////////////////////////////////
 189 // VoicePlayer ////////////////////////////////////////////////////////////////
 190 ///////////////////////////////////////////////////////////////////////////////
 191
 192 void
 193 TCAS::VoicePlayer::init(void)
 194 {
 195     FGVoicePlayer::init();
 196
 197     ADD_VOICE(Voices.pTrafficTraffic, "traffic",                 true);
 198     ADD_VOICE(Voices.pClear,          "clear",                   false);
 199     ADD_VOICE(Voices.pClimb,          "climb",                   true);
 200     ADD_VOICE(Voices.pClimbNow,       "climb_now",               true);
 201     ADD_VOICE(Voices.pClimbCrossing,  "climb_crossing",          true);
 202     ADD_VOICE(Voices.pClimbIncrease,  "increase_climb",          false);
 203     ADD_VOICE(Voices.pDescend,        "descend",                 true);
 204     ADD_VOICE(Voices.pDescendNow,     "descend_now",             true);
 205     ADD_VOICE(Voices.pDescendCrossing,"descend_crossing",        true);
 206     ADD_VOICE(Voices.pDescendIncrease,"increase_descent",        false);
 207     ADD_VOICE(Voices.pAdjustVSpeed,   "adjust_vertical_speed",   false);
 208     ADD_VOICE(Voices.pMaintVSpeed,    "maintain_vertical_speed", false);
 209     ADD_VOICE(Voices.pMonitorVSpeed,  "monitor_vertical_speed",  false);
 210     ADD_VOICE(Voices.pLevelOff,       "level_off",               false);
 211     ADD_VOICE(Voices.pTestOk,         "test_ok",                 false);
 212     ADD_VOICE(Voices.pTestFail,       "test_fail",               false);
 213
 214     speaker.update_configuration();
 215 }
 216
 217 /////////////////////////////////////////////////////////////////////////////
 218 // TCAS::Annunciator ////////////////////////////////////////////////////////
 219 /////////////////////////////////////////////////////////////////////////////
 220
 221 TCAS::Annunciator::Annunciator(TCAS* tcas) :
 222     pLastVoice(NULL),
 223     voicePlayer(tcas)
 224 {
 225     clear();
 226 }
 227
 228 void TCAS::Annunciator::clear(void)
 229 {
 230     previous.threatLevel = ThreatNone;
 231     previous.RA          = AdvisoryClear;
 232     previous.RAOption    = OptionNone;
 233     pLastVoice = NULL;
 234 }
 235
 236 void
 237 TCAS::Annunciator::bind(SGPropertyNode* node)
 238 {
 239     voicePlayer.bind(node, "Sounds/tcas/");
 240 }
 241
 242 void
 243 TCAS::Annunciator::init(void)
 244 {
 245     //TODO link to GPWS module/audio-on signal must be configurable
 246     nodeGpwsAlertOn = fgGetNode("/instrumentation/mk-viii/outputs/discretes/audio-on", true);
 247     voicePlayer.init();
 248 }
 249
 250 void
 251 TCAS::Annunciator::update(void)
 252 {
 253     voicePlayer.update();
 254
 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())
 259         voicePlayer.pause();
 260     else
 261         voicePlayer.resume();
 262 }
 263
 264 /** Trigger voice sample for current alert. */
 265 void
 266 TCAS::Annunciator::trigger(const ResolutionAdvisory& current, bool revertedRA)
 267 {
 268     int RA = current.RA;
 269     int RAOption = current.RAOption;
 270
 271     if (RA == AdvisoryClear)
 272     {
 273         if (previous.RA != AdvisoryClear)
 274         {
 275             voicePlayer.play(voicePlayer.Voices.pClear, VoicePlayer::PLAY_NOW);
 276             previous = current;
 277         }
 278         pLastVoice = NULL;
 279         return;
 280     }
 281
 282     if (previous.RA == AdvisoryClear)
 283     {
 284         voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW);
 285     }
 286
 287     // pick voice sample
 288     VoicePlayer::Voice* pVoice = NULL;
 289     switch(RA)
 290     {
 291         case AdvisoryClimb:
 292             if (revertedRA)
 293                 pVoice = voicePlayer.Voices.pClimbNow;
 294             else
 295             if (AVAILABLE_RA(RAOption, OptionIncreaseClimb))
 296                 pVoice = voicePlayer.Voices.pClimbIncrease;
 297             else
 298             if (AVAILABLE_RA(RAOption, OptionCrossingClimb))
 299                 pVoice = voicePlayer.Voices.pClimbCrossing;
 300             else
 301                 pVoice = voicePlayer.Voices.pClimb;
 302             break;
 303
 304         case AdvisoryDescend:
 305             if (revertedRA)
 306                 pVoice = voicePlayer.Voices.pDescendNow;
 307             else
 308             if (AVAILABLE_RA(RAOption, OptionIncreaseDescend))
 309                 pVoice = voicePlayer.Voices.pDescendIncrease;
 310             else
 311             if (AVAILABLE_RA(RAOption, OptionCrossingDescent))
 312                 pVoice = voicePlayer.Voices.pDescendCrossing;
 313             else
 314                 pVoice = voicePlayer.Voices.pDescend;
 315             break;
 316
 317         case AdvisoryAdjustVSpeed:
 318             pVoice = voicePlayer.Voices.pAdjustVSpeed;
 319             break;
 320
 321         case AdvisoryMaintVSpeed:
 322             pVoice = voicePlayer.Voices.pMaintVSpeed;
 323             break;
 324
 325         case AdvisoryMonitorVSpeed:
 326             pVoice = voicePlayer.Voices.pMonitorVSpeed;
 327             break;
 328
 329         case AdvisoryLevelOff:
 330             pVoice = voicePlayer.Voices.pLevelOff;
 331             break;
 332
 333         case AdvisoryIntrusion:
 334             break;
 335
 336         default:
 337             RA = AdvisoryIntrusion;
 338             break;
 339     }
 340
 341     previous = current;
 342
 343     if (pLastVoice == pVoice)
 344     {
 345         // don't repeat annunciation
 346         return;
 347     }
 348     pLastVoice = pVoice;
 349     if (pVoice)
 350         voicePlayer.play(pVoice);
 351
 352 #ifdef FEATURE_TCAS_DEBUG_ANNUNCIATOR
 353     cout << "Annunciating TCAS RA " << RA << endl;
 354 #endif
 355 }
 356
 357 void
 358 TCAS::Annunciator::test(bool testOk)
 359 {
 360     if (testOk)
 361         voicePlayer.play(voicePlayer.Voices.pTestOk);
 362     else
 363         voicePlayer.play(voicePlayer.Voices.pTestFail);
 364 }
 365
 366 /////////////////////////////////////////////////////////////////////////////
 367 // TCAS::AdvisoryCoordinator ////////////////////////////////////////////////
 368 /////////////////////////////////////////////////////////////////////////////
 369
 370 TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) :
 371   tcas(_tcas),
 372   lastTATime(0)
 373 {
 374     init();
 375 }
 376
 377 void
 378 TCAS::AdvisoryCoordinator::init(void)
 379 {
 380     clear();
 381     previous = current;
 382 }
 383
 384 void
 385 TCAS::AdvisoryCoordinator::bind(SGPropertyNode* node)
 386 {
 387     nodeTAWarning = node->getNode("outputs/traffic-alert", true);
 388     nodeTAWarning->setBoolValue(false);
 389 }
 390
 391 void
 392 TCAS::AdvisoryCoordinator::clear(void)
 393 {
 394     current.threatLevel = ThreatNone;
 395     current.RA          = AdvisoryClear;
 396     current.RAOption    = OptionNone;
 397 }
 398
 399 /** Add all suitable resolution advisories for a single threat. */
 400 void
 401 TCAS::AdvisoryCoordinator::add(const ResolutionAdvisory& newAdvisory)
 402 {
 403     if (newAdvisory.RA == AdvisoryClear)
 404         return;
 405
 406     if ((current.RA == AdvisoryClear)||
 407         (current.threatLevel == ThreatTA))
 408     {
 409         current = newAdvisory;
 410     }
 411     else
 412     {
 413         // combine with other advisories so far
 414         current.RA &= newAdvisory.RA;
 415         // remember any advisory modifier
 416         current.RAOption |= newAdvisory.RAOption;
 417     }
 418 }
 419
 420 /** Pick and trigger suitable resolution advisory. */
 421 void
 422 TCAS::AdvisoryCoordinator::update(int mode)
 423 {
 424     bool revertedRA = false; // has advisory changed?
 425     double currentTime = globals->get_sim_time_sec();
 426
 427     if (current.RA == AdvisoryClear)
 428     {
 429         // filter: wait 5 seconds after last TA/RA before announcing TA clearance
 430         if ((previous.RA != AdvisoryClear)&&
 431              (currentTime - lastTATime < 5.0))
 432             return;
 433     }
 434     else
 435     {
 436         // intruder detected
 437
 438 #ifdef FEATURE_TCAS_DEBUG_COORDINATOR
 439         cout << "TCAS::Annunciator::update: previous: " << previous.RA << ", new: " << current.RA << endl;
 440 #endif
 441
 442         lastTATime = currentTime;
 443         if ((previous.RA == AdvisoryClear)||
 444             (previous.RA == AdvisoryIntrusion)||
 445             ((current.RA & previous.RA) != previous.RA))
 446         {
 447             // no RA yet, or we can't keep previous RA: pick one - in order of priority
 448
 449             if (AVAILABLE_RA(current.RA, AdvisoryMonitorVSpeed))
 450             {
 451                 // prio 1: monitor vertical speed only
 452                 current.RA = AdvisoryMonitorVSpeed;
 453             }
 454             else
 455             if (AVAILABLE_RA(current.RA, AdvisoryMaintVSpeed))
 456             {
 457                 // prio 2: maintain vertical speed
 458                 current.RA = AdvisoryMaintVSpeed;
 459             }
 460             else
 461             if (AVAILABLE_RA(current.RA, AdvisoryAdjustVSpeed))
 462             {
 463                 // prio 3: adjust vertical speed (TCAS II 7.0 only)
 464                 current.RA = AdvisoryAdjustVSpeed;
 465             }
 466             else
 467             if (AVAILABLE_RA(current.RA, AdvisoryLevelOff))
 468             {
 469                 // prio 3: adjust vertical speed (TCAS II 7.1 only, [EUROACAS]: CP115)
 470                 current.RA = AdvisoryLevelOff;
 471             }
 472             else
 473             if (AVAILABLE_RA(current.RA, AdvisoryClimb))
 474             {
 475                 // prio 4: climb
 476                 current.RA = AdvisoryClimb;
 477             }
 478             else
 479             if (AVAILABLE_RA(current.RA, AdvisoryDescend))
 480             {
 481                 // prio 5: descend
 482                 current.RA = AdvisoryDescend;
 483             }
 484             else
 485             {
 486                 // no RA, issue a TA only
 487                 current.RA = AdvisoryIntrusion;
 488             }
 489
 490             // check if earlier advisory was reverted
 491             revertedRA = ((previous.RA != current.RA)&&
 492                           (previous.RA != 0)&&
 493                           (previous.RA != AdvisoryIntrusion));
 494         }
 495         else
 496         {
 497             // keep earlier RA
 498             current.RA = previous.RA;
 499         }
 500     }
 501
 502     /* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */
 503     if ((tcas->threatDetector.getAlt() > 500)&&
 504         (mode >= SwitchTaOnly))
 505         tcas->annunciator.trigger(current, revertedRA);
 506     else
 507     if (current.RA == AdvisoryClear)
 508     {
 509         /* explicitly clear traffic alert (since aural annunciation disabled) */
 510         tcas->annunciator.clear();
 511     }
 512
 513     previous = current;
 514
 515     /* [TCASII] "[..] also performs the function of setting flags that control the displays.
 516      *   The traffic display, the RA display, [..] use these flags to alert the pilot to
 517      *   the presence of TAs and RAs." */
 518     nodeTAWarning->setBoolValue(current.RA != AdvisoryClear);
 519 }
 520
 521 ///////////////////////////////////////////////////////////////////////////////
 522 // TCAS::ThreatDetector ///////////////////////////////////////////////////////
 523 ///////////////////////////////////////////////////////////////////////////////
 524
 525 TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
 526     tcas(_tcas),
 527     pAlarmThresholds(&sensitivityLevels[0])
 528 {
 529     unitTest();
 530 }
 531
 532 void
 533 TCAS::ThreatDetector::init(void)
 534 {
 535     nodeLat         = fgGetNode("/position/latitude-deg",          true);
 536     nodeLon         = fgGetNode("/position/longitude-deg",         true);
 537     nodeAlt         = fgGetNode("/position/altitude-ft",           true);
 538     nodeHeading     = fgGetNode("/orientation/heading-deg",        true);
 539     nodeVelocity    = fgGetNode("/velocities/airspeed-kt",         true);
 540     nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps",  true);
 541
 542     tcas->advisoryGenerator.init(&self,&currentThreat);
 543 }
 544
 545 /** Update local position and threat sensitivity levels. */
 546 void
 547 TCAS::ThreatDetector::update(void)
 548 {
 549     // update local position
 550     self.lat         = nodeLat->getDoubleValue();
 551     self.lon         = nodeLon->getDoubleValue();
 552     self.altFt       = nodeAlt->getDoubleValue();
 553     self.heading     = nodeHeading->getDoubleValue();
 554     self.velocityKt  = nodeVelocity->getDoubleValue();
 555     self.verticalFps = nodeVerticalFps->getDoubleValue();
 556
 557     checkCount = 0;
 558
 559     // determine current altitude's "Sensitivity Level Definition and Alarm Thresholds"
 560     int sl=0;
 561     for (sl=0;((self.altFt > sensitivityLevels[sl].maxAltitude)&&
 562                (sensitivityLevels[sl].maxAltitude));sl++);
 563     pAlarmThresholds = &sensitivityLevels[sl];
 564     tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds);
 565 }
 566
 567 /** Check if plane's transponder is enabled. */
 568 bool
 569 TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float velocityKt)
 570 {
 571     const string name = pModel->getName();
 572     if (name != "multiplayer" && name != "aircraft")
 573     {
 574         // assume non-MP/non-AI planes (e.g. ships) have no transponder
 575         return false;
 576     }
 577
 578     if (velocityKt < 40)
 579     {
 580         /* assume all pilots have their transponder switched off while taxiing/parking
 581          * (at low speed) */
 582         return false;
 583     }
 584
 585     return true;
 586 }
 587
 588 /** Check if plane is a threat. */
 589 int
 590 TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
 591 {
 592     checkCount++;
 593
 594     float velocityKt  = pModel->getDoubleValue("velocities/true-airspeed-kt");
 595
 596     if (!checkTransponder(pModel, velocityKt))
 597         return ThreatInvisible;
 598
 599     int threatLevel = ThreatNone;
 600     float altFt = pModel->getDoubleValue("position/altitude-ft");
 601     currentThreat.relativeAltitudeFt = altFt - self.altFt;
 602
 603     // save computation time: don't care when relative altitude is excessive
 604     if (fabs(currentThreat.relativeAltitudeFt) > 10000)
 605         return threatLevel;
 606
 607     // position data of current intruder
 608     double lat        = pModel->getDoubleValue("position/latitude-deg");
 609     double lon        = pModel->getDoubleValue("position/longitude-deg");
 610     float heading     = pModel->getDoubleValue("orientation/true-heading-deg");
 611
 612     double distanceNm, bearing;
 613     calcRangeBearing(self.lat, self.lon, lat, lon, distanceNm, bearing);
 614
 615     // save computation time: don't care for excessive distances (also captures NaNs...)
 616     if ((distanceNm > 10)||(distanceNm < 0))
 617         return threatLevel;
 618
 619     // first stage: vertical movement
 620     currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps");
 621
 622     /* Detect proximity targets
 623      * [TCASII]: "Any target that is less than 6 nmi in range and within +/-1200ft
 624      *  vertically, but that does not meet the intruder or threat criteria." */
 625     if ((distanceNm < 6)&&
 626         (fabs(currentThreat.relativeAltitudeFt) < 1200))
 627     {
 628         // at least a proximity target
 629         threatLevel = ThreatProximity;
 630     }
 631
 632     checkVerticalThreat();
 633
 634     // stop processing when no vertical threat
 635     if (!currentThreat.verticalTA)
 636         return threatLevel;
 637
 638     // second stage: horizontal movement
 639     horizontalThreat(bearing, distanceNm, heading, velocityKt);
 640
 641     // no horizontal threat?
 642     if (!currentThreat.horizontalTA)
 643         return threatLevel;
 644
 645     if ((currentThreat.horizontalTau < 0)||
 646         (currentThreat.verticalTau < 0))
 647     {
 648         // do not trigger new alerts when Tau is negative, but keep existing alerts
 649         int previousThreatLevel = pModel->getIntValue("tcas/threat-level", 0);
 650         if (previousThreatLevel == 0)
 651             return threatLevel;
 652     }
 653
 654 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 655     cout << "#" << checkCount << ": " << pModel->getStringValue("callsign") << endl;
 656 #endif
 657
 658     threatLevel = ThreatTA;
 659     /* at least a TA-level threat, may also have a RA-level threat...
 660      * [TCASII]: "For either a TA or an RA to be issued, both the range and
 661      *    vertical criteria, in terms of tau or the fixed thresholds, must be
 662      *    satisfied only one of the criteria is satisfied, TCAS will not issue
 663      *    an advisory." */
 664     if (currentThreat.horizontalRA && currentThreat.verticalRA)
 665         threatLevel = ThreatRA;
 666
 667     // find all resolution options for this conflict
 668     tcas->advisoryGenerator.resolution(mode, threatLevel, distanceNm, altFt, heading, velocityKt);
 669
 670 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 671     printf("  threat: distance: %4.1f, bearing: %4.1f, alt: %5.1f, velocity: %4.1f, heading: %4.1f, vspeed: %4.1f, "
 672            "own alt: %5.1f, own heading: %4.1f, own velocity: %4.1f, vertical tau: %3.2f"
 673            //", closing speed: %f"
 674            "\n",
 675            distanceNm, relAngle(bearing, self.heading), altFt, velocityKt, heading, currentThreat.verticalFps,
 676            self.altFt, self.heading, self.velocityKt
 677            //, currentThreat.closingSpeedKt
 678            ,currentThreat.verticalTau
 679            );
 680 #endif
 681
 682     return threatLevel;
 683 }
 684
 685 /** Check if plane is a vertical threat. */
 686 void
 687 TCAS::ThreatDetector::checkVerticalThreat(void)
 688 {
 689     // calculate relative vertical speed and altitude
 690     float dV = self.verticalFps - currentThreat.verticalFps;
 691     float dA = currentThreat.relativeAltitudeFt;
 692
 693     currentThreat.verticalTA  = false;
 694     currentThreat.verticalRA  = false;
 695     currentThreat.verticalTau = 0;
 696
 697     /* [TCASII]: "The vertical tau is equal to the altitude separation (feet)
 698      *   divided by the combined vertical speed of the two aircraft (feet/minute)
 699      *   times 60." */
 700     float tau = 0;
 701     if (fabs(dV) > 0.1)
 702         tau = dA/dV;
 703
 704     /* [TCASII]: "When the combined vertical speed of the TCAS and the intruder aircraft
 705      *    is low, TCAS will use a fixed-altitude threshold to determine whether a TA or
 706      *    an RA should be issued." */
 707     if ((fabs(dV) < 3.0)||
 708         ((tau < 0) && (tau > -5)))
 709     {
 710         /* vertical closing speed is low (below 180fpm/3fps), check
 711          * fixed altitude range. */
 712         float abs_dA = fabs(dA);
 713         if (abs_dA < pAlarmThresholds->RA.ALIM)
 714         {
 715             // continuous intrusion at RA-level
 716             currentThreat.verticalTA = true;
 717             currentThreat.verticalRA = true;
 718         }
 719         else
 720         if (abs_dA < pAlarmThresholds->TA.ALIM)
 721         {
 722             // continuous intrusion: with TA-level, but no RA-threat
 723             currentThreat.verticalTA = true;
 724         }
 725         // else: no RA/TA threat
 726     }
 727     else
 728     {
 729         if ((tau < pAlarmThresholds->TA.Tau)&&
 730             (tau >= -5))
 731         {
 732             currentThreat.verticalTA = true;
 733             currentThreat.verticalRA = (tau<pAlarmThresholds->RA.Tau);
 734         }
 735     }
 736     currentThreat.verticalTau = tau;
 737
 738 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 739     if (currentThreat.verticalTA)
 740         printf("  vertical dV=%f (%f-%f), dA=%f\n", dV, self.verticalFps, currentThreat.verticalFps, dA);
 741 #endif
 742 }
 743
 744 /** Check if plane is a horizontal threat. */
 745 void
 746 TCAS::ThreatDetector::horizontalThreat(float bearing, float distanceNm, float heading, float velocityKt)
 747 {
 748     // calculate speed
 749     float vxKt = sin(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - sin(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
 750     float vyKt = cos(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - cos(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
 751
 752     // calculate horizontal closing speed
 753     float closingSpeedKt2 = vxKt*vxKt+vyKt*vyKt;
 754     float closingSpeedKt  = sqrt(closingSpeedKt2);
 755
 756     /* [TCASII]: "The range tau is equal to the slant range (nmi) divided by the closing speed
 757      *    (knots) multiplied by 3600."
 758      * => calculate allowed slant range (nmi) based on known maximum tau */
 759     float TA_rangeNm = (pAlarmThresholds->TA.Tau*closingSpeedKt)/3600;
 760     float RA_rangeNm = (pAlarmThresholds->RA.Tau*closingSpeedKt)/3600;
 761
 762     if (closingSpeedKt < 100)
 763     {
 764         /* [TCASII]: "In events where the rate of closure is very low, [..]
 765          *    an intruder aircraft can come very close in range without crossing the
 766          *    range tau boundaries [..]. To provide protection in these types of
 767          *    advisories, the range tau boundaries are modified [..] to use
 768          *    a fixed-range threshold to issue TAs and RAs in these slow closure
 769          *    encounters." */
 770         TA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->TA.DMOD/100.0);
 771         RA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->RA.DMOD/100.0);
 772     }
 773     if (TA_rangeNm < pAlarmThresholds->TA.DMOD)
 774         TA_rangeNm = pAlarmThresholds->TA.DMOD;
 775     if (RA_rangeNm < pAlarmThresholds->RA.DMOD)
 776         RA_rangeNm = pAlarmThresholds->RA.DMOD;
 777
 778     currentThreat.horizontalTA   = (distanceNm < TA_rangeNm);
 779     currentThreat.horizontalRA   = (distanceNm < RA_rangeNm);
 780     currentThreat.horizontalTau  = -1;
 781
 782     if ((currentThreat.horizontalRA)&&
 783         (currentThreat.verticalRA))
 784     {
 785         /* an RA will be issued. Prepare extra data for the
 786          * traffic resolution stage, i.e. calculate
 787          * exact time tau to horizontal CPA.
 788          */
 789
 790         /* relative position of intruder is
 791          *   Sx(t) = sx + vx*t
 792          *   Sy(t) = sy + vy*t
 793          * horizontal distance to intruder is r(t)
 794          *   r(t) = sqrt( Sx(t)^2 + Sy(t)^2 )
 795          * => horizontal CPA at time t=tau, where r(t) has minimum
 796          * r2(t) := r^2(t) = Sx(t)^2 + Sy(t)^2
 797          * since r(t)>0 for all t => minimum of r(t) is also minimum of r2(t)
 798          * => (d/dt) r2(t) = r2'(t) is 0 for t=tau
 799          *    r2(t) = ((Sx(t)^2 + Sy(t))^2) = c + b*t + a*t^2
 800          * => r2'(t) = b + a*2*t
 801          * at t=tau:
 802          *    r2'(tau) = 0 = b + 2*a*tau
 803          * => tau = -b/(2*a)
 804          */
 805         float sx = sin(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
 806         float sy = cos(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
 807         float vx = vxKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
 808         float vy = vyKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
 809         float a  = vx*vx + vy*vy;
 810         float b  = 2*(sx*vx + sy*vy);
 811         float tau = 0;
 812         if (a > 0.0001)
 813             tau = -b/(2*a);
 814 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 815         printf("  Time to horizontal CPA: %4.2f\n",tau);
 816 #endif
 817         if (tau > pAlarmThresholds->RA.Tau)
 818             tau = pAlarmThresholds->RA.Tau;
 819
 820         // remember time to horizontal CPA
 821         currentThreat.horizontalTau = tau;
 822     }
 823 }
 824
 825 /** Test threat detection logic. */
 826 void
 827 TCAS::ThreatDetector::unitTest(void)
 828 {
 829     pAlarmThresholds = &sensitivityLevels[1];
 830 #if 0
 831     // vertical tests
 832     self.verticalFps = 0;
 833     self.altFt = 1000;
 834     cout << "identical altitude and vspeed " << endl;
 835     checkVerticalThreat(self.altFt, self.verticalFps);
 836     cout << "1000ft alt offset, dV=100 " << endl;
 837     checkVerticalThreat(self.altFt+1000, 100);
 838     cout << "-1000ft alt offset, dV=100 " << endl;
 839     checkVerticalThreat(self.altFt-1000, 100);
 840     cout << "3000ft alt offset, dV=10 " << endl;
 841     checkVerticalThreat(self.altFt+3000, 10);
 842     cout << "500ft alt offset, dV=100 " << endl;
 843     checkVerticalThreat(self.altFt+500, 100);
 844     cout << "500ft alt offset, dV=-100 " << endl;
 845     checkVerticalThreat(self.altFt+500, -100);
 846
 847     // horizontal tests
 848     self.heading = 0;
 849     self.velocityKt = 0;
 850     cout << "10nm behind, overtaking with 1Nm/s" << endl;
 851     horizontalThreat(-180, 10, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
 852
 853     cout << "10nm ahead, departing with 1Nm/s" << endl;
 854     horizontalThreat(0, 20, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
 855
 856     self.heading = 90;
 857     self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
 858     cout << "10nm behind, overtaking with 1Nm/s at 90 degrees" << endl;
 859     horizontalThreat(-90, 20, 90, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
 860
 861     self.heading = 20;
 862     self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
 863     cout << "10nm behind, overtaking with 1Nm/s at 20 degrees" << endl;
 864     horizontalThreat(200, 20, 20, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
 865 #endif
 866 }
 867
 868 ///////////////////////////////////////////////////////////////////////////////
 869 // TCAS::AdvisoryGenerator ////////////////////////////////////////////////////
 870 ///////////////////////////////////////////////////////////////////////////////
 871
 872 TCAS::AdvisoryGenerator::AdvisoryGenerator(TCAS* _tcas) :
 873     tcas(_tcas),
 874     pSelf(NULL),
 875     pCurrentThreat(NULL),
 876     pAlarmThresholds(NULL)
 877 {
 878 }
 879
 880 void
 881 TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, const ThreatInfo* _pCurrentThreat)
 882 {
 883     pCurrentThreat = _pCurrentThreat;
 884     pSelf = _pSelf;
 885 }
 886
 887 void
 888 TCAS::AdvisoryGenerator::setAlarmThresholds(const SensitivityLevel* _pAlarmThresholds)
 889 {
 890     pAlarmThresholds = _pAlarmThresholds;
 891 }
 892
 893 /** Calculate projected vertical separation at horizontal CPA. */
 894 float
 895 TCAS::AdvisoryGenerator::verticalSeparation(float newVerticalFps)
 896 {
 897     // calculate relative vertical speed and altitude
 898     float dV = pCurrentThreat->verticalFps - newVerticalFps;
 899     float tau = pCurrentThreat->horizontalTau;
 900     // don't use negative tau to project future separation...
 901     if (tau < 0.5)
 902         tau = 0.5;
 903     return pCurrentThreat->relativeAltitudeFt + tau * dV;
 904 }
 905
 906 /** Determine RA sense. */
 907 void
 908 TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing)
 909 {
 910     /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
 911              *   geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
 912     RASense = 0;
 913     isCrossing = false;
 914
 915     /* [TCASII]: "Based on the range and altitude tracks of the intruder, the CAS logic models the
 916      *   intruder's flight path from its present position to CPA. The CAS logic then models upward
 917      *   and downward sense RAs for own aircraft [..] to determine which sense provides the most
 918      *   vertical separation at CPA." */
 919     float upSenseRelAltFt   = verticalSeparation(+2000/60.0);
 920     float downSenseRelAltFt = verticalSeparation(-2000/60.0);
 921     if (fabs(upSenseRelAltFt) >= fabs(downSenseRelAltFt))
 922         RASense = +1;  // upward
 923     else
 924         RASense = -1; // downward
 925
 926     /* [TCASII]: "In encounters where either of the senses results in the TCAS aircraft crossing through
 927      *   the intruder's altitude, TCAS is designed to select the nonaltitude crossing sense if the
 928      *   noncrossing sense provides the desired vertical separation, known as ALIM, at CPA." */
 929     /* [TCASII]: "If ALIM cannot be obtained in the nonaltitude crossing sense, an altitude
 930      *   crossing RA will be issued." */
 931     if ((RASense > 0)&&
 932         (pCurrentThreat->relativeAltitudeFt > 200))
 933     {
 934         // threat is above and RA is crossing
 935         if (fabs(downSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
 936         {
 937             // non-crossing descend is sufficient
 938             RASense = -1;
 939         }
 940         else
 941         {
 942             // keep crossing climb RA
 943             isCrossing = true;
 944         }
 945     }
 946     else
 947     if ((RASense < 0)&&
 948         (pCurrentThreat->relativeAltitudeFt < -200))
 949     {
 950         // threat is below and RA is crossing
 951         if (fabs(upSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
 952         {
 953             // non-crossing climb is sufficient
 954             RASense = 1;
 955         }
 956         else
 957         {
 958             // keep crossing descent RA
 959             isCrossing = true;
 960         }
 961     }
 962     // else: threat is at same altitude, keep optimal RA sense (non-crossing)
 963
 964 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
 965         printf("  RASense: %i, crossing: %u, relAlt: %4.1f, upward separation: %4.1f, downward separation: %4.1f\n",
 966                RASense,isCrossing,
 967                pCurrentThreat->relativeAltitudeFt,
 968                upSenseRelAltFt,downSenseRelAltFt);
 969 #endif
 970 }
 971
 972 /** Determine suitable resolution advisories. */
 973 int
 974 TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, float altFt,
 975                                  float heading, float velocityKt)
 976 {
 977     int RAOption = OptionNone;
 978     int RA       = AdvisoryIntrusion;
 979
 980     // RAs are disabled under certain conditions
 981     if (threatLevel == ThreatRA)
 982     {
 983         /* [TCASII]: "... less than 360 feet, TCAS considers the reporting aircraft
 984          *   to be on the ground. If TCAS determines the intruder to be on the ground, it
 985          *   inhibits the generation of advisories against this aircraft."*/
 986         if (altFt < 360)
 987             threatLevel = ThreatTA;
 988
 989         /* [EUROACAS]: "Certain RAs are inhibited at altitudes based on inputs from the radio altimeter:
 990          *   [..] (c)1000ft (+/- 100ft) and below, all RAs are inhibited;" */
 991         if (pSelf->altFt < 1000)
 992             threatLevel = ThreatTA;
 993
 994         // RAs only issued in mode "Auto" (= "TA/RA" mode)
 995         if (mode != SwitchAuto)
 996             threatLevel = ThreatTA;
 997     }
 998
 999     bool isCrossing = false;
1000     int RASense = 0;
1001     // determine suitable RAs
1002     if (threatLevel == ThreatRA)
1003     {
1004         /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
1005          *   geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
1006         determineRAsense(RASense, isCrossing);
1007
1008         /* second step: determine required strength */
1009         if (RASense > 0)
1010         {
1011             // upward
1012
1013             if ((pSelf->verticalFps < -1000/60.0)&&
1014                 (!isCrossing))
1015             {
1016                 // currently descending, see if reducing current descent is sufficient
1017                 float relAltFt = verticalSeparation(-500/60.0);
1018                 if (relAltFt > pAlarmThresholds->TA.ALIM)
1019                     RA |= AdvisoryAdjustVSpeed;
1020             }
1021             RA |= AdvisoryClimb;
1022             if (isCrossing)
1023                 RAOption |= OptionCrossingClimb;
1024         }
1025
1026         if (RASense < 0)
1027         {
1028             // downward
1029
1030             if ((pSelf->verticalFps > 1000/60.0)&&
1031                 (!isCrossing))
1032             {
1033                 // currently climbing, see if reducing current climb is sufficient
1034                 float relAltFt = verticalSeparation(500/60.0);
1035                 if (relAltFt < -pAlarmThresholds->TA.ALIM)
1036                     RA |= AdvisoryAdjustVSpeed;
1037             }
1038             RA |= AdvisoryDescend;
1039             if (isCrossing)
1040                 RAOption |= OptionCrossingDescent;
1041         }
1042
1043         //TODO
1044         /* [TCASII]: "When two TCAS-equipped aircraft are converging vertically with opposite rates
1045          *   and are currently well separated in altitude, TCAS will first issue a vertical speed
1046          *   limit (Negative) RA to reinforce the pilots' likely intention to level off at adjacent
1047          *   flight levels." */
1048
1049         //TODO
1050         /* [TCASII]: "[..] if the CAS logic determines that the response to a Positive RA has provided
1051          *   ALIM feet of vertical separation before CPA, the initial RA will be weakened to either a
1052          *   Do Not Descend RA (after an initial Climb RA) or a Do Not Climb RA (after an initial
1053          *   Descend RA)." */
1054
1055         //TODO
1056         /* [TCASII]: "TCAS is designed to inhibit Increase Descent RAs below 1450 feet AGL; */
1057
1058         /* [TCASII]: "Descend RAs below 1100 feet AGL;" (inhibited) */
1059         if (pSelf->altFt < 1100)
1060         {
1061             RA &= ~AdvisoryDescend;
1062             //TODO Support "Do not descend" RA
1063             RA |= AdvisoryIntrusion;
1064         }
1065     }
1066
1067 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
1068     cout << "  resolution advisory: " << RA << endl;
1069 #endif
1070
1071     ResolutionAdvisory newAdvisory;
1072     newAdvisory.RAOption    = RAOption;
1073     newAdvisory.RA          = RA;
1074     newAdvisory.threatLevel = threatLevel;
1075     tcas->advisoryCoordinator.add(newAdvisory);
1076
1077     return RA;
1078 }
1079
1080 ///////////////////////////////////////////////////////////////////////////////
1081 // TCAS ///////////////////////////////////////////////////////////////////////
1082 ///////////////////////////////////////////////////////////////////////////////
1083
1084 TCAS::TCAS(SGPropertyNode* pNode) :
1085     name("tcas"),
1086     num(0),
1087     nextUpdateTime(0),
1088     selfTestStep(0),
1089     properties_handler(this),
1090     threatDetector(this),
1091     advisoryCoordinator(this),
1092     advisoryGenerator(this),
1093     annunciator(this)
1094 {
1095     for (int i = 0; i < pNode->nChildren(); ++i)
1096     {
1097         SGPropertyNode* pChild = pNode->getChild(i);
1098         string cname = pChild->getName();
1099         string cval = pChild->getStringValue();
1100
1101         if (cname == "name")
1102             name = cval;
1103         else if (cname == "number")
1104             num = pChild->getIntValue();
1105         else
1106         {
1107             SG_LOG(SG_INSTR, SG_WARN, "Error in TCAS config logic");
1108             if (name.length())
1109                 SG_LOG(SG_INSTR, SG_WARN, "Section = " << name);
1110         }
1111     }
1112 }
1113
1114 void
1115 TCAS::init(void)
1116 {
1117     annunciator.init();
1118     advisoryCoordinator.init();
1119     threatDetector.init();
1120 }
1121
1122 void
1123 TCAS::bind(void)
1124 {
1125     SGPropertyNode* node = fgGetNode(("/instrumentation/" + name).c_str(), num, true);
1126
1127     nodeServiceable  = node->getNode("serviceable", true);
1128
1129     // TCAS mode selection (0=off, 1=standby, 2=TA only, 3=auto(TA/RA) )
1130     nodeModeSwitch   = node->getNode("inputs/mode", true);
1131     // self-test button
1132     nodeSelfTest     = node->getNode("inputs/self-test", true);
1133     // default value
1134     nodeSelfTest->setBoolValue(false);
1135
1136 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1137     SGPropertyNode* nodeDebug = node->getNode("debug", true);
1138     // debug triggers
1139     nodeDebugTrigger = nodeDebug->getNode("threat-trigger", true);
1140     nodeDebugRA      = nodeDebug->getNode("threat-RA",      true);
1141     nodeDebugThreat  = nodeDebug->getNode("threat-level",   true);
1142     // default values
1143     nodeDebugTrigger->setBoolValue(false);
1144     nodeDebugRA->setIntValue(3);
1145     nodeDebugThreat->setIntValue(1);
1146 #endif
1147
1148     annunciator.bind(node);
1149     advisoryCoordinator.bind(node);
1150 }
1151
1152 void
1153 TCAS::unbind(void)
1154 {
1155     properties_handler.unbind();
1156 }
1157
1158 /** Monitor traffic for safety threats. */
1159 void
1160 TCAS::update(double dt)
1161 {
1162     if (!nodeServiceable->getBoolValue())
1163         return;
1164     int mode = nodeModeSwitch->getIntValue();
1165     if (mode == SwitchOff)
1166         return;
1167
1168     nextUpdateTime -= dt;
1169     if (nextUpdateTime <= 0.0 )
1170     {
1171         nextUpdateTime = 1.0;
1172
1173         // get aircrafts current position/speed/heading
1174         threatDetector.update();
1175
1176         // clear old threats
1177         advisoryCoordinator.clear();
1178
1179         if (nodeSelfTest->getBoolValue())
1180         {
1181             if (threatDetector.getVelocityKt() >= 40)
1182             {
1183                 // disable self-test when plane moves above taxiing speed
1184                 nodeSelfTest->setBoolValue(false);
1185                 selfTestStep = 0;
1186             }
1187             else
1188             {
1189                 selfTest();
1190                 // speed-up self test
1191                 nextUpdateTime = 0;
1192                 // no further TCAS processing during self-test
1193                 return;
1194             }
1195         }
1196
1197 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1198         if (nodeDebugTrigger->getBoolValue())
1199         {
1200             // debugging test
1201             ResolutionAdvisory debugAdvisory;
1202             debugAdvisory.RAOption    = OptionNone;
1203             debugAdvisory.RA          = nodeDebugRA->getIntValue();
1204             debugAdvisory.threatLevel = nodeDebugThreat->getIntValue();
1205             advisoryCoordinator.add(debugAdvisory);
1206         }
1207         else
1208 #endif
1209         {
1210             SGPropertyNode* pAi = fgGetNode("/ai/models", true);
1211
1212             // check all aircraft
1213             for (int i = pAi->nChildren() - 1; i >= -1; i--)
1214             {
1215                 SGPropertyNode* pModel = pAi->getChild(i);
1216                 if ((pModel)&&(pModel->nChildren()))
1217                 {
1218                     int threatLevel = threatDetector.checkThreat(mode, pModel);
1219                     /* expose aircraft threat-level (to be used by other instruments,
1220                      * i.e. TCAS display) */
1221                     pModel->setIntValue("tcas/threat-level", threatLevel);
1222                 }
1223             }
1224         }
1225         advisoryCoordinator.update(mode);
1226     }
1227     annunciator.update();
1228 }
1229
1230 /** Run a single self-test iteration. */
1231 void
1232 TCAS::selfTest(void)
1233 {
1234     annunciator.update();
1235     if (annunciator.isPlaying())
1236     {
1237         return;
1238     }
1239
1240     ResolutionAdvisory newAdvisory;
1241     newAdvisory.threatLevel = ThreatRA;
1242     newAdvisory.RA          = AdvisoryClear;
1243     newAdvisory.RAOption    = OptionNone;
1244     // TCAS audio is disabled below 500ft
1245     threatDetector.setAlt(501);
1246
1247     // trigger various advisories
1248     switch(selfTestStep)
1249     {
1250         case 0:
1251             newAdvisory.RA = AdvisoryIntrusion;
1252             newAdvisory.threatLevel = ThreatTA;
1253             break;
1254         case 1:
1255             newAdvisory.RA = AdvisoryClimb;
1256             break;
1257         case 2:
1258             newAdvisory.RA = AdvisoryClimb;
1259             newAdvisory.RAOption = OptionIncreaseClimb;
1260             break;
1261         case 3:
1262             newAdvisory.RA = AdvisoryClimb;
1263             newAdvisory.RAOption = OptionCrossingClimb;
1264             break;
1265         case 4:
1266             newAdvisory.RA = AdvisoryDescend;
1267             break;
1268         case 5:
1269             newAdvisory.RA = AdvisoryDescend;
1270             newAdvisory.RAOption = OptionIncreaseDescend;
1271             break;
1272         case 6:
1273             newAdvisory.RA = AdvisoryDescend;
1274             newAdvisory.RAOption = OptionCrossingDescent;
1275             break;
1276         case 7:
1277             newAdvisory.RA = AdvisoryAdjustVSpeed;
1278             break;
1279         case 8:
1280             newAdvisory.RA = AdvisoryMaintVSpeed;
1281             break;
1282         case 9:
1283             newAdvisory.RA = AdvisoryMonitorVSpeed;
1284             break;
1285         case 10:
1286             newAdvisory.threatLevel = ThreatNone;
1287             newAdvisory.RA = AdvisoryClear;
1288             break;
1289         case 11:
1290             annunciator.test(true);
1291             selfTestStep+=2;
1292             return;
1293         default:
1294             nodeSelfTest->setBoolValue(false);
1295             selfTestStep = 0;
1296             return;
1297     }
1298
1299     advisoryCoordinator.add(newAdvisory);
1300     advisoryCoordinator.update(SwitchAuto);
1301
1302     selfTestStep++;
1303 }