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