src/FDM/YASim/Surface.cpp

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