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