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     Math::mul3(1/vel, v, out);
 162
 163     // Convert to the surface's coordinates
 164     Math::vmul33(_orient, out, out);
 165
 166     // "Rotate" by the incidence angle.  Assume small angles, so we
 167     // need to diddle only the Z component, X is relatively unchanged
 168     // by small rotations.
 169     float incidence = _incidence + _twist;
 170     out[2] += incidence * out[0]; // z' = z + incidence * x
 171
 172     // Hold onto the local wind vector so we can multiply the induced
 173     // drag at the end.
 174     float lwind[3];
 175     Math::set3(out, lwind);
 176
 177     // Diddle the Z force according to our configuration
 178     float stallMul = stallFunc(out);
 179     stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
 180     float stallLift = (stallMul - 1) * _cz * out[2];
 181     float flaplift = flapLift(out[2]);
 182
 183     out[2] *= _cz;       // scaling factor
 184     out[2] += _cz*_cz0;  // zero-alpha lift
 185     out[2] += stallLift;
 186     out[2] += flaplift;
 187
 188     // Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
 189     // torque) around the Y axis, while flap lift pushes down.  Both
 190     // forces are considered to act at one third chord from the
 191     // edge.  Convert to local (i.e. airplane) coordiantes and store
 192     // into "torque".
 193     torque[0] = 0;
 194     torque[1] = 0.1667f * _chord * (flaplift - (_cz*_cz0 + stallLift));
 195     torque[2] = 0;
 196     Math::tmul33(_orient, torque, torque);
 197
 198     // The X (drag) force gets diddled for control deflection
 199     out[0] = controlDrag(out[2], _cx * out[0]);
 200
 201     // Add in any specific Y (side force) coefficient.
 202     out[1] *= _cy;
 203
 204     // Diddle the induced drag
 205     float IDMUL = 0.5;
 206     Math::mul3(-1*_inducedDrag*out[2]*lwind[2], lwind, lwind);
 207     Math::add3(lwind, out, out);
 208
 209     // Reverse the incidence rotation to get back to surface
 210     // coordinates.
 211     out[2] -= incidence * out[0];
 212
 213     // Convert back to external coordinates
 214     Math::tmul33(_orient, out, out);
 215
 216     // Add in the units to make a real force:
 217     float scale = 0.5f*rho*vel*vel*_c0;
 218     Math::mul3(scale, out, out);
 219     Math::mul3(scale, torque, torque);
 220 }
 221
 222 // Returns a multiplier for the "plain" force equations that
 223 // approximates an airfoil's lift/stall curve.
 224 float Surface::stallFunc(float* v)
 225 {
 226     // Sanity check to treat FPU psychopathology
 227     if(v[0] == 0) return 1;
 228
 229     float alpha = Math::abs(v[2]/v[0]);
 230
 231     // Wacky use of indexing, see setStall*() methods.
 232     int fwdBak = v[0] > 0; // set if this is "backward motion"
 233     int posNeg = v[2] < 0; // set if the lift is toward -z
 234     int i = (fwdBak<<1) | posNeg;
 235
 236     float stallAlpha = _stalls[i];
 237     if(stallAlpha == 0)
 238         return 1;
 239
 240     if(i == 0)
 241         stallAlpha += _slatAlpha;
 242
 243     // Beyond the stall
 244     if(alpha > stallAlpha+_widths[i])
 245         return 1;
 246
 247     // (note mask: we want to use the "positive" stall angle here)
 248     float scale = 0.5f*_peaks[fwdBak]/_stalls[i&2];
 249
 250     // Before the stall
 251     if(alpha <= stallAlpha)
 252         return scale;
 253
 254     // Inside the stall.  Compute a cubic interpolation between the
 255     // pre-stall "scale" value and the post-stall unity.
 256     float frac = (alpha - stallAlpha) / _widths[i];
 257     frac = frac*frac*(3-2*frac);
 258
 259     return scale*(1-frac) + frac;
 260 }
 261
 262 // Similar to the above -- interpolates out the flap lift past the
 263 // stall alpha
 264 float Surface::flapLift(float alpha)
 265 {
 266     float flapLift = _cz * _flapPos * (_flapLift-1);
 267
 268     if(alpha < 0) alpha = -alpha;
 269     if(alpha < _stalls[0])
 270         return flapLift;
 271     else if(alpha > _stalls[0] + _widths[0])
 272         return 1;
 273
 274     float frac = (alpha - _stalls[0]) / _widths[0];
 275     frac = frac*frac*(3-2*frac);
 276     return flapLift * (1-frac) + frac;
 277 }
 278
 279 float Surface::controlDrag(float lift, float drag)
 280 {
 281     // Negative flap deflections don't affect drag until their lift
 282     // multiplier exceeds the "camber" (cz0) of the surface.  Use a
 283     // synthesized "fp" number instead of the actual flap position.
 284     float fp = _flapPos;
 285     if(fp < 0) {
 286         fp = -fp;
 287         fp -= _cz0/(_flapLift-1);
 288         if(fp < 0) fp = 0;
 289     }
 290
 291     // Calculate an "effective" drag -- this is the drag that would
 292     // have been produced by an unflapped surface at the same lift.
 293     float flapDragAoA = (_flapLift - 1 - _cz0) * _stalls[0];
 294     float fd = Math::abs(lift * flapDragAoA * fp);
 295     if(drag < 0) fd = -fd;
 296     drag += fd;
 297
 298     // Now multiply by the various control factors
 299     drag *= 1 + fp * (_flapDrag - 1);
 300     drag *= 1 + _spoilerPos * (_spoilerDrag - 1);
 301     drag *= 1 + _slatPos * (_slatDrag - 1);
 302
 303     return drag;
 304 }
 305
 306 }; // namespace yasim