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         _ubodyN = fgGetNode("/velocities/uBody-fps", true);
  59         _vbodyN = fgGetNode("/velocities/vBody-fps", true);
  60
  61         // output
  62         _drift_uN = n->getNode("drift-u-kt", true);
  63         _drift_vN = n->getNode("drift-v-kt", true);
  64         _drift_speedN = n->getNode("drift-speed-kt", true);
  65         _drift_angleN = n->getNode("drift-angle-deg", true);
  66 }
  67
  68
  69 void GSDI::update(double /*delta_time_sec*/)
  70 {
  71         if (!_serviceableN->getBoolValue())
  72                 return;
  73
  74         double u = _ubodyN->getDoubleValue() * SG_FPS_TO_KT;
  75         double v = _vbodyN->getDoubleValue() * SG_FPS_TO_KT;
  76
  77         double speed = sqrt(u * u + v * v);
  78         double angle = atan2(v, u) * SGD_RADIANS_TO_DEGREES;
  79
  80         _drift_uN->setDoubleValue(u);
  81         _drift_vN->setDoubleValue(v);
  82         _drift_speedN->setDoubleValue(speed);
  83         _drift_angleN->setDoubleValue(angle);
  84 }
  85
  86 // end of gsdi.cxx