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] = 0;
  15         _widths[i] = 0.01; // half a degree
  16     }
  17     _orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
  18     _orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
  19     _orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
  20
  21     _chord = 0;
  22     _incidence = 0;
  23     _twist = 0;
  24     _slatPos = _spoilerPos = _flapPos = 0;
  25     _slatDrag = _spoilerDrag = _flapDrag = 1;
  26
  27     _flapLift = 0;
  28     _slatAlpha = 0;
  29     _spoilerLift = 1;
  30     _inducedDrag = 1;
  31 }
  32
  33 void Surface::setPosition(float* p)
  34 {
  35     int i;
  36     for(i=0; i<3; i++) _pos[i] = p[i];
  37 }
  38
  39 void Surface::getPosition(float* out)
  40 {
  41     int i;
  42     for(i=0; i<3; i++) out[i] = _pos[i];
  43 }
  44
  45 void Surface::setChord(float chord)
  46 {
  47     _chord = chord;
  48 }
  49
  50 void Surface::setTotalDrag(float c0)
  51 {
  52     _c0 = c0;
  53 }
  54
  55 float Surface::getTotalDrag()
  56 {
  57     return _c0;
  58 }
  59
  60 void Surface::setXDrag(float cx)
  61 {
  62     _cx = cx;
  63 }
  64
  65 void Surface::setYDrag(float cy)
  66 {
  67     _cy = cy;
  68 }
  69
  70 void Surface::setZDrag(float cz)
  71 {
  72     _cz = cz;
  73 }
  74
  75 void Surface::setBaseZDrag(float cz0)
  76 {
  77     _cz0 = cz0;
  78 }
  79
  80 void Surface::setStallPeak(int i, float peak)
  81 {
  82     _peaks[i] = peak;
  83 }
  84
  85 void Surface::setStall(int i, float alpha)
  86 {
  87     _stalls[i] = alpha;
  88 }
  89
  90 void Surface::setStallWidth(int i, float width)
  91 {
  92     _widths[i] = width;
  93 }
  94
  95 void Surface::setOrientation(float* o)
  96 {
  97     int i;
  98     for(i=0; i<9; i++)
  99         _orient[i] = o[i];
 100 }
 101
 102 void Surface::setIncidence(float angle)
 103 {
 104     _incidence = angle;
 105 }
 106
 107 void Surface::setTwist(float angle)
 108 {
 109     _twist = angle;
 110 }
 111
 112 void Surface::setSlatParams(float stallDelta, float dragPenalty)
 113 {
 114     _slatAlpha = stallDelta;
 115     _slatDrag = dragPenalty;
 116 }
 117
 118 void Surface::setFlapParams(float liftAdd, float dragPenalty)
 119 {
 120     _flapLift = liftAdd;
 121     _flapDrag = dragPenalty;
 122 }
 123
 124 void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
 125 {
 126     _spoilerLift = liftPenalty;
 127     _spoilerDrag = dragPenalty;
 128 }
 129
 130 void Surface::setFlap(float pos)
 131 {
 132     _flapPos = pos;
 133 }
 134
 135 void Surface::setSlat(float pos)
 136 {
 137     _slatPos = pos;
 138 }
 139
 140 void Surface::setSpoiler(float pos)
 141 {
 142     _spoilerPos = pos;
 143 }
 144
 145 // Calculate the aerodynamic force given a wind vector v (in the
 146 // aircraft's "local" coordinates) and an air density rho.  Returns a
 147 // torque about the Y axis, too.
 148 void Surface::calcForce(float* v, float rho, float* out, float* torque)
 149 {
 150     // Split v into magnitude and direction:
 151     float vel = Math::mag3(v);
 152
 153     // Handle the blowup condition.  Zero velocity means zero force by
 154     // definition.
 155     if(vel == 0) {
 156         int i;
 157         for(i=0; i<3; i++) out[i] = torque[i] = 0;
 158         return;
 159     }
 160
 161     // special case this so the logic below doesn't produce a non-zero
 162     // force; should probably have a "no force" flag instead...
 163     if(_cx == 0. && _cy == 0. && _cz == 0.) {
 164         for(int i=0; i<3; i++) out[i] = torque[i] = 0.;
 165         return;
 166     }
 167
 168     Math::mul3(1/vel, v, out);
 169
 170     // Convert to the surface's coordinates
 171     Math::vmul33(_orient, out, out);
 172
 173     // "Rotate" by the incidence angle.  Assume small angles, so we
 174     // need to diddle only the Z component, X is relatively unchanged
 175     // by small rotations.
 176     float incidence = _incidence + _twist;
 177     out[2] += incidence * out[0]; // z' = z + incidence * x
 178
 179     // Hold onto the local wind vector so we can multiply the induced
 180     // drag at the end.
 181     float lwind[3];
 182     Math::set3(out, lwind);
 183
 184     // Diddle the Z force according to our configuration
 185     float stallMul = stallFunc(out);
 186     stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
 187     float stallLift = (stallMul - 1) * _cz * out[2];
 188     float flaplift = flapLift(out[2]);
 189
 190     out[2] *= _cz;       // scaling factor
 191     out[2] += _cz*_cz0;  // zero-alpha lift
 192     out[2] += stallLift;
 193     out[2] += flaplift;
 194
 195     // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
 196     // torque) around the Y axis, while flap lift pushes down.  Both
 197     // forces are considered to act at one third chord from the
 198     // edge.  Convert to local (i.e. airplane) coordiantes and store
 199     // into "torque".
 200     torque[0] = 0;
 201     torque[1] = 0.1667f * _chord * (flaplift - (_cz*_cz0 + stallLift));
 202     torque[2] = 0;
 203     Math::tmul33(_orient, torque, torque);
 204
 205     // The X (drag) force gets diddled for control deflection
 206     out[0] = controlDrag(out[2], _cx * out[0]);
 207
 208     // Add in any specific Y (side force) coefficient.
 209     out[1] *= _cy;
 210
 211     // Diddle the induced drag
 212     Math::mul3(-1*_inducedDrag*out[2]*lwind[2], lwind, lwind);
 213     Math::add3(lwind, out, out);
 214
 215     // Reverse the incidence rotation to get back to surface
 216     // coordinates.
 217     out[2] -= incidence * out[0];
 218
 219     // Convert back to external coordinates
 220     Math::tmul33(_orient, out, out);
 221
 222     // Add in the units to make a real force:
 223     float scale = 0.5f*rho*vel*vel*_c0;
 224     Math::mul3(scale, out, out);
 225     Math::mul3(scale, torque, torque);
 226 }
 227
 228 #if 0
 229 void Surface::test()
 230 {
 231     static const float DEG2RAD = 0.0174532925199;
 232     float v[3], force[3], torque[3];
 233     float rho = Atmosphere::getStdDensity(0);
 234     float spd = 30;
 235
 236     setFlap(0);
 237     setSlat(0);
 238     setSpoiler(0);
 239
 240     for(float angle = -90; angle<90; angle += 0.01) {
 241         float rad = angle * DEG2RAD;
 242         v[0] = spd * -Math::cos(rad);
 243         v[1] = 0;
 244         v[2] = spd * Math::sin(rad);
 245         calcForce(v, rho, force, torque);
 246         float lift = force[2] * Math::cos(rad) + force[0] * Math::sin(rad);
 247         //__builtin_printf("%f %f\n", angle, lift);
 248         __builtin_printf("%f %f\n", angle, torque[2]);
 249     }
 250 }
 251 #endif
 252
 253 // Returns a multiplier for the "plain" force equations that
 254 // approximates an airfoil's lift/stall curve.
 255 float Surface::stallFunc(float* v)
 256 {
 257     // Sanity check to treat FPU psychopathology
 258     if(v[0] == 0) return 1;
 259
 260     float alpha = Math::abs(v[2]/v[0]);
 261
 262     // Wacky use of indexing, see setStall*() methods.
 263     int fwdBak = v[0] > 0; // set if this is "backward motion"
 264     int posNeg = v[2] < 0; // set if the airflow is toward -z
 265     int i = (fwdBak<<1) | posNeg;
 266
 267     float stallAlpha = _stalls[i];
 268     if(stallAlpha == 0)
 269         return 1;
 270
 271     if(i == 0)
 272         stallAlpha += _slatAlpha;
 273
 274     // Beyond the stall
 275     if(alpha > stallAlpha+_widths[i])
 276         return 1;
 277
 278     // (note mask: we want to use the "positive" stall angle here)
 279     float scale = 0.5f*_peaks[fwdBak]/_stalls[i&2];
 280
 281     // Before the stall
 282     if(alpha <= stallAlpha)
 283         return scale;
 284
 285     // Inside the stall.  Compute a cubic interpolation between the
 286     // pre-stall "scale" value and the post-stall unity.
 287     float frac = (alpha - stallAlpha) / _widths[i];
 288     frac = frac*frac*(3-2*frac);
 289
 290     return scale*(1-frac) + frac;
 291 }
 292
 293 // Similar to the above -- interpolates out the flap lift past the
 294 // stall alpha
 295 float Surface::flapLift(float alpha)
 296 {
 297     float flapLift = _cz * _flapPos * (_flapLift-1);
 298     if(_stalls[0] == 0)
 299         return 0;
 300
 301     if(alpha < 0) alpha = -alpha;
 302     if(alpha < _stalls[0])
 303         return flapLift;
 304     else if(alpha > _stalls[0] + _widths[0])
 305         return 0;
 306
 307     float frac = (alpha - _stalls[0]) / _widths[0];
 308     frac = frac*frac*(3-2*frac);
 309     return flapLift * (1-frac);
 310 }
 311
 312 float Surface::controlDrag(float lift, float drag)
 313 {
 314     // Negative flap deflections don't affect drag until their lift
 315     // multiplier exceeds the "camber" (cz0) of the surface.  Use a
 316     // synthesized "fp" number instead of the actual flap position.
 317     float fp = _flapPos;
 318     if(fp < 0) {
 319         fp = -fp;
 320         fp -= _cz0/(_flapLift-1);
 321         if(fp < 0) fp = 0;
 322     }
 323
 324     // Calculate an "effective" drag -- this is the drag that would
 325     // have been produced by an unflapped surface at the same lift.
 326     float flapDragAoA = (_flapLift - 1 - _cz0) * _stalls[0];
 327     float fd = Math::abs(lift * flapDragAoA * fp);
 328     if(drag < 0) fd = -fd;
 329     drag += fd;
 330
 331     // Now multiply by the various control factors
 332     drag *= 1 + fp * (_flapDrag - 1);
 333     drag *= 1 + _spoilerPos * (_spoilerDrag - 1);
 334     drag *= 1 + _slatPos * (_slatDrag - 1);
 335
 336     return drag;
 337 }
 338
 339 }; // namespace yasim