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