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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, 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/sound/soundmgr_openal.hxx>
 104 #include <simgear/sound/sample_group.hxx>
 105 #include <simgear/structure/exception.hxx>
 106
 107 using std::string;
 108
 109 #if defined( HAVE_VERSION_H ) && HAVE_VERSION_H
 110 #  include <Include/version.h>
 111 #else
 112 #  include <Include/no_version.h>
 113 #endif
 114
 115 ///////////////////////////////////////////////////////////////////////////////
 116 // debug switches /////////////////////////////////////////////////////////////
 117 ///////////////////////////////////////////////////////////////////////////////
 118 //#define FEATURE_TCAS_DEBUG_ANNUNCIATOR
 119 //#define FEATURE_TCAS_DEBUG_COORDINATOR
 120 //#define FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 121 //#define FEATURE_TCAS_DEBUG_TRACKER
 122 //#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
 123 //#define FEATURE_TCAS_DEBUG_PROPERTIES
 124
 125 #include <Main/fg_props.hxx>
 126 #include <Main/globals.hxx>
 127 #include "instrument_mgr.hxx"
 128 #include "tcas.hxx"
 129
 130 ///////////////////////////////////////////////////////////////////////////////
 131 // constants //////////////////////////////////////////////////////////////////
 132 ///////////////////////////////////////////////////////////////////////////////
 133
 134 /** Sensitivity Level Definition and Alarm Thresholds (TCASII, Version 7)
 135  *  Max Own        |    |TA-level                |RA-level
 136  *  Altitude(ft)   | SL |Tau(s),DMOD(nm),ALIM(ft)|Tau(s),DMOD(nm),ALIM(ft) */
 137 const TCAS::SensitivityLevel
 138 TCAS::ThreatDetector::sensitivityLevels[] = {
 139    { 1000,           2,  {20,   0.30,     850},  {0,    0,        0  }},
 140    { 2350,           3,  {25,   0.33,     850},  {15,   0.20,     300}},
 141    { 5000,           4,  {30,   0.48,     850},  {20,   0.35,     300}},
 142    {10000,           5,  {40,   0.75,     850},  {25,   0.55,     350}},
 143    {20000,           6,  {45,   1.00,     850},  {30,   0.80,     400}},
 144    {42000,           7,  {48,   1.30,     850},  {35,   1.10,     600}},
 145    {0,               8,  {48,   1.30,    1200},  {35,   1.10,     700}}
 146 };
 147
 148 ///////////////////////////////////////////////////////////////////////////////
 149 // helpers ////////////////////////////////////////////////////////////////////
 150 ///////////////////////////////////////////////////////////////////////////////
 151
 152 #define ADD_VOICE(Var, Sample, SayTwice)    \
 153     { make_voice(&Var);                    \
 154       append(Var, Sample);                  \
 155       if (SayTwice) append(Var, Sample); }
 156
 157 #define AVAILABLE_RA(Options, Advisory) (Advisory == (Advisory & Options))
 158
 159 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 160 /** calculate relative angle in between two headings */
 161 static float
 162 relAngle(float Heading1, float Heading2)
 163 {
 164     Heading1 -= Heading2;
 165
 166     while (Heading1 >= 360.0)
 167         Heading1 -= 360.0;
 168
 169     while (Heading1 < 0.0)
 170         Heading1 += 360;
 171
 172     return Heading1;
 173 }
 174 #endif
 175
 176 /** calculate range and bearing of lat2/lon2 relative to lat1/lon1 */
 177 static void
 178 calcRangeBearing(double lat1, double lon1, double lat2, double lon2,
 179                  double &rangeNm, double &bearing)
 180 {
 181     // calculate the bearing and range of the second pos from the first
 182     double az2, distanceM;
 183     geo_inverse_wgs_84(lat1, lon1, lat2, lon2, &bearing, &az2, &distanceM);
 184     rangeNm = distanceM * SG_METER_TO_NM;
 185 }
 186
 187 ///////////////////////////////////////////////////////////////////////////////
 188 // VoicePlayer ////////////////////////////////////////////////////////////////
 189 ///////////////////////////////////////////////////////////////////////////////
 190
 191 void
 192 TCAS::VoicePlayer::init(void)
 193 {
 194     FGVoicePlayer::init();
 195
 196     ADD_VOICE(Voices.pTrafficTraffic, "traffic",                 true);
 197     ADD_VOICE(Voices.pClear,          "clear",                   false);
 198     ADD_VOICE(Voices.pClimb,          "climb",                   true);
 199     ADD_VOICE(Voices.pClimbNow,       "climb_now",               true);
 200     ADD_VOICE(Voices.pClimbCrossing,  "climb_crossing",          true);
 201     ADD_VOICE(Voices.pClimbIncrease,  "increase_climb",          false);
 202     ADD_VOICE(Voices.pDescend,        "descend",                 true);
 203     ADD_VOICE(Voices.pDescendNow,     "descend_now",             true);
 204     ADD_VOICE(Voices.pDescendCrossing,"descend_crossing",        true);
 205     ADD_VOICE(Voices.pDescendIncrease,"increase_descent",        false);
 206     ADD_VOICE(Voices.pAdjustVSpeed,   "adjust_vertical_speed",   false);
 207     ADD_VOICE(Voices.pMaintVSpeed,    "maintain_vertical_speed", false);
 208     ADD_VOICE(Voices.pMonitorVSpeed,  "monitor_vertical_speed",  false);
 209     ADD_VOICE(Voices.pLevelOff,       "level_off",               false);
 210     ADD_VOICE(Voices.pTestOk,         "test_ok",                 false);
 211     ADD_VOICE(Voices.pTestFail,       "test_fail",               false);
 212
 213     speaker.update_configuration();
 214 }
 215
 216 /////////////////////////////////////////////////////////////////////////////
 217 // TCAS::Annunciator ////////////////////////////////////////////////////////
 218 /////////////////////////////////////////////////////////////////////////////
 219
 220 TCAS::Annunciator::Annunciator(TCAS* _tcas) :
 221     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/female/");
 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         (tcas->tracker.newTraffic()))
 284     {
 285         voicePlayer.play(voicePlayer.Voices.pTrafficTraffic, VoicePlayer::PLAY_NOW);
 286     }
 287
 288     // pick voice sample
 289     VoicePlayer::Voice* pVoice = NULL;
 290     switch(RA)
 291     {
 292         case AdvisoryClimb:
 293             if (revertedRA)
 294                 pVoice = voicePlayer.Voices.pClimbNow;
 295             else
 296             if (AVAILABLE_RA(RAOption, OptionIncreaseClimb))
 297                 pVoice = voicePlayer.Voices.pClimbIncrease;
 298             else
 299             if (AVAILABLE_RA(RAOption, OptionCrossingClimb))
 300                 pVoice = voicePlayer.Voices.pClimbCrossing;
 301             else
 302                 pVoice = voicePlayer.Voices.pClimb;
 303             break;
 304
 305         case AdvisoryDescend:
 306             if (revertedRA)
 307                 pVoice = voicePlayer.Voices.pDescendNow;
 308             else
 309             if (AVAILABLE_RA(RAOption, OptionIncreaseDescend))
 310                 pVoice = voicePlayer.Voices.pDescendIncrease;
 311             else
 312             if (AVAILABLE_RA(RAOption, OptionCrossingDescent))
 313                 pVoice = voicePlayer.Voices.pDescendCrossing;
 314             else
 315                 pVoice = voicePlayer.Voices.pDescend;
 316             break;
 317
 318         case AdvisoryAdjustVSpeed:
 319             pVoice = voicePlayer.Voices.pAdjustVSpeed;
 320             break;
 321
 322         case AdvisoryMaintVSpeed:
 323             pVoice = voicePlayer.Voices.pMaintVSpeed;
 324             break;
 325
 326         case AdvisoryMonitorVSpeed:
 327             pVoice = voicePlayer.Voices.pMonitorVSpeed;
 328             break;
 329
 330         case AdvisoryLevelOff:
 331             pVoice = voicePlayer.Voices.pLevelOff;
 332             break;
 333
 334         case AdvisoryIntrusion:
 335             break;
 336
 337         default:
 338             RA = AdvisoryIntrusion;
 339             break;
 340     }
 341
 342     previous = current;
 343
 344     if ((pLastVoice == pVoice)&&
 345         (!tcas->tracker.newTraffic()))
 346     {
 347         // don't repeat annunciation
 348         return;
 349     }
 350     pLastVoice = pVoice;
 351     if (pVoice)
 352         voicePlayer.play(pVoice);
 353
 354 #ifdef FEATURE_TCAS_DEBUG_ANNUNCIATOR
 355     cout << "Annunciating TCAS RA " << RA << endl;
 356 #endif
 357 }
 358
 359 void
 360 TCAS::Annunciator::test(bool testOk)
 361 {
 362     if (testOk)
 363         voicePlayer.play(voicePlayer.Voices.pTestOk);
 364     else
 365         voicePlayer.play(voicePlayer.Voices.pTestFail);
 366 }
 367
 368 /////////////////////////////////////////////////////////////////////////////
 369 // TCAS::AdvisoryCoordinator ////////////////////////////////////////////////
 370 /////////////////////////////////////////////////////////////////////////////
 371
 372 TCAS::AdvisoryCoordinator::AdvisoryCoordinator(TCAS* _tcas) :
 373   tcas(_tcas),
 374   lastTATime(0)
 375 {
 376     init();
 377 }
 378
 379 void
 380 TCAS::AdvisoryCoordinator::init(void)
 381 {
 382     reinit();
 383 }
 384
 385 void
 386 TCAS::AdvisoryCoordinator::reinit(void)
 387 {
 388     clear();
 389     previous = current;
 390 }
 391
 392 void
 393 TCAS::AdvisoryCoordinator::bind(SGPropertyNode* node)
 394 {
 395     nodeTAWarning = node->getNode("outputs/traffic-alert", true);
 396     nodeTAWarning->setBoolValue(false);
 397 }
 398
 399 void
 400 TCAS::AdvisoryCoordinator::clear(void)
 401 {
 402     current.threatLevel = ThreatNone;
 403     current.RA          = AdvisoryClear;
 404     current.RAOption    = OptionNone;
 405 }
 406
 407 /** Add all suitable resolution advisories for a single threat. */
 408 void
 409 TCAS::AdvisoryCoordinator::add(const ResolutionAdvisory& newAdvisory)
 410 {
 411     if ((newAdvisory.RA == AdvisoryClear)||
 412         (newAdvisory.threatLevel < current.threatLevel))
 413         return;
 414
 415     if (current.threatLevel == newAdvisory.threatLevel)
 416     {
 417         // combine with other advisories so far
 418         current.RA &= newAdvisory.RA;
 419         // remember any advisory modifier
 420         current.RAOption |= newAdvisory.RAOption;
 421     }
 422     else
 423     {
 424         current = newAdvisory;
 425     }
 426 }
 427
 428 /** Pick and trigger suitable resolution advisory. */
 429 void
 430 TCAS::AdvisoryCoordinator::update(int mode)
 431 {
 432     bool revertedRA = false; // has advisory changed?
 433     double currentTime = globals->get_sim_time_sec();
 434
 435     if (current.RA == AdvisoryClear)
 436     {
 437         // filter: wait 5 seconds after last TA/RA before announcing TA clearance
 438         if ((previous.RA != AdvisoryClear)&&
 439              (currentTime - lastTATime < 5.0))
 440             return;
 441     }
 442     else
 443     {
 444         // intruder detected
 445
 446 #ifdef FEATURE_TCAS_DEBUG_COORDINATOR
 447         cout << "TCAS::Annunciator::update: previous: " << previous.RA << ", new: " << current.RA << endl;
 448 #endif
 449
 450         lastTATime = currentTime;
 451         if ((previous.RA == AdvisoryClear)||
 452             (previous.RA == AdvisoryIntrusion)||
 453             ((current.RA & previous.RA) != previous.RA))
 454         {
 455             // no RA yet, or we can't keep previous RA: pick one - in order of priority
 456
 457             if (AVAILABLE_RA(current.RA, AdvisoryMonitorVSpeed))
 458             {
 459                 // prio 1: monitor vertical speed only
 460                 current.RA = AdvisoryMonitorVSpeed;
 461             }
 462             else
 463             if (AVAILABLE_RA(current.RA, AdvisoryMaintVSpeed))
 464             {
 465                 // prio 2: maintain vertical speed
 466                 current.RA = AdvisoryMaintVSpeed;
 467             }
 468             else
 469             if (AVAILABLE_RA(current.RA, AdvisoryAdjustVSpeed))
 470             {
 471                 // prio 3: adjust vertical speed (TCAS II 7.0 only)
 472                 current.RA = AdvisoryAdjustVSpeed;
 473             }
 474             else
 475             if (AVAILABLE_RA(current.RA, AdvisoryLevelOff))
 476             {
 477                 // prio 3: adjust vertical speed (TCAS II 7.1 only, [EUROACAS]: CP115)
 478                 current.RA = AdvisoryLevelOff;
 479             }
 480             else
 481             if (AVAILABLE_RA(current.RA, AdvisoryClimb))
 482             {
 483                 // prio 4: climb
 484                 current.RA = AdvisoryClimb;
 485             }
 486             else
 487             if (AVAILABLE_RA(current.RA, AdvisoryDescend))
 488             {
 489                 // prio 5: descend
 490                 current.RA = AdvisoryDescend;
 491             }
 492             else
 493             {
 494                 // no RA, issue a TA only
 495                 current.RA = AdvisoryIntrusion;
 496             }
 497
 498             // check if earlier advisory was reverted
 499             revertedRA = ((previous.RA != current.RA)&&
 500                           (previous.RA != 0)&&
 501                           (previous.RA != AdvisoryIntrusion));
 502         }
 503         else
 504         {
 505             // keep earlier RA
 506             current.RA = previous.RA;
 507         }
 508     }
 509
 510     /* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */
 511     if ((tcas->threatDetector.getRadarAlt() > 500)&&
 512         (mode >= SwitchTaOnly))
 513         tcas->annunciator.trigger(current, revertedRA);
 514     else
 515     if (current.RA == AdvisoryClear)
 516     {
 517         /* explicitly clear traffic alert (since aural annunciation disabled) */
 518         tcas->annunciator.clear();
 519     }
 520
 521     previous = current;
 522
 523     /* [TCASII] "[..] also performs the function of setting flags that control the displays.
 524      *   The traffic display, the RA display, [..] use these flags to alert the pilot to
 525      *   the presence of TAs and RAs." */
 526     nodeTAWarning->setBoolValue(current.RA != AdvisoryClear);
 527 }
 528
 529 ///////////////////////////////////////////////////////////////////////////////
 530 // TCAS::ThreatDetector ///////////////////////////////////////////////////////
 531 ///////////////////////////////////////////////////////////////////////////////
 532
 533 TCAS::ThreatDetector::ThreatDetector(TCAS* _tcas) :
 534     tcas(_tcas),
 535     pAlarmThresholds(&sensitivityLevels[0])
 536 {
 537 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 538     checkCount = 0;
 539 #endif
 540     self.radarAltFt = 0.0;
 541     unitTest();
 542 }
 543
 544 void
 545 TCAS::ThreatDetector::init(void)
 546 {
 547     nodeLat         = fgGetNode("/position/latitude-deg",          true);
 548     nodeLon         = fgGetNode("/position/longitude-deg",         true);
 549     nodePressureAlt = fgGetNode("/position/altitude-ft",           true);
 550     nodeRadarAlt    = fgGetNode("/position/altitude-agl-ft",       true);
 551     nodeHeading     = fgGetNode("/orientation/heading-deg",        true);
 552     nodeVelocity    = fgGetNode("/velocities/airspeed-kt",         true);
 553     nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps",  true);
 554
 555     tcas->advisoryGenerator.init(&self,&currentThreat);
 556 }
 557
 558 /** Update local position and threat sensitivity levels. */
 559 void
 560 TCAS::ThreatDetector::update(void)
 561 {
 562     // update local position
 563     self.lat           = nodeLat->getDoubleValue();
 564     self.lon           = nodeLon->getDoubleValue();
 565     self.pressureAltFt = nodePressureAlt->getDoubleValue();
 566     self.heading       = nodeHeading->getDoubleValue();
 567     self.velocityKt    = nodeVelocity->getDoubleValue();
 568     self.verticalFps   = nodeVerticalFps->getDoubleValue();
 569
 570     /* radar altimeter provides a lot of spikes due to uneven terrain
 571      * MK-VIII GPWS-spec requires smoothing the radar altitude with a
 572      * 10second moving average. Likely the TCAS spec requires the same.
 573      * => We use a cheap 10 second exponential average method.
 574      */
 575     const double SmoothingFactor = 0.3;
 576     self.radarAltFt = nodeRadarAlt->getDoubleValue()*SmoothingFactor +
 577             (1-SmoothingFactor)*self.radarAltFt;
 578
 579 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 580     printf("TCAS::ThreatDetector::update: radarAlt = %f\n",self.radarAltFt);
 581     checkCount = 0;
 582 #endif
 583
 584     // determine current altitude's "Sensitivity Level Definition and Alarm Thresholds"
 585     int sl=0;
 586     for (sl=0;((self.radarAltFt > sensitivityLevels[sl].maxAltitude)&&
 587                (sensitivityLevels[sl].maxAltitude));sl++);
 588     pAlarmThresholds = &sensitivityLevels[sl];
 589     tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds);
 590 }
 591
 592 /** Check if plane's transponder is enabled. */
 593 bool
 594 TCAS::ThreatDetector::checkTransponder(const SGPropertyNode* pModel, float velocityKt)
 595 {
 596     const string name = pModel->getName();
 597     if (name != "multiplayer" && name != "aircraft")
 598     {
 599         // assume non-MP/non-AI planes (e.g. ships) have no transponder
 600         return false;
 601     }
 602
 603     if (velocityKt < 40)
 604     {
 605         /* assume all pilots have their transponder switched off while taxiing/parking
 606          * (at low speed) */
 607         return false;
 608     }
 609
 610     if ((name == "multiplayer")&&
 611         (pModel->getBoolValue("controls/invisible")))
 612     {
 613         // ignored MP plane: pretend transponder is switched off
 614         return false;
 615     }
 616
 617     return true;
 618 }
 619
 620 /** Check if plane is a threat. */
 621 int
 622 TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
 623 {
 624 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 625     checkCount++;
 626 #endif
 627
 628     float velocityKt  = pModel->getDoubleValue("velocities/true-airspeed-kt");
 629
 630     if (!checkTransponder(pModel, velocityKt))
 631         return ThreatInvisible;
 632
 633     int threatLevel = ThreatNone;
 634     float altFt = pModel->getDoubleValue("position/altitude-ft");
 635     currentThreat.relativeAltitudeFt = altFt - self.pressureAltFt;
 636
 637     // save computation time: don't care when relative altitude is excessive
 638     if (fabs(currentThreat.relativeAltitudeFt) > 10000)
 639         return threatLevel;
 640
 641     // position data of current intruder
 642     double lat        = pModel->getDoubleValue("position/latitude-deg");
 643     double lon        = pModel->getDoubleValue("position/longitude-deg");
 644     float heading     = pModel->getDoubleValue("orientation/true-heading-deg");
 645
 646     double distanceNm, bearing;
 647     calcRangeBearing(self.lat, self.lon, lat, lon, distanceNm, bearing);
 648
 649     // save computation time: don't care for excessive distances (also captures NaNs...)
 650     if ((distanceNm > 10)||(distanceNm < 0))
 651         return threatLevel;
 652
 653     currentThreat.verticalFps = pModel->getDoubleValue("velocities/vertical-speed-fps");
 654
 655     /* Detect proximity targets
 656      * [TCASII]: "Any target that is less than 6 nmi in range and within +/-1200ft
 657      *  vertically, but that does not meet the intruder or threat criteria." */
 658     if ((distanceNm < 6)&&
 659         (fabs(currentThreat.relativeAltitudeFt) < 1200))
 660     {
 661         // at least a proximity target
 662         threatLevel = ThreatProximity;
 663     }
 664
 665     /* do not detect any threats when in standby or on ground and taxiing */
 666     if ((mode <= SwitchStandby)||
 667         ((self.radarAltFt < 360)&&(self.velocityKt < 40)))
 668     {
 669         return threatLevel;
 670     }
 671
 672     if (tcas->tracker.active())
 673     {
 674         currentThreat.callsign = pModel->getStringValue("callsign");
 675         currentThreat.isTracked = tcas->tracker.isTracked(currentThreat.callsign);
 676     }
 677     else
 678         currentThreat.isTracked = false;
 679
 680     // first stage: vertical movement
 681     checkVerticalThreat();
 682
 683     // stop processing when no vertical threat
 684     if ((!currentThreat.verticalTA)&&
 685         (!currentThreat.isTracked))
 686         return threatLevel;
 687
 688     // second stage: horizontal movement
 689     horizontalThreat(bearing, distanceNm, heading, velocityKt);
 690
 691     if (!currentThreat.isTracked)
 692     {
 693         // no horizontal threat?
 694         if (!currentThreat.horizontalTA)
 695             return threatLevel;
 696
 697         if ((currentThreat.horizontalTau < 0)||
 698             (currentThreat.verticalTau < 0))
 699         {
 700             // do not trigger new alerts when Tau is negative, but keep existing alerts
 701             int previousThreatLevel = pModel->getIntValue("tcas/threat-level", 0);
 702             if (previousThreatLevel == 0)
 703                 return threatLevel;
 704         }
 705     }
 706
 707 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 708     cout << "#" << checkCount << ": " << pModel->getStringValue("callsign") << endl;
 709 #endif
 710
 711
 712     /* [TCASII]: "For either a TA or an RA to be issued, both the range and
 713      *    vertical criteria, in terms of tau or the fixed thresholds, must be
 714      *    satisfied only one of the criteria is satisfied, TCAS will not issue
 715      *    an advisory." */
 716     if (currentThreat.horizontalTA && currentThreat.verticalTA)
 717         threatLevel = ThreatTA;
 718     if (currentThreat.horizontalRA && currentThreat.verticalRA)
 719         threatLevel = ThreatRA;
 720
 721     if (!tcas->tracker.active())
 722         currentThreat.callsign = pModel->getStringValue("callsign");
 723
 724     tcas->tracker.add(currentThreat.callsign, threatLevel);
 725
 726     // check existing threat level
 727     if (currentThreat.isTracked)
 728     {
 729         int oldLevel = tcas->tracker.getThreatLevel(currentThreat.callsign);
 730         if (oldLevel > threatLevel)
 731             threatLevel = oldLevel;
 732     }
 733
 734     // find all resolution options for this conflict
 735     threatLevel = tcas->advisoryGenerator.resolution(mode, threatLevel, distanceNm, altFt, heading, velocityKt);
 736
 737
 738 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 739     printf("  threat: distance: %4.1f, bearing: %4.1f, alt: %5.1f, velocity: %4.1f, heading: %4.1f, vspeed: %4.1f, "
 740            "own alt: %5.1f, own heading: %4.1f, own velocity: %4.1f, vertical tau: %3.2f"
 741            //", closing speed: %f"
 742            "\n",
 743            distanceNm, relAngle(bearing, self.heading), altFt, velocityKt, heading, currentThreat.verticalFps,
 744            self.altFt, self.heading, self.velocityKt
 745            //, currentThreat.closingSpeedKt
 746            ,currentThreat.verticalTau
 747            );
 748 #endif
 749
 750     return threatLevel;
 751 }
 752
 753 /** Check if plane is a vertical threat. */
 754 void
 755 TCAS::ThreatDetector::checkVerticalThreat(void)
 756 {
 757     // calculate relative vertical speed and altitude
 758     float dV = self.verticalFps - currentThreat.verticalFps;
 759     float dA = currentThreat.relativeAltitudeFt;
 760
 761     currentThreat.verticalTA  = false;
 762     currentThreat.verticalRA  = false;
 763     currentThreat.verticalTau = 0;
 764
 765     /* [TCASII]: "The vertical tau is equal to the altitude separation (feet)
 766      *   divided by the combined vertical speed of the two aircraft (feet/minute)
 767      *   times 60." */
 768     float tau = 0;
 769     if (fabs(dV) > 0.1)
 770         tau = dA/dV;
 771
 772     /* [TCASII]: "When the combined vertical speed of the TCAS and the intruder aircraft
 773      *    is low, TCAS will use a fixed-altitude threshold to determine whether a TA or
 774      *    an RA should be issued." */
 775     if ((fabs(dV) < 3.0)||
 776         ((tau < 0) && (tau > -5)))
 777     {
 778         /* vertical closing speed is low (below 180fpm/3fps), check
 779          * fixed altitude range. */
 780         float abs_dA = fabs(dA);
 781         if (abs_dA < pAlarmThresholds->RA.ALIM)
 782         {
 783             // continuous intrusion at RA-level
 784             currentThreat.verticalTA = true;
 785             currentThreat.verticalRA = true;
 786         }
 787         else
 788         if (abs_dA < pAlarmThresholds->TA.ALIM)
 789         {
 790             // continuous intrusion: with TA-level, but no RA-threat
 791             currentThreat.verticalTA = true;
 792         }
 793         // else: no RA/TA threat
 794     }
 795     else
 796     {
 797         if ((tau < pAlarmThresholds->TA.Tau)&&
 798             (tau >= -5))
 799         {
 800             currentThreat.verticalTA = true;
 801             currentThreat.verticalRA = (tau < pAlarmThresholds->RA.Tau);
 802         }
 803     }
 804     currentThreat.verticalTau = tau;
 805
 806 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 807     if (currentThreat.verticalTA)
 808         printf("  vertical dV=%f (%f-%f), dA=%f\n", dV, self.verticalFps, currentThreat.verticalFps, dA);
 809 #endif
 810 }
 811
 812 /** Check if plane is a horizontal threat. */
 813 void
 814 TCAS::ThreatDetector::horizontalThreat(float bearing, float distanceNm, float heading, float velocityKt)
 815 {
 816     // calculate speed
 817     float vxKt = sin(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - sin(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
 818     float vyKt = cos(heading*SGD_DEGREES_TO_RADIANS)*velocityKt - cos(self.heading*SGD_DEGREES_TO_RADIANS)*self.velocityKt;
 819
 820     // calculate horizontal closing speed
 821     float closingSpeedKt2 = vxKt*vxKt+vyKt*vyKt;
 822     float closingSpeedKt  = sqrt(closingSpeedKt2);
 823
 824     /* [TCASII]: "The range tau is equal to the slant range (nmi) divided by the closing speed
 825      *    (knots) multiplied by 3600."
 826      * => calculate allowed slant range (nmi) based on known maximum tau */
 827     float TA_rangeNm = (pAlarmThresholds->TA.Tau*closingSpeedKt)/3600;
 828     float RA_rangeNm = (pAlarmThresholds->RA.Tau*closingSpeedKt)/3600;
 829
 830     if (closingSpeedKt < 100)
 831     {
 832         /* [TCASII]: "In events where the rate of closure is very low, [..]
 833          *    an intruder aircraft can come very close in range without crossing the
 834          *    range tau boundaries [..]. To provide protection in these types of
 835          *    advisories, the range tau boundaries are modified [..] to use
 836          *    a fixed-range threshold to issue TAs and RAs in these slow closure
 837          *    encounters." */
 838         TA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->TA.DMOD/100.0);
 839         RA_rangeNm += (100.0-closingSpeedKt)*(pAlarmThresholds->RA.DMOD/100.0);
 840     }
 841     if (TA_rangeNm < pAlarmThresholds->TA.DMOD)
 842         TA_rangeNm = pAlarmThresholds->TA.DMOD;
 843     if (RA_rangeNm < pAlarmThresholds->RA.DMOD)
 844         RA_rangeNm = pAlarmThresholds->RA.DMOD;
 845
 846     currentThreat.horizontalTA   = (distanceNm < TA_rangeNm);
 847     currentThreat.horizontalRA   = (distanceNm < RA_rangeNm);
 848     currentThreat.horizontalTau  = -1;
 849
 850     if ((currentThreat.horizontalRA)&&
 851         (currentThreat.verticalRA))
 852     {
 853         /* an RA will be issued. Prepare extra data for the
 854          * traffic resolution stage, i.e. calculate
 855          * exact time tau to horizontal CPA.
 856          */
 857
 858         /* relative position of intruder is
 859          *   Sx(t) = sx + vx*t
 860          *   Sy(t) = sy + vy*t
 861          * horizontal distance to intruder is r(t)
 862          *   r(t) = sqrt( Sx(t)^2 + Sy(t)^2 )
 863          * => horizontal CPA at time t=tau, where r(t) has minimum
 864          * r2(t) := r^2(t) = Sx(t)^2 + Sy(t)^2
 865          * since r(t)>0 for all t => minimum of r(t) is also minimum of r2(t)
 866          * => (d/dt) r2(t) = r2'(t) is 0 for t=tau
 867          *    r2(t) = ((Sx(t)^2 + Sy(t))^2) = c + b*t + a*t^2
 868          * => r2'(t) = b + a*2*t
 869          * at t=tau:
 870          *    r2'(tau) = 0 = b + 2*a*tau
 871          * => tau = -b/(2*a)
 872          */
 873         float sx = sin(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
 874         float sy = cos(bearing*SGD_DEGREES_TO_RADIANS)*distanceNm;
 875         float vx = vxKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
 876         float vy = vyKt * (SG_KT_TO_MPS*SG_METER_TO_NM);
 877         float a  = vx*vx + vy*vy;
 878         float b  = 2*(sx*vx + sy*vy);
 879         float tau = 0;
 880         if (a > 0.0001)
 881             tau = -b/(2*a);
 882 #ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
 883         printf("  Time to horizontal CPA: %4.2f\n",tau);
 884 #endif
 885         if (tau > pAlarmThresholds->RA.Tau)
 886             tau = pAlarmThresholds->RA.Tau;
 887
 888         // remember time to horizontal CPA
 889         currentThreat.horizontalTau = tau;
 890     }
 891 }
 892
 893 /** Test threat detection logic. */
 894 void
 895 TCAS::ThreatDetector::unitTest(void)
 896 {
 897     pAlarmThresholds = &sensitivityLevels[1];
 898 #if 0
 899     // vertical tests
 900     self.verticalFps = 0;
 901     self.altFt = 1000;
 902     cout << "identical altitude and vspeed " << endl;
 903     checkVerticalThreat(self.altFt, self.verticalFps);
 904     cout << "1000ft alt offset, dV=100 " << endl;
 905     checkVerticalThreat(self.altFt+1000, 100);
 906     cout << "-1000ft alt offset, dV=100 " << endl;
 907     checkVerticalThreat(self.altFt-1000, 100);
 908     cout << "3000ft alt offset, dV=10 " << endl;
 909     checkVerticalThreat(self.altFt+3000, 10);
 910     cout << "500ft alt offset, dV=100 " << endl;
 911     checkVerticalThreat(self.altFt+500, 100);
 912     cout << "500ft alt offset, dV=-100 " << endl;
 913     checkVerticalThreat(self.altFt+500, -100);
 914
 915     // horizontal tests
 916     self.heading = 0;
 917     self.velocityKt = 0;
 918     cout << "10nm behind, overtaking with 1Nm/s" << endl;
 919     horizontalThreat(-180, 10, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
 920
 921     cout << "10nm ahead, departing with 1Nm/s" << endl;
 922     horizontalThreat(0, 20, 0, 1/(SG_KT_TO_MPS*SG_METER_TO_NM));
 923
 924     self.heading = 90;
 925     self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
 926     cout << "10nm behind, overtaking with 1Nm/s at 90 degrees" << endl;
 927     horizontalThreat(-90, 20, 90, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
 928
 929     self.heading = 20;
 930     self.velocityKt = 1/(SG_KT_TO_MPS*SG_METER_TO_NM);
 931     cout << "10nm behind, overtaking with 1Nm/s at 20 degrees" << endl;
 932     horizontalThreat(200, 20, 20, 2/(SG_KT_TO_MPS*SG_METER_TO_NM));
 933 #endif
 934 }
 935
 936 ///////////////////////////////////////////////////////////////////////////////
 937 // TCAS::AdvisoryGenerator ////////////////////////////////////////////////////
 938 ///////////////////////////////////////////////////////////////////////////////
 939
 940 TCAS::AdvisoryGenerator::AdvisoryGenerator(TCAS* _tcas) :
 941     tcas(_tcas),
 942     pSelf(NULL),
 943     pCurrentThreat(NULL),
 944     pAlarmThresholds(NULL)
 945 {
 946 }
 947
 948 void
 949 TCAS::AdvisoryGenerator::init(const LocalInfo* _pSelf, ThreatInfo* _pCurrentThreat)
 950 {
 951     pCurrentThreat = _pCurrentThreat;
 952     pSelf = _pSelf;
 953 }
 954
 955 void
 956 TCAS::AdvisoryGenerator::setAlarmThresholds(const SensitivityLevel* _pAlarmThresholds)
 957 {
 958     pAlarmThresholds = _pAlarmThresholds;
 959 }
 960
 961 /** Calculate projected vertical separation at horizontal CPA. */
 962 float
 963 TCAS::AdvisoryGenerator::verticalSeparation(float newVerticalFps)
 964 {
 965     // calculate relative vertical speed and altitude
 966     float dV = pCurrentThreat->verticalFps - newVerticalFps;
 967     float tau = pCurrentThreat->horizontalTau;
 968     // don't use negative tau to project future separation...
 969     if (tau < 0.5)
 970         tau = 0.5;
 971     return pCurrentThreat->relativeAltitudeFt + tau * dV;
 972 }
 973
 974 /** Determine RA sense. */
 975 void
 976 TCAS::AdvisoryGenerator::determineRAsense(int& RASense, bool& isCrossing)
 977 {
 978     /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
 979              *   geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
 980     RASense = 0;
 981     isCrossing = false;
 982
 983     /* [TCASII]: "Based on the range and altitude tracks of the intruder, the CAS logic models the
 984      *   intruder's flight path from its present position to CPA. The CAS logic then models upward
 985      *   and downward sense RAs for own aircraft [..] to determine which sense provides the most
 986      *   vertical separation at CPA." */
 987     float upSenseRelAltFt   = verticalSeparation(+2000/60.0);
 988     float downSenseRelAltFt = verticalSeparation(-2000/60.0);
 989     if (fabs(upSenseRelAltFt) >= fabs(downSenseRelAltFt))
 990         RASense = +1;  // upward
 991     else
 992         RASense = -1; // downward
 993
 994     /* [TCASII]: "In encounters where either of the senses results in the TCAS aircraft crossing through
 995      *   the intruder's altitude, TCAS is designed to select the nonaltitude crossing sense if the
 996      *   noncrossing sense provides the desired vertical separation, known as ALIM, at CPA." */
 997     /* [TCASII]: "If ALIM cannot be obtained in the nonaltitude crossing sense, an altitude
 998      *   crossing RA will be issued." */
 999     if ((RASense > 0)&&
1000         (pCurrentThreat->relativeAltitudeFt > 200))
1001     {
1002         // threat is above and RA is crossing
1003         if (fabs(downSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
1004         {
1005             // non-crossing descend is sufficient
1006             RASense = -1;
1007         }
1008         else
1009         {
1010             // keep crossing climb RA
1011             isCrossing = true;
1012         }
1013     }
1014     else
1015     if ((RASense < 0)&&
1016         (pCurrentThreat->relativeAltitudeFt < -200))
1017     {
1018         // threat is below and RA is crossing
1019         if (fabs(upSenseRelAltFt) > pAlarmThresholds->TA.ALIM)
1020         {
1021             // non-crossing climb is sufficient
1022             RASense = 1;
1023         }
1024         else
1025         {
1026             // keep crossing descent RA
1027             isCrossing = true;
1028         }
1029     }
1030     // else: threat is at same altitude, keep optimal RA sense (non-crossing)
1031
1032     pCurrentThreat->RASense = RASense;
1033
1034 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
1035         printf("  RASense: %i, crossing: %u, relAlt: %4.1f, upward separation: %4.1f, downward separation: %4.1f\n",
1036                RASense,isCrossing,
1037                pCurrentThreat->relativeAltitudeFt,
1038                upSenseRelAltFt,downSenseRelAltFt);
1039 #endif
1040 }
1041
1042 /** Determine suitable resolution advisories. */
1043 int
1044 TCAS::AdvisoryGenerator::resolution(int mode, int threatLevel, float rangeNm, float altFt,
1045                                     float heading, float velocityKt)
1046 {
1047     int RAOption = OptionNone;
1048     int RA       = AdvisoryIntrusion;
1049
1050     // RAs are disabled under certain conditions
1051     if (threatLevel == ThreatRA)
1052     {
1053         /* [TCASII]: "... less than 360 feet, TCAS considers the reporting aircraft
1054          *   to be on the ground. If TCAS determines the intruder to be on the ground, it
1055          *   inhibits the generation of advisories against this aircraft."*/
1056         if (altFt < 360)
1057             threatLevel = ThreatTA;
1058
1059         /* [EUROACAS]: "Certain RAs are inhibited at altitudes based on inputs from the radio altimeter:
1060          *   [..] (c)1000ft (+/- 100ft) and below, all RAs are inhibited;" */
1061         if (pSelf->radarAltFt < 1000)
1062             threatLevel = ThreatTA;
1063
1064         // RAs only issued in mode "Auto" (= "TA/RA" mode)
1065         if (mode != SwitchAuto)
1066             threatLevel = ThreatTA;
1067     }
1068
1069     bool isCrossing = false;
1070     int RASense = 0;
1071     // determine suitable RAs
1072     if (threatLevel == ThreatRA)
1073     {
1074         /* [TCASII]: "[..] a two step process is used to select the appropriate RA for the encounter
1075          *   geometry. The first step in the process is to select the RA sense, i.e., upward or downward." */
1076         determineRAsense(RASense, isCrossing);
1077
1078         /* second step: determine required strength */
1079         if (RASense > 0)
1080         {
1081             // upward
1082
1083             if ((pSelf->verticalFps < -1000/60.0)&&
1084                 (!isCrossing))
1085             {
1086                 // currently descending, see if reducing current descent is sufficient
1087                 float relAltFt = verticalSeparation(-500/60.0);
1088                 if (relAltFt > pAlarmThresholds->TA.ALIM)
1089                     RA |= AdvisoryAdjustVSpeed;
1090             }
1091             RA |= AdvisoryClimb;
1092             if (isCrossing)
1093                 RAOption |= OptionCrossingClimb;
1094         }
1095
1096         if (RASense < 0)
1097         {
1098             // downward
1099
1100             if ((pSelf->verticalFps > 1000/60.0)&&
1101                 (!isCrossing))
1102             {
1103                 // currently climbing, see if reducing current climb is sufficient
1104                 float relAltFt = verticalSeparation(500/60.0);
1105                 if (relAltFt < -pAlarmThresholds->TA.ALIM)
1106                     RA |= AdvisoryAdjustVSpeed;
1107             }
1108             RA |= AdvisoryDescend;
1109             if (isCrossing)
1110                 RAOption |= OptionCrossingDescent;
1111         }
1112
1113         //TODO
1114         /* [TCASII]: "When two TCAS-equipped aircraft are converging vertically with opposite rates
1115          *   and are currently well separated in altitude, TCAS will first issue a vertical speed
1116          *   limit (Negative) RA to reinforce the pilots' likely intention to level off at adjacent
1117          *   flight levels." */
1118
1119         //TODO
1120         /* [TCASII]: "[..] if the CAS logic determines that the response to a Positive RA has provided
1121          *   ALIM feet of vertical separation before CPA, the initial RA will be weakened to either a
1122          *   Do Not Descend RA (after an initial Climb RA) or a Do Not Climb RA (after an initial
1123          *   Descend RA)." */
1124
1125         //TODO
1126         /* [TCASII]: "TCAS is designed to inhibit Increase Descent RAs below 1450 feet AGL; */
1127
1128         /* [TCASII]: "Descend RAs below 1100 feet AGL;" (inhibited) */
1129         if (pSelf->radarAltFt < 1100)
1130         {
1131             RA &= ~AdvisoryDescend;
1132             //TODO Support "Do not descend" RA
1133             RA |= AdvisoryIntrusion;
1134         }
1135     }
1136
1137 #ifdef FEATURE_TCAS_DEBUG_ADV_GENERATOR
1138     cout << "  resolution advisory: " << RA << endl;
1139 #endif
1140
1141     ResolutionAdvisory newAdvisory;
1142     newAdvisory.RAOption    = RAOption;
1143     newAdvisory.RA          = RA;
1144     newAdvisory.threatLevel = threatLevel;
1145     tcas->advisoryCoordinator.add(newAdvisory);
1146
1147     return threatLevel;
1148 }
1149
1150 ///////////////////////////////////////////////////////////////////////////////
1151 // TCAS ///////////////////////////////////////////////////////////////////////
1152 ///////////////////////////////////////////////////////////////////////////////
1153
1154 TCAS::TCAS(SGPropertyNode* pNode) :
1155     name("tcas"),
1156     num(0),
1157     nextUpdateTime(0),
1158     selfTestStep(0),
1159     properties_handler(this),
1160     threatDetector(this),
1161     tracker(this),
1162     advisoryCoordinator(this),
1163     advisoryGenerator(this),
1164     annunciator(this)
1165 {
1166     for (int i = 0; i < pNode->nChildren(); ++i)
1167     {
1168         SGPropertyNode* pChild = pNode->getChild(i);
1169         string cname = pChild->getName();
1170         string cval = pChild->getStringValue();
1171
1172         if (cname == "name")
1173             name = cval;
1174         else if (cname == "number")
1175             num = pChild->getIntValue();
1176         else
1177         {
1178             SG_LOG(SG_INSTR, SG_WARN, "Error in TCAS config logic");
1179             if (name.length())
1180                 SG_LOG(SG_INSTR, SG_WARN, "Section = " << name);
1181         }
1182     }
1183 }
1184
1185 void
1186 TCAS::init(void)
1187 {
1188     annunciator.init();
1189     advisoryCoordinator.init();
1190     threatDetector.init();
1191 }
1192
1193 void
1194 TCAS::reinit(void)
1195 {
1196     nextUpdateTime = 0;
1197     advisoryCoordinator.reinit();
1198 }
1199
1200 void
1201 TCAS::bind(void)
1202 {
1203     SGPropertyNode* node = fgGetNode(("/instrumentation/" + name).c_str(), num, true);
1204
1205     nodeServiceable  = node->getNode("serviceable", true);
1206
1207     // TCAS mode selection (0=off, 1=standby, 2=TA only, 3=auto(TA/RA) )
1208     nodeModeSwitch   = node->getNode("inputs/mode", true);
1209     // self-test button
1210     nodeSelfTest     = node->getNode("inputs/self-test", true);
1211     // default value
1212     nodeSelfTest->setBoolValue(false);
1213
1214 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1215     SGPropertyNode* nodeDebug = node->getNode("debug", true);
1216     // debug triggers
1217     nodeDebugTrigger = nodeDebug->getNode("threat-trigger", true);
1218     nodeDebugRA      = nodeDebug->getNode("threat-RA",      true);
1219     nodeDebugThreat  = nodeDebug->getNode("threat-level",   true);
1220     // default values
1221     nodeDebugTrigger->setBoolValue(false);
1222     nodeDebugRA->setIntValue(3);
1223     nodeDebugThreat->setIntValue(1);
1224 #endif
1225
1226     annunciator.bind(node);
1227     advisoryCoordinator.bind(node);
1228 }
1229
1230 void
1231 TCAS::unbind(void)
1232 {
1233     properties_handler.unbind();
1234 }
1235
1236 /** Monitor traffic for safety threats. */
1237 void
1238 TCAS::update(double dt)
1239 {
1240     if (!nodeServiceable->getBoolValue())
1241         return;
1242     int mode = nodeModeSwitch->getIntValue();
1243     if (mode == SwitchOff)
1244         return;
1245
1246     nextUpdateTime -= dt;
1247     if (nextUpdateTime <= 0.0 )
1248     {
1249         nextUpdateTime = 1.0;
1250
1251         // remove obsolete targets
1252         tracker.update();
1253
1254         // get aircrafts current position/speed/heading
1255         threatDetector.update();
1256
1257         // clear old threats
1258         advisoryCoordinator.clear();
1259
1260         if (nodeSelfTest->getBoolValue())
1261         {
1262             if (threatDetector.getVelocityKt() >= 40)
1263             {
1264                 // disable self-test when plane moves above taxiing speed
1265                 nodeSelfTest->setBoolValue(false);
1266                 selfTestStep = 0;
1267             }
1268             else
1269             {
1270                 selfTest();
1271                 // speed-up self test
1272                 nextUpdateTime = 0;
1273                 // no further TCAS processing during self-test
1274                 return;
1275             }
1276         }
1277
1278 #ifdef FEATURE_TCAS_DEBUG_PROPERTIES
1279         if (nodeDebugTrigger->getBoolValue())
1280         {
1281             // debugging test
1282             ResolutionAdvisory debugAdvisory;
1283             debugAdvisory.RAOption    = OptionNone;
1284             debugAdvisory.RA          = nodeDebugRA->getIntValue();
1285             debugAdvisory.threatLevel = nodeDebugThreat->getIntValue();
1286             advisoryCoordinator.add(debugAdvisory);
1287         }
1288         else
1289 #endif
1290         {
1291             SGPropertyNode* pAi = fgGetNode("/ai/models", true);
1292
1293             // check all aircraft
1294             for (int i = pAi->nChildren() - 1; i >= -1; i--)
1295             {
1296                 SGPropertyNode* pModel = pAi->getChild(i);
1297                 if ((pModel)&&(pModel->nChildren()))
1298                 {
1299                     int threatLevel = threatDetector.checkThreat(mode, pModel);
1300                     /* expose aircraft threat-level (to be used by other instruments,
1301                      * i.e. TCAS display) */
1302                     if (threatLevel==ThreatRA)
1303                         pModel->setIntValue("tcas/ra-sense", -threatDetector.getRASense());
1304                     pModel->setIntValue("tcas/threat-level", threatLevel);
1305                 }
1306             }
1307         }
1308         advisoryCoordinator.update(mode);
1309     }
1310     annunciator.update();
1311 }
1312
1313 /** Run a single self-test iteration. */
1314 void
1315 TCAS::selfTest(void)
1316 {
1317     annunciator.update();
1318     if (annunciator.isPlaying())
1319     {
1320         return;
1321     }
1322
1323     ResolutionAdvisory newAdvisory;
1324     newAdvisory.threatLevel = ThreatRA;
1325     newAdvisory.RA          = AdvisoryClear;
1326     newAdvisory.RAOption    = OptionNone;
1327     // TCAS audio is disabled below 500ft AGL
1328     threatDetector.setRadarAlt(501);
1329
1330     // trigger various advisories
1331     switch(selfTestStep)
1332     {
1333         case 0:
1334             newAdvisory.RA = AdvisoryIntrusion;
1335             newAdvisory.threatLevel = ThreatTA;
1336             break;
1337         case 1:
1338             newAdvisory.RA = AdvisoryClimb;
1339             break;
1340         case 2:
1341             newAdvisory.RA = AdvisoryClimb;
1342             newAdvisory.RAOption = OptionIncreaseClimb;
1343             break;
1344         case 3:
1345             newAdvisory.RA = AdvisoryClimb;
1346             newAdvisory.RAOption = OptionCrossingClimb;
1347             break;
1348         case 4:
1349             newAdvisory.RA = AdvisoryDescend;
1350             break;
1351         case 5:
1352             newAdvisory.RA = AdvisoryDescend;
1353             newAdvisory.RAOption = OptionIncreaseDescend;
1354             break;
1355         case 6:
1356             newAdvisory.RA = AdvisoryDescend;
1357             newAdvisory.RAOption = OptionCrossingDescent;
1358             break;
1359         case 7:
1360             newAdvisory.RA = AdvisoryAdjustVSpeed;
1361             break;
1362         case 8:
1363             newAdvisory.RA = AdvisoryMaintVSpeed;
1364             break;
1365         case 9:
1366             newAdvisory.RA = AdvisoryMonitorVSpeed;
1367             break;
1368         case 10:
1369             newAdvisory.threatLevel = ThreatNone;
1370             newAdvisory.RA = AdvisoryClear;
1371             break;
1372         case 11:
1373             annunciator.test(true);
1374             selfTestStep+=2;
1375             return;
1376         default:
1377             nodeSelfTest->setBoolValue(false);
1378             selfTestStep = 0;
1379             return;
1380     }
1381
1382     advisoryCoordinator.add(newAdvisory);
1383     advisoryCoordinator.update(SwitchAuto);
1384
1385     selfTestStep++;
1386 }
1387
1388 ///////////////////////////////////////////////////////////////////////////////
1389 // TCAS::Tracker //////////////////////////////////////////////////////////////
1390 ///////////////////////////////////////////////////////////////////////////////
1391
1392 TCAS::Tracker::Tracker(TCAS* _tcas) :
1393     tcas(_tcas),
1394     currentTime(0),
1395     haveTargets(false),
1396     newTargets(false)
1397 {
1398     targets.clear();
1399 }
1400
1401 void
1402 TCAS::Tracker::update(void)
1403 {
1404     currentTime = globals->get_sim_time_sec();
1405     newTargets = false;
1406
1407     if (haveTargets)
1408     {
1409         // remove outdated targets
1410         TrackerTargets::iterator it = targets.begin();
1411         while (it != targets.end())
1412         {
1413             TrackerTarget* pTarget = it->second;
1414             if (currentTime - pTarget->TAtimestamp > 10.0)
1415             {
1416                 TrackerTargets::iterator temp = it;
1417                 ++it;
1418 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1419                 printf("target %s no longer a TA threat.\n",temp->first.c_str());
1420 #endif
1421                 targets.erase(temp->first);
1422                 delete pTarget;
1423                 pTarget = NULL;
1424             }
1425             else
1426             {
1427                 if ((pTarget->threatLevel == ThreatRA)&&
1428                     (currentTime - pTarget->RAtimestamp > 7.0))
1429                 {
1430                     pTarget->threatLevel = ThreatTA;
1431 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1432                     printf("target %s no longer an RA threat.\n",it->first.c_str());
1433 #endif
1434                 }
1435                 ++it;
1436             }
1437         }
1438         haveTargets = !targets.empty();
1439     }
1440 }
1441
1442 void
1443 TCAS::Tracker::add(const string callsign, int detectedLevel)
1444 {
1445     TrackerTarget* pTarget = NULL;
1446     if (haveTargets)
1447     {
1448         TrackerTargets::iterator it = targets.find(callsign);
1449         if (it != targets.end())
1450         {
1451             pTarget = it->second;
1452         }
1453     }
1454
1455     if (!pTarget)
1456     {
1457         pTarget = new TrackerTarget();
1458         pTarget->TAtimestamp = 0;
1459         pTarget->RAtimestamp = 0;
1460         pTarget->threatLevel = 0;
1461         newTargets = true;
1462         targets[callsign] = pTarget;
1463 #ifdef FEATURE_TCAS_DEBUG_TRACKER
1464         printf("new target: %s, level: %i\n",callsign.c_str(),detectedLevel);
1465 #endif
1466     }
1467
1468     if (detectedLevel > pTarget->threatLevel)
1469         pTarget->threatLevel = detectedLevel;
1470
1471     if (detectedLevel >= ThreatTA)
1472         pTarget->TAtimestamp = currentTime;
1473
1474     if (detectedLevel >= ThreatRA)
1475         pTarget->RAtimestamp = currentTime;
1476
1477     haveTargets = true;
1478 }
1479
1480 bool
1481 TCAS::Tracker::_isTracked(string callsign)
1482 {
1483     return targets.find(callsign) != targets.end();
1484 }
1485
1486 int
1487 TCAS::Tracker::getThreatLevel(string callsign)
1488 {
1489     TrackerTargets::iterator it = targets.find(callsign);
1490     if (it != targets.end())
1491         return it->second->threatLevel;
1492     else
1493         return 0;
1494 }