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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  79  USA or view http://www.gnu.org/copyleft/gpl.html.
  80
  81 **********************************************************************/
  82
  83 #include <math.h>
  84
  85 #include "uiuc_aerodeflections.h"
  86
  87 void uiuc_aerodeflections( double dt )
  88 {
  89   double demax_remain;
  90   double demin_remain;
  91   static double elev_trim;
  92
  93   if (use_uiuc_network) {
  94       if (pitch_trim_up)
  95         elev_trim += 0.001;
  96       if (pitch_trim_down)
  97         elev_trim -= 0.001;
  98       if (elev_trim > 1.0)
  99         elev_trim = 1;
 100       if (elev_trim < -1.0)
 101         elev_trim = -1;
 102       Flap_handle = flap_percent * flap_max;
 103       if (outside_control) {
 104         pilot_elev_no = true;
 105         pilot_ail_no = true;
 106         pilot_rud_no = true;
 107         pilot_throttle_no = true;
 108         Long_trim = elev_trim;
 109       }
 110   }
 111
 112   if (zero_Long_trim) {
 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     if (Long_trim <= 0) {
 124       elevator = Long_trim * demax * DEG_TO_RAD;
 125       demax_remain = demax + Long_trim * demax;
 126       demin_remain = -Long_trim * demax + demin;
 127       if (Long_control <= 0)
 128         elevator += Long_control * demax_remain * DEG_TO_RAD;
 129       else
 130         elevator += Long_control * demin_remain * DEG_TO_RAD;
 131     } else {
 132       elevator = Long_trim * demin * DEG_TO_RAD;
 133       demin_remain = demin - Long_trim * demin;
 134       demax_remain = Long_trim * demin + demax;
 135       if (Long_control >=0)
 136         elevator += Long_control * demin_remain * DEG_TO_RAD;
 137       else
 138         elevator += Long_control * demax_remain * DEG_TO_RAD;
 139     }
 140   } else {
 141     if ((Long_control+Long_trim) <= 0)
 142       elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
 143     else
 144       elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
 145   }
 146
 147   if (Rudder_pedal <= 0)
 148     rudder = -Rudder_pedal * drmin * DEG_TO_RAD;
 149   else
 150     rudder = -Rudder_pedal * drmax * DEG_TO_RAD;
 151
 152   /* at this point aileron, elevator, and rudder commands are known (in radians)
 153    * add in SAS and any other mixing control laws
 154    */
 155
 156   // SAS for pitch, positive elevator is TED
 157   if (use_elevator_stick_gain)
 158     elevator *= elevator_stick_gain;
 159   if (use_elevator_sas_type1) {
 160     elevator_sas = elevator_sas_KQ * Q_body;
 161     if (use_elevator_sas_max && (elevator_sas > (elevator_sas_max * DEG_TO_RAD))) {
 162       elevator += elevator_sas_max * DEG_TO_RAD;
 163       //elevator_sas = elevator_sas_max;
 164     } else if (use_elevator_sas_min && (elevator_sas < (elevator_sas_min * DEG_TO_RAD))) {
 165       elevator += elevator_sas_min * DEG_TO_RAD;
 166       //elevator_sas = elevator_sas_min;
 167     }
 168     else
 169       elevator += elevator_sas;
 170     // don't exceed the elevator deflection limits
 171     if (elevator > demax * DEG_TO_RAD)
 172       elevator = demax * DEG_TO_RAD;
 173     else if (elevator < (-demin * DEG_TO_RAD))
 174       elevator = -demin * DEG_TO_RAD;
 175   }
 176
 177   // SAS for roll, positive aileron is right aileron TED
 178   if (use_aileron_stick_gain)
 179     aileron *= aileron_stick_gain;
 180   if (use_aileron_sas_type1) {
 181     aileron_sas = aileron_sas_KP * P_body;
 182     if (use_aileron_sas_max && (fabs(aileron_sas) > (aileron_sas_max * DEG_TO_RAD)))
 183       if (aileron_sas >= 0) {
 184         aileron +=  aileron_sas_max * DEG_TO_RAD;
 185         //aileron_sas = aileron_sas_max;
 186       } else {
 187         aileron += -aileron_sas_max * DEG_TO_RAD;
 188         //aileron_sas = -aileron_sas;
 189       }
 190     else
 191       aileron += aileron_sas;
 192     // don't exceed aileron deflection limits
 193     if (fabs(aileron) > (damax * DEG_TO_RAD))
 194       if (aileron > 0)
 195         aileron =  damax * DEG_TO_RAD;
 196       else
 197         aileron = -damax * DEG_TO_RAD;
 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   /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
 225      The second part of the if-block is rewritten below.
 226   double prevFlapHandle = 0.0f;
 227   double flap_transit_rate;
 228   bool flaps_in_transit = false;
 229
 230   if (old_flap_routine) {
 231     // old flap routine
 232     // check for lowest flap setting
 233     if (Flap_handle < dfArray[1]) {
 234       Flap_handle    = dfArray[1];
 235       prevFlapHandle = Flap_handle;
 236       flap           = Flap_handle;
 237     } else if (Flap_handle > dfArray[ndf]) {
 238       // check for highest flap setting
 239       Flap_handle      = dfArray[ndf];
 240       prevFlapHandle   = Flap_handle;
 241       flap             = Flap_handle;
 242     } else {
 243       // otherwise in between
 244       if(Flap_handle != prevFlapHandle)
 245         flaps_in_transit = true;
 246       if(flaps_in_transit) {
 247         int iflap = 0;
 248         while (dfArray[iflap] < Flap_handle)
 249           iflap++;
 250         if (flap < Flap_handle) {
 251           if (TimeArray[iflap] > 0)
 252             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
 253           else
 254             flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
 255         } else {
 256           if (TimeArray[iflap+1] > 0)
 257             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
 258           else
 259             flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
 260         }
 261         if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
 262           flap += flap_transit_rate * dt;
 263         else {
 264           flaps_in_transit = false;
 265           flap = Flap_handle;
 266         }
 267       }
 268     }
 269     prevFlapHandle = Flap_handle;
 270   } else {
 271     // new flap routine
 272     // designed for the twin otter non-linear model
 273     if (!outside_control) {
 274       flap_percent     = Flap_handle / 30.0;    // percent of flaps desired
 275       if (flap_percent>=0.31 && flap_percent<=0.35)
 276         flap_percent = 1.0 / 3.0;
 277       if (flap_percent>=0.65 && flap_percent<=0.69)
 278         flap_percent = 2.0 / 3.0;
 279     }
 280     flap_cmd_deg        = flap_percent * flap_max;  // angle of flaps desired
 281     flap_moving_rate = flap_rate * dt;           // amount flaps move per time step
 282
 283     // determine flap position with respect to the flap goal
 284     if (flap_pos < flap_cmd_deg) {
 285       flap_pos += flap_moving_rate;
 286       if (flap_pos > flap_cmd_deg)
 287         flap_pos = flap_cmd_deg;
 288     } else if (flap_pos > flap_cmd_deg) {
 289       flap_pos -= flap_moving_rate;
 290       if (flap_pos < flap_cmd_deg)
 291         flap_pos = flap_cmd_deg;
 292     }
 293   }
 294
 295 */
 296
 297   if (!outside_control && use_flaps) {
 298     // angle of flaps desired [rad]
 299     flap_cmd = Flap_handle * DEG_TO_RAD;
 300     // amount flaps move per time step [rad/sec]
 301     flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
 302     // determine flap position [rad] with respect to flap command
 303     flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
 304     // get the normalized position
 305     flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
 306   }
 307
 308
 309   if(use_spoilers) {
 310     // angle of spoilers desired [rad]
 311     spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
 312     // amount spoilers move per time step [rad/sec]
 313     spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
 314     // determine spoiler position [rad] with respect to spoiler command
 315     spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
 316     // get the normailized position
 317     spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
 318   }
 319
 320
 321   return;
 322 }
 323
 324 // end uiuc_aerodeflections.cpp