src/FDM/UIUCModel/uiuc_auto_pilot.cpp

   1 /**********************************************************************
   2
   3  FILENAME:     uiuc_auto_pilot.cpp
   4
   5 ----------------------------------------------------------------------
   6
   7  DESCRIPTION:  calls autopilot routines
   8
   9 ----------------------------------------------------------------------
  10
  11  STATUS:       alpha version
  12
  13 ----------------------------------------------------------------------
  14
  15  REFERENCES:
  16
  17 ----------------------------------------------------------------------
  18
  19  HISTORY:      09/04/2003   initial release (with PAH)
  20                10/31/2003   added ALH autopilot
  21                11/04/2003   added RAH and HH autopilots
  22
  23 ----------------------------------------------------------------------
  24
  25  AUTHOR(S):    Robert Deters      <rdeters@uiuc.edu>
  26
  27 ----------------------------------------------------------------------
  28
  29  VARIABLES:
  30
  31 ----------------------------------------------------------------------
  32
  33  INPUTS:       -V_rel_wind (or U_body)
  34                -dyn_on_speed
  35                -ice on/off
  36                -phugoid on/off
  37
  38 ----------------------------------------------------------------------
  39
  40  OUTPUTS:      -CL
  41                -CD
  42                -Cm
  43                -CY
  44                -Cl
  45                -Cn
  46
  47 ----------------------------------------------------------------------
  48
  49  CALLED BY:    uiuc_coefficients
  50
  51 ----------------------------------------------------------------------
  52
  53  CALLS TO:     uiuc_coef_lift
  54                uiuc_coef_drag
  55
  56 ----------------------------------------------------------------------
  57
  58  COPYRIGHT:    (C) 2003 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  72
  73 **********************************************************************/
  74
  75 #include "uiuc_auto_pilot.h"
  76 //#include <stdio.h>
  77 void uiuc_auto_pilot(double dt)
  78 {
  79   double V_rel_wind_ms;
  80   double Altitude_m;
  81   //static bool ap_pah_on_prev = false;
  82   static int ap_pah_init = 0;
  83   //static bool ap_alh_on_prev = false;
  84   static int ap_alh_init = 0;
  85   static int ap_rah_init = 0;
  86   static int ap_hh_init = 0;
  87   double ap_alt_ref_m;
  88   double bw_m;
  89   double ail_rud[2];
  90   V_rel_wind_ms = V_rel_wind * 0.3048;
  91   Altitude_m = Altitude * 0.3048;
  92
  93   if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time)
  94     {
  95       //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD;
  96       //if (ap_pah_on_prev == false) {
  97       //ap_pah_init = 1;
  98       //ap_pah_on_prev = true;
  99       //}
 100       elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt,
 101                         ap_pah_init);
 102       if (elevator*RAD_TO_DEG <= -1*demax)
 103         elevator = -1*demax * DEG_TO_RAD;
 104       if (elevator*RAD_TO_DEG >= demin)
 105         elevator = demin * DEG_TO_RAD;
 106       pilot_elev_no=true;
 107       ap_pah_init=1;
 108       //printf("elv1=%f\n",elevator);
 109     }
 110
 111   if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time)
 112     {
 113       ap_alt_ref_m = ap_alt_ref_ft * 0.3048;
 114       //if (ap_alh_on_prev == false)
 115       //ap_alh_init = 0;
 116       elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m,
 117                         V_rel_wind_ms, dt, ap_alh_init);
 118       if (elevator*RAD_TO_DEG <= -1*demax)
 119         elevator = -1*demax * DEG_TO_RAD;
 120       if (elevator*RAD_TO_DEG >= demin)
 121         elevator = demin * DEG_TO_RAD;
 122       pilot_elev_no=true;
 123       ap_alh_init = 1;
 124     }
 125
 126   if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time)
 127     {
 128       bw_m = bw * 0.3048;
 129       rah_ap(Phi, Phi_dot, ap_Phi_ref_rad, V_rel_wind_ms, dt,
 130              bw_m, Psi_dot, ail_rud, ap_rah_init);
 131       aileron = ail_rud[0];
 132       rudder = ail_rud[1];
 133       if (aileron*RAD_TO_DEG <= -1*damax)
 134         aileron = -1*damax * DEG_TO_RAD;
 135       if (aileron*RAD_TO_DEG >= damin)
 136         aileron = damin * DEG_TO_RAD;
 137       if (rudder*RAD_TO_DEG <= -1*drmax)
 138         rudder = -1*drmax * DEG_TO_RAD;
 139       if (rudder*RAD_TO_DEG >= drmin)
 140         rudder = drmin * DEG_TO_RAD;
 141       pilot_ail_no=true;
 142       pilot_rud_no=true;
 143       ap_rah_init = 1;
 144     }
 145
 146   if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time)
 147     {
 148       bw_m = bw * 0.3048;
 149       hh_ap(Phi, Psi, Phi_dot, ap_Psi_ref_rad, V_rel_wind_ms, dt,
 150             bw_m, Psi_dot, ail_rud, ap_hh_init);
 151       aileron = ail_rud[0];
 152       rudder = ail_rud[1];
 153       if (aileron*RAD_TO_DEG <= -1*damax)
 154         aileron = -1*damax * DEG_TO_RAD;
 155       if (aileron*RAD_TO_DEG >= damin)
 156         aileron = damin * DEG_TO_RAD;
 157       if (rudder*RAD_TO_DEG <= -1*drmax)
 158         rudder = -1*drmax * DEG_TO_RAD;
 159       if (rudder*RAD_TO_DEG >= drmin)
 160         rudder = drmin * DEG_TO_RAD;
 161       pilot_ail_no=true;
 162       pilot_rud_no=true;
 163       ap_hh_init = 1;
 164     }
 165 }