X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmag_compass.cxx;h=30dc1fb7372d422afedace374ca4cb670f1151e4;hb=34433c1fa1d10e389ffd84ec115d6c8036c5b885;hp=c7b13553c37f2d9793eb47c00620590de4341945;hpb=0ab2a40c2a2c75d117744f63a1bd74ccc7ea3a4e;p=flightgear.git diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx index c7b13553c..30dc1fb73 100644 --- a/src/Instrumentation/mag_compass.cxx +++ b/src/Instrumentation/mag_compass.cxx @@ -10,43 +10,32 @@ # include #endif -#include +#include +#include -#include "mag_compass.hxx" #include
#include
+#include "mag_compass.hxx" MagCompass::MagCompass ( SGPropertyNode *node ) - : _error_deg(0.0), - _rate_degps(0.0), - name("magnetic-compass"), - num(0) + : _rate_degps(0.0), + _name(node->getStringValue("name", "magnetic-compass")), + _num(node->getIntValue("number", 0)) { - int i; - for ( i = 0; i < node->nChildren(); ++i ) { - SGPropertyNode *child = node->getChild(i); - string cname = child->getName(); - string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "number" ) { - num = child->getIntValue(); - } else { - SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" ); - if ( name.length() ) { - SG_LOG( SG_INSTR, SG_WARN, "Section = " << name ); - } - } + SGPropertyNode_ptr n = node->getNode( "deviation", false ); + if( n ) { + SGPropertyNode_ptr deviation_table_node = n->getNode( "table", false ); + if( NULL != deviation_table_node ) { + _deviation_table = new SGInterpTable( deviation_table_node ); + } else { + std::string deviation_node_name = n->getStringValue(); + if( false == deviation_node_name.empty() ) + _deviation_node = fgGetNode( deviation_node_name, true ); + } } } -MagCompass::MagCompass () - : _error_deg(0.0), - _rate_degps(0.0) -{ -} - MagCompass::~MagCompass () { } @@ -54,96 +43,160 @@ MagCompass::~MagCompass () void MagCompass::init () { - string branch; - branch = "/instrumentation/" + name; + std::string branch; + branch = "/instrumentation/" + _name; - SGPropertyNode *node = fgGetNode(branch.c_str(), num, true ); + SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true ); _serviceable_node = node->getChild("serviceable", 0, true); - _heading_node = - fgGetNode("/orientation/heading-deg", true); - _beta_node = - fgGetNode("/orientation/side-slip-deg", true); - _variation_node = - fgGetNode("/environment/magnetic-variation-deg", true); - _dip_node = - fgGetNode("/environment/magnetic-dip-deg", true); - _north_accel_node = - fgGetNode("/accelerations/ned/north-accel-fps_sec", true); - _east_accel_node = - fgGetNode("/accelerations/ned/east-accel-fps_sec", true); - _down_accel_node = - fgGetNode("/accelerations/ned/down-accel-fps_sec", true); + _pitch_offset_node = node->getChild("pitch-offset-deg", 0, true); + _roll_node = fgGetNode("/orientation/roll-deg", true); + _pitch_node = fgGetNode("/orientation/pitch-deg", true); + _heading_node = fgGetNode("/orientation/heading-magnetic-deg", true); + _beta_node = fgGetNode("/orientation/side-slip-deg", true); + _dip_node = fgGetNode("/environment/magnetic-dip-deg", true); + _x_accel_node = fgGetNode("/accelerations/pilot/x-accel-fps_sec", true); + _y_accel_node = fgGetNode("/accelerations/pilot/y-accel-fps_sec", true); + _z_accel_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true); _out_node = node->getChild("indicated-heading-deg", 0, true); - _serviceable_node->setBoolValue(true); + reinit(); +} + +void +MagCompass::reinit () +{ + _rate_degps = 0.0; } void MagCompass::update (double delta_time_sec) { - // algorithm from Alex Perry - // possibly broken by David Megginson + // This is the real magnetic + // which would be displayed + // if the compass had no errors. + //double heading_mag_deg = _heading_node->getDoubleValue(); - // don't update if it's broken + + // don't update if the compass + // is broken if (!_serviceable_node->getBoolValue()) return; - // jam on a sideslip of 12 degrees or more + /* + * Vassilii: commented out because this way, even when parked, + * w/o any accelerations and level, the compass is jammed. + * If somebody wants to model jamming, real forces (i.e. accelerations) + * and not sideslip angle must be considered. + */ +#if 0 + // jam on excessive sideslip if (fabs(_beta_node->getDoubleValue()) > 12.0) { _rate_degps = 0.0; - _error_deg = _heading_node->getDoubleValue() - - _out_node->getDoubleValue(); return; } +#endif + - double accelN = _north_accel_node->getDoubleValue(); - double accelE = _east_accel_node->getDoubleValue(); - double accelU = _down_accel_node->getDoubleValue() - 32.0; // why? - - // force vector towards magnetic north pole - double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - double cosdip = cos(dip); - double forceN = cosdip * cos(var); - double forceE = cosdip * sin(var); - double forceU = sin(dip); - - // rotation is around acceleration axis - // (magnitude doesn't matter) - double accel = accelN * accelN + accelE * accelE + accelU * accelU; - if (accel > 1.0) - accel = sqrt(accel); - else - accel = 1.0; - - // North marking on compass card - double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS); - double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS); - double edgeU = 0.0; - - // apply the force to that edge to get torques - double torqueN = edgeE * forceU - edgeU * forceE; - double torqueE = edgeU * forceN - edgeN * forceU; - double torqueU = edgeN * forceE - edgeE * forceN; - - // get the component parallel to the axis - double torque = (torqueN * accelN + - torqueE * accelE + - torqueU * accelU) * 5.0 / accel; - - // the compass has angular momentum, - // so apply a torque and wait - if (delta_time_sec < 1.0) { - _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque; - _error_deg += delta_time_sec * _rate_degps; + /* + Formula for northernly turning error from + http://williams.best.vwh.net/compass/node4.html: + + Hc: compass heading + psi: magnetic heading + theta: bank angle (right positive; should be phi here) + mu: dip angle (down positive) + + Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm)) + + This function changes the variable names to the more common psi + for the heading, theta for the pitch, and phi for the roll (and + target_deg for Hc). It also modifies the equation to + incorporate pitch as well as roll, as suggested by Chris + Metzler. + */ + + // bank angle (radians) + double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; + + // pitch angle (radians) + double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS + + _pitch_offset_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; + + // magnetic heading (radians) + double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; + + // magnetic dip (radians) + double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; + + + /* + Tilt adjustments for accelerations. + + The magnitudes of these are totally made up, but in real life, + they would depend on the fluid level, the amount of friction, + etc. anyway. Basically, the compass float tilts forward for + acceleration and backward for deceleration. Tilt about 4 + degrees (0.07 radians) for every G (32 fps/sec) of + acceleration. + + TODO: do something with the vertical acceleration. + */ + double x_accel_g = _x_accel_node->getDoubleValue() / 32; + double y_accel_g = _y_accel_node->getDoubleValue() / 32; + //double z_accel_g = _z_accel_node->getDoubleValue() / 32; + + theta -= 0.07 * x_accel_g; + phi -= 0.07 * y_accel_g; + + //////////////////////////////////////////////////////////////////// + // calculate target compass heading degrees + //////////////////////////////////////////////////////////////////// + + // these are expensive: don't repeat + double sin_phi = sin(phi); + double sin_theta = sin(theta); + double sin_mu = sin(mu); + double cos_theta = cos(theta); + double cos_psi = cos(psi); + double cos_mu = cos(mu); + + double a = cos(phi) * sin(psi) * cos_mu + - sin_phi * cos_theta * sin_mu + - sin_phi* sin_theta * cos_mu * cos_psi; + + double b = cos_theta * cos_psi * cos(mu) + - sin_theta * sin_mu; + + // This is the value that the compass + // is *trying* to display. + double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES; + + if( _deviation_node ) { + target_deg -= _deviation_node->getDoubleValue(); + } else if( _deviation_table ) { + target_deg -= _deviation_table->interpolate( SGMiscd::normalizePeriodic( 0.0, 360.0, target_deg ) ); } - if (_error_deg > 180.0) - _error_deg -= 360.0; - else if (_error_deg < -180.0) - _error_deg += 360.0; - // Set the indicated heading - _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg); + double old_deg = _out_node->getDoubleValue(); + + while ((target_deg - old_deg) > 180.0) + target_deg -= 360.0; + while ((target_deg - old_deg) < -180.0) + target_deg += 360.0; + + // The compass has a current rate of + // rotation -- move the rate of rotation + // towards one that will turn the compass + // to the correct heading, but lag a bit. + // (so that the compass can keep overshooting + // and coming back). + double error = target_deg - old_deg; + _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0); + double indicated_deg = old_deg + _rate_degps * delta_time_sec; + SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0); + + // That's it -- set the messed-up heading. + _out_node->setDoubleValue(indicated_deg); } // end of altimeter.cxx