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