src/FDM/UIUCModel/uiuc_pah_ap.cpp

   1 // *                                                   *
   2 // *   pah_ap.C                                        *
   3 // *                                                   *
   4 // *  Pah autopilot function. takes in the state       *
   5 // *  variables and reference angle 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 2/11/02 by Vikrant Sharma               *
  12 // *                                                   *
  13 // *****************************************************
  14
  15 //#include <iostream.h>
  16 //#include <stddef.h>
  17
  18 // define u2prev,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 // pitch - Current pitch angle
  24 // pitchrate - current rate of change of pitch angle
  25 // pitch_ref - reference pitch angle to be tracked
  26 // sample_t - sampling time
  27 // V - aircraft's current velocity
  28 // u2prev - u2 value at the previous time step.
  29 // x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  30 // x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  31 // x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
  32 // the autpilot function (pah_ap) changes these values at every time step.
  33 // so the simulator guys don't have to do it. Since these values are
  34 // passed by reference to the function.
  35
  36 // (RD) Units for the variables
  37 // pitch = radians
  38 // pitchrate = rad/s
  39 // pitch_ref = rad
  40 // sample_t = seconds
  41 // V = m/s
  42
  43 // (RD) changed from float to double
  44
  45 #include "uiuc_pah_ap.h"
  46 //#include <stdio.h>
  47 double pah_ap(double pitch, double pitchrate, double pitch_ref, double V,
  48               double sample_time, int init)
  49 {
  50   // changes by RD so function keeps previous values
  51   static double u2prev;
  52   static double x1prev;
  53   static double x2prev;
  54   static double x3prev;
  55
  56   if (init == 0)
  57     {
  58       u2prev = 0;
  59       x1prev = 0;
  60       x2prev = 0;
  61       x3prev = 0;
  62     }
  63   // end changes
  64
  65   double Ki;
  66   double Ktheta;
  67   double Kq;
  68   double deltae;
  69   double x1, x2, x3;
  70   Ktheta = -0.0004*V*V + 0.0479*V - 2.409;
  71   Kq = -0.0005*V*V + 0.054*V - 1.5931;
  72   Ki = 0.5;
  73   double u1,u2,u3;
  74   u1 = Ktheta*(pitch_ref-pitch);
  75   u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_time;
  76   u3 = Kq*pitchrate;
  77   double totalU;
  78   totalU = u1 + u2 - u3;
  79   //printf("\nu1=%f\n",u1);
  80   //printf("u2=%f\n",u2);
  81   //printf("u3=%f\n",u3);
  82   //printf("totalU=%f\n",totalU);
  83   u2prev = u2;
  84   // the following is using the actuator dynamics given in Beaver.
  85   // the actuator dynamics for Twin Otter are still unavailable.
  86   x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev +
  87                 25.1568*totalU)*sample_time;
  88   x2 = x2prev + x3prev*sample_time;
  89   x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev +
  90                  5.8694*totalU)*sample_time;
  91   deltae = 57.2958*x2;
  92   //deltae = x2;
  93   //printf("x1prev=%f\n",x1prev);
  94   //printf("x2prev=%f\n",x2prev);
  95   //printf("x3prev=%f\n",x3prev);
  96   x1prev = x1;
  97   x2prev = x2;
  98   x3prev = x3;
  99   //printf("x1=%f\n",x1);
 100   //printf("x2=%f\n",x2);
 101   //printf("x3=%f\n",x3);
 102   //printf("deltae=%f\n",deltae);
 103   return deltae;
 104 }
 105
 106
 107
 108