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 #include <simgear/compiler.h>
  10 #include <iostream>
  11 #include <string>
  12 #include <sstream>
  13
  14 #include <Main/fg_props.hxx>
  15 #include <Main/util.hxx>
  16
  17 #include "heading_indicator_dg.hxx"
  18
  19
  20 HeadingIndicatorDG::HeadingIndicatorDG ( SGPropertyNode *node ) :
  21     name("heading-indicator-dg"),
  22     num(0)
  23 {
  24     int i;
  25     for ( i = 0; i < node->nChildren(); ++i ) {
  26         SGPropertyNode *child = node->getChild(i);
  27         string cname = child->getName();
  28         string cval = child->getStringValue();
  29         if ( cname == "name" ) {
  30             name = cval;
  31         } else if ( cname == "number" ) {
  32             num = child->getIntValue();
  33         } else {
  34             SG_LOG( SG_INSTR, SG_WARN, "Error in DG heading-indicator config logic" );
  35             if ( name.length() ) {
  36                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  37             }
  38         }
  39     }
  40 }
  41
  42 HeadingIndicatorDG::HeadingIndicatorDG ()
  43 {
  44 }
  45
  46 HeadingIndicatorDG::~HeadingIndicatorDG ()
  47 {
  48 }
  49
  50 void
  51 HeadingIndicatorDG::init ()
  52 {
  53     string branch;
  54     branch = "/instrumentation/" + name;
  55
  56     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  57     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  58     _offset_node = node->getChild("offset-deg", 0, true);
  59     _serviceable_node = node->getChild("serviceable", 0, true);
  60     _error_node = node->getChild("heading-bug-error-deg", 0, true);
  61     _nav1_error_node = node->getChild("nav1-course-error-deg", 0, true);
  62     _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
  63     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  64                          _offset_node->getDoubleValue());
  65     _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true);
  66 }
  67
  68 void
  69 HeadingIndicatorDG::bind ()
  70 {
  71     std::ostringstream temp;
  72     string branch;
  73     temp << num;
  74     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  75
  76     fgTie((branch + "/serviceable").c_str(),
  77           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  78     fgTie((branch + "/spin").c_str(),
  79           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  80 }
  81
  82 void
  83 HeadingIndicatorDG::unbind ()
  84 {
  85     std::ostringstream temp;
  86     string branch;
  87     temp << num;
  88     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  89
  90     fgUntie((branch + "/serviceable").c_str());
  91     fgUntie((branch + "/spin").c_str());
  92 }
  93
  94 void
  95 HeadingIndicatorDG::update (double dt)
  96 {
  97                                 // Get the spin from the gyro
  98     _gyro.set_power_norm(_electrical_node->getDoubleValue());
  99
 100     _gyro.update(dt);
 101     double spin = _gyro.get_spin_norm();
 102
 103                                 // No time-based precession     for a flux gate compass
 104                                 // No magvar
 105     double offset = 0;
 106
 107                                 // TODO: movement-induced error
 108
 109                                 // Next, calculate the indicated heading,
 110                                 // introducing errors.
 111     double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
 112     double heading = _heading_in_node->getDoubleValue();
 113
 114                                 // Now, we have to get the current
 115                                 // heading and the last heading into
 116                                 // the same range.
 117     while ((heading - _last_heading_deg) > 180)
 118         _last_heading_deg += 360;
 119     while ((heading - _last_heading_deg) < -180)
 120         _last_heading_deg -= 360;
 121
 122     heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
 123     _last_heading_deg = heading;
 124
 125     heading += offset;
 126     while (heading < 0)
 127         heading += 360;
 128     while (heading > 360)
 129         heading -= 360;
 130
 131     _heading_out_node->setDoubleValue(heading);
 132
 133                                  // calculate the difference between the indicacted heading
 134                                  // and the selected heading for use with an autopilot
 135     static SGPropertyNode *bnode
 136         = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
 137     if ( bnode ) {
 138         double diff = bnode->getDoubleValue() - heading;
 139         if ( diff < -180.0 ) { diff += 360.0; }
 140         if ( diff > 180.0 ) { diff -= 360.0; }
 141         _error_node->setDoubleValue( diff );
 142     }
 143                                  // calculate the difference between the indicated heading
 144                                  // and the selected nav1 radial for use with an autopilot
 145     SGPropertyNode *nnode
 146         = fgGetNode( "/instrumentation/nav/radials/selected-deg", false );
 147     if ( nnode ) {
 148         double diff = nnode->getDoubleValue() - heading;
 149         if ( diff < -180.0 ) { diff += 360.0; }
 150         if ( diff > 180.0 ) { diff -= 360.0; }
 151         _nav1_error_node->setDoubleValue( diff );
 152     }
 153 }
 154
 155 // end of heading_indicator_fg.cxx