MapWidget for the GUI, initial commit.

[flightgear.git] / src / Instrumentation / mrg.cxx

diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx
index 3e7c07162776da24a31db79b280aaa7422194229..539231da46bd85c12d7f724045f6c5cffa27f18c 100644 (file)
--- a/src/Instrumentation/mrg.cxx
+++ b/src/Instrumentation/mrg.cxx
@@ -7,6 +7,8 @@
 // - better spin-up
 
 #include <simgear/compiler.h>
+#include <simgear/sg_inlines.h>
+
 
 #include <iostream>
 #include <string>
@@ -41,6 +43,7 @@ MasterReferenceGyro::init ()
        _indicated_hdg_rate = 0;
        _indicated_roll_rate = 0;
        _indicated_pitch_rate = 0;
+        _erect_time = 0;
        
        string branch;
        branch = "/instrumentation/" + _name;
@@ -48,26 +51,33 @@ MasterReferenceGyro::init ()
        _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
        _roll_in_node = fgGetNode("/orientation/roll-deg", true);
        _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
+       _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
        _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
        _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
        _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
-       _g_in_node =   fgGetNode("/accelerations/pilot-g-damped", true);
+       _g_in_node =   fgGetNode("/accelerations/pilot-g", true);
        _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
+       _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
        
        SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
        _off_node = node->getChild("off-flag", 0, true);
        _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
        _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
        _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
+       _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
        _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
        _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
        _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
        _responsiveness_node = node->getChild("responsiveness", 0, true);
-       _error_out_node = node->getChild("heading-error-deg", 0, true);
+       _error_out_node = node->getChild("heading-bug-error-deg", 0, true);
+       _hdg_input_source_node = node->getChild("heading-source", 0, true);
+        _fast_erect_node = node->getChild("fast-erect", 0, true);
 
        _electrical_node->setDoubleValue(0);
        _responsiveness_node->setDoubleValue(0.75);
        _off_node->setBoolValue(false);
+       _hdg_input_source_node->setBoolValue(false);
+        _fast_erect_node->setBoolValue(false);
 }
 
 void
@@ -112,31 +122,44 @@ MasterReferenceGyro::update (double dt)
        double spin = _gyro.get_spin_norm();
        
        // set the "off-flag"
-       if ( _electrical_node->getDoubleValue() == 0 ) {
-               _off_node->setBoolValue(true);
-       } else if ( spin == 1 ) {
+        if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
                _off_node->setBoolValue(false);
        } else {
                _off_node->setBoolValue(true);
+        return;
        }
 
        // Get the input values
+       double hdg = _hdg_mag_in_node->getDoubleValue();
+
+       if(_hdg_input_source_node->getBoolValue())
+               hdg = _hdg_in_node->getDoubleValue();
+
        double roll = _roll_in_node->getDoubleValue();
        double pitch = _pitch_in_node->getDoubleValue();
-       double hdg = _hdg_in_node->getDoubleValue();
+       
        double roll_rate = _yaw_rate_node->getDoubleValue();
        double pitch_rate = _yaw_rate_node->getDoubleValue();
        double yaw_rate = _yaw_rate_node->getDoubleValue();
+       double g = _g_in_node->getDoubleValue();
 
        //modulate the input by the spin rate
        double responsiveness = spin * spin * spin * spin * spin * spin;
        roll = fgGetLowPass( _last_roll, roll, responsiveness );
        pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
+
+       if ((hdg - _last_hdg) > 180)
+               _last_hdg += 360;
+       if ((hdg - _last_hdg) < -180)
+               _last_hdg -= 360;
+
        hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
 
        //but we need to filter the hdg and yaw_rate as well - yuk!
        responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
        yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
+        g = fgGetLowPass( _last_g , g, 0.015 );
+
 
        // store the new values
        _last_roll = roll;
@@ -145,11 +168,24 @@ MasterReferenceGyro::update (double dt)
        _last_roll_rate = roll_rate;
        _last_pitch_rate = pitch_rate;
        _last_yaw_rate = yaw_rate;
+       _last_g = g;
+
+    if (_erect_time > 0){
+
+        if ( !_fast_erect_node->getBoolValue() )
+            _erect_time -= dt;
+        else 
+            _erect_time -= 2 * dt;
+
+    }
+
+    //SG_LOG(SG_GENERAL, SG_ALERT,
+    //    "g input " << g <<" _erect_time " << _erect_time << " spin " << spin);
 
        //the gyro only erects inside limits
        if ( fabs ( yaw_rate ) <= 5
-                       && (_g_in_node->getDoubleValue() <= 1.5
-                       || _g_in_node->getDoubleValue() >= -0.5) ) {
+            && ( g <= 1.5 || g >= -0.5)
+            && _erect_time <=0 ) {
                indicated_roll = _last_roll;
                indicated_pitch = _last_pitch;
                indicated_hdg = _last_hdg;
@@ -160,26 +196,33 @@ MasterReferenceGyro::update (double dt)
                indicated_roll_rate = 0;
                indicated_pitch_rate = 0;
                indicated_hdg_rate = 0;
+        
+        if (_erect_time <= 0 )
+            _erect_time = 34;
+
        }
 
        // calculate the difference between the indicated heading
        // and the selected heading for use with an autopilot
        static SGPropertyNode *bnode
-               = fgGetNode( "autopilot/settings/target-heading-deg", false );
+               = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
 
        if ( bnode ) {
                double diff = bnode->getDoubleValue() - indicated_hdg;
                if ( diff < -180.0 ) { diff += 360.0; }
                if ( diff > 180.0 ) { diff -= 360.0; }
                _error_out_node->setDoubleValue( diff );
+               //SG_LOG(SG_GENERAL, SG_ALERT,
+               //"autopilot input " << bnode->getDoubleValue() 
+               //<< " output " << _error_out_node->getDoubleValue()<<);
        }
-       //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
+       
        //smooth the output
        double factor = _responsiveness_node->getDoubleValue() * dt;
 
        indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
        indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
-       indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
+       //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
 
        indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
        indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );