From ad6236cbe7ae6d12842d4904cac5f899cd148435 Mon Sep 17 00:00:00 2001
From: david <david>
Date: Fri, 4 Apr 2003 03:25:02 +0000
Subject: [PATCH] Add the ability for the AI to tumble in extreme attitudes, if

  /instrumentation/attitude-indicator/config/tumble-flag

is true.  The AI will take up to five minutes to reerect itself
completely.
---
 src/Instrumentation/attitude_indicator.cxx | 49 +++++++++++++++++++---
 src/Instrumentation/attitude_indicator.hxx |  4 ++
 2 files changed, 47 insertions(+), 6 deletions(-)

diff --git a/src/Instrumentation/attitude_indicator.cxx b/src/Instrumentation/attitude_indicator.cxx
index cb0c5aa4f..36a8a2d86 100644
--- a/src/Instrumentation/attitude_indicator.cxx
+++ b/src/Instrumentation/attitude_indicator.cxx
@@ -4,14 +4,15 @@
 // This file is in the Public Domain and comes with no warranty.
 
 // TODO:
-// - tumble
 // - better spin-up
 
 #include "attitude_indicator.hxx"
 #include <Main/fg_props.hxx>
+#include <Main/util.hxx>
 
 
 AttitudeIndicator::AttitudeIndicator ()
+    : _tumble(0)
 {
 }
 
@@ -27,6 +28,9 @@ AttitudeIndicator::init ()
     _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
     _roll_in_node = fgGetNode("/orientation/roll-deg", true);
     _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
+    _tumble_flag_node =
+        fgGetNode("/instrumentation/attitude-indicator/config/tumble-flag",
+                  true);
     _pitch_out_node =
         fgGetNode("/instrumentation/attitude-indicator/indicated-pitch-deg",
                   true);
@@ -59,14 +63,47 @@ AttitudeIndicator::update (double dt)
     _gyro.update(dt);
     double spin = _gyro.get_spin_norm();
 
-                                // Next, calculate the indicated roll
-                                // and pitch, introducing errors.
-    double factor = 1.0 - ((1.0 - spin) * (1.0 - spin) * (1.0 - spin));
+                                // Calculate the responsiveness
+    double responsiveness = spin * spin * spin * spin * spin * spin;
+
+                                // Get the indicated roll and pitch
     double roll = _roll_in_node->getDoubleValue();
     double pitch = _pitch_in_node->getDoubleValue();
-    roll = 35 + (factor * (roll - 35));
-    pitch = 15 + (factor * (pitch - 15));
 
+                                // Calculate the tumble for the
+                                // next pass.
+    if (_tumble_flag_node->getBoolValue()) {
+        if (_tumble < 1.0) {
+            if (fabs(roll) > 45.0) {
+                double target = (fabs(roll) - 45.0) / 45.0;
+                if (_tumble < target)
+                    _tumble = target;
+            }
+            if (fabs(pitch) > 45.0) {
+                double target = (fabs(pitch) - 45.0) / 45.0;
+                if (_tumble < target)
+                    _tumble = target;
+            }
+            if (_tumble > 1.0) {
+                _tumble = 1.0;
+            }
+        }
+                                // Reerect in 5 minutes
+        _tumble -= dt/300.0;
+        if (_tumble < 0.0)
+            _tumble = 0.0;
+
+        responsiveness *= ((1.0 - _tumble) * (1.0 - _tumble) *
+                           (1.0 - _tumble) * (1.0 - _tumble));
+
+    }
+
+    roll = fgGetLowPass(_roll_out_node->getDoubleValue(), roll,
+                        responsiveness);
+    pitch = fgGetLowPass(_pitch_out_node->getDoubleValue(), pitch,
+                         responsiveness);
+
+                                // Assign the new values
     _roll_out_node->setDoubleValue(roll);
     _pitch_out_node->setDoubleValue(pitch);
 }
diff --git a/src/Instrumentation/attitude_indicator.hxx b/src/Instrumentation/attitude_indicator.hxx
index 32bc8b72f..b75ccd317 100644
--- a/src/Instrumentation/attitude_indicator.hxx
+++ b/src/Instrumentation/attitude_indicator.hxx
@@ -24,6 +24,7 @@
  *
  * Input properties:
  *
+ * /instrumentation/attitude-indicator/config/tumble-flag
  * /instrumentation/attitude-indicator/serviceable
  * /orientation/pitch-deg
  * /orientation/roll-deg
@@ -51,6 +52,9 @@ private:
 
     Gyro _gyro;
 
+    double _tumble;
+
+    SGPropertyNode_ptr _tumble_flag_node;
     SGPropertyNode_ptr _pitch_in_node;
     SGPropertyNode_ptr _roll_in_node;
     SGPropertyNode_ptr _suction_node;