337 lines
9.9 KiB
C++
337 lines
9.9 KiB
C++
#include <Main/fg_props.hxx>
|
|
#include "yasim-common.hpp"
|
|
#include "Math.hpp"
|
|
#include "Surface.hpp"
|
|
|
|
namespace yasim {
|
|
int Surface::s_idGenerator = 0;
|
|
|
|
Surface::Surface(Version* version, float* pos, float dragCoefficient = 1 ) :
|
|
_version(version),
|
|
_c0(dragCoefficient)
|
|
{
|
|
_id = s_idGenerator++;
|
|
|
|
_orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
|
|
_orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
|
|
_orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
|
|
|
|
Math::set3(pos, _pos);
|
|
|
|
_surfN = fgGetNode("/fdm/yasim/debug/surfaces", true);
|
|
if (_surfN != 0) {
|
|
_surfN = _surfN->getChild("surface", _id, true);
|
|
_fxN = _surfN->getNode("f-x", true);
|
|
_fyN = _surfN->getNode("f-y", true);
|
|
_fzN = _surfN->getNode("f-z", true);
|
|
_fabsN = _surfN->getNode("f-abs", true);
|
|
_alphaN = _surfN->getNode("alpha", true);
|
|
_stallAlphaN = _surfN->getNode("stall-alpha", true);
|
|
_flapN = _surfN->getNode("flap-pos", true);
|
|
_slatN = _surfN->getNode("slat-pos", true);
|
|
_spoilerN = _surfN->getNode("spoiler-pos", true);
|
|
_surfN->getNode("pos-x", true)->setFloatValue(pos[0]);
|
|
_surfN->getNode("pos-y", true)->setFloatValue(pos[1]);
|
|
_surfN->getNode("pos-z", true)->setFloatValue(pos[2]);
|
|
_surfN->getNode("chord",true)->setFloatValue(0);
|
|
_surfN->getNode("axis-x", true)->setFloatValue(0);
|
|
_surfN->getNode("axis-y", true)->setFloatValue(0);
|
|
_surfN->getNode("axis-z", true)->setFloatValue(0);
|
|
}
|
|
}
|
|
|
|
|
|
void Surface::setPosition(const float* pos)
|
|
{
|
|
Math::set3(pos, _pos);
|
|
if (_surfN != 0) {
|
|
_surfN->getNode("pos-x", true)->setFloatValue(pos[0]);
|
|
_surfN->getNode("pos-y", true)->setFloatValue(pos[1]);
|
|
_surfN->getNode("pos-z", true)->setFloatValue(pos[2]);
|
|
}
|
|
}
|
|
|
|
void Surface::setChord(float chord)
|
|
{
|
|
_chord = chord;
|
|
if (_surfN != 0) {
|
|
_surfN->getNode("chord",true)->setFloatValue(_chord);
|
|
}
|
|
}
|
|
|
|
void Surface::setOrientation(const float* o)
|
|
{
|
|
for(int i=0; i<9; i++) _orient[i] = o[i];
|
|
if (_surfN) {
|
|
// export the chord line (transformed into aircraft coordiantes)
|
|
float xaxis[3] {1,0,0};
|
|
Math::tmul33(_orient,xaxis, xaxis);
|
|
_surfN->getNode("axis-x", true)->setFloatValue(xaxis[0]);
|
|
_surfN->getNode("axis-y", true)->setFloatValue(xaxis[1]);
|
|
_surfN->getNode("axis-z", true)->setFloatValue(xaxis[2]);
|
|
}
|
|
}
|
|
|
|
|
|
void Surface::setSlatParams(float stallDelta, float dragPenalty)
|
|
{
|
|
_slatAlpha = stallDelta;
|
|
_slatDrag = dragPenalty;
|
|
}
|
|
|
|
void Surface::setFlapParams(float liftAdd, float dragPenalty)
|
|
{
|
|
_flapLift = liftAdd;
|
|
_flapDrag = dragPenalty;
|
|
}
|
|
|
|
void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
|
|
{
|
|
_spoilerLift = liftPenalty;
|
|
_spoilerDrag = dragPenalty;
|
|
}
|
|
|
|
void Surface::setFlapPos(float pos)
|
|
{
|
|
if (_flapPos != pos) {
|
|
_flapPos = pos;
|
|
if (_surfN != 0) {
|
|
_flapN->setFloatValue(pos);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Surface::setSlatPos(float pos)
|
|
{
|
|
if (_slatPos != pos) {
|
|
_slatPos = pos;
|
|
if (_surfN != 0) {
|
|
_slatN->setFloatValue(pos);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Surface::setSpoilerPos(float pos)
|
|
{
|
|
if (_spoilerPos != pos) {
|
|
_spoilerPos = pos;
|
|
if (_surfN != 0) _spoilerN->setFloatValue(pos);
|
|
}
|
|
}
|
|
|
|
// Calculate the aerodynamic force given a wind vector v (in the
|
|
// aircraft's "local" coordinates) and an air density rho. Returns a
|
|
// torque about the Y axis ("pitch"), too.
|
|
void Surface::calcForce(const float* v, const float rho, float* out, float* torque)
|
|
{
|
|
// initialize outputs to zero
|
|
Math::zero3(out);
|
|
Math::zero3(torque);
|
|
|
|
// Split v into magnitude and direction:
|
|
float vel = Math::mag3(v);
|
|
|
|
// Zero velocity means zero force by definition (also prevents div0).
|
|
if(vel == 0) {
|
|
return;
|
|
}
|
|
|
|
// special case this so the logic below doesn't produce a non-zero
|
|
// force; should probably have a "no force" flag instead...
|
|
if(_cx == 0. && _cy == 0. && _cz == 0.) {
|
|
return;
|
|
}
|
|
|
|
// Normalize wind and convert to the surface's coordinates
|
|
Math::mul3(1/vel, v, out);
|
|
Math::vmul33(_orient, out, out);
|
|
|
|
// "Rotate" by the incidence angle. Assume small angles, so we
|
|
// need to diddle only the Z component, X is relatively unchanged
|
|
// by small rotations. sin(a) ~ a, cos(a) ~ 1 for small a
|
|
float incidence = _incidence + _twist;
|
|
out[2] += incidence * out[0]; // z' = z + incidence * x
|
|
|
|
// Hold onto the local wind vector so we can multiply the induced
|
|
// drag at the end.
|
|
float lwind[3];
|
|
Math::set3(out, lwind);
|
|
|
|
// Diddle the Z force according to our configuration
|
|
float stallMul = stallFunc(out);
|
|
stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
|
|
float stallLift = (stallMul - 1) * _cz * out[2];
|
|
float flaplift = flapLift(out[2]);
|
|
|
|
out[2] *= _cz; // scaling factor
|
|
out[2] += _cz*_cz0; // zero-alpha lift
|
|
out[2] += stallLift;
|
|
out[2] += flaplift;
|
|
|
|
// Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
|
|
// torque) around the Y axis, while flap lift pushes down. Both
|
|
// forces are considered to act at one third chord from the
|
|
// edge. Convert to local (i.e. airplane) coordiantes and store
|
|
// into "torque".
|
|
torque[0] = 0;
|
|
torque[1] = 0.1667f * _chord * (flaplift - (_cz*_cz0 + stallLift));
|
|
torque[2] = 0;
|
|
Math::tmul33(_orient, torque, torque);
|
|
|
|
// The X (drag) force gets diddled for control deflection
|
|
out[0] = controlDrag(out[2], _cx * out[0]);
|
|
|
|
// Add in any specific Y (side force) coefficient.
|
|
out[1] *= _cy;
|
|
|
|
// Diddle the induced drag
|
|
Math::mul3(-1*_inducedDrag*out[2]*lwind[2], lwind, lwind);
|
|
Math::add3(lwind, out, out);
|
|
|
|
// Reverse the incidence rotation to get back to surface
|
|
// coordinates. Since out[] is now the force vector and is
|
|
// roughly parallel with Z, the small-angle approximation
|
|
// must change its X component.
|
|
if( _version->isVersionOrNewer( Version::YASIM_VERSION_32 )) {
|
|
out[0] += incidence * out[2];
|
|
} else {
|
|
out[2] -= incidence * out[0];
|
|
}
|
|
|
|
// Convert back to external coordinates
|
|
Math::tmul33(_orient, out, out);
|
|
|
|
// Add in the units to make a real force:
|
|
float scale = 0.5f*rho*vel*vel*_c0;
|
|
Math::mul3(scale, out, out);
|
|
Math::mul3(scale, torque, torque);
|
|
// if we have a property tree, export info
|
|
if (_surfN != 0) {
|
|
_fabsN->setFloatValue(Math::mag3(out));
|
|
_fxN->setFloatValue(out[0]);
|
|
_fyN->setFloatValue(out[1]);
|
|
_fzN->setFloatValue(out[2]);
|
|
_alphaN->setFloatValue(_alpha);
|
|
_stallAlphaN->setFloatValue(_stallAlpha);
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
void Surface::test()
|
|
{
|
|
static const float DEG2RAD = 0.0174532925199;
|
|
float v[3], force[3], torque[3];
|
|
float rho = Atmosphere::getStdDensity(0);
|
|
float spd = 30;
|
|
|
|
setFlapPos(0);
|
|
setSlatPos(0);
|
|
setSpoilerPos(0);
|
|
|
|
for(float angle = -90; angle<90; angle += 0.01) {
|
|
float rad = angle * DEG2RAD;
|
|
v[0] = spd * -Math::cos(rad);
|
|
v[1] = 0;
|
|
v[2] = spd * Math::sin(rad);
|
|
calcForce(v, rho, force, torque);
|
|
float lift = force[2] * Math::cos(rad) + force[0] * Math::sin(rad);
|
|
//__builtin_printf("%f %f\n", angle, lift);
|
|
__builtin_printf("%f %f\n", angle, torque[2]);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
// Returns a multiplier for the "plain" force equations that
|
|
// approximates an airfoil's lift/stall curve.
|
|
float Surface::stallFunc(float* v)
|
|
{
|
|
// Sanity check to treat FPU psychopathology
|
|
if(v[0] == 0) return 1;
|
|
|
|
_alpha = Math::abs(v[2]/v[0]);
|
|
|
|
// Wacky use of indexing, see setStall*() methods.
|
|
int fwdBak = v[0] > 0; // set if this is "backward motion"
|
|
int posNeg = v[2] < 0; // set if the airflow is toward -z
|
|
int i = (fwdBak<<1) | posNeg;
|
|
|
|
_stallAlpha = _stalls[i];
|
|
if(_stallAlpha == 0)
|
|
return 1;
|
|
|
|
// consider slat position, moves the stall aoa some degrees
|
|
if(i == 0) {
|
|
if( _version->isVersionOrNewer( Version::YASIM_VERSION_32 )) {
|
|
_stallAlpha += _slatPos * _slatAlpha;
|
|
} else {
|
|
_stallAlpha += _slatAlpha;
|
|
}
|
|
}
|
|
|
|
// Beyond the stall
|
|
if(_alpha > _stallAlpha+_widths[i])
|
|
return 1;
|
|
|
|
// (note mask: we want to use the "positive" stall angle here)
|
|
float scale = 0.5f*_peaks[fwdBak]/_stalls[i&2];
|
|
|
|
// Before the stall
|
|
if(_alpha <= _stallAlpha)
|
|
return scale;
|
|
|
|
// Inside the stall. Compute a cubic interpolation between the
|
|
// pre-stall "scale" value and the post-stall unity.
|
|
float frac = (_alpha - _stallAlpha) / _widths[i];
|
|
frac = frac*frac*(3-2*frac);
|
|
|
|
return scale*(1-frac) + frac;
|
|
}
|
|
|
|
// Similar to the above -- interpolates out the flap lift past the
|
|
// stall alpha
|
|
float Surface::flapLift(float alpha)
|
|
{
|
|
float flapLift = _cz * _flapPos * (_flapLift-1) * _flapEffectiveness;
|
|
|
|
if(_stalls[0] == 0)
|
|
return 0;
|
|
|
|
if(alpha < 0) alpha = -alpha;
|
|
if(alpha < _stalls[0])
|
|
return flapLift;
|
|
else if(alpha > _stalls[0] + _widths[0])
|
|
return 0;
|
|
|
|
float frac = (alpha - _stalls[0]) / _widths[0];
|
|
frac = frac*frac*(3-2*frac);
|
|
return flapLift * (1-frac);
|
|
}
|
|
|
|
float Surface::controlDrag(float lift, float drag)
|
|
{
|
|
// Negative flap deflections don't affect drag until their lift
|
|
// multiplier exceeds the "camber" (cz0) of the surface. Use a
|
|
// synthesized "fp" number instead of the actual flap position.
|
|
float fp = _flapPos;
|
|
if(fp < 0) {
|
|
fp = -fp;
|
|
fp -= _cz0/(_flapLift-1);
|
|
if(fp < 0) fp = 0;
|
|
}
|
|
|
|
// Calculate an "effective" drag -- this is the drag that would
|
|
// have been produced by an unflapped surface at the same lift.
|
|
float flapDragAoA = (_flapLift - 1 - _cz0) * _stalls[0];
|
|
float fd = Math::abs(lift * flapDragAoA * fp);
|
|
if(drag < 0) fd = -fd;
|
|
drag += fd;
|
|
|
|
// Now multiply by the various control factors
|
|
drag *= 1 + fp * (_flapDrag - 1);
|
|
drag *= 1 + _spoilerPos * (_spoilerDrag - 1);
|
|
drag *= 1 + _slatPos * (_slatDrag - 1);
|
|
|
|
return drag;
|
|
}
|
|
|
|
}; // namespace yasim
|