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 addition to
  26                             elevator calculation
  27                11/12/2001   (RD) added new flap routine.  Needed for
  28                             Twin Otter non-linear model
  29                12/28/2002   (MSS) added simple SAS capability
  30                03/03/2003   (RD) changed flap code to call
  31                             uiuc_find_position to determine
  32                             flap position
  33                08/20/2003   (RD) changed spoiler variables and code
  34                             to match flap conventions.  Changed
  35                             flap_pos_pct to flap_pos_norm
  36
  37 ----------------------------------------------------------------------
  38
  39  AUTHOR(S):    Jeff Scott         http://www.jeffscott.net/
  40                Robert Deters      <rdeters@uiuc.edu>
  41                Michael Selig      <m-selig@uiuc.edu>
  42
  43 ----------------------------------------------------------------------
  44
  45  VARIABLES:
  46
  47 ----------------------------------------------------------------------
  48
  49  INPUTS:       *
  50
  51 ----------------------------------------------------------------------
  52
  53  OUTPUTS:      *
  54
  55 ----------------------------------------------------------------------
  56
  57  CALLED BY:    *
  58
  59 ----------------------------------------------------------------------
  60
  61  CALLS TO:     *
  62
  63 ----------------------------------------------------------------------
  64
  65  COPYRIGHT:    (C) 2000 by Michael Selig
  66
  67  This program is free software; you can redistribute it and/or
  68  modify it under the terms of the GNU General Public License
  69  as published by the Free Software Foundation.
  70
  71  This program is distributed in the hope that it will be useful,
  72  but WITHOUT ANY WARRANTY; without even the implied warranty of
  73  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  74  GNU General Public License for more details.
  75
  76  You should have received a copy of the GNU General Public License
  77  along with this program; if not, write to the Free Software
  78  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  79
  80 **********************************************************************/
  81
  82 #include <math.h>
  83
  84 #include "uiuc_aerodeflections.h"
  85
  86 void uiuc_aerodeflections( double dt )
  87 {
  88   double demax_remain;
  89   double demin_remain;
  90   static double elev_trim;
  91
  92   if (use_uiuc_network) {
  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       Flap_handle = flap_percent * flap_max;
 102       if (outside_control) {
 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     Long_trim = 0;
 113     //elevator_tab = 0;
 114   }
 115
 116   if (Lat_control <= 0)
 117     aileron = - Lat_control * damin * DEG_TO_RAD;
 118   else
 119     aileron = - Lat_control * damax * DEG_TO_RAD;
 120
 121   if (trim_case_2) {
 122     if (Long_trim <= 0) {
 123       elevator = Long_trim * demax * DEG_TO_RAD;
 124       demax_remain = demax + Long_trim * demax;
 125       demin_remain = -Long_trim * demax + demin;
 126       if (Long_control <= 0)
 127         elevator += Long_control * demax_remain * DEG_TO_RAD;
 128       else
 129         elevator += Long_control * demin_remain * DEG_TO_RAD;
 130     } else {
 131       elevator = Long_trim * demin * DEG_TO_RAD;
 132       demin_remain = demin - Long_trim * demin;
 133       demax_remain = Long_trim * demin + demax;
 134       if (Long_control >=0)
 135         elevator += Long_control * demin_remain * DEG_TO_RAD;
 136       else
 137         elevator += Long_control * demax_remain * DEG_TO_RAD;
 138     }
 139   } else {
 140     if ((Long_control+Long_trim) <= 0)
 141       elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
 142     else
 143       elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
 144   }
 145
 146   if (Rudder_pedal <= 0)
 147     rudder = -Rudder_pedal * drmin * DEG_TO_RAD;
 148   else
 149     rudder = -Rudder_pedal * drmax * DEG_TO_RAD;
 150
 151   /* at this point aileron, elevator, and rudder commands are known (in radians)
 152    * add in SAS and any other mixing control laws
 153    */
 154
 155   // SAS for pitch, positive elevator is TED
 156   if (use_elevator_stick_gain)
 157     elevator *= elevator_stick_gain;
 158   if (use_elevator_sas_type1) {
 159     elevator_sas = elevator_sas_KQ * Q_body;
 160     if (use_elevator_sas_max && (elevator_sas > (elevator_sas_max * DEG_TO_RAD))) {
 161       elevator += elevator_sas_max * DEG_TO_RAD;
 162       //elevator_sas = elevator_sas_max;
 163     } else if (use_elevator_sas_min && (elevator_sas < (elevator_sas_min * DEG_TO_RAD))) {
 164       elevator += elevator_sas_min * DEG_TO_RAD;
 165       //elevator_sas = elevator_sas_min;
 166     }
 167     else
 168       elevator += elevator_sas;
 169     // don't exceed the elevator deflection limits
 170     if (elevator > demax * DEG_TO_RAD)
 171       elevator = demax * DEG_TO_RAD;
 172     else if (elevator < (-demin * DEG_TO_RAD))
 173       elevator = -demin * DEG_TO_RAD;
 174   }
 175
 176   // SAS for roll, positive aileron is right aileron TED
 177   if (use_aileron_stick_gain)
 178     aileron *= aileron_stick_gain;
 179   if (use_aileron_sas_type1) {
 180     aileron_sas = aileron_sas_KP * P_body;
 181     if (use_aileron_sas_max && (fabs(aileron_sas) > (aileron_sas_max * DEG_TO_RAD)))
 182       if (aileron_sas >= 0) {
 183         aileron +=  aileron_sas_max * DEG_TO_RAD;
 184         //aileron_sas = aileron_sas_max;
 185       } else {
 186         aileron += -aileron_sas_max * DEG_TO_RAD;
 187         //aileron_sas = -aileron_sas;
 188       }
 189     else
 190       aileron += aileron_sas;
 191     // don't exceed aileron deflection limits
 192     if (fabs(aileron) > (damax * DEG_TO_RAD))
 193       if (aileron > 0)
 194         aileron =  damax * DEG_TO_RAD;
 195       else
 196         aileron = -damax * DEG_TO_RAD;
 197   }
 198
 199   // SAS for yaw, positive rudder deflection is TEL
 200   if (use_rudder_stick_gain)
 201     rudder *= rudder_stick_gain;
 202   if (use_rudder_sas_type1) {
 203     rudder_sas = rudder_sas_KR * R_body;
 204     if (use_rudder_sas_max && (fabs(rudder_sas) > (rudder_sas_max*DEG_TO_RAD)))
 205       if (rudder_sas >= 0) {
 206         rudder +=  rudder_sas_max * DEG_TO_RAD;
 207         //rudder_sas = rudder_sas_max;
 208       } else {
 209         rudder += -rudder_sas_max * DEG_TO_RAD;
 210         //rudder_sas = rudder_sas_max;
 211       }
 212     else
 213       rudder += rudder_sas;
 214     // don't exceed rudder deflection limits, assumes drmax = drmin,
 215     // i.e. equal rudder throws left and right
 216     if (fabs(rudder) > drmax)
 217       if (rudder > 0)
 218         rudder =  drmax * DEG_TO_RAD;
 219       else
 220         rudder = -drmax * DEG_TO_RAD;
 221   }
 222
 223   /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
 224      The second part of the if-block is rewritten below.
 225   double prevFlapHandle = 0.0f;
 226   double flap_transit_rate;
 227   bool flaps_in_transit = false;
 228
 229   if (old_flap_routine) {
 230     // old flap routine
 231     // check for lowest flap setting
 232     if (Flap_handle < dfArray[1]) {
 233       Flap_handle    = dfArray[1];
 234       prevFlapHandle = Flap_handle;
 235       flap           = Flap_handle;
 236     } else if (Flap_handle > dfArray[ndf]) {
 237       // check for highest flap setting
 238       Flap_handle      = dfArray[ndf];
 239       prevFlapHandle   = Flap_handle;
 240       flap             = Flap_handle;
 241     } else {
 242       // otherwise in between
 243       if(Flap_handle != prevFlapHandle)
 244         flaps_in_transit = true;
 245       if(flaps_in_transit) {
 246         int iflap = 0;
 247         while (dfArray[iflap] < Flap_handle)
 248           iflap++;
 249         if (flap < Flap_handle) {
 250           if (TimeArray[iflap] > 0)
 251             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
 252           else
 253             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
 254         } else {
 255           if (TimeArray[iflap+1] > 0)
 256             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
 257           else
 258             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
 259         }
 260         if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
 261           flap += flap_transit_rate * dt;
 262         else {
 263           flaps_in_transit = false;
 264           flap = Flap_handle;
 265         }
 266       }
 267     }
 268     prevFlapHandle = Flap_handle;
 269   } else {
 270     // new flap routine
 271     // designed for the twin otter non-linear model
 272     if (!outside_control) {
 273       flap_percent     = Flap_handle / 30.0;    // percent of flaps desired
 274       if (flap_percent>=0.31 && flap_percent<=0.35)
 275         flap_percent = 1.0 / 3.0;
 276       if (flap_percent>=0.65 && flap_percent<=0.69)
 277         flap_percent = 2.0 / 3.0;
 278     }
 279     flap_cmd_deg        = flap_percent * flap_max;  // angle of flaps desired
 280     flap_moving_rate = flap_rate * dt;           // amount flaps move per time step
 281
 282     // determine flap position with respect to the flap goal
 283     if (flap_pos < flap_cmd_deg) {
 284       flap_pos += flap_moving_rate;
 285       if (flap_pos > flap_cmd_deg)
 286         flap_pos = flap_cmd_deg;
 287     } else if (flap_pos > flap_cmd_deg) {
 288       flap_pos -= flap_moving_rate;
 289       if (flap_pos < flap_cmd_deg)
 290         flap_pos = flap_cmd_deg;
 291     }
 292   }
 293
 294 */
 295
 296   if (!outside_control && use_flaps) {
 297     // angle of flaps desired [rad]
 298     flap_cmd = Flap_handle * DEG_TO_RAD;
 299     // amount flaps move per time step [rad/sec]
 300     flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
 301     // determine flap position [rad] with respect to flap command
 302     flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
 303     // get the normalized position
 304     flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
 305   }
 306
 307
 308   if(use_spoilers) {
 309     // angle of spoilers desired [rad]
 310     spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
 311     // amount spoilers move per time step [rad/sec]
 312     spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
 313     // determine spoiler position [rad] with respect to spoiler command
 314     spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
 315     // get the normailized position
 316     spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
 317   }
 318
 319
 320   return;
 321 }
 322
 323 // end uiuc_aerodeflections.cpp