src/FDM/UIUCModel/uiuc_alh_ap.cpp

   1 // *                                                   *
   2 // *   alh_ap.C                                        *
   3 // *                                                   *
   4 // *  ALH autopilot function. takes in the state       *
   5 // *  variables and reference height as arguments      *
   6 // *  (there are other variable too as arguments       *
   7 // *  as listed below)                                 *
   8 // *  and returns the elevator deflection angle at     *
   9 // *  every time step.                                 *
  10 // *                                                   *
  11 // *   Written 7/8/02 by Vikrant Sharma               *
  12 // *                                                   *
  13 // *****************************************************
  14
  15 //#include <iostream.h>
  16 //#include <stddef.h>
  17
  18 // define u2prev,ubarprev,x1prev,x2prev and x3prev in the main function
  19 // that uses this autopilot function. give them initial values at origin.
  20 // Pass these values to the A/P function as an argument and pass by
  21 // reference
  22 // Parameters passed as arguments to the function:
  23 // H - Current Height of the a/c at the current simulation time.
  24 // pitch - Current pitch angle ,,,,,,,,,,,,
  25 // pitchrate - current rate of change of pitch angle
  26 // H_ref - reference Height differential wanted
  27 // sample_t - sampling time
  28 // V - aircraft's current velocity
  29 // u2prev - u2 value at the previous time step.
  30 // ubar prev - ubar ,,,,,,,,,,,,,,,,,,,,,,,
  31 // x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  32 // x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  33 // x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  34 // the autpilot function (alh_ap) changes these values at every time step.
  35 // so the simulator guys don't have to do it. Since these values are
  36 // passed by reference to the function.
  37
  38 // RD changed float to double
  39
  40 #include "uiuc_alh_ap.h"
  41
  42 double alh_ap(double pitch, double pitchrate, double H_ref, double H,
  43               double V, double sample_t, int init)
  44 {
  45   // changes by RD so function keeps previous values
  46   static double u2prev;
  47   static double x1prev;
  48   static double x2prev;
  49   static double x3prev;
  50   static double ubarprev;
  51
  52   double pi = 3.14159;
  53
  54   if (init == 0)
  55     {
  56       u2prev = 0;
  57       x1prev = 0;
  58       x2prev = 0;
  59       x3prev = 0;
  60       ubarprev = 0;
  61     }
  62
  63         double Ki;
  64         double Ktheta;
  65         double Kq;
  66         double deltae;
  67         double Kh,Kd;
  68         double x1, x2, x3;
  69         Ktheta = -0.0004*V*V + 0.0479*V - 2.409;
  70         Kq = -0.0005*V*V + 0.054*V - 1.5931;
  71         Ki = 0.5;
  72         Kh = -0.25*pi/180 + (((-0.15 + 0.25)*pi/180)/(20))*(V-60);
  73         Kd = -0.0025*V + 0.2875;
  74         double u1,u2,u3,ubar;
  75         ubar = (1-Kd*sample_t)*ubarprev + Ktheta*pitchrate*sample_t;
  76         u1 = Kh*(H_ref-H) - ubar;
  77         u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_t;
  78         u3 = Kq*pitchrate;
  79         double totalU;
  80         totalU = u1 + u2 - u3;
  81         u2prev = u2;
  82         ubarprev = ubar;
  83 // the following is using the actuator dynamics given in Beaver.
  84 // the actuator dynamics for Twin Otter are still unavailable.
  85         x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev +
  86 25.1568*totalU)*sample_t;
  87         x2 = x2prev + x3prev*sample_t;
  88         x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev +
  89 5.8694*totalU)*sample_t;
  90         deltae = 57.2958*x2;
  91         x1prev = x1;
  92         x2prev = x2;
  93         x3prev = x3;
  94 return deltae;
  95 }