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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  72  USA or view http://www.gnu.org/copyleft/gpl.html.
  73
  74 **********************************************************************/
  75
  76 #include "uiuc_auto_pilot.h"
  77 //#include <stdio.h>
  78 void uiuc_auto_pilot(double dt)
  79 {
  80   double V_rel_wind_ms;
  81   double Altitude_m;
  82   //static bool ap_pah_on_prev = false;
  83   static int ap_pah_init = 0;
  84   //static bool ap_alh_on_prev = false;
  85   static int ap_alh_init = 0;
  86   static int ap_rah_init = 0;
  87   static int ap_hh_init = 0;
  88   double ap_alt_ref_m;
  89   double bw_m;
  90   double ail_rud[2];
  91   V_rel_wind_ms = V_rel_wind * 0.3048;
  92   Altitude_m = Altitude * 0.3048;
  93
  94   if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time)
  95     {
  96       //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD;
  97       //if (ap_pah_on_prev == false) {
  98       //ap_pah_init = 1;
  99       //ap_pah_on_prev = true;
 100       //}
 101       elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt,
 102                         ap_pah_init);
 103       if (elevator*RAD_TO_DEG <= -1*demax)
 104         elevator = -1*demax * DEG_TO_RAD;
 105       if (elevator*RAD_TO_DEG >= demin)
 106         elevator = demin * DEG_TO_RAD;
 107       pilot_elev_no=true;
 108       ap_pah_init=1;
 109       //printf("elv1=%f\n",elevator);
 110     }
 111
 112   if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time)
 113     {
 114       ap_alt_ref_m = ap_alt_ref_ft * 0.3048;
 115       //if (ap_alh_on_prev == false)
 116       //ap_alh_init = 0;
 117       elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m,
 118                         V_rel_wind_ms, dt, ap_alh_init);
 119       if (elevator*RAD_TO_DEG <= -1*demax)
 120         elevator = -1*demax * DEG_TO_RAD;
 121       if (elevator*RAD_TO_DEG >= demin)
 122         elevator = demin * DEG_TO_RAD;
 123       pilot_elev_no=true;
 124       ap_alh_init = 1;
 125     }
 126
 127   if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time)
 128     {
 129       bw_m = bw * 0.3048;
 130       rah_ap(Phi, Phi_dot, ap_Phi_ref_rad, V_rel_wind_ms, dt,
 131              bw_m, Psi_dot, ail_rud, ap_rah_init);
 132       aileron = ail_rud[0];
 133       rudder = ail_rud[1];
 134       if (aileron*RAD_TO_DEG <= -1*damax)
 135         aileron = -1*damax * DEG_TO_RAD;
 136       if (aileron*RAD_TO_DEG >= damin)
 137         aileron = damin * DEG_TO_RAD;
 138       if (rudder*RAD_TO_DEG <= -1*drmax)
 139         rudder = -1*drmax * DEG_TO_RAD;
 140       if (rudder*RAD_TO_DEG >= drmin)
 141         rudder = drmin * DEG_TO_RAD;
 142       pilot_ail_no=true;
 143       pilot_rud_no=true;
 144       ap_rah_init = 1;
 145     }
 146
 147   if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time)
 148     {
 149       bw_m = bw * 0.3048;
 150       hh_ap(Phi, Psi, Phi_dot, ap_Psi_ref_rad, V_rel_wind_ms, dt,
 151             bw_m, Psi_dot, ail_rud, ap_hh_init);
 152       aileron = ail_rud[0];
 153       rudder = ail_rud[1];
 154       if (aileron*RAD_TO_DEG <= -1*damax)
 155         aileron = -1*damax * DEG_TO_RAD;
 156       if (aileron*RAD_TO_DEG >= damin)
 157         aileron = damin * DEG_TO_RAD;
 158       if (rudder*RAD_TO_DEG <= -1*drmax)
 159         rudder = -1*drmax * DEG_TO_RAD;
 160       if (rudder*RAD_TO_DEG >= drmin)
 161         rudder = drmin * DEG_TO_RAD;
 162       pilot_ail_no=true;
 163       pilot_rud_no=true;
 164       ap_hh_init = 1;
 165     }
 166 }