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