Updated to match changes in radiostack.[ch]xx

[flightgear.git] / src / FDM / UIUCModel / uiuc_aerodeflections.cpp

diff --git a/src/FDM/UIUCModel/uiuc_aerodeflections.cpp b/src/FDM/UIUCModel/uiuc_aerodeflections.cpp
index e2692d99e84814bf5d753682e5d060ccd1dc55ee..d87f608b82812ad9991f00ac37dd123240706843 100644 (file)
--- a/src/FDM/UIUCModel/uiuc_aerodeflections.cpp
+++ b/src/FDM/UIUCModel/uiuc_aerodeflections.cpp
@@ -21,10 +21,16 @@
 ----------------------------------------------------------------------
 
  HISTORY:      01/30/2000   initial release
+               04/05/2000   (JS) added zero_Long_trim command
+              07/05/2001   (RD) removed elevator_tab addidtion to
+                           elevator calculation
+              11/12/2001   (RD) added new flap routine.  Needed for
+                           Twin Otter non-linear model
 
 ----------------------------------------------------------------------
 
  AUTHOR(S):    Jeff Scott         <jscott@mail.com>
+               Robert Deters      <rdeters@uiuc.edu>
                Michael Selig      <m-selig@uiuc.edu>
 
 ----------------------------------------------------------------------
@@ -67,22 +73,116 @@
 
 **********************************************************************/
 
+#include <math.h>
+
 #include "uiuc_aerodeflections.h"
 
-void uiuc_aerodeflections()
+void uiuc_aerodeflections( double dt )
 {
-
-  // for now, consider deflections to be equal
-  // damin = damax
-  aileron = - Lat_control * damax * DEG_TO_RAD;
-
-  // for now, consider deflections to be equal
-  // demin = demax
-  elevator = Long_control * demax * DEG_TO_RAD + Long_trim;
-
-  // for now, consider deflections to be equal
-  // drmin = drmax
-  rudder = - Rudder_pedal * drmax * DEG_TO_RAD; 
+  double prevFlapHandle = 0.0f;
+  double flap_transit_rate;
+  bool flaps_in_transit = false;
+
+  if (zero_Long_trim)
+    {
+      Long_trim = 0;
+      //elevator_tab = 0;
+    }
+
+  if (Lat_control <= 0)
+    aileron = - Lat_control * damin * DEG_TO_RAD;
+  else
+    aileron = - Lat_control * damax * DEG_TO_RAD;
+
+  if ((Long_control+Long_trim) <= 0)
+    elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
+  else
+    elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
+
+  if (Rudder_pedal <= 0)
+    rudder = - Rudder_pedal * drmin * DEG_TO_RAD;
+  else
+    rudder = - Rudder_pedal * drmax * DEG_TO_RAD;
+
+
+  // new flap routine
+  // designed for the twin otter non-linear model
+  flap_percent     = Flap_handle / 30.0;       // percent of flaps desired
+  if (flap_percent>=0.31 && flap_percent<=0.35)
+    flap_percent = 1.0 / 3.0;
+  if (flap_percent>=0.65 && flap_percent<=0.69)
+    flap_percent = 2.0 / 3.0;
+  flap_goal        = flap_percent * flap_max;  // angle of flaps desired
+  flap_moving_rate = flap_rate * dt;           // amount flaps move per time step
+  
+  // determine flap position with respect to the flap goal
+  if (flap_pos < flap_goal)
+    {
+      flap_pos += flap_moving_rate;
+      if (flap_pos > flap_goal)
+       flap_pos = flap_goal;
+    }
+  else if (flap_pos > flap_goal)
+    {
+      flap_pos -= flap_moving_rate;
+      if (flap_pos < flap_goal)
+       flap_pos = flap_goal;
+    }
+
+
+  // old flap routine
+  // check for lowest flap setting
+  if (Flap_handle < dfArray[1])
+    {
+      Flap_handle    = dfArray[1];
+      prevFlapHandle = Flap_handle;
+      flap           = Flap_handle;
+    }
+  // check for highest flap setting
+  else if (Flap_handle > dfArray[ndf])
+    {
+      Flap_handle      = dfArray[ndf];
+      prevFlapHandle   = Flap_handle;
+      flap             = Flap_handle;
+    }
+  // otherwise in between
+  else          
+    {
+      if(Flap_handle != prevFlapHandle)
+        {
+          flaps_in_transit = true;
+        }
+      if(flaps_in_transit)
+        {
+          int iflap = 0;
+          while (dfArray[iflap] < Flap_handle)
+            {
+              iflap++;
+            }
+          if (flap < Flap_handle)
+            {
+              if (TimeArray[iflap] > 0)
+                flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
+              else
+                flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
+            }
+          else 
+            {
+              if (TimeArray[iflap+1] > 0)
+                flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
+              else
+                flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
+            }
+          if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
+            flap += flap_transit_rate * dt;
+          else
+            {
+              flaps_in_transit = false;
+              flap = Flap_handle;
+            }
+        }
+    }
+  prevFlapHandle = Flap_handle;
 
   return;
 }