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
 200   // SAS for yaw, positive rudder deflection is TEL
 201   if (use_rudder_stick_gain)
 202     rudder *= rudder_stick_gain;
 203   if (use_rudder_sas_type1) {
 204     rudder_sas = rudder_sas_KR * R_body;
 205     if (use_rudder_sas_max && (fabs(rudder_sas) > (rudder_sas_max*DEG_TO_RAD)))
 206       if (rudder_sas >= 0) {
 207         rudder +=  rudder_sas_max * DEG_TO_RAD;
 208         //rudder_sas = rudder_sas_max;
 209       } else {
 210         rudder += -rudder_sas_max * DEG_TO_RAD;
 211         //rudder_sas = rudder_sas_max;
 212       }
 213     else
 214       rudder += rudder_sas;
 215     // don't exceed rudder deflection limits, assumes drmax = drmin,
 216     // i.e. equal rudder throws left and right
 217     if (fabs(rudder) > drmax) {
 218       if (rudder > 0)
 219         rudder =  drmax * DEG_TO_RAD;
 220       else
 221         rudder = -drmax * DEG_TO_RAD;
 222     }
 223   }
 224
 225   /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
 226      The second part of the if-block is rewritten below.
 227   double prevFlapHandle = 0.0f;
 228   double flap_transit_rate;
 229   bool flaps_in_transit = false;
 230
 231   if (old_flap_routine) {
 232     // old flap routine
 233     // check for lowest flap setting
 234     if (Flap_handle < dfArray[1]) {
 235       Flap_handle    = dfArray[1];
 236       prevFlapHandle = Flap_handle;
 237       flap           = Flap_handle;
 238     } else if (Flap_handle > dfArray[ndf]) {
 239       // check for highest flap setting
 240       Flap_handle      = dfArray[ndf];
 241       prevFlapHandle   = Flap_handle;
 242       flap             = Flap_handle;
 243     } else {
 244       // otherwise in between
 245       if(Flap_handle != prevFlapHandle)
 246         flaps_in_transit = true;
 247       if(flaps_in_transit) {
 248         int iflap = 0;
 249         while (dfArray[iflap] < Flap_handle)
 250           iflap++;
 251         if (flap < Flap_handle) {
 252           if (TimeArray[iflap] > 0)
 253             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
 254           else
 255             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
 256         } else {
 257           if (TimeArray[iflap+1] > 0)
 258             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
 259           else
 260             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
 261         }
 262         if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
 263           flap += flap_transit_rate * dt;
 264         else {
 265           flaps_in_transit = false;
 266           flap = Flap_handle;
 267         }
 268       }
 269     }
 270     prevFlapHandle = Flap_handle;
 271   } else {
 272     // new flap routine
 273     // designed for the twin otter non-linear model
 274     if (!outside_control) {
 275       flap_percent     = Flap_handle / 30.0;    // percent of flaps desired
 276       if (flap_percent>=0.31 && flap_percent<=0.35)
 277         flap_percent = 1.0 / 3.0;
 278       if (flap_percent>=0.65 && flap_percent<=0.69)
 279         flap_percent = 2.0 / 3.0;
 280     }
 281     flap_cmd_deg        = flap_percent * flap_max;  // angle of flaps desired
 282     flap_moving_rate = flap_rate * dt;           // amount flaps move per time step
 283
 284     // determine flap position with respect to the flap goal
 285     if (flap_pos < flap_cmd_deg) {
 286       flap_pos += flap_moving_rate;
 287       if (flap_pos > flap_cmd_deg)
 288         flap_pos = flap_cmd_deg;
 289     } else if (flap_pos > flap_cmd_deg) {
 290       flap_pos -= flap_moving_rate;
 291       if (flap_pos < flap_cmd_deg)
 292         flap_pos = flap_cmd_deg;
 293     }
 294   }
 295
 296 */
 297
 298   if (!outside_control && use_flaps) {
 299     // angle of flaps desired [rad]
 300     flap_cmd = Flap_handle * DEG_TO_RAD;
 301     // amount flaps move per time step [rad/sec]
 302     flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
 303     // determine flap position [rad] with respect to flap command
 304     flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
 305     // get the normalized position
 306     flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
 307   }
 308
 309
 310   if(use_spoilers) {
 311     // angle of spoilers desired [rad]
 312     spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
 313     // amount spoilers move per time step [rad/sec]
 314     spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
 315     // determine spoiler position [rad] with respect to spoiler command
 316     spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
 317     // get the normailized position
 318     spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
 319   }
 320
 321
 322   return;
 323 }
 324
 325 // end uiuc_aerodeflections.cpp