//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
//
///////////////////////////////////////////////////////////////////////////////
#include <simgear/debug/logstream.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/sound/soundmgr_openal.hxx>
+#include <simgear/sound/sample_group.hxx>
#include <simgear/structure/exception.hxx>
using std::string;
# include <Include/no_version.h>
#endif
-#include <Main/fg_props.hxx>
-#include <Main/globals.hxx>
-#include "instrument_mgr.hxx"
-#include "tcas.hxx"
-
///////////////////////////////////////////////////////////////////////////////
// debug switches /////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//#define FEATURE_TCAS_DEBUG_ADV_GENERATOR
//#define FEATURE_TCAS_DEBUG_PROPERTIES
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include "instrument_mgr.hxx"
+#include "tcas.hxx"
+
///////////////////////////////////////////////////////////////////////////////
// constants //////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void
TCAS::AdvisoryCoordinator::init(void)
+{
+ reinit();
+}
+
+void
+TCAS::AdvisoryCoordinator::reinit(void)
{
clear();
previous = current;
}
/* [TCASII]: "Aural annunciations are inhibited below 500+/-100 feet AGL." */
- if ((tcas->threatDetector.getAlt() > 500)&&
+ if ((tcas->threatDetector.getRadarAlt() > 500)&&
(mode >= SwitchTaOnly))
tcas->annunciator.trigger(current, revertedRA);
else
tcas(_tcas),
pAlarmThresholds(&sensitivityLevels[0])
{
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
+ checkCount = 0;
+#endif
+ self.radarAltFt = 0.0;
unitTest();
}
{
nodeLat = fgGetNode("/position/latitude-deg", true);
nodeLon = fgGetNode("/position/longitude-deg", true);
- nodeAlt = fgGetNode("/position/altitude-ft", true);
+ nodePressureAlt = fgGetNode("/position/altitude-ft", true);
+ nodeRadarAlt = fgGetNode("/position/altitude-agl-ft", true);
nodeHeading = fgGetNode("/orientation/heading-deg", true);
nodeVelocity = fgGetNode("/velocities/airspeed-kt", true);
nodeVerticalFps = fgGetNode("/velocities/vertical-speed-fps", true);
TCAS::ThreatDetector::update(void)
{
// update local position
- self.lat = nodeLat->getDoubleValue();
- self.lon = nodeLon->getDoubleValue();
- self.altFt = nodeAlt->getDoubleValue();
- self.heading = nodeHeading->getDoubleValue();
- self.velocityKt = nodeVelocity->getDoubleValue();
- self.verticalFps = nodeVerticalFps->getDoubleValue();
+ self.lat = nodeLat->getDoubleValue();
+ self.lon = nodeLon->getDoubleValue();
+ self.pressureAltFt = nodePressureAlt->getDoubleValue();
+ self.heading = nodeHeading->getDoubleValue();
+ self.velocityKt = nodeVelocity->getDoubleValue();
+ self.verticalFps = nodeVerticalFps->getDoubleValue();
+
+ /* radar altimeter provides a lot of spikes due to uneven terrain
+ * MK-VIII GPWS-spec requires smoothing the radar altitude with a
+ * 10second moving average. Likely the TCAS spec requires the same.
+ * => We use a cheap 10 second exponential average method.
+ */
+ const double SmoothingFactor = 0.3;
+ self.radarAltFt = nodeRadarAlt->getDoubleValue()*SmoothingFactor +
+ (1-SmoothingFactor)*self.radarAltFt;
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
+ printf("TCAS::ThreatDetector::update: radarAlt = %f\n",self.radarAltFt);
checkCount = 0;
+#endif
// determine current altitude's "Sensitivity Level Definition and Alarm Thresholds"
int sl=0;
- for (sl=0;((self.altFt > sensitivityLevels[sl].maxAltitude)&&
+ for (sl=0;((self.radarAltFt > sensitivityLevels[sl].maxAltitude)&&
(sensitivityLevels[sl].maxAltitude));sl++);
pAlarmThresholds = &sensitivityLevels[sl];
tcas->advisoryGenerator.setAlarmThresholds(pAlarmThresholds);
int
TCAS::ThreatDetector::checkThreat(int mode, const SGPropertyNode* pModel)
{
+#ifdef FEATURE_TCAS_DEBUG_THREAT_DETECTOR
checkCount++;
+#endif
float velocityKt = pModel->getDoubleValue("velocities/true-airspeed-kt");
int threatLevel = ThreatNone;
float altFt = pModel->getDoubleValue("position/altitude-ft");
- currentThreat.relativeAltitudeFt = altFt - self.altFt;
+ currentThreat.relativeAltitudeFt = altFt - self.pressureAltFt;
// save computation time: don't care when relative altitude is excessive
if (fabs(currentThreat.relativeAltitudeFt) > 10000)
/* do not detect any threats when in standby or on ground and taxiing */
if ((mode <= SwitchStandby)||
- ((self.altFt < 360)&&(self.velocityKt < 40)))
+ ((self.radarAltFt < 360)&&(self.velocityKt < 40)))
{
return threatLevel;
}
/* [EUROACAS]: "Certain RAs are inhibited at altitudes based on inputs from the radio altimeter:
* [..] (c)1000ft (+/- 100ft) and below, all RAs are inhibited;" */
- if (pSelf->altFt < 1000)
+ if (pSelf->radarAltFt < 1000)
threatLevel = ThreatTA;
// RAs only issued in mode "Auto" (= "TA/RA" mode)
/* [TCASII]: "TCAS is designed to inhibit Increase Descent RAs below 1450 feet AGL; */
/* [TCASII]: "Descend RAs below 1100 feet AGL;" (inhibited) */
- if (pSelf->altFt < 1100)
+ if (pSelf->radarAltFt < 1100)
{
RA &= ~AdvisoryDescend;
//TODO Support "Do not descend" RA
threatDetector.init();
}
+void
+TCAS::reinit(void)
+{
+ nextUpdateTime = 0;
+ advisoryCoordinator.reinit();
+}
+
void
TCAS::bind(void)
{
newAdvisory.threatLevel = ThreatRA;
newAdvisory.RA = AdvisoryClear;
newAdvisory.RAOption = OptionNone;
- // TCAS audio is disabled below 500ft
- threatDetector.setAlt(501);
+ // TCAS audio is disabled below 500ft AGL
+ threatDetector.setRadarAlt(501);
// trigger various advisories
switch(selfTestStep)
// TCAS::Tracker //////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
-TCAS::Tracker::Tracker(TCAS* _tcas) :
- tcas(_tcas),
+TCAS::Tracker::Tracker(TCAS*) :
currentTime(0),
haveTargets(false),
newTargets(false)