From 2a40f9876fa609ecf800f9e494c64ba433e7ddbf Mon Sep 17 00:00:00 2001
From: mfranz <mfranz>
Date: Sun, 25 Jun 2006 12:36:19 +0000
Subject: [PATCH] add GSDI (Ground Speed Drift Angle Indicator) instrument for
helicopters
---
src/Instrumentation/Makefile.am | 1 +
src/Instrumentation/gsdi.cxx | 114 +++++++++++++++++++++++++
src/Instrumentation/gsdi.hxx | 70 +++++++++++++++
src/Instrumentation/instrument_mgr.cxx | 4 +
4 files changed, 189 insertions(+)
create mode 100644 src/Instrumentation/gsdi.cxx
create mode 100644 src/Instrumentation/gsdi.hxx
diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am
index 9775844e1..3bcf9d05f 100644
--- a/src/Instrumentation/Makefile.am
+++ b/src/Instrumentation/Makefile.am
@@ -13,6 +13,7 @@ libInstrumentation_a_SOURCES = \
dme.cxx dme.hxx \
encoder.cxx encoder.hxx \
gps.cxx gps.hxx \
+ gsdi.cxx gsdi.hxx \
gyro.cxx gyro.hxx \
heading_indicator.cxx heading_indicator.hxx \
heading_indicator_fg.cxx heading_indicator_fg.hxx \
diff --git a/src/Instrumentation/gsdi.cxx b/src/Instrumentation/gsdi.cxx
new file mode 100644
index 000000000..b624e6405
--- /dev/null
+++ b/src/Instrumentation/gsdi.cxx
@@ -0,0 +1,114 @@
+// gsdi.cxx - Ground Speed Drift Angle Indicator (known as GSDI or GSDA)
+// Written by Melchior FRANZ, started 2006.
+//
+// Copyright (C) 2006 Melchior FRANZ - mfranz#aon:at
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/sg_inlines.h>
+#include <simgear/constants.h>
+
+#include <Main/fg_props.hxx>
+#include "gsdi.hxx"
+
+
+/*
+ * Failures or inaccuracies are currently not modeled due to lack of data.
+ * The Doppler based GSDI should output unreliable data with increasing
+ * pitch, roll, vertical acceleration and altitude-agl.
+ */
+
+
+GSDI::GSDI(SGPropertyNode *node) :
+ _name("gsdi"),
+ _num(0)
+{
+ for (int i = 0; i < node->nChildren(); ++i) {
+ SGPropertyNode *child = node->getChild(i);
+ string name = child->getName();
+
+ if (name == "name") {
+ _name = child->getStringValue();
+ } else if (name == "number") {
+ _num = child->getIntValue();
+ } else {
+ SG_LOG(SG_INSTR, SG_WARN, "Error in gsdi config logic");
+ if (_name.length())
+ SG_LOG(SG_INSTR, SG_WARN, "Section = " << _name);
+ }
+ }
+}
+
+
+GSDI::GSDI()
+{
+}
+
+
+GSDI::~GSDI()
+{
+}
+
+
+void GSDI::init()
+{
+ string branch;
+ branch = "/instrumentation/" + _name;
+ SGPropertyNode *n = fgGetNode(branch.c_str(), _num, true);
+ _serviceableN = n->getNode("serviceable", true);
+
+ // input
+ _headingN = fgGetNode("/orientation/heading-deg", true);
+ _ubodyN = fgGetNode("/velocities/uBody-fps", true);
+ _vbodyN = fgGetNode("/velocities/vBody-fps", true);
+ _wind_dirN = fgGetNode("/environment/wind-from-heading-deg", true);
+ _wind_speedN = fgGetNode("/environment/wind-speed-kt", true);
+
+ // output
+ _drift_uN = n->getNode("drift-u-kt", true);
+ _drift_vN = n->getNode("drift-v-kt", true);
+ _drift_speedN = n->getNode("drift-speed-kt", true);
+ _drift_angleN = n->getNode("drift-angle-deg", true);
+}
+
+
+void GSDI::update(double /*delta_time_sec*/)
+{
+ if (!_serviceableN->getBoolValue())
+ return;
+
+ double wind_speed = _wind_speedN->getDoubleValue();
+ double rel_wind_dir = (_headingN->getDoubleValue() - _wind_dirN->getDoubleValue())
+ * SGD_DEGREES_TO_RADIANS;
+ double wind_u = wind_speed * cos(rel_wind_dir);
+ double wind_v = wind_speed * sin(rel_wind_dir);
+
+ double u = _ubodyN->getDoubleValue() * SG_FPS_TO_KT - wind_u;
+ double v = _vbodyN->getDoubleValue() * SG_FPS_TO_KT + wind_v;
+
+ double speed = sqrt(u * u + v * v);
+ double angle = atan2(v, u) * SGD_RADIANS_TO_DEGREES;
+
+ _drift_uN->setDoubleValue(u);
+ _drift_vN->setDoubleValue(v);
+ _drift_speedN->setDoubleValue(speed);
+ _drift_angleN->setDoubleValue(angle);
+}
+
+// end of gsdi.cxx
diff --git a/src/Instrumentation/gsdi.hxx b/src/Instrumentation/gsdi.hxx
new file mode 100644
index 000000000..b63566bc9
--- /dev/null
+++ b/src/Instrumentation/gsdi.hxx
@@ -0,0 +1,70 @@
+// gsdi.cxx - Ground Speed Drift Angle Indicator (known as GSDI or GSDA)
+// Written by Melchior FRANZ, started 2006.
+//
+// Copyright (C) 2006 Melchior FRANZ - mfranz#aon:at
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+
+#ifndef __INSTRUMENTS_GSDI_HXX
+#define __INSTRUMENTS_GSDI_HXX 1
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+
+/**
+ * Input properties:
+ *
+ * /instrumentation/gsdi/serviceable
+ * /orientation/heading-deg
+ * /velocities/uBody-fps
+ * /velocities/vBody-fps
+ * /environment/wind-from-heading-deg
+ * /environment/wind-speed-kt
+ *
+ * Output properties:
+ *
+ * /instrumentation/gsdi/drift-u-kt
+ * /instrumentation/gsdi/drift-v-kt
+ * /instrumentation/gsdi/drift-speed-kt
+ * /instrumentation/gsdi/drift-angle-deg
+ */
+class GSDI : public SGSubsystem
+{
+public:
+ GSDI(SGPropertyNode *node);
+ GSDI();
+ virtual ~GSDI();
+
+ virtual void init();
+ virtual void update(double dt);
+
+private:
+ string _name;
+ int _num;
+
+ SGPropertyNode_ptr _serviceableN;
+ SGPropertyNode_ptr _headingN;
+ SGPropertyNode_ptr _ubodyN;
+ SGPropertyNode_ptr _vbodyN;
+ SGPropertyNode_ptr _wind_speedN;
+ SGPropertyNode_ptr _wind_dirN;
+ SGPropertyNode_ptr _drift_uN;
+ SGPropertyNode_ptr _drift_vN;
+ SGPropertyNode_ptr _drift_speedN;
+ SGPropertyNode_ptr _drift_angleN;
+};
+
+#endif // _INSTRUMENTS_GSDI_HXX
diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx
index 65e3c1e86..1da1abb71 100644
--- a/src/Instrumentation/instrument_mgr.cxx
+++ b/src/Instrumentation/instrument_mgr.cxx
@@ -29,6 +29,7 @@
#include "dme.hxx"
#include "encoder.hxx"
#include "gps.hxx"
+#include "gsdi.hxx"
#include "heading_indicator.hxx"
#include "heading_indicator_fg.hxx"
#include "kr_87.hxx"
@@ -122,6 +123,9 @@ bool FGInstrumentMgr::build ()
} else if ( name == "gps" ) {
set_subsystem( "instrument" + temp.str(),
new GPS( node ), 0.45 );
+ } else if ( name == "gsdi" ) {
+ set_subsystem( "instrument" + temp.str(),
+ new GSDI( node ) );
} else if ( name == "heading-indicator" ) {
set_subsystem( "instrument" + temp.str(),
new HeadingIndicator( node ) );