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