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