X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmag_compass.cxx;h=1f57f0791923154a44fd9e98301f3ec08225a268;hb=2302f040953b030612e1c0cb47fc47414ddbed31;hp=e44981cf577cd9606ca2199bc5c5ee3fae5c3157;hpb=b0c1fb7755c87948aa3db42780f1c3921815b0c0;p=flightgear.git diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx index e44981cf5..1f57f0791 100644 --- a/src/Instrumentation/mag_compass.cxx +++ b/src/Instrumentation/mag_compass.cxx @@ -10,7 +10,7 @@ # include #endif -#include +#include #include "mag_compass.hxx" #include
@@ -20,30 +20,8 @@ MagCompass::MagCompass ( SGPropertyNode *node ) : _error_deg(0.0), _rate_degps(0.0), - name("magnetic-compass"), - num(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 ); - } - } - } -} - -MagCompass::MagCompass () - : _error_deg(0.0), - _rate_degps(0.0) + _name(node->getStringValue("name", "magnetic-compass")), + _num(node->getIntValue("number", 0)) { } @@ -55,29 +33,19 @@ void MagCompass::init () { string branch; - branch = "/instrumentation/" + name; + 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); - _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); + _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); } @@ -85,22 +53,29 @@ void MagCompass::update (double delta_time_sec) { // This is the real magnetic - // heading, which will almost - // never appear. - double heading_mag_deg = _heading_node->getDoubleValue(); + // 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 an excessive sideslip. - */ + * 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; return; } +#endif /* @@ -114,10 +89,11 @@ MagCompass::update (double delta_time_sec) 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. It also modifies the equation to incorporate pitch - as well as roll, as suggested by Chris Metzler. + 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) @@ -147,13 +123,13 @@ MagCompass::update (double delta_time_sec) */ 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; + //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 Hc in degrees + // calculate target compass heading degrees //////////////////////////////////////////////////////////////////// // these are expensive: don't repeat @@ -172,13 +148,28 @@ MagCompass::update (double delta_time_sec) - sin_theta * sin_mu; // This is the value that the compass - // is *trying* to display, but it - // takes time to move there, and because - // of momentum, the compass will often - // overshoot. - double Hc = atan2(a, b) * SGD_RADIANS_TO_DEGREES; - - _out_node->setDoubleValue(Hc); + // is *trying* to display. + double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES; + 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