src/Instrumentation/gsdi.cxx

   1 // gsdi.cxx - Ground Speed Drift Angle Indicator (known as GSDI or GSDA)
   2 // Written by Melchior FRANZ, started 2006.
   3 //
   4 // Copyright (C) 2006  Melchior FRANZ - mfranz#aon:at
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19
  20 #ifdef HAVE_CONFIG_H
  21 #  include <config.h>
  22 #endif
  23
  24 #include <simgear/sg_inlines.h>
  25 #include <simgear/constants.h>
  26
  27 #include <Main/fg_props.hxx>
  28 #include "gsdi.hxx"
  29
  30
  31 /*
  32  * Failures or inaccuracies are currently not modeled due to lack of data.
  33  * The Doppler based GSDI should output unreliable data with increasing
  34  * pitch, roll, vertical acceleration and altitude-agl.
  35  */
  36
  37
  38 GSDI::GSDI(SGPropertyNode *node) :
  39         _name(node->getStringValue("name", "gsdi")),
  40         _num(node->getIntValue("number", 0))
  41 {
  42 }
  43
  44
  45 GSDI::~GSDI()
  46 {
  47 }
  48
  49
  50 void GSDI::init()
  51 {
  52   std::string branch;
  53         branch = "/instrumentation/" + _name;
  54         SGPropertyNode *n = fgGetNode(branch.c_str(), _num, true);
  55         _serviceableN = n->getNode("serviceable", true);
  56
  57         // input
  58         _headingN = fgGetNode("/orientation/heading-deg", true);
  59         _ubodyN = fgGetNode("/velocities/uBody-fps", true);
  60         _vbodyN = fgGetNode("/velocities/vBody-fps", true);
  61         _wind_dirN = fgGetNode("/environment/wind-from-heading-deg", true);
  62         _wind_speedN = fgGetNode("/environment/wind-speed-kt", true);
  63
  64         // output
  65         _drift_uN = n->getNode("drift-u-kt", true);
  66         _drift_vN = n->getNode("drift-v-kt", true);
  67         _drift_speedN = n->getNode("drift-speed-kt", true);
  68         _drift_angleN = n->getNode("drift-angle-deg", true);
  69 }
  70
  71
  72 void GSDI::update(double /*delta_time_sec*/)
  73 {
  74         if (!_serviceableN->getBoolValue())
  75                 return;
  76
  77         double wind_speed = _wind_speedN->getDoubleValue();
  78         double rel_wind_dir = (_headingN->getDoubleValue() - _wind_dirN->getDoubleValue())
  79                         * SGD_DEGREES_TO_RADIANS;
  80         double wind_u = wind_speed * cos(rel_wind_dir);
  81         double wind_v = wind_speed * sin(rel_wind_dir);
  82
  83         double u = _ubodyN->getDoubleValue() * SG_FPS_TO_KT - wind_u;
  84         double v = _vbodyN->getDoubleValue() * SG_FPS_TO_KT + wind_v;
  85
  86         double speed = sqrt(u * u + v * v);
  87         double angle = atan2(v, u) * SGD_RADIANS_TO_DEGREES;
  88
  89         _drift_uN->setDoubleValue(u);
  90         _drift_vN->setDoubleValue(v);
  91         _drift_speedN->setDoubleValue(speed);
  92         _drift_angleN->setDoubleValue(angle);
  93 }
  94
  95 // end of gsdi.cxx