src/FDM/YASim/Surface.cpp

   1 #include "Math.hpp"
   2 #include "Surface.hpp"
   3 namespace yasim {
   4
   5 Surface::Surface()
   6 {
   7     // Start in a "sane" mode, so unset stuff doesn't freak us out
   8     _c0 = 1;
   9     _cx = _cy = _cz = 1;
  10     _cz0 = 0;
  11     _peaks[0] = _peaks[1] = 1;
  12     for(int i=0; i<4; i++)
  13         _stalls[i] = _widths[i] = 0;
  14     _orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
  15     _orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
  16     _orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
  17
  18     _incidence = 0;
  19     _slatPos = _spoilerPos = _flapPos = 0;
  20     _slatDrag = _spoilerDrag = _flapDrag = 1;
  21     _flapLift = 0;
  22     _slatAlpha = 0;
  23     _spoilerLift = 1;
  24 }
  25
  26 void Surface::setPosition(float* p)
  27 {
  28     for(int i=0; i<3; i++) _pos[i] = p[i];
  29 }
  30
  31 void Surface::getPosition(float* out)
  32 {
  33     for(int i=0; i<3; i++) out[i] = _pos[i];
  34 }
  35
  36 void Surface::setChord(float chord)
  37 {
  38     _chord = chord;
  39 }
  40
  41 void Surface::setTotalDrag(float c0)
  42 {
  43     _c0 = c0;
  44 }
  45
  46 float Surface::getTotalDrag()
  47 {
  48     return _c0;
  49 }
  50
  51 void Surface::setXDrag(float cx)
  52 {
  53     _cx = cx;
  54 }
  55
  56 void Surface::setYDrag(float cy)
  57 {
  58     _cy = cy;
  59 }
  60
  61 void Surface::setZDrag(float cz)
  62 {
  63     _cz = cz;
  64 }
  65
  66 void Surface::setBaseZDrag(float cz0)
  67 {
  68     _cz0 = cz0;
  69 }
  70
  71 void Surface::setStallPeak(int i, float peak)
  72 {
  73     _peaks[i] = peak;
  74 }
  75
  76 void Surface::setStall(int i, float alpha)
  77 {
  78     _stalls[i] = alpha;
  79 }
  80
  81 void Surface::setStallWidth(int i, float width)
  82 {
  83     _widths[i] = width;
  84 }
  85
  86 void Surface::setOrientation(float* o)
  87 {
  88     for(int i=0; i<9; i++)
  89         _orient[i] = o[i];
  90 }
  91
  92 void Surface::setIncidence(float angle)
  93 {
  94     _incidence = angle;
  95 }
  96
  97 void Surface::setSlatParams(float stallDelta, float dragPenalty)
  98 {
  99     _slatAlpha = stallDelta;
 100     _slatDrag = dragPenalty;
 101 }
 102
 103 void Surface::setFlapParams(float liftAdd, float dragPenalty)
 104 {
 105     _flapLift = liftAdd;
 106     _flapDrag = dragPenalty;
 107 }
 108
 109 void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
 110 {
 111     _spoilerLift = liftPenalty;
 112     _spoilerDrag = dragPenalty;
 113 }
 114
 115 void Surface::setFlap(float pos)
 116 {
 117     _flapPos = pos;
 118 }
 119
 120 void Surface::setSlat(float pos)
 121 {
 122     _slatPos = pos;
 123 }
 124
 125 void Surface::setSpoiler(float pos)
 126 {
 127     _spoilerPos = pos;
 128 }
 129
 130 // Calculate the aerodynamic force given a wind vector v (in the
 131 // aircraft's "local" coordinates) and an air density rho.  Returns a
 132 // torque about the Y axis, too.
 133 void Surface::calcForce(float* v, float rho, float* out, float* torque)
 134 {
 135     // Split v into magnitude and direction:
 136     float vel = Math::mag3(v);
 137
 138     // Handle the blowup condition.  Zero velocity means zero force by
 139     // definition.
 140     if(vel == 0) {
 141         for(int i=0; i<3; i++) out[i] = torque[i] = 0;
 142         return;
 143     }
 144
 145     Math::mul3(1/vel, v, out);
 146
 147     // Convert to the surface's coordinates
 148     Math::vmul33(_orient, out, out);
 149
 150     // "Rotate" by the incidence angle.  Assume small angles, so we
 151     // need to diddle only the Z component, X is relatively unchanged
 152     // by small rotations.
 153     out[2] += _incidence * out[0]; // z' = z + incidence * x
 154
 155     // Diddle the Z force according to our configuration
 156     float stallMul = stallFunc(out);
 157     stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
 158     float stallLift = (stallMul - 1) * _cz * out[2];
 159     float flapLift = _cz * _flapPos * (_flapLift-1);
 160
 161     out[2] *= _cz;       // scaling factor
 162     out[2] += _cz*_cz0;  // zero-alpha lift
 163     out[2] += stallLift;
 164     out[2] += flapLift;
 165
 166     // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
 167     // torque) around the Y axis, while flap lift pushes down.  Both
 168     // forces are considered to act at one third chord from the
 169     // center.  Convert to local (i.e. airplane) coordiantes and store
 170     // into "torque".
 171     torque[0] = 0;
 172     torque[1] = 0.33 * _chord * (flapLift - (_cz*_cz0 + stallLift));
 173     torque[2] = 0;
 174     Math::tmul33(_orient, torque, torque);
 175
 176     // Diddle X (drag) and Y (side force) in the same manner
 177     out[0] *= _cx * controlDrag();
 178     out[1] *= _cy;
 179
 180     // Reverse the incidence rotation to get back to surface
 181     // coordinates.
 182     out[2] -= _incidence * out[0];
 183
 184     // Convert back to external coordinates
 185     Math::tmul33(_orient, out, out);
 186
 187     // Add in the units to make a real force:
 188     float scale = 0.5*rho*vel*vel*_c0;
 189     Math::mul3(scale, out, out);
 190     Math::mul3(scale, torque, torque);
 191 }
 192
 193 // Returns a multiplier for the "plain" force equations that
 194 // approximates an airfoil's lift/stall curve.
 195 float Surface::stallFunc(float* v)
 196 {
 197     // Sanity check to treat FPU psychopathology
 198     if(v[0] == 0) return 1;
 199
 200     float alpha = Math::abs(v[2]/v[0]);
 201
 202     // Wacky use of indexing, see setStall*() methods.
 203     int fwdBak = v[0] > 0; // set if this is "backward motion"
 204     int posNeg = v[2] < 0; // set if the lift is toward -z
 205     int i = (fwdBak<<1) | posNeg;
 206
 207     float stallAlpha = _stalls[i];
 208     if(stallAlpha == 0)
 209         return 1;
 210
 211     if(i == 0)
 212         stallAlpha += _slatAlpha;
 213
 214     // Beyond the stall
 215     if(alpha > stallAlpha+_widths[i])
 216         return 1;
 217
 218     // (note mask: we want to use the "positive" stall angle here)
 219     float scale = 0.5*_peaks[fwdBak]/_stalls[i&2];
 220
 221     // Before the stall
 222     if(alpha <= stallAlpha)
 223         return scale;
 224
 225     // Inside the stall.  Compute a cubic interpolation between the
 226     // pre-stall "scale" value and the post-stall unity.
 227     float frac = (alpha - stallAlpha) / _widths[i];
 228     frac = frac*frac*(3-2*frac);
 229
 230     return scale*(1-frac) + frac;
 231 }
 232
 233 float Surface::controlDrag()
 234 {
 235     float d = 1;
 236     d *= 1 + _spoilerPos * (_spoilerDrag - 1);
 237     d *= 1 + _slatPos * (_slatDrag - 1);
 238
 239     // FIXME: flaps should REDUCE drag when the reduce lift!
 240     d *= 1 + Math::abs(_flapPos) * (_flapDrag - 1);
 241
 242     return d;
 243 }
 244
 245 }; // namespace yasim