src/Instrumentation/heading_indicator_dg.cxx

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