src/FDM/UIUCModel/uiuc_aerodeflections.cpp

   1 /**********************************************************************
   2
   3  FILENAME:     uiuc_aerodeflections.cpp
   4
   5 ----------------------------------------------------------------------
   6
   7  DESCRIPTION:  determine the aero control surface deflections
   8                elevator [rad]
   9                aileron [rad]
  10                rudder [rad]
  11
  12 ----------------------------------------------------------------------
  13
  14  STATUS:       alpha version
  15
  16 ----------------------------------------------------------------------
  17
  18  REFERENCES:   based on deflection portions of c172_aero.c and
  19                uiuc_aero.c
  20
  21 ----------------------------------------------------------------------
  22
  23  HISTORY:      01/30/2000   initial release
  24                04/05/2000   (JS) added zero_Long_trim command
  25                07/05/2001   (RD) removed elevator_tab addidtion to
  26                             elevator calculation
  27                11/12/2001   (RD) added new flap routine.  Needed for
  28                             Twin Otter non-linear model
  29
  30 ----------------------------------------------------------------------
  31
  32  AUTHOR(S):    Jeff Scott         <jscott@mail.com>
  33                Robert Deters      <rdeters@uiuc.edu>
  34                Michael Selig      <m-selig@uiuc.edu>
  35
  36 ----------------------------------------------------------------------
  37
  38  VARIABLES:
  39
  40 ----------------------------------------------------------------------
  41
  42  INPUTS:       *
  43
  44 ----------------------------------------------------------------------
  45
  46  OUTPUTS:      *
  47
  48 ----------------------------------------------------------------------
  49
  50  CALLED BY:    *
  51
  52 ----------------------------------------------------------------------
  53
  54  CALLS TO:     *
  55
  56 ----------------------------------------------------------------------
  57
  58  COPYRIGHT:    (C) 2000 by Michael Selig
  59
  60  This program is free software; you can redistribute it and/or
  61  modify it under the terms of the GNU General Public License
  62  as published by the Free Software Foundation.
  63
  64  This program is distributed in the hope that it will be useful,
  65  but WITHOUT ANY WARRANTY; without even the implied warranty of
  66  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  67  GNU General Public License for more details.
  68
  69  You should have received a copy of the GNU General Public License
  70  along with this program; if not, write to the Free Software
  71  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  72  USA or view http://www.gnu.org/copyleft/gpl.html.
  73
  74 **********************************************************************/
  75
  76 //#include <math.h>
  77
  78 #include "uiuc_aerodeflections.h"
  79
  80 void uiuc_aerodeflections( double dt )
  81 {
  82   double prevFlapHandle = 0.0f;
  83   double flap_transit_rate;
  84   bool flaps_in_transit = false;
  85   double demax_remain;
  86   double demin_remain;
  87   static double elev_trim;
  88
  89   //if (use_uiuc_network)
  90   //  {
  91       // receive data
  92   //    uiuc_network(1);
  93   //    if (pitch_trim_up)
  94         //elev_trim += 0.001;
  95   //    if (pitch_trim_down)
  96         //elev_trim -= 0.001;
  97   //    if (elev_trim > 1.0)
  98         //elev_trim = 1;
  99   //    if (elev_trim < -1.0)
 100         //elev_trim = -1;
 101   //    if (outside_control)
 102         //{
 103         //  pilot_elev_no = true;
 104         //  pilot_ail_no = true;
 105         //  pilot_rud_no = true;
 106         //  pilot_throttle_no = true;
 107         //  Long_trim = elev_trim;
 108         //}
 109   //  }
 110
 111   if (zero_Long_trim)
 112     {
 113       Long_trim = 0;
 114       //elevator_tab = 0;
 115     }
 116
 117   if (Lat_control <= 0)
 118     aileron = - Lat_control * damin * DEG_TO_RAD;
 119   else
 120     aileron = - Lat_control * damax * DEG_TO_RAD;
 121
 122   if (trim_case_2)
 123     {
 124       if (Long_trim <= 0)
 125         {
 126           elevator = Long_trim * demax * DEG_TO_RAD;
 127           demax_remain = demax + Long_trim * demax;
 128           demin_remain = -1*Long_trim * demax + demin;
 129           if (Long_control <= 0)
 130             elevator += Long_control * demax_remain * DEG_TO_RAD;
 131           else
 132             elevator += Long_control * demin_remain * DEG_TO_RAD;
 133         }
 134       else
 135         {
 136           elevator = Long_trim * demin * DEG_TO_RAD;
 137           demin_remain = demin - Long_trim * demin;
 138           demax_remain = Long_trim * demin + demax;
 139           if (Long_control >=0)
 140             elevator += Long_control * demin_remain * DEG_TO_RAD;
 141           else
 142             elevator += Long_control * demax_remain * DEG_TO_RAD;
 143         }
 144     }
 145   else
 146     {
 147       if ((Long_control+Long_trim) <= 0)
 148         elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
 149       else
 150         elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
 151     }
 152
 153   if (Rudder_pedal <= 0)
 154     rudder = - Rudder_pedal * drmin * DEG_TO_RAD;
 155   else
 156     rudder = - Rudder_pedal * drmax * DEG_TO_RAD;
 157
 158
 159   if (old_flap_routine)
 160     {
 161       // old flap routine
 162       // check for lowest flap setting
 163       if (Flap_handle < dfArray[1])
 164         {
 165           Flap_handle    = dfArray[1];
 166           prevFlapHandle = Flap_handle;
 167           flap           = Flap_handle;
 168         }
 169       // check for highest flap setting
 170       else if (Flap_handle > dfArray[ndf])
 171         {
 172           Flap_handle      = dfArray[ndf];
 173           prevFlapHandle   = Flap_handle;
 174           flap             = Flap_handle;
 175         }
 176       // otherwise in between
 177       else
 178         {
 179           if(Flap_handle != prevFlapHandle)
 180             {
 181               flaps_in_transit = true;
 182             }
 183           if(flaps_in_transit)
 184             {
 185               int iflap = 0;
 186               while (dfArray[iflap] < Flap_handle)
 187                 {
 188                   iflap++;
 189                 }
 190               if (flap < Flap_handle)
 191                 {
 192                   if (TimeArray[iflap] > 0)
 193                     flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
 194                   else
 195                     flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
 196                 }
 197               else
 198                 {
 199                   if (TimeArray[iflap+1] > 0)
 200                     flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
 201                   else
 202                     flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
 203                 }
 204               if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
 205                 flap += flap_transit_rate * dt;
 206               else
 207                 {
 208                   flaps_in_transit = false;
 209                   flap = Flap_handle;
 210                 }
 211             }
 212         }
 213       prevFlapHandle = Flap_handle;
 214     }
 215   else
 216     {
 217       // new flap routine
 218       // designed for the twin otter non-linear model
 219       if (outside_control == false)
 220         {
 221           flap_percent     = Flap_handle / 30.0;    // percent of flaps desired
 222           if (flap_percent>=0.31 && flap_percent<=0.35)
 223             flap_percent = 1.0 / 3.0;
 224           if (flap_percent>=0.65 && flap_percent<=0.69)
 225             flap_percent = 2.0 / 3.0;
 226         }
 227       flap_goal        = flap_percent * flap_max;  // angle of flaps desired
 228       flap_moving_rate = flap_rate * dt;           // amount flaps move per time step
 229
 230       // determine flap position with respect to the flap goal
 231       if (flap_pos < flap_goal)
 232         {
 233           flap_pos += flap_moving_rate;
 234           if (flap_pos > flap_goal)
 235             flap_pos = flap_goal;
 236         }
 237       else if (flap_pos > flap_goal)
 238         {
 239           flap_pos -= flap_moving_rate;
 240           if (flap_pos < flap_goal)
 241             flap_pos = flap_goal;
 242         }
 243     }
 244
 245   return;
 246 }
 247
 248 // end uiuc_aerodeflections.cpp