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("gsdi"),
  40         _num(0)
  41 {
  42         for (int i = 0; i < node->nChildren(); ++i) {
  43                 SGPropertyNode *child = node->getChild(i);
  44                 string name = child->getName();
  45
  46                 if (name == "name") {
  47                         _name = child->getStringValue();
  48                 } else if (name == "number") {
  49                         _num = child->getIntValue();
  50                 } else {
  51                         SG_LOG(SG_INSTR, SG_WARN, "Error in gsdi config logic");
  52                         if (_name.length())
  53                                 SG_LOG(SG_INSTR, SG_WARN, "Section = " << _name);
  54                 }
  55         }
  56 }
  57
  58
  59 GSDI::GSDI()
  60 {
  61 }
  62
  63
  64 GSDI::~GSDI()
  65 {
  66 }
  67
  68
  69 void GSDI::init()
  70 {
  71         string branch;
  72         branch = "/instrumentation/" + _name;
  73         SGPropertyNode *n = fgGetNode(branch.c_str(), _num, true);
  74         _serviceableN = n->getNode("serviceable", true);
  75
  76         // input
  77         _headingN = fgGetNode("/orientation/heading-deg", true);
  78         _ubodyN = fgGetNode("/velocities/uBody-fps", true);
  79         _vbodyN = fgGetNode("/velocities/vBody-fps", true);
  80         _wind_dirN = fgGetNode("/environment/wind-from-heading-deg", true);
  81         _wind_speedN = fgGetNode("/environment/wind-speed-kt", true);
  82
  83         // output
  84         _drift_uN = n->getNode("drift-u-kt", true);
  85         _drift_vN = n->getNode("drift-v-kt", true);
  86         _drift_speedN = n->getNode("drift-speed-kt", true);
  87         _drift_angleN = n->getNode("drift-angle-deg", true);
  88 }
  89
  90
  91 void GSDI::update(double /*delta_time_sec*/)
  92 {
  93         if (!_serviceableN->getBoolValue())
  94                 return;
  95
  96         double wind_speed = _wind_speedN->getDoubleValue();
  97         double rel_wind_dir = (_headingN->getDoubleValue() - _wind_dirN->getDoubleValue())
  98                         * SGD_DEGREES_TO_RADIANS;
  99         double wind_u = wind_speed * cos(rel_wind_dir);
 100         double wind_v = wind_speed * sin(rel_wind_dir);
 101
 102         double u = _ubodyN->getDoubleValue() * SG_FPS_TO_KT - wind_u;
 103         double v = _vbodyN->getDoubleValue() * SG_FPS_TO_KT + wind_v;
 104
 105         double speed = sqrt(u * u + v * v);
 106         double angle = atan2(v, u) * SGD_RADIANS_TO_DEGREES;
 107
 108         _drift_uN->setDoubleValue(u);
 109         _drift_vN->setDoubleValue(v);
 110         _drift_speedN->setDoubleValue(speed);
 111         _drift_angleN->setDoubleValue(angle);
 112 }
 113
 114 // end of gsdi.cxx