src/Instrumentation/heading_indicator_dg.cxx

   1 // heading_indicator_dg.cxx - a Directional Gyro (DG) compass.
   2 // Based on the vacuum driven Heading Indicator Written by David Megginson,
   3 // started 2002.
   4 //
   5 // Written by Vivian Meazza, started 2005.
   6 //
   7 // This file is in the Public Domain and comes with no warranty.
   8
   9 #ifdef HAVE_CONFIG_H
  10 #  include "config.h"
  11 #endif
  12
  13 #include <simgear/compiler.h>
  14 #include <simgear/sg_inlines.h>
  15 #include <simgear/math/SGMath.hxx>
  16 #include <iostream>
  17 #include <string>
  18 #include <sstream>
  19
  20 #include <Main/fg_props.hxx>
  21 #include <Main/util.hxx>
  22
  23 #include "heading_indicator_dg.hxx"
  24
  25 /** Macro calculating x^6 (faster than super-slow math/pow). */
  26 #define POW6(x) (x*x*x*x*x*x)
  27
  28 HeadingIndicatorDG::HeadingIndicatorDG ( SGPropertyNode *node ) :
  29     name("heading-indicator-dg"),
  30     num(0)
  31 {
  32     int i;
  33     for ( i = 0; i < node->nChildren(); ++i ) {
  34         SGPropertyNode *child = node->getChild(i);
  35         std::string cname = child->getName();
  36         std::string cval = child->getStringValue();
  37         if ( cname == "name" ) {
  38             name = cval;
  39         } else if ( cname == "number" ) {
  40             num = child->getIntValue();
  41         } else {
  42             SG_LOG( SG_INSTR, SG_WARN, "Error in DG heading-indicator config logic" );
  43             if ( name.length() ) {
  44                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  45             }
  46         }
  47     }
  48 }
  49
  50 HeadingIndicatorDG::~HeadingIndicatorDG ()
  51 {
  52 }
  53
  54 void
  55 HeadingIndicatorDG::init ()
  56 {
  57     std::string branch;
  58     branch = "/instrumentation/" + name;
  59
  60     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  61     _yaw_rate_node   = fgGetNode("/orientation/yaw-rate-degps", true);
  62      _g_node         = fgGetNode("/accelerations/pilot-g", true);
  63
  64     SGPropertyNode *node    = fgGetNode(branch.c_str(), num, true );
  65     _offset_node            = node->getChild("offset-deg", 0, true);
  66     _serviceable_node       = node->getChild("serviceable", 0, true);
  67     _heading_bug_error_node = node->getChild("heading-bug-error-deg", 0, true);
  68     _error_node             = node->getChild("error-deg", 0, true);
  69     _nav1_error_node        = node->getChild("nav1-course-error-deg", 0, true);
  70     _heading_out_node       = node->getChild("indicated-heading-deg", 0, true);
  71     _align_node             = node->getChild("align-deg", 0, true);
  72
  73     _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true);
  74
  75     reinit();
  76 }
  77
  78 void
  79 HeadingIndicatorDG::bind ()
  80 {
  81     std::ostringstream temp;
  82     std::string branch;
  83     temp << num;
  84     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  85
  86     fgTie((branch + "/serviceable").c_str(),
  87           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  88     fgTie((branch + "/spin").c_str(),
  89           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  90 }
  91
  92 void
  93 HeadingIndicatorDG::unbind ()
  94 {
  95     std::ostringstream temp;
  96     std::string branch;
  97     temp << num;
  98     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  99
 100     fgUntie((branch + "/serviceable").c_str());
 101     fgUntie((branch + "/spin").c_str());
 102 }
 103
 104 void
 105 HeadingIndicatorDG::reinit (void)
 106 {
 107     // reset errors/drift values
 108     _align_node->setDoubleValue(0.0);
 109     _error_node->setDoubleValue(0.0);
 110     _offset_node->setDoubleValue(0.0);
 111
 112     _last_heading_deg = _heading_in_node->getDoubleValue();
 113     _last_indicated_heading_dg = _last_heading_deg;
 114
 115     _gyro.reinit();
 116 }
 117
 118 void
 119 HeadingIndicatorDG::update (double dt)
 120 {
 121                                 // Get the spin from the gyro
 122     _gyro.set_power_norm(_electrical_node->getDoubleValue());
 123
 124     _gyro.update(dt);
 125
 126     // read inputs
 127     double spin = _gyro.get_spin_norm();
 128     double heading = _heading_in_node->getDoubleValue();
 129     double offset = _offset_node->getDoubleValue();
 130
 131     // calculate scaling factor
 132     double factor = POW6(spin);
 133
 134     // calculate time-based precession (scaled by spin factor, since
 135     // there is no precession when the gyro is stuck).
 136     offset -= dt * (0.25 / 60.0) * factor; // 360deg/day
 137
 138     // indication should get more and more stuck at low gyro spins
 139     if (spin < 0.9)
 140     {
 141         // when gyro spin is low, then any heading change results in
 142         // increasing the offset
 143         double diff = SGMiscd::normalizePeriodic(-180.0, 180.0, _last_heading_deg - heading);
 144         // scaled by 1-factor, so indication is fully stuck at spin==0 (offset compensates
 145         // any heading change)
 146         offset += diff * (1.0-factor);
 147     }
 148     _last_heading_deg = heading;
 149
 150     // normalize offset
 151     offset = SGMiscd::normalizePeriodic(-180.0,180.0,offset);
 152     _offset_node->setDoubleValue(offset);
 153
 154                                 // No magvar - set the alignment manually
 155     double align = _align_node->getDoubleValue();
 156
 157                                 // Movement-induced error
 158     double yaw_rate = _yaw_rate_node->getDoubleValue();
 159     double error = _error_node->getDoubleValue();
 160     double g = _g_node->getDoubleValue();
 161     if ( fabs ( yaw_rate ) > 5 ) {
 162         error += 0.033 * -yaw_rate * dt * factor;
 163     }
 164
 165     if ( g > 1.5 || g < -0.5){
 166         error += 0.033 * g * dt * factor;
 167     }
 168     _error_node->setDoubleValue(error);
 169
 170                                 // Now, we have to get the current
 171                                 // heading and the last heading into
 172                                 // the same range.
 173     while ((heading - _last_indicated_heading_dg) > 180)
 174         _last_indicated_heading_dg += 360;
 175     while ((heading - _last_indicated_heading_dg) < -180)
 176         _last_indicated_heading_dg -= 360;
 177     heading = fgGetLowPass(_last_indicated_heading_dg, heading, dt * 100);
 178     _last_indicated_heading_dg = heading;
 179
 180     heading += offset + align + error;
 181     heading = SGMiscd::normalizePeriodic(0.0,360.0,heading);
 182
 183     _heading_out_node->setDoubleValue(heading);
 184
 185                                  // calculate the difference between the indicated heading
 186                                  // and the selected heading for use with an autopilot
 187     SGPropertyNode *bnode
 188         = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
 189     if ( bnode ) {
 190         double diff = bnode->getDoubleValue() - heading;
 191         if ( diff < -180.0 ) { diff += 360.0; }
 192         if ( diff > 180.0 ) { diff -= 360.0; }
 193             _heading_bug_error_node->setDoubleValue( diff );
 194     }
 195                                  // calculate the difference between the indicated heading
 196                                  // and the selected nav1 radial for use with an autopilot
 197     SGPropertyNode *nnode
 198         = fgGetNode( "/instrumentation/nav/radials/selected-deg", false );
 199     if ( nnode ) {
 200         double diff = nnode->getDoubleValue() - heading;
 201         if ( diff < -180.0 ) { diff += 360.0; }
 202         if ( diff > 180.0 ) { diff -= 360.0; }
 203         _nav1_error_node->setDoubleValue( diff );
 204     }
 205 }
 206
 207 // end of heading_indicator_dg.cxx