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