src/FDM/UIUCModel/uiuc_hh_ap.cpp

   1 // *                                                   *
   2 // *   hh_ap.C                                         *
   3 // *                                                   *
   4 // *  Heading Hold autopilot function. takes in        *
   5 // *  the state                                        *
   6 // *  variables and reference angle as arguments       *
   7 // *  (there are other variable too as arguments       *
   8 // *  as listed below)                                 *
   9 // *  and returns the aileron and rudder deflection    *
  10 // *  angle at every time step                         *
  11 // *                                                   *
  12 // *                                                   *
  13 // *   Written 2/14/03 by Vikrant Sharma               *
  14 // *                                                   *
  15 // *****************************************************
  16
  17 //#include <iostream.h>
  18 //#include <stddef.h>
  19
  20 // define u2prev,x1prev,x2prev,x3prev,x4prev,x5prev and x6prev in the main
  21 // function
  22 // that uses this autopilot function. give them initial values at origin.
  23 // Pass these values to the A/P function as an argument and pass by
  24 // reference
  25 // Parameters passed as arguments to the function:
  26 // yaw - current yaw angle difference from the trim value
  27 // phi - Current roll angle ,,,,,,,,,,,,,,,,,,,,
  28 // rollrate - current rate of change of roll angle
  29 // yawrate - current rate of change of yaw angle
  30 // b - the wingspan
  31 // yaw_ref - reference yaw angle to be tracked (amount of increase/decrease desired from the trim)
  32 // sample_t - sampling time
  33 // V - aircraft's current velocity
  34 // u2prev - u2 value at the previous time step.
  35 // x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  36 // x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  37 // x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  38 // x4prev - x4 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  39 // x5prev - x5 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  40 // x6prev - x6 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  41 // the autpilot function (rah_ap) changes these values at every time step.
  42 // so the simulator guys don't have to do it. Since these values are
  43 // passed by reference to the function.
  44
  45 // Another important thing to do and for you simulator guys to check is the
  46 // way I return the the deltaa (aileron deflection) and deltar (rudder deflection).
  47 // I expect you guys to define an array that holds two float values. The first entry
  48 // is for deltaa and the second for deltar and the pointer to this array (ctr_ptr) is also
  49 // one of the arguments for the function rah_ap and then this function updates the value for
  50 // deltae and deltaa every time its called. PLEASE CHECK IF THE SYNTAX IS RIGHT. Also note that
  51 // these values have to be added to the initial trim values of the control deflection to get the
  52 // entire control deflection.
  53
  54 #include "uiuc_hh_ap.h"
  55
  56 // (RD) changed float to double
  57
  58 void hh_ap(double phi, double yaw, double phirate, double yaw_ref,
  59            double V, double sample_time, double b, double yawrate,
  60            double ctr_defl[2], int init)
  61 {
  62
  63   static double u2prev;
  64   static double x1prev;
  65   static double x2prev;
  66   static double x3prev;
  67   static double x4prev;
  68   static double x5prev;
  69   static double x6prev;
  70
  71   if (init==0)
  72     {
  73       u2prev = 0;
  74       x1prev = 0;
  75       x2prev = 0;
  76       x3prev = 0;
  77       x4prev = 0;
  78       x5prev = 0;
  79       x6prev = 0;
  80     }
  81
  82         double Kphi, Kyaw;
  83         double Kr;
  84         double Ki;
  85         double drr;
  86         double dar;
  87         double deltar;
  88         double deltaa;
  89         double x1, x2, x3, x4, x5, x6, phi_ref;
  90         Kphi = 0.000975*V*V - 0.108*V + 2.335625;
  91         Kr = -4;
  92         Ki = 0.25;
  93         Kyaw = 0.05*V-1.1;
  94         dar = 0.165;
  95         drr = -0.000075*V*V + 0.0095*V -0.4606;
  96         double u1,u2,u3,u4,u5,u6,u7,ubar,udbar;
  97         phi_ref = Kyaw*(yaw_ref-yaw);
  98         u1 = Kphi*(phi_ref-phi);
  99         u2 = u2prev + Ki*Kphi*(phi_ref-phi)*sample_time;
 100         u3 = dar*yawrate;
 101         u4 = u1 + u2 + u3;
 102         u2prev = u2;
 103         double K1,K2;
 104         K1 = Kr*9.8*sin(phi)/V;
 105         K2 = drr - Kr;
 106         u5 = K2*yawrate;
 107         u6 = K1*phi;
 108         u7 = u5 + u6;
 109         ubar = phirate*b/(2*V);
 110         udbar = yawrate*b/(2*V);
 111 // the following is using the actuator dynamics to get the aileron
 112 // angle, given in Beaver.
 113 // the actuator dynamics for Twin Otter are still unavailable.
 114         x1 = x1prev +(-10.6*x1prev - 2.64*x2prev -7.58*x3prev +
 115 27.46*u4 -0.0008*ubar)*sample_time;
 116         x2 = x2prev + x3prev*sample_time;
 117         x3 = x3prev + (1.09*x1prev - 558.86*x2prev - 23.35*x3prev +
 118 3.02*u4 - 0.164*ubar)*sample_time;
 119         deltaa = 57.3*x2;
 120         x1prev = x1;
 121         x2prev = x2;
 122         x3prev = x3;
 123
 124 // the following is using the actuator dynamics to get the rudder
 125 // angle, given in Beaver.
 126 // the actuator dynamics for Twin Otter are still unavailable.
 127         x4 = x4prev +(-9.2131*x4prev + 5.52*x5prev + 16*x6prev +
 128 24.571*u7 + 0.0036*udbar)*sample_time;
 129         x5 = x5prev + x6prev*sample_time;
 130         x6 = x6prev + (0.672*x4prev - 919.78*x5prev - 56.453*x6prev +
 131 7.54*u7 - 0.636*udbar)*sample_time;
 132         deltar = 57.3*x5;
 133         x4prev = x4;
 134         x5prev = x5;
 135         x6prev = x6;
 136         ctr_defl[0] = deltaa;
 137         ctr_defl[1] = deltar;
 138 return;
 139 }