Re-interpret gyro wander under g-forces

Signed-off-by: Vivian Meazza <vivian.meazza@lineone.net>

src/Instrumentation/mrg.cxx

@@ -20,6 +20,7 @@
 
 #include "mrg.hxx"
 
+const double MasterReferenceGyro::gravity = -32.1740485564;
 
 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
 _name(node->getStringValue("name", "master-reference-gyro")),
@@ -43,7 +44,9 @@ MasterReferenceGyro::init ()
     _indicated_hdg_rate = 0;
     _indicated_roll_rate = 0;
     _indicated_pitch_rate = 0;
-        _erect_time = 0;
+    _erect_time = 180;
+    _last_g = 1;
+    _g_error = 10;
 
     string branch;
     branch = "/instrumentation/" + _name;
@@ -55,6 +58,7 @@ MasterReferenceGyro::init ()
     _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/z-accel-fps_sec", 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);
@@ -78,6 +82,7 @@ MasterReferenceGyro::init ()
     _off_node->setBoolValue(false);
     _hdg_input_source_node->setBoolValue(false);
     _fast_erect_node->setBoolValue(false);
+    _g_in_node->setDoubleValue(1);
 }
 
 void
@@ -109,12 +114,22 @@ MasterReferenceGyro::unbind ()
 void
 MasterReferenceGyro::update (double dt)
 {
+    //sanity check
+    if (!fgGetBool("/sim/fdm-initialized", false)) {
+        return;
+    }
+
     double indicated_roll = 0;
     double indicated_pitch = 0;
     double indicated_hdg = 0;
     double indicated_roll_rate = 0;
     double indicated_pitch_rate = 0;
     double indicated_hdg_rate = 0;
+    double hdg = 0;
+    double erect_time_factor = 1;
+
+    const double erect_time = 180;
+    const double max_g_error = 10.0;
 
     //Get the spin from the gyro
     _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
@@ -130,18 +145,19 @@ MasterReferenceGyro::update (double dt)
     }
 
     // Get the input values
-	double hdg = _hdg_mag_in_node->getDoubleValue();
-
-	if(_hdg_input_source_node->getBoolValue())
+    if(_hdg_input_source_node->getBoolValue()){
         hdg = _hdg_in_node->getDoubleValue();
+    } else {
+        hdg = _hdg_mag_in_node->getDoubleValue();
+    }
 
     double roll = _roll_in_node->getDoubleValue();
     double pitch = _pitch_in_node->getDoubleValue();
+    double g = _g_in_node->getDoubleValue()/* / gravity*/;
 
     double roll_rate = _yaw_rate_node->getDoubleValue();
-	double pitch_rate = _yaw_rate_node->getDoubleValue();
+    double pitch_rate = _pitch_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;
@@ -158,7 +174,7 @@ MasterReferenceGyro::update (double dt)
     //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 );
+    g = fgGetLowPass( _last_g , g, 1.5);
 
 
     // store the new values
@@ -170,38 +186,38 @@ MasterReferenceGyro::update (double dt)
     _last_yaw_rate = yaw_rate;
     _last_g = g;
 
-    if (_erect_time > 0){
+    //the gyro only erects inside limits
+    if ( fabs ( yaw_rate ) <= 5
+        && g <= 1.5 && g >= -0.5){
 
-        if ( !_fast_erect_node->getBoolValue() )
-            _erect_time -= dt;
-        else 
-            _erect_time -= 2 * dt;
+            if ( !_fast_erect_node->getBoolValue() ){
+                erect_time_factor = 1;
+            } else {
+                erect_time_factor = 2;
+            }
+
+        _g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor;
+    } else {
+        _g_error += (max_g_error /(erect_time * 0.33)) * dt * 2; 
+
+        //SG_LOG(SG_GENERAL, SG_ALERT,_num <<
+        //    " g input " << _g_in_node->getDoubleValue() * gravity
+        //    <<" _erect_time " << _erect_time 
+        //    << " yaw " <<   yaw_rate
+        //    << " pitch " << _pitch_rate_node->getDoubleValue()
+        //    << " roll " << _roll_rate_node->getDoubleValue());
 
     }
 
-    //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 <= 1.5 || g >= -0.5)
-            && _erect_time <=0 ) {
-		indicated_roll = _last_roll;
-		indicated_pitch = _last_pitch;
-		indicated_hdg = _last_hdg;
+    //cout << "_g_error "<< _g_error << endl;
+    _g_error = SGMiscd::clip(_g_error, 0, 10);
+//     cout << "_g_error clip "<< _g_error << endl;
+    indicated_roll = _last_roll + _g_error;
+    indicated_pitch = _last_pitch + _g_error;
+    indicated_hdg = _last_hdg + _g_error;
     indicated_roll_rate = _last_roll_rate;
     indicated_pitch_rate = _last_pitch_rate;
     indicated_hdg_rate = _last_yaw_rate;
-	} else {
-		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

src/Instrumentation/mrg.hxx

@@ -49,6 +49,8 @@ public:
 
 private:
 
+    static const double gravity; //conversion factor
+
     string _name;
     int _num;
 
@@ -66,6 +68,7 @@ private:
     double _last_yaw_rate;
     double _last_g;
     double _erect_time;
+    double _g_error;
 
     Gyro _gyro;