From 2a40f9876fa609ecf800f9e494c64ba433e7ddbf Mon Sep 17 00:00:00 2001 From: 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 +#endif + +#include +#include + +#include
+#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 +#include + + +/** + * 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 ) ); -- 2.39.5