fly/flightgear
1
0
Fork 0
Code Activity

Merge /u/jsb1685/flightgear/ branch yasim2 into next

Browse source 
https://sourceforge.net/p/flightgear/flightgear/merge-requests/110/
This commit is contained in:
James Turner 2017-12-19 23:06:03 +00:00
commit edc1d63b26
23 changed files with 2039 additions and 1554 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

192
src/FDM/YASim/Airplane.cpp
View file

@ -60,9 +60,9 @@ Airplane::~Airplane()
for(i=0; i<_solveWeights.size(); i++)
delete (SolveWeight*)_solveWeights.get(i);
for(i=0; i<_cruiseConfig.controls.size(); i++)
delete (Control*)_cruiseConfig.controls.get(i);
delete (ControlSetting*)_cruiseConfig.controls.get(i);
for(i=0; i<_approachConfig.controls.size(); i++) {
Control* c = (Control*)_approachConfig.controls.get(i);
ControlSetting* c = (ControlSetting*)_approachConfig.controls.get(i);
if(c != &_approachElevator)
delete c;
}
@ -111,15 +111,31 @@ void Airplane::getPilotAccel(float* out)
// FIXME: rotational & centripetal acceleration needed
}
Wing* Airplane::getWing()
{
if (_wing == nullptr) {
_wing = new Wing((Version*)this, true);
}
return _wing;
}
Wing* Airplane::getTail()
{
if (_tail == nullptr) {
_tail = new Wing((Version*)this, true);
}
return _tail;
}
void Airplane::updateGearState()
{
for(int i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
float ext = gr->gear->getExtension();
gr->surf->setXDrag(ext);
gr->surf->setDragCoefficient(ext);
gr->surf->setYDrag(ext);
gr->surf->setZDrag(ext);
gr->surf->setLiftCoefficient(ext);
}
}
@ -143,33 +159,32 @@ void Airplane::setCruise(float speed, float altitude, float fuel, float gla)
_cruiseConfig.glideAngle = gla;
}
void Airplane::setElevatorControl(int control)
void Airplane::setElevatorControl(const char* prop)
{
_approachElevator.control = control;
_approachElevator.propHandle = getControlMap()->getInputPropertyHandle(prop);
_approachElevator.val = 0;
_approachConfig.controls.add(&_approachElevator);
}
void Airplane::addApproachControl(int control, float val)
void Airplane::addControlSetting(Configuration cfg, const char* prop, float val)
{
Control* c = new Control();
c->control = control;
ControlSetting* c = new ControlSetting();
c->propHandle = getControlMap()->getInputPropertyHandle(prop);
c->val = val;
switch (cfg) {
case APPROACH:
_approachConfig.controls.add(c);
}
void Airplane::addCruiseControl(int control, float val)
{
Control* c = new Control();
c->control = control;
c->val = val;
break;
case CRUISE:
_cruiseConfig.controls.add(c);
break;
}
}
void Airplane::addSolutionWeight(bool approach, int idx, float wgt)
void Airplane::addSolutionWeight(Configuration cfg, int idx, float wgt)
{
SolveWeight* w = new SolveWeight();
w->approach = approach;
w->approach = (cfg == APPROACH);
w->idx = idx;
w->wgt = wgt;
_solveWeights.add(w);
@ -238,9 +253,7 @@ int Airplane::addWeight(float* pos, float size)
WeightRec* wr = new WeightRec();
wr->handle = _model.getBody()->addMass(0, pos);
wr->surf = new Surface(this);
wr->surf->setPosition(pos);
wr->surf->setTotalDrag(size*size);
wr->surf = new Surface(this, pos, size*size);
_model.addSurface(wr->surf);
_surfs.add(wr->surf);
@ -257,13 +270,13 @@ void Airplane::setWeight(int handle, float mass)
// Kill the aerodynamic drag if the mass is exactly zero. This is
// how we simulate droppable stores.
if(mass == 0) {
wr->surf->setXDrag(0);
wr->surf->setDragCoefficient(0);
wr->surf->setYDrag(0);
wr->surf->setZDrag(0);
wr->surf->setLiftCoefficient(0);
} else {
wr->surf->setXDrag(1);
wr->surf->setDragCoefficient(1);
wr->surf->setYDrag(1);
wr->surf->setZDrag(1);
wr->surf->setLiftCoefficient(1);
}
}
@ -308,49 +321,9 @@ float Airplane::compileWing(Wing* w)
addContactPoint(tip);
tip[1] *= -1; //undo mirror
}
if (_wingsN != 0) {
_wingsN->getNode("tip-x", true)->setFloatValue(tip[0]);
_wingsN->getNode("tip-y", true)->setFloatValue(tip[1]);
_wingsN->getNode("tip-z", true)->setFloatValue(tip[2]);
w->getBase(tip);
_wingsN->getNode("base-x", true)->setFloatValue(tip[0]);
_wingsN->getNode("base-y", true)->setFloatValue(tip[1]);
_wingsN->getNode("base-z", true)->setFloatValue(tip[2]);
_wingsN->getNode("wing-span", true)->setFloatValue(w->getSpan());
_wingsN->getNode("wing-area", true)->setFloatValue(w->getArea());
_wingsN->getNode("aspect-ratio", true)->setFloatValue(w->getAspectRatio());
_wingsN->getNode("standard-mean-chord", true)->setFloatValue(w->getSMC());
_wingsN->getNode("mac", true)->setFloatValue(w->getMAC());
_wingsN->getNode("mac-x", true)->setFloatValue(w->getMACx());
_wingsN->getNode("mac-y", true)->setFloatValue(w->getMACy());
}
float wgt = 0;
float dragSum = 0;
for(int i=0; i<w->numSurfaces(); i++) {
Surface* s = (Surface*)w->getSurface(i);
float td = s->getTotalDrag();
int sid = s->getID();
_model.addSurface(s);
float mass = w->getSurfaceWeight(i);
mass = mass * Math::sqrt(mass);
float pos[3];
s->getPosition(pos);
int mid = _model.getBody()->addMass(mass, pos, true);
if (_wingsN != 0) {
SGPropertyNode_ptr n = _wingsN->getNode("surfaces", true)->getChild("surface", sid, true);
n->getNode("drag", true)->setFloatValue(td);
n->getNode("mass-id", true)->setIntValue(mid);
}
wgt += mass;
dragSum += td;
}
if (_wingsN != 0) {
_wingsN->getNode("weight", true)->setFloatValue(wgt);
_wingsN->getNode("drag", true)->setFloatValue(dragSum);
}
wgt = w->updateModel(&_model);
return wgt;
}
@ -379,7 +352,6 @@ float Airplane::compileFuselage(Fuselage* f)
int j;
for(j=0; j<segs; j++) {
float frac = (j+0.5f) / segs;
float scale = 1;
if(frac < f->mid)
scale = f->taper+(1-f->taper) * (frac / f->mid);
@ -403,9 +375,6 @@ float Airplane::compileFuselage(Fuselage* f)
_model.getBody()->addMass(mass, pos, true);
wgt += mass;
// Make a Surface too
Surface* s = new Surface(this);
s->setPosition(pos);
// The following is the original YASim value for sideDrag.
// Originally YASim calculated the fuselage's lateral drag
@ -430,17 +399,18 @@ float Airplane::compileFuselage(Fuselage* f)
// XML parameters: "cy" for the sides, "cz" for top and bottom.
sideDrag = 0.5;
}
float dragCoefficient = scale*segWgt*f->_cx;
if( isVersionOrNewer( YASIM_VERSION_32 ) ) {
s->setXDrag(f->_cx);
dragCoefficient = scale*segWgt;
}
// Make a Surface too
Surface* s = new Surface(this, pos, dragCoefficient);
if( isVersionOrNewer( YASIM_VERSION_32 ) ) {
s->setDragCoefficient(f->_cx);
}
s->setYDrag(sideDrag*f->_cy);
s->setZDrag(sideDrag*f->_cz);
if( isVersionOrNewer( YASIM_VERSION_32 ) ) {
s->setTotalDrag(scale*segWgt);
} else {
s->setTotalDrag(scale*segWgt*f->_cx);
}
s->setLiftCoefficient(sideDrag*f->_cz);
s->setInducedDrag(f->_idrag);
// FIXME: fails for fuselages aligned along the Y axis
@ -465,9 +435,6 @@ void Airplane::compileGear(GearRec* gr)
{
Gear* g = gr->gear;
// Make a Surface object for the aerodynamic behavior
Surface* s = new Surface(this);
gr->surf = s;
// Put the surface at the half-way point on the gear strut, give
// it a drag coefficient equal to a square of the same dimension
@ -480,8 +447,9 @@ void Airplane::compileGear(GearRec* gr)
Math::mul3(0.5, cmp, cmp);
Math::add3(pos, cmp, pos);
s->setPosition(pos);
s->setTotalDrag(length*length);
// Make a Surface object for the aerodynamic behavior
Surface* s = new Surface(this, pos, length*length);
gr->surf = s;
_model.addGear(g);
_model.addSurface(s);
@ -543,12 +511,15 @@ void Airplane::compile()
// The Wing objects
if (_wing)
{
if (baseN != 0) _wingsN = baseN->getChild("wing", 0, true);
if (baseN != 0) {
_wingsN = baseN->getChild("wing", 0, true);
_wing->setPropertyNode(_wingsN);
}
aeroWgt += compileWing(_wing);
// convert % to absolute x coordinates
_cgDesiredFront = _wing->getMACx() - _wing->getMAC()*_cgDesiredMin;
_cgDesiredAft = _wing->getMACx() - _wing->getMAC()*_cgDesiredMax;
_cgDesiredFront = _wing->getMACx() - _wing->getMACLength()*_cgDesiredMin;
_cgDesiredAft = _wing->getMACx() - _wing->getMACLength()*_cgDesiredMax;
if (baseN != 0) {
SGPropertyNode_ptr n = fgGetNode("/fdm/yasim/model", true);
n->getNode("cg-x-range-front", true)->setFloatValue(_cgDesiredFront);
@ -557,14 +528,21 @@ void Airplane::compile()
}
if (_tail)
{
if (baseN != 0) _wingsN = baseN->getChild("tail", 0, true);
if (baseN != 0) {
_wingsN = baseN->getChild("tail", 0, true);
_tail->setPropertyNode(_wingsN);
}
aeroWgt += compileWing(_tail);
}
int i;
for(i=0; i<_vstabs.size(); i++)
{
if (baseN != 0) _wingsN = baseN->getChild("stab", i, true);
aeroWgt += compileWing((Wing*)_vstabs.get(i));
Wing* vs = (Wing*)_vstabs.get(i);
if (baseN != 0) {
_wingsN = baseN->getChild("stab", i, true);
vs->setPropertyNode(_wingsN);
}
aeroWgt += compileWing(vs);
}
// The fuselage(s)
@ -578,9 +556,13 @@ void Airplane::compile()
// Rescale to the specified empty weight
float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
for(i=firstMass; i<body->numMasses(); i++)
for(i=firstMass; i<body->numMasses(); i++) {
body->setMass(i, body->getMass(i)*wscale);
}
if (_wingsN != nullptr) {
float w = _wingsN->getNode("weight", true)->getFloatValue();
_wingsN->getNode("mass", true)->setFloatValue(w * wscale);
}
// Add the thruster masses
for(i=0; i<_thrusters.size(); i++) {
ThrustRec* t = (ThrustRec*)_thrusters.get(i);
@ -745,13 +727,13 @@ void Airplane::setupWeights(bool isApproach)
}
}
/// load values for controls as defined in cruise configuration
/// load values for controls as defined in cruise/approach configuration
void Airplane::loadControls(const Vector& controls)
{
_controls.reset();
for(int i=0; i < controls.size(); i++) {
Control* c = (Control*)controls.get(i);
_controls.setInput(c->control, c->val);
ControlSetting* c = (ControlSetting*)controls.get(i);
_controls.setInput(c->propHandle, c->val);
}
_controls.applyControls();
}
@ -800,13 +782,13 @@ void Airplane::applyDragFactor(float factor)
float applied = Math::pow(factor, SOLVE_TWEAK);
_dragFactor *= applied;
if(_wing)
_wing->setDragScale(_wing->getDragScale() * applied);
_wing->multiplyDragCoefficient(applied);
if(_tail)
_tail->setDragScale(_tail->getDragScale() * applied);
_tail->multiplyDragCoefficient(applied);
int i;
for(i=0; i<_vstabs.size(); i++) {
Wing* w = (Wing*)_vstabs.get(i);
w->setDragScale(w->getDragScale() * applied);
w->multiplyDragCoefficient(applied);
}
for(i=0; i<_fuselages.size(); i++) {
Fuselage* f = (Fuselage*)_fuselages.get(i);
@ -822,21 +804,21 @@ void Airplane::applyDragFactor(float factor)
// it won't be affected by the streamlining done to reduce
// longitudinal drag. So the solver should only adjust the
// fuselage's longitudinal (X axis) drag coefficient.
s->setXDrag(s->getXDrag() * applied);
s->setDragCoefficient(s->getDragCoefficient() * applied);
} else {
// Originally YASim applied the drag factor to all axes
// for Fuselage Surfaces.
s->setTotalDrag(s->getTotalDrag() * applied);
s->mulTotalForceCoefficient(applied);
}
}
}
for(i=0; i<_weights.size(); i++) {
WeightRec* wr = (WeightRec*)_weights.get(i);
wr->surf->setTotalDrag(wr->surf->getTotalDrag() * applied);
wr->surf->mulTotalForceCoefficient(applied);
}
for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
gr->surf->setTotalDrag(gr->surf->getTotalDrag() * applied);
gr->surf->mulTotalForceCoefficient(applied);
}
}
@ -846,13 +828,13 @@ void Airplane::applyLiftRatio(float factor)
float applied = Math::pow(factor, SOLVE_TWEAK);
_liftRatio *= applied;
if(_wing)
_wing->setLiftRatio(_wing->getLiftRatio() * applied);
_wing->multiplyLiftRatio(applied);
if(_tail)
_tail->setLiftRatio(_tail->getLiftRatio() * applied);
_tail->multiplyLiftRatio(applied);
int i;
for(i=0; i<_vstabs.size(); i++) {
Wing* w = (Wing*)_vstabs.get(i);
w->setLiftRatio(w->getLiftRatio() * applied);
w->multiplyLiftRatio(applied);
}
}
@ -1034,7 +1016,7 @@ float Airplane::getCGMAC()
if (_wing) {
float cg[3];
_model.getBody()->getCG(cg);
return (_wing->getMACx() - cg[0]) / _wing->getMAC();
return (_wing->getMACx() - cg[0]) / _wing->getMACLength();
}
return 0;
}

24
src/FDM/YASim/Airplane.hpp
View file

@ -24,6 +24,11 @@ public:
Airplane();
~Airplane();
enum Configuration {
APPROACH,
CRUISE,
};
void iterate(float dt);
void calcFuelWeights();
@ -37,9 +42,9 @@ public:
void setEmptyWeight(float weight) { _emptyWeight = weight; }
void setWing(Wing* wing) { _wing = wing; }
Wing* getWing() { return _wing; }
void setTail(Wing* tail) { _tail = tail; }
Wing* getWing();
bool hasWing() const { return (_wing != nullptr); }
Wing* getTail();
void addVStab(Wing* vstab) { _vstabs.add(vstab); }
void addFuselage(float* front, float* back, float width,
@ -59,11 +64,10 @@ public:
void setApproach(float speed, float altitude, float aoa, float fuel, float gla);
void setCruise(float speed, float altitude, float fuel, float gla);
void setElevatorControl(int control);
void addApproachControl(int control, float val);
void addCruiseControl(int control, float val);
void setElevatorControl(const char* prop);
void addControlSetting(Configuration cfg, const char* prop, float val);
void addSolutionWeight(bool approach, int idx, float wgt);
void addSolutionWeight(Configuration cfg, int idx, float wgt);
int numGear() const { return _gears.size(); }
Gear* getGear(int g) { return ((GearRec*)_gears.get(g))->gear; }
@ -134,8 +138,8 @@ private:
float cg[3];
float mass;
};
struct Control {
int control;
struct ControlSetting {
int propHandle;
float val;
};
struct WeightRec {
@ -210,7 +214,7 @@ private:
float _dragFactor {1};
float _liftRatio {1};
float _tailIncidence {0};
Control _approachElevator;
ControlSetting _approachElevator;
const char* _failureMsg {0};
float _cgMax {-1e6}; // hard limits for cg from gear position

2
src/FDM/YASim/Atmosphere.cpp
View file

@ -211,7 +211,7 @@ bool Atmosphere::test() {
fprintf(stderr, "Columns = %d\n", numColumns);
fprintf(stderr, "Rows = %d\n", rows);
for (int alt = 0; alt < maxTableIndex(); alt++) {
for (int alt = 0; alt <= maxTableIndex(); alt++) {
float density = calcStdDensity(data[alt][PRESSURE], data[alt][TEMPERATURE]);
float delta = data[alt][DENSITY] - density;
fprintf(stderr, "%d : %f \n", alt, delta);

1
src/FDM/YASim/CMakeLists.txt
View file

@ -26,6 +26,7 @@ set(COMMON
Turbulence.cpp
Wing.cpp
Version.cpp
yasim-common.cpp
)
set(SOURCES

327
src/FDM/YASim/ControlMap.cpp
View file

@ -2,6 +2,8 @@
# include "config.h"
#endif
#include <cstring>
#include "Jet.hpp"
#include "Thruster.hpp"
#include "PropEngine.hpp"
@ -41,18 +43,14 @@ ControlMap::~ControlMap()
}
/**
input : index to _inputs
type: identifier (see enum OutputType)
prop: name of input property
control: identifier (see enum OutputType)
object: object to which this input belongs to
options: bits OPT_INVERT, OPT_SPLIT, OPT_SQUARE
*/
void ControlMap::addMapping(int input, int type, void* object, int options,
float src0, float src1, float dst0, float dst1)
void ControlMap::addMapping(const char* prop, Control control, ObjectID id, int options, float src0, float src1, float dst0, float dst1)
{
addMapping(input, type, object, options);
// The one we just added is last in the list (ugly, awful hack!)
Vector* maps = (Vector*)_inputs.get(input);
MapRec* m = (MapRec*)maps->get(maps->size() - 1);
MapRec* m = (MapRec*)addMapping(prop, control, id, options);
m->src0 = src0;
m->src1 = src1;
m->dst0 = dst0;
@ -60,28 +58,32 @@ void ControlMap::addMapping(int input, int type, void* object, int options,
}
/**
input : index to _inputs
type: identifier (see enum OutputType)
prop: name of input property
control: identifier (see enum OutputType)
object: object to which this input belongs to
options: bits OPT_INVERT, OPT_SPLIT, OPT_SQUARE
*/
void ControlMap::addMapping(int input, int type, void* object, int options)
void* ControlMap::addMapping(const char* prop, Control control, ObjectID id, int options)
{
int inputPropHandle = getInputPropertyHandle(prop);
// See if the output object already exists
OutRec* out = 0;
OutRec* out {nullptr};
int i;
for(i = 0; i < _outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
if(o->object == object && o->type == type) {
if(o->oid.object == id.object && o->oid.subObj == id.subObj
&& o->control == control)
{
out = o;
break;
}
}
// Create one if it doesn't
if(out == 0) {
if(out == nullptr) {
out = new OutRec();
out->type = type;
out->object = object;
out->oldL = out->oldR = out->time = 0;
out->control = control;
out->oid = id;
_outputs.add(out);
}
@ -92,12 +94,13 @@ void ControlMap::addMapping(int input, int type, void* object, int options)
map->idx = out->maps.add(map);
// The default ranges differ depending on type!
map->src1 = map->dst1 = rangeMax(type);
map->src0 = map->dst0 = rangeMin(type);
map->src1 = map->dst1 = rangeMax(control);
map->src0 = map->dst0 = rangeMin(control);
// And add it to the approproate vectors.
Vector* maps = (Vector*)_inputs.get(input);
Vector* maps = (Vector*)_inputs.get(inputPropHandle);
maps->add(map);
return map;
}
void ControlMap::reset()
@ -105,38 +108,36 @@ void ControlMap::reset()
// Set all the values to zero
for(int i = 0; i < _outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
for(int j=0; j<o->maps.size(); j++)
for(int j = 0; j < o->maps.size(); j++) {
((MapRec*)(o->maps.get(j)))->val = 0;
}
}
}
void ControlMap::setInput(int input, float val)
{
Vector* maps = (Vector*)_inputs.get(input);
for(int i = 0; i < maps->size(); i++) {
MapRec* m = (MapRec*)maps->get(i);
float val2 = val;
// Do the scaling operation. Clamp to [src0:src1], rescale to
// [0:1] within that range, then map to [dst0:dst1].
if(val2 < m->src0) val2 = m->src0;
if(val2 > m->src1) val2 = m->src1;
val2 = Math::clamp(val2, m->src0, m->src1);
val2 = (val2 - m->src0) / (m->src1 - m->src0);
val2 = m->dst0 + val2 * (m->dst1 - m->dst0);
m->val = val2;
m->val = m->dst0 + val2 * (m->dst1 - m->dst0);
}
}
int ControlMap::getOutputHandle(void* obj, int type)
int ControlMap::getOutputHandle(ObjectID id, Control control)
{
for(int i = 0; i < _outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
if(o->object == obj && o->type == type)
if(o->oid.object == id.object && o->oid.subObj == id.subObj
&& o->control == control)
return i;
}
return 0;
fprintf(stderr, "ControlMap::getOutputHandle cannot find *%ld, control %d \nMissing <control-input ...> in XML?!", (long)id.object, control);
return -1;
}
void ControlMap::setTransitionTime(int handle, float time)
@ -157,7 +158,8 @@ float ControlMap::getOutputR(int handle)
void ControlMap::applyControls(float dt)
{
int outrec;
for(outrec=0; outrec<_outputs.size(); outrec++) {
for(outrec=0; outrec<_outputs.size(); outrec++)
{
OutRec* o = (OutRec*)_outputs.get(outrec);
// Generate a summed value. Note the check for "split"
@ -187,7 +189,7 @@ void ControlMap::applyControls(float dt)
float adl = Math::abs(dl);
float adr = Math::abs(dr);
float max = (dt/o->time) * (rangeMax(o->type) - rangeMin(o->type));
float max = (dt/o->time) * (rangeMax(o->control) - rangeMin(o->control));
if(adl > max) dl = dl*max/adl;
if(adr > max) dr = dr*max/adr;
@ -198,66 +200,143 @@ void ControlMap::applyControls(float dt)
o->oldL = lval;
o->oldR = rval;
void* obj = o->object;
switch(o->type) {
case THROTTLE: ((Thruster*)obj)->setThrottle(lval); break;
case MIXTURE: ((Thruster*)obj)->setMixture(lval); break;
case CONDLEVER: ((TurbineEngine*)((PropEngine*)
obj)->getEngine())->setCondLever(lval); break;
case STARTER: ((Thruster*)obj)->setStarter(lval != 0.0); break;
case MAGNETOS: ((PropEngine*)obj)->setMagnetos((int)lval); break;
case ADVANCE: ((PropEngine*)obj)->setAdvance(lval); break;
case PROPPITCH: ((PropEngine*)obj)->setPropPitch(lval); break;
case PROPFEATHER: ((PropEngine*)obj)->setPropFeather((int)lval); break;
case REHEAT: ((Jet*)obj)->setReheat(lval); break;
case VECTOR: ((Jet*)obj)->setRotation(lval); break;
case BRAKE: ((Gear*)obj)->setBrake(lval); break;
case STEER: ((Gear*)obj)->setRotation(lval); break;
case EXTEND: ((Gear*)obj)->setExtension(lval); break;
case HEXTEND: ((Hook*)obj)->setExtension(lval); break;
case LEXTEND: ((Launchbar*)obj)->setExtension(lval); break;
case LACCEL: ((Launchbar*)obj)->setAcceleration(lval); break;
case CASTERING:((Gear*)obj)->setCastering(lval != 0); break;
case SLAT: ((Wing*)obj)->setSlatPos(lval); break;
case FLAP0: ((Wing*)obj)->setFlap0Pos(lval, rval); break;
case FLAP0EFFECTIVENESS: ((Wing*)obj)->setFlap0Effectiveness(lval); break;
case FLAP1: ((Wing*)obj)->setFlap1Pos(lval, rval); break;
case FLAP1EFFECTIVENESS: ((Wing*)obj)->setFlap1Effectiveness(lval); break;
case SPOILER: ((Wing*)obj)->setSpoilerPos(lval, rval); break;
case COLLECTIVE: ((Rotor*)obj)->setCollective(lval); break;
case CYCLICAIL: ((Rotor*)obj)->setCyclicail(lval,rval); break;
case CYCLICELE: ((Rotor*)obj)->setCyclicele(lval,rval); break;
case TILTPITCH: ((Rotor*)obj)->setTiltPitch(lval); break;
case TILTYAW: ((Rotor*)obj)->setTiltYaw(lval); break;
case TILTROLL: ((Rotor*)obj)->setTiltRoll(lval); break;
void* obj = o->oid.object;
switch(o->control) {
case THROTTLE:
((Thruster*)obj)->setThrottle(lval);
break;
case MIXTURE:
((Thruster*)obj)->setMixture(lval);
break;
case CONDLEVER:
((TurbineEngine*)((PropEngine*)obj)->getEngine())->setCondLever(lval);
break;
case STARTER:
((Thruster*)obj)->setStarter(lval != 0.0);
break;
case MAGNETOS:
((PropEngine*)obj)->setMagnetos((int)lval);
break;
case ADVANCE:
((PropEngine*)obj)->setAdvance(lval);
break;
case PROPPITCH:
((PropEngine*)obj)->setPropPitch(lval);
break;
case PROPFEATHER:
((PropEngine*)obj)->setPropFeather((int)lval);
break;
case REHEAT:
((Jet*)obj)->setReheat(lval);
break;
case VECTOR:
((Jet*)obj)->setRotation(lval);
break;
case BRAKE:
((Gear*)obj)->setBrake(lval);
break;
case STEER:
((Gear*)obj)->setRotation(lval);
break;
case EXTEND:
((Gear*)obj)->setExtension(lval);
break;
case HEXTEND:
((Hook*)obj)->setExtension(lval);
break;
case LEXTEND:
((Launchbar*)obj)->setExtension(lval);
break;
case LACCEL:
((Launchbar*)obj)->setAcceleration(lval);
break;
case CASTERING:
((Gear*)obj)->setCastering(lval != 0);
break;
case SLAT:
((Wing*)obj)->setFlapPos(WING_SLAT,lval);
break;
case FLAP0:
((Wing*)obj)->setFlapPos(WING_FLAP0, lval, rval);
break;
case FLAP0EFFECTIVENESS:
((Wing*)obj)->setFlapEffectiveness(WING_FLAP0,lval);
break;
case FLAP1:
((Wing*)obj)->setFlapPos(WING_FLAP1,lval, rval);
break;
case FLAP1EFFECTIVENESS:
((Wing*)obj)->setFlapEffectiveness(WING_FLAP1,lval);
break;
case SPOILER:
((Wing*)obj)->setFlapPos(WING_SPOILER, lval, rval);
break;
case COLLECTIVE:
((Rotor*)obj)->setCollective(lval);
break;
case CYCLICAIL:
((Rotor*)obj)->setCyclicail(lval,rval);
break;
case CYCLICELE:
((Rotor*)obj)->setCyclicele(lval,rval);
break;
case TILTPITCH:
((Rotor*)obj)->setTiltPitch(lval);
break;
case TILTYAW:
((Rotor*)obj)->setTiltYaw(lval);
break;
case TILTROLL:
((Rotor*)obj)->setTiltRoll(lval);
break;
case ROTORBALANCE:
((Rotor*)obj)->setRotorBalance(lval); break;
case ROTORBRAKE: ((Rotorgear*)obj)->setRotorBrake(lval); break;
((Rotor*)obj)->setRotorBalance(lval);
break;
case ROTORBRAKE:
((Rotorgear*)obj)->setRotorBrake(lval);
break;
case ROTORENGINEON:
((Rotorgear*)obj)->setEngineOn((int)lval); break;
((Rotorgear*)obj)->setEngineOn((int)lval);
break;
case ROTORENGINEMAXRELTORQUE:
((Rotorgear*)obj)->setRotorEngineMaxRelTorque(lval); break;
((Rotorgear*)obj)->setRotorEngineMaxRelTorque(lval);
break;
case ROTORRELTARGET:
((Rotorgear*)obj)->setRotorRelTarget(lval); break;
case REVERSE_THRUST: ((Jet*)obj)->setReverse(lval != 0); break;
((Rotorgear*)obj)->setRotorRelTarget(lval);
break;
case REVERSE_THRUST:
((Jet*)obj)->setReverse(lval != 0);
break;
case BOOST:
((PistonEngine*)((Thruster*)obj)->getEngine())->setBoost(lval);
break;
case WASTEGATE:
((PistonEngine*)((Thruster*)obj)->getEngine())->setWastegate(lval);
break;
case WINCHRELSPEED: ((Hitch*)obj)->setWinchRelSpeed(lval); break;
case HITCHOPEN: ((Hitch*)obj)->setOpen(lval!=0); break;
case PLACEWINCH: ((Hitch*)obj)->setWinchPositionAuto(lval!=0); break;
case FINDAITOW: ((Hitch*)obj)->findBestAIObject(lval!=0); break;
case WINCHRELSPEED:
((Hitch*)obj)->setWinchRelSpeed(lval);
break;
case HITCHOPEN:
((Hitch*)obj)->setOpen(lval!=0);
break;
case PLACEWINCH:
((Hitch*)obj)->setWinchPositionAuto(lval!=0);
break;
case FINDAITOW:
((Hitch*)obj)->findBestAIObject(lval!=0);
break;
case PROP:
break;
case INCIDENCE:
break;
}
}
}
float ControlMap::rangeMin(int type)
float ControlMap::rangeMin(Control control)
{
// The minimum of the range for each type of control
switch(type) {
switch(control) {
case FLAP0: return -1; // [-1:1]
case FLAP1: return -1;
case STEER: return -1;
@ -272,10 +351,10 @@ float ControlMap::rangeMin(int type)
}
}
float ControlMap::rangeMax(int type)
float ControlMap::rangeMax(Control control)
{
// The maximum of the range for each type of control
switch(type) {
switch(control) {
case FLAP0: return 1; // [-1:1]
case FLAP1: return 1;
case STEER: return 1;
@ -286,38 +365,18 @@ float ControlMap::rangeMax(int type)
}
}
/// duplicate null-terminated string
char* ControlMap::dup(const char* s)
{
int len=0;
while(s[len++]);
char* s2 = new char[len+1];
char* p = s2;
while((*p++ = *s++));
s2[len] = 0;
return s2;
}
/// compare null-terminated strings
bool ControlMap::eq(const char* a, const char* b)
{
while(*a && *b && *a == *b) { a++; b++; }
// equal if both a and b points to null chars
return !(*a || *b);
}
/// register property name, return ID (int)
int ControlMap::propertyHandle(const char* name)
int ControlMap::getInputPropertyHandle(const char* name)
{
for(int i=0; i < _properties.size(); i++) {
PropHandle* p = (PropHandle*)_properties.get(i);
if(eq(p->name, name))
if(!strcmp(p->name, name))
return p->handle;
}
// create new
PropHandle* p = new PropHandle();
p->name = dup(name);
p->name = strdup(name);
fgGetNode(p->name, true);
@ -327,4 +386,64 @@ int ControlMap::propertyHandle(const char* name)
return p->handle;
}
ControlMap::Control ControlMap::parseControl(const char* name)
{
if(!strcmp(name, "THROTTLE")) return THROTTLE;
if(!strcmp(name, "MIXTURE")) return MIXTURE;
if(!strcmp(name, "CONDLEVER")) return CONDLEVER;
if(!strcmp(name, "STARTER")) return STARTER;
if(!strcmp(name, "MAGNETOS")) return MAGNETOS;
if(!strcmp(name, "ADVANCE")) return ADVANCE;
if(!strcmp(name, "REHEAT")) return REHEAT;
if(!strcmp(name, "BOOST")) return BOOST;
if(!strcmp(name, "VECTOR")) return VECTOR;
if(!strcmp(name, "PROP")) return PROP;
if(!strcmp(name, "BRAKE")) return BRAKE;
if(!strcmp(name, "STEER")) return STEER;
if(!strcmp(name, "EXTEND")) return EXTEND;
if(!strcmp(name, "HEXTEND")) return HEXTEND;
if(!strcmp(name, "LEXTEND")) return LEXTEND;
if(!strcmp(name, "LACCEL")) return LACCEL;
if(!strcmp(name, "INCIDENCE")) return INCIDENCE;
if(!strcmp(name, "FLAP0")) return FLAP0;
if(!strcmp(name, "FLAP0EFFECTIVENESS")) return FLAP0EFFECTIVENESS;
if(!strcmp(name, "FLAP1")) return FLAP1;
if(!strcmp(name, "FLAP1EFFECTIVENESS")) return FLAP1EFFECTIVENESS;
if(!strcmp(name, "SLAT")) return SLAT;
if(!strcmp(name, "SPOILER")) return SPOILER;
if(!strcmp(name, "CASTERING")) return CASTERING;
if(!strcmp(name, "PROPPITCH")) return PROPPITCH;
if(!strcmp(name, "PROPFEATHER")) return PROPFEATHER;
if(!strcmp(name, "COLLECTIVE")) return COLLECTIVE;
if(!strcmp(name, "CYCLICAIL")) return CYCLICAIL;
if(!strcmp(name, "CYCLICELE")) return CYCLICELE;
if(!strcmp(name, "TILTROLL")) return TILTROLL;
if(!strcmp(name, "TILTPITCH")) return TILTPITCH;
if(!strcmp(name, "TILTYAW")) return TILTYAW;
if(!strcmp(name, "ROTORGEARENGINEON")) return ROTORENGINEON;
if(!strcmp(name, "ROTORBRAKE")) return ROTORBRAKE;
if(!strcmp(name, "ROTORENGINEMAXRELTORQUE")) return ROTORENGINEMAXRELTORQUE;
if(!strcmp(name, "ROTORRELTARGET")) return ROTORRELTARGET;
if(!strcmp(name, "ROTORBALANCE")) return ROTORBALANCE;
if(!strcmp(name, "REVERSE_THRUST")) return REVERSE_THRUST;
if(!strcmp(name, "WASTEGATE")) return WASTEGATE;
if(!strcmp(name, "WINCHRELSPEED")) return WINCHRELSPEED;
if(!strcmp(name, "HITCHOPEN")) return HITCHOPEN;
if(!strcmp(name, "PLACEWINCH")) return PLACEWINCH;
if(!strcmp(name, "FINDAITOW")) return FINDAITOW;
SG_LOG(SG_FLIGHT,SG_ALERT,"Unrecognized control type '" << name
<< "' in YASim aircraft description.");
exit(1);
}
ControlMap::ObjectID ControlMap::getObjectID(void* object, int subObj)
{
assert(object != nullptr);
ObjectID o;
o.object = object;
o.subObj = subObj;
return o;
}
} // namespace yasim

126
src/FDM/YASim/ControlMap.hpp
View file

@ -10,41 +10,83 @@ class ControlMap {
public:
~ControlMap();
enum OutputType { THROTTLE, MIXTURE, CONDLEVER, STARTER, MAGNETOS,
ADVANCE, REHEAT, PROP,
BRAKE, STEER, EXTEND, HEXTEND, LEXTEND, LACCEL,
INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER, VECTOR,
FLAP0EFFECTIVENESS, FLAP1EFFECTIVENESS,
BOOST, CASTERING, PROPPITCH, PROPFEATHER,
COLLECTIVE, CYCLICAIL, CYCLICELE, ROTORENGINEON,
TILTYAW, TILTPITCH, TILTROLL,
ROTORBRAKE, ROTORENGINEMAXRELTORQUE, ROTORRELTARGET,
ROTORBALANCE, REVERSE_THRUST, WASTEGATE,
WINCHRELSPEED, HITCHOPEN, PLACEWINCH, FINDAITOW
enum Control {
THROTTLE,
MIXTURE,
CONDLEVER,
STARTER,
MAGNETOS,
ADVANCE,
REHEAT,
PROP,
BRAKE,
STEER,
EXTEND,
HEXTEND,
LEXTEND,
LACCEL,
INCIDENCE,
FLAP0,
FLAP1,
SLAT,
SPOILER,
VECTOR,
FLAP0EFFECTIVENESS,
FLAP1EFFECTIVENESS,
BOOST,
CASTERING,
PROPPITCH,
PROPFEATHER,
COLLECTIVE,
CYCLICAIL,
CYCLICELE,
ROTORENGINEON,
TILTYAW,
TILTPITCH,
TILTROLL,
ROTORBRAKE,
ROTORENGINEMAXRELTORQUE,
ROTORRELTARGET,
ROTORBALANCE,
REVERSE_THRUST,
WASTEGATE,
WINCHRELSPEED,
HITCHOPEN,
PLACEWINCH,
FINDAITOW
};
enum { OPT_SPLIT = 0x01,
enum {
OPT_SPLIT = 0x01,
OPT_INVERT = 0x02,
OPT_SQUARE = 0x04 };
OPT_SQUARE = 0x04
};
struct PropHandle { char* name; int handle; };
struct PropHandle {
char* name {nullptr};
int handle {0};
};
struct ObjectID {
void* object {nullptr};
int subObj {0};
};
// Adds a mapping to between input handle and a particular setting
// on an output object. The value of output MUST match the type
// of object!
void addMapping(int input, int output, void* object, int options=0);
// map control name to int (enum)
Control parseControl(const char* name);
// create ID from object and optional sub index (e.g. for wing section)
ObjectID getObjectID(void* object, int subObj = 0);
// An additional form to specify a mapping range. Input values
// outside of [src0:src1] are clamped, and are then mapped to
// [dst0:dst1] before being set on the object.
void addMapping(int input, int output, void* object, int options,
float src0, float src1, float dst0, float dst1);
// add input property for a control to an object
// same with limits. Input values are clamped to [src0:src1] and then mapped to
// [dst0:dst1] before being set on the objects control.
void addMapping(const char* prop, Control control, ObjectID id, int options, float src0, float src1, float dst0, float dst1);
// Resets our accumulated input values. Call before any
// setInput() invokations.
void reset();
// Sets the specified input (as returned by propertyHandle) to the
// Sets the specified input (as returned by getPropertyHandle()) to the
// specified value.
void setInput(int propHandle, float value);
@ -55,12 +97,12 @@ public:
// Returns the input/output range appropriate for the given
// control. Ailerons go from -1 to 1, while throttles are never
// lower than zero, etc...
static float rangeMin(int type);
static float rangeMax(int type);
static float rangeMin(Control control);
static float rangeMax(Control control);
// Each output record is identified by both an object/type tuple
// and a numeric handle.
int getOutputHandle(void* obj, int type);
int getOutputHandle(ObjectID id, Control control);
// Sets the transition time for the control output to swing
// through its full range.
@ -73,19 +115,29 @@ public:
float getOutputR(int handle);
// register property name, return handle
int propertyHandle(const char* name);
int getInputPropertyHandle(const char* name);
int numProperties() { return _properties.size(); }
PropHandle* getProperty(const int i) { return ((PropHandle*)_properties.get(i)); }
// helper
char* dup(const char* s);
bool eq(const char* a, const char* b);
private:
struct OutRec { int type; void* object; Vector maps;
float oldL, oldR, time; };
struct MapRec { OutRec* out; int idx; int opt; float val;
float src0; float src1; float dst0; float dst1; };
struct OutRec {
Control control;
ObjectID oid;
Vector maps;
float oldL {0};
float oldR {0};
float time {0};
};
struct MapRec {
OutRec* out {nullptr};
int idx {0};
int opt {0};
float val {0};
float src0 {0};
float src1 {0};
float dst0 {0};
float dst1 {0};
};
// A list of (sub)Vectors containing a bunch of MapRec objects for
// each input handle.
@ -95,6 +147,8 @@ private:
Vector _outputs;
// control properties
Vector _properties;
void* addMapping(const char* prop, Control control, ObjectID id, int options = 0);
};
}; // namespace yasim

1003
src/FDM/YASim/FGFDM.cpp
View file

File diff suppressed because it is too large Load diff

88
src/FDM/YASim/FGFDM.hpp
View file

@ -4,7 +4,6 @@
#include <simgear/xml/easyxml.hxx>
#include <simgear/props/props.hxx>
#include "yasim-common.hpp"
#include "Airplane.hpp"
#include "Vector.hpp"
@ -32,30 +31,68 @@ public:
float getVehicleRadius(void) const { return _vehicle_radius; }
private:
struct EngRec { char* prefix; Thruster* eng; };
struct WeightRec { char* prop; float size; int handle; };
struct PropOut { SGPropertyNode* prop; int handle, type; bool left;
float min, max; };
struct EngRec {
char* prefix {nullptr};
Thruster* eng {nullptr};
};
struct WeightRec {
char* prop {nullptr};
float size {0};
int handle {0};
};
struct PropOut {
SGPropertyNode* prop {nullptr};
int handle {0};
ControlMap::Control control;
bool left {false};
float min {0};
float max {0};
};
void parseAirplane(const XMLAttributes* a);
void parseApproachCruise(const XMLAttributes* a, const char* name);
void parseSolveWeight(const XMLAttributes* a);
void parseCockpit(const XMLAttributes* a);
void setOutputProperties(float dt);
Rotor* parseRotor(XMLAttributes* a, const char* name);
Wing* parseWing(XMLAttributes* a, const char* name, Version * version);
void parseRotor(const XMLAttributes* a, const char* name);
void parseRotorGear(const XMLAttributes* a);
void parseWing(const XMLAttributes* a, const char* name, Airplane* airplane);
int parseOutput(const char* name);
void parseWeight(XMLAttributes* a);
void parseTurbineEngine(XMLAttributes* a);
void parsePistonEngine(XMLAttributes* a);
void parsePropeller(XMLAttributes* a);
bool eq(const char* a, const char* b);
bool caseeq(const char* a, const char* b);
char* dup(const char* s);
int attri(XMLAttributes* atts, const char* attr);
int attri(XMLAttributes* atts, const char* attr, int def);
float attrf(XMLAttributes* atts, const char* attr);
float attrf(XMLAttributes* atts, const char* attr, float def);
double attrd(XMLAttributes* atts, const char* attr);
double attrd(XMLAttributes* atts, const char* attr, double def);
bool attrb(XMLAttributes* atts, const char* attr);
void parseWeight(const XMLAttributes* a);
void parseStall(const XMLAttributes* a);
void parseFlap(const XMLAttributes* a, const char* name);
void parseTurbineEngine(const XMLAttributes* a);
void parsePistonEngine(const XMLAttributes* a);
void parsePropeller(const XMLAttributes* a);
void parseThruster(const XMLAttributes* a);
void parseJet(const XMLAttributes* a);
void parseHitch(const XMLAttributes* a);
void parseTow(const XMLAttributes* a);
void parseWinch(const XMLAttributes* a);
void parseGear(const XMLAttributes* a);
void parseHook(const XMLAttributes* a);
void parseLaunchbar(const XMLAttributes* a);
void parseFuselage(const XMLAttributes* a);
void parseTank(const XMLAttributes* a);
void parseBallast(const XMLAttributes* a);
void parseControlSetting(const XMLAttributes* a);
void parseControlIn(const XMLAttributes* a);
void parseControlOut(const XMLAttributes* a);
void parseControlSpeed(const XMLAttributes* a);
int attri(const XMLAttributes* atts, const char* attr);
int attri(const XMLAttributes* atts, const char* attr, int def);
float attrf(const XMLAttributes* atts, const char* attr);
float attrf(const XMLAttributes* atts, const char* attr, float def);
void attrf_xyz(const XMLAttributes* atts, float* out);
double attrd(const XMLAttributes* atts, const char* attr);
double attrd(const XMLAttributes* atts, const char* attr, double def);
bool attrb(const XMLAttributes* atts, const char* attr);
// The core Airplane object we manage.
Airplane _airplane;
@ -73,12 +110,13 @@ private:
Vector _controlProps;
// Radius of the vehicle, for intersection testing.
float _vehicle_radius;
float _vehicle_radius {0};
// Parsing temporaries
void* _currObj;
bool _cruiseCurr;
int _nextEngine;
void* _currObj {nullptr};
Airplane::Configuration _airplaneCfg;
int _nextEngine {0};
int _wingSection {0};
class FuelProps
{

2
src/FDM/YASim/Math.hpp
View file

@ -50,6 +50,8 @@ public:
out[2] = v[2];
}
static void zero3(float* out) { out[0] = out[1] = out[2] = 0; }
static inline float dot3(const float* a, const float* b) {
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

15
src/FDM/YASim/Model.cpp
View file

@ -61,6 +61,12 @@ Model::Model()
_fAeroXN = _modelN->getNode("f-x-drag", true);
_fAeroYN = _modelN->getNode("f-y-side", true);
_fAeroZN = _modelN->getNode("f-z-lift", true);
_fGravXN = _modelN->getNode("gravity-x", true);
_fGravYN = _modelN->getNode("gravity-y", true);
_fGravZN = _modelN->getNode("gravity-z", true);
_fSumXN = _modelN->getNode("f-sum-x", true);
_fSumYN = _modelN->getNode("f-sum-y", true);
_fSumZN = _modelN->getNode("f-sum-z", true);
_gefxN = fgGetNode("/fdm/yasim/debug/ground-effect/ge-f-x", true);
_gefyN = fgGetNode("/fdm/yasim/debug/ground-effect/ge-f-y", true);
@ -293,8 +299,7 @@ void Model::calcForces(State* s)
// Do each surface, remembering that the local velocity at each
// point is different due to rotation.
float faero[3];
faero[0] = faero[1] = faero[2] = 0;
float faero[3] {0,0,0};
for(i=0; i<_surfaces.size(); i++) {
Surface* sf = (Surface*)_surfaces.get(i);
@ -374,6 +379,12 @@ void Model::calcForces(State* s)
_fAeroXN->setFloatValue(faero[0]);
_fAeroYN->setFloatValue(faero[1]);
_fAeroZN->setFloatValue(faero[2]);
_fGravXN->setFloatValue(grav[0]);
_fGravYN->setFloatValue(grav[1]);
_fGravZN->setFloatValue(grav[2]);
_fSumXN->setFloatValue(faero[0]+grav[0]);
_fSumYN->setFloatValue(faero[1]+grav[1]);
_fSumZN->setFloatValue(faero[2]+grav[2]);
}
// Convert the velocity and rotation vectors to local coordinates
float lrot[3], lv[3];

6
src/FDM/YASim/Model.hpp
View file

@ -118,6 +118,12 @@ private:
SGPropertyNode_ptr _fAeroXN;
SGPropertyNode_ptr _fAeroYN;
SGPropertyNode_ptr _fAeroZN;
SGPropertyNode_ptr _fSumXN;
SGPropertyNode_ptr _fSumYN;
SGPropertyNode_ptr _fSumZN;
SGPropertyNode_ptr _fGravXN;
SGPropertyNode_ptr _fGravYN;
SGPropertyNode_ptr _fGravZN;
SGPropertyNode_ptr _gefxN;
SGPropertyNode_ptr _gefyN;
SGPropertyNode_ptr _gefzN;

36
src/FDM/YASim/RigidBody.cpp
View file

@ -69,9 +69,7 @@ void RigidBody::setMass(int handle, float mass, const float* pos, bool isStatic)
void RigidBody::getMassPosition(int handle, float* out) const
{
out[0] = _masses[handle].p[0];
out[1] = _masses[handle].p[1];
out[2] = _masses[handle].p[2];
Math::set3(_masses[handle].p, out);
}
// Calcualtes the rotational velocity of a particular point. All
@ -86,19 +84,17 @@ void RigidBody::_recalcStatic()
{
// aggregate all masses that do not change (e.g. fuselage, wings) into one point mass
_staticMass.m = 0;
_staticMass.p[0] = 0;
_staticMass.p[1] = 0;
_staticMass.p[2] = 0;
Math::zero3(_staticMass.p);
int i;
int s = 0;
for(i=0; i<_nMasses; i++) {
if (_masses[i].isStatic) {
s++;
float m = _masses[i].m;
_staticMass.m += m;
_staticMass.p[0] += m * _masses[i].p[0];
_staticMass.p[1] += m * _masses[i].p[1];
_staticMass.p[2] += m * _masses[i].p[2];
float mass = _masses[i].m;
_staticMass.m += mass;
float momentum[3];
Math::mul3(mass, _masses[i].p, momentum);
Math::add3(momentum, _staticMass.p, _staticMass.p);
}
}
Math::mul3(1/_staticMass.m, _staticMass.p, _staticMass.p);
@ -151,18 +147,16 @@ void RigidBody::recalc()
// Calculate the c.g and total mass
// init with pre-calculated static mass
_totalMass = _staticMass.m;
_cg[0] = _staticMass.m * _staticMass.p[0];
_cg[1] = _staticMass.m * _staticMass.p[1];
_cg[2] = _staticMass.m * _staticMass.p[2];
Math::mul3(_staticMass.m, _staticMass.p, _cg);
int i;
for(i=0; i<_nMasses; i++) {
// only masses we did not aggregate
if (!_masses[i].isStatic) {
float m = _masses[i].m;
_totalMass += m;
_cg[0] += m * _masses[i].p[0];
_cg[1] += m * _masses[i].p[1];
_cg[2] += m * _masses[i].p[2];
float mass = _masses[i].m;
_totalMass += mass;
float momentum[3];
Math::mul3(mass, _masses[i].p, momentum);
Math::add3(momentum, _cg, _cg);
}
}
Math::mul3(1/_totalMass, _cg, _cg);
@ -201,8 +195,8 @@ void RigidBody::recalc()
void RigidBody::reset()
{
_torque[0] = _torque[1] = _torque[2] = 0;
_force[0] = _force[1] = _force[2] = 0;
Math::zero3(_torque);
Math::zero3(_force);
}
void RigidBody::addForce(const float* pos, const float* force)

61
src/FDM/YASim/Surface.cpp
View file

@ -1,11 +1,14 @@
#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 ) :
_version(version)
Surface::Surface(Version* version, float* pos, float dragCoefficient = 1 ) :
_version(version),
_c0(dragCoefficient)
{
_id = s_idGenerator++;
@ -13,6 +16,8 @@ Surface::Surface( Version * version ) :
_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);
@ -25,24 +30,45 @@ Surface::Surface( Version * version ) :
_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* p)
void Surface::setPosition(const float* pos)
{
int i;
for(i=0; i<3; i++) _pos[i] = p[i];
Math::set3(pos, _pos);
if (_surfN != 0) {
_surfN->getNode("pos-x", true)->setFloatValue(p[0]);
_surfN->getNode("pos-y", true)->setFloatValue(p[1]);
_surfN->getNode("pos-z", true)->setFloatValue(p[2]);
_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) {
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]);
}
}
@ -68,7 +94,9 @@ void Surface::setFlapPos(float pos)
{
if (_flapPos != pos) {
_flapPos = pos;
if (_surfN != 0) _flapN->setFloatValue(pos);
if (_surfN != 0) {
_flapN->setFloatValue(pos);
}
}
}
@ -76,7 +104,9 @@ void Surface::setSlatPos(float pos)
{
if (_slatPos != pos) {
_slatPos = pos;
if (_surfN != 0) _slatN->setFloatValue(pos);
if (_surfN != 0) {
_slatN->setFloatValue(pos);
}
}
}
@ -90,23 +120,24 @@ void Surface::setSpoilerPos(float 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, too.
// 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) {
int i;
for(i=0; i<3; i++) out[i] = torque[i] = 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.) {
for(int i=0; i<3; i++) out[i] = torque[i] = 0.;
return;
}
@ -116,7 +147,7 @@ void Surface::calcForce(const float* v, const float rho, float* out, float* torq
// "Rotate" by the incidence angle. Assume small angles, so we
// need to diddle only the Z component, X is relatively unchanged
// by small rotations.
// 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

22
src/FDM/YASim/Surface.hpp
View file

@ -16,7 +16,7 @@ class Surface
int _id; //index for property tree
public:
Surface( Version * version );
Surface(Version * version, float* pos, float dragCoefficient);
int getID() const { return _id; };
static void resetIDgen() { s_idGenerator = 0; };
@ -25,8 +25,8 @@ public:
void setPosition(const float* p);
void getPosition(float* out) const { Math::set3(_pos, out); }
// Distance scale along the X axis
void setChord(float chord) { _chord = chord; }
// Distance scale along the X axis used for torque (pitch) calculation
void setChord(float chord);
// Slats act to move the stall peak by the specified angle, and
// increase drag by the multiplier specified.
@ -59,16 +59,18 @@ public:
// The offset from base incidence for this surface.
void setTwist(float angle) { _twist = angle; }
void setTotalDrag(float c0) { _c0 = c0; }
float getTotalDrag() const { return _c0; }
void setTotalForceCoefficient(float c0) { _c0 = c0; }
void mulTotalForceCoefficient(float factor) { _c0 *= factor; }
float getTotalForceCoefficient() const { return _c0; }
void setXDrag(float cx) { _cx = cx; }
void setDragCoefficient(float cx) { _cx = cx; }
float getDragCoefficient() const { return _cx; }
void setYDrag(float cy) { _cy = cy; }
void setZDrag(float cz) { _cz = cz; }
float getXDrag() const { return _cx; }
void setLiftCoefficient(float cz) { _cz = cz; }
float getLiftCoefficient() const { return _cz; }
// zero-alpha Z drag ("camber") specified as a fraction of cz
void setBaseZDrag(float cz0) { _cz0 = cz0; }
void setZeroAlphaLift(float cz0) { _cz0 = cz0; }
// i: 0 == forward, 1 == backwards
void setStallPeak(int i, float peak) { _peaks[i] = peak; }
@ -87,6 +89,7 @@ public:
private:
SGPropertyNode_ptr _surfN;
Version * _version;
float stallFunc(float* v);
float flapLift(float alpha);
@ -123,7 +126,6 @@ private:
float _stallAlpha {0};
float _alpha {0};
Version * _version;
SGPropertyNode* _fxN;
SGPropertyNode* _fyN;
SGPropertyNode* _fzN;

15
src/FDM/YASim/Vector.hpp
View file

@ -1,6 +1,10 @@
#ifndef _VECTOR_HPP
#define _VECTOR_HPP
#include <stdio.h>
#include <cassert>
namespace yasim {
//
// Excruciatingly simple vector-of-pointers class. Easy & useful.
// No support for addition of elements anywhere but at the end of the
@ -19,9 +23,9 @@ public:
private:
void realloc();
int _nelem;
int _sz;
void** _array;
int _nelem {0};
int _sz {0};
void** _array {nullptr};
};
inline Vector::Vector()
@ -46,11 +50,14 @@ inline int Vector::add(void* p)
inline void* Vector::get(int i) const
{
assert(i >= 0 and i < _sz);
return _array[i];
}
inline void Vector::set(int i, void* p)
{
assert(i >= 0 and i < _sz);
_array[i] = p;
}
@ -73,5 +80,5 @@ inline void Vector::realloc()
delete[] _array;
_array = array;
}
}; //namespace yasim
#endif // _VECTOR_HPP

2
src/FDM/YASim/Version.cpp
View file

@ -18,6 +18,8 @@ void Version::setVersion( const char * version )
_version = YASIM_VERSION_32;
} else if( v == "2017.2" ) {
_version = YASIM_VERSION_2017_2;
} else if( v == "2018.1" ) {
_version = YASIM_VERSION_2018_1;
} else if( v == "YASIM_VERSION_CURRENT" ) {
_version = YASIM_VERSION_CURRENT;
} else {

3
src/FDM/YASim/Version.hpp
View file

@ -12,7 +12,8 @@ public:
YASIM_VERSION_ORIGINAL = 0,
YASIM_VERSION_32,
YASIM_VERSION_2017_2,
YASIM_VERSION_CURRENT = YASIM_VERSION_2017_2
YASIM_VERSION_2018_1,
YASIM_VERSION_CURRENT = YASIM_VERSION_2018_1
} YASIM_VERSION;
void setVersion( const char * version );

521
src/FDM/YASim/Wing.cpp
View file

@ -1,135 +1,104 @@
#include "yasim-common.hpp"
#include "Model.hpp"
#include "Surface.hpp"
#include "Wing.hpp"
namespace yasim {
Wing::Wing(Version *ver, bool mirror, float* base, float chord,
float length, float taper, float sweep, float dihedral, float twist) :
Wing::Wing(Version *ver, bool mirror) :
_version(ver),
_mirror(mirror),
_chord(chord),
_length(length),
_taper(taper),
_sweep(sweep),
_dihedral(dihedral),
_twist(twist)
_mirror(mirror)
{
Math::set3(base, _base);
_meanChord = _chord*(_taper+1)*0.5f;
calculateWingCoordinateSystem();
calculateTip();
calculateSpan();
calculateMAC();
}
Wing::~Wing()
{
for(int i=0; i<_surfs.size(); i++) {
SurfRec* s = (SurfRec*)_surfs.get(i);
WingSection* ws;
for (int s=0; s < _sections.size(); s++){
ws = (WingSection*)_sections.get(s);
for(int i=0; i<ws->_surfs.size(); i++) {
SurfRec* s = (SurfRec*)ws->_surfs.get(i);
delete s->surface;
delete s;
}
}
}
void Wing::setIncidence(float incidence)
int Wing::addWingSection(float* base, float chord, float wingLength, float taper,
float sweep, float dihedral, float twist, float camber,
float idrag, float incidence)
{
_incidence = incidence;
for(int i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
WingSection* ws = new WingSection;
if (_sections.size() == 0) {
// first section
Math::set3(base, _base);
ws->_rootChord = _float2chord(base, chord);
ws->_sectionIncidence = incidence;
} else {
WingSection* prev = (WingSection*)_sections.get(_sections.size()-1);
//use old wing tip instead of base argument
ws->_rootChord = prev->_tipChord;
ws->_sectionIncidence = prev->_sectionIncidence + prev->_twist;
}
ws->_length = wingLength;
ws->_taper = taper;
ws->_sweepAngleCenterLine = sweep;
ws->_dihedral = dihedral;
ws->_twist = twist;
ws->_camber = camber;
ws->_inducedDrag = idrag;
ws->calculateGeometry();
int idx = _sections.add(ws);
// first / only section
if (idx == 0) {
_mac = ws->getMAC();
_wingspan = ws->_wingspan;
_area = ws->getArea();
_meanChord = ws->_meanChord;
}
// append section: Calculate wing MAC from MACs of section and prev wing
else {
_mac = Wing::calculateMAC(_mac, ws->getMAC());
_wingspan += ws->_wingspan;
_area += ws->getArea();
_meanChord = _meanChord * ws->_meanChord * 0.5f;
}
_chord2float(ws->_tipChord, _tip);
return idx;
}
void Wing::setFlap0Params(float start, float end, float lift, float drag)
Chord Wing::_float2chord(float* pos, float lenght)
{
_flap0Start = start;
_flap0End = end;
_flap0Lift = lift;
_flap0Drag = drag;
Chord c;
c.x = pos[0];
c.y = pos[1];
c.z = pos[2];
c.length = lenght;
return c;
}
void Wing::setFlap1Params(float start, float end, float lift, float drag)
void Wing::_chord2float(Chord c, float* pos)
{
_flap1Start = start;
_flap1End = end;
_flap1Lift = lift;
_flap1Drag = drag;
pos[0] = c.x;
pos[1] = c.y;
pos[2] = c.z;
}
void Wing::setSlatParams(float start, float end, float aoa, float drag)
void Wing::WingSection::calculateGeometry()
{
_slatStart = start;
_slatEnd = end;
_slatAoA = aoa;
_slatDrag = drag;
_meanChord = _rootChord.length*(_taper+1)*0.5f;
calculateWingCoordinateSystem();
calculateTipChord();
calculateSpan();
calculateMAC();
}
void Wing::setSpoilerParams(float start, float end, float lift, float drag)
{
_spoilerStart = start;
_spoilerEnd = end;
_spoilerLift = lift;
_spoilerDrag = drag;
}
void Wing::setFlap0Pos(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
for(int i=0; i<_flap0Surfs.size(); i++) {
((Surface*)_flap0Surfs.get(i))->setFlapPos(lval);
if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlapPos(rval);
}
}
void Wing::setFlap0Effectiveness(float lval)
{
lval = Math::clamp(lval, 1, 10);
for(int i=0; i<_flap0Surfs.size(); i++) {
((Surface*)_flap0Surfs.get(i))->setFlapEffectiveness(lval);
}
}
void Wing::setFlap1Pos(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
for(int i=0; i<_flap1Surfs.size(); i++) {
((Surface*)_flap1Surfs.get(i))->setFlapPos(lval);
if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlapPos(rval);
}
}
void Wing::setFlap1Effectiveness(float lval)
{
lval = Math::clamp(lval, 1, 10);
for(int i=0; i<_flap1Surfs.size(); i++) {
((Surface*)_flap1Surfs.get(i))->setFlapEffectiveness(lval);
}
}
void Wing::setSpoilerPos(float lval, float rval)
{
lval = Math::clamp(lval, 0, 1);
rval = Math::clamp(rval, 0, 1);
for(int i=0; i<_spoilerSurfs.size(); i++) {
((Surface*)_spoilerSurfs.get(i))->setSpoilerPos(lval);
if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoilerPos(rval);
}
}
void Wing::setSlatPos(float val)
{
val = Math::clamp(val, 0, 1);
for(int i=0; i<_slatSurfs.size(); i++)
((Surface*)_slatSurfs.get(i))->setSlatPos(val);
}
void Wing::calculateWingCoordinateSystem() {
void Wing::WingSection::calculateWingCoordinateSystem() {
// prepare wing coordinate system, ignoring incidence and twist for now
// (tail incidence is varied by the solver)
// Generating a unit vector pointing out the left wing.
float left[3];
left[0] = -Math::tan(_sweep);
left[0] = -Math::tan(_sweepAngleCenterLine);
left[1] = Math::cos(_dihedral);
left[2] = Math::sin(_dihedral);
Math::unit3(left, left);
@ -155,33 +124,132 @@ void Wing::calculateWingCoordinateSystem() {
for(i=3; i<6; i++) _rightOrient[i] = -_rightOrient[i];
}
void Wing::calculateTip() {
void Wing::WingSection::calculateTipChord() {
float *y = _orient+3;
Math::mul3(_length, y, _tip);
Math::add3(_base, _tip, _tip);
_tipChord.x = _rootChord.x + _length * y[0];
_tipChord.y = _rootChord.y + _length * y[1];
_tipChord.z = _rootChord.z + _length * y[2];
_tipChord.length = _rootChord.length * _taper;
}
void Wing::calculateSpan()
void Wing::WingSection::calculateSpan()
{
// wingspan in y-direction (not for vstab)
_wingspan = Math::abs(2*_tip[1]);
_netSpan = Math::abs(2*(_tip[1]-_base[1]));
_wingspan = Math::abs(2*_tipChord.y);
_aspectRatio = _wingspan / _meanChord;
}
void Wing::calculateMAC()
void Wing::WingSection::calculateMAC()
{
//FIXME call static method, use absolute y values
// http://www.nasascale.org/p2/wp-content/uploads/mac-calculator.htm
const float commonFactor = _chord*(0.5+_taper)/(3*_chord*(1+_taper));
_mac = _chord-(2*_chord*(1-_taper)*commonFactor);
_macRootDistance = _netSpan*commonFactor;
_macX = _base[0]-Math::tan(_sweep) * _macRootDistance + _mac/2;
const float commonFactor = _rootChord.length*(0.5+_taper)/(3*_rootChord.length*(1+_taper));
_mac.length = _rootChord.length-(2*_rootChord.length*(1-_taper)*commonFactor);
_mac.y = Math::abs(2*(_tipChord.y-_rootChord.y))*commonFactor;
_mac.x = _rootChord.x-Math::tan(_sweepAngleCenterLine)*_mac.y + _mac.length/2;
_mac.y += _rootChord.y;
}
float Wing::WingSection::calculateSweepAngleLeadingEdge()
{
if (_length == 0) {
return 0;
}
return Math::atan(
(sin(_sweepAngleCenterLine)+(1-_taper)*_rootChord.length/(2*_length)) /
cos(_sweepAngleCenterLine)
);
}
void Wing::WingSection::setIncidence(float incidence)
{
//update surface
for(int i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence + _sectionIncidence);
}
// root and tip (x,y) coordinates are on leading edge
Chord Wing::calculateMAC(Chord root, Chord tip)
{
assert(root.length > 0);
//taper = tip.length / root.length;
const float commonFactor = (root.length*0.5+tip.length)/(3*(root.length+tip.length));
Chord m;
m.length = root.length-(2*(root.length - tip.length)*commonFactor);
m.y = Math::abs(2*(tip.y - root.y))*commonFactor + root.y;
m.x = root.x - (root.x - tip.x)*(root.y - m.y)/(root.y - tip.y);
return m;
}
void Wing::setFlapParams(int section, WingFlaps type, FlapParams fp)
{
((WingSection*)_sections.get(section))->_flapParams[type] = fp;
}
void Wing::setSectionDrag(int section, float pdrag)
{
((WingSection*)_sections.get(section))->_dragScale = pdrag;
}
void Wing::setSectionStallParams(int section, StallParams sp)
{
((WingSection*)_sections.get(section))->_stallParams = sp;
}
void Wing::setFlapPos(WingFlaps type,float lval, float rval)
{
float min {-1};
if (type == WING_SPOILER || type == WING_SLAT) {
min = 0;
}
lval = Math::clamp(lval, min, 1);
rval = Math::clamp(rval, min, 1);
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
for(int i=0; i < ws->_flapSurfs[type].size(); i++) {
switch (type) {
case WING_FLAP0:
case WING_FLAP1:
((Surface*)ws->_flapSurfs[type].get(i))->setFlapPos(lval);
if(_mirror) ((Surface*)ws->_flapSurfs[type].get(++i))->setFlapPos(rval);
break;
case WING_SLAT:
((Surface*)ws->_flapSurfs[type].get(i))->setSlatPos(lval);
break;
case WING_SPOILER:
((Surface*)ws->_flapSurfs[type].get(i))->setSpoilerPos(lval);
if(_mirror) ((Surface*)ws->_flapSurfs[type].get(++i))->setSpoilerPos(rval);
break;
}
}
}
}
void Wing::setFlapEffectiveness(WingFlaps f, float lval)
{
lval = Math::clamp(lval, 1, 10);
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
for(int i=0; i<ws->_flapSurfs[f].size(); i++) {
((Surface*)ws->_flapSurfs[f].get(i))->setFlapEffectiveness(lval);
}
}
}
void Wing::compile()
{
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
// Have we already been compiled?
if(! _surfs.empty()) return;
if(! ws->_surfs.empty()) return;
// Assemble the start/end coordinates of all control surfaces
// and the wing itself into an array, sort them,
@ -189,16 +257,19 @@ void Wing::compile()
// segments.
const int NUM_BOUNDS {10};
float bounds[NUM_BOUNDS];
bounds[0] = _flap0Start; bounds[1] = _flap0End;
bounds[2] = _flap1Start; bounds[3] = _flap1End;
bounds[4] = _spoilerStart; bounds[5] = _spoilerEnd;
bounds[6] = _slatStart; bounds[7] = _slatEnd;
bounds[0] = ws->_flapParams[WING_FLAP0].start;
bounds[1] = ws->_flapParams[WING_FLAP0].end;
bounds[2] = ws->_flapParams[WING_FLAP1].start;
bounds[3] = ws->_flapParams[WING_FLAP1].end;
bounds[4] = ws->_flapParams[WING_SPOILER].start;
bounds[5] = ws->_flapParams[WING_SPOILER].end;
bounds[6] = ws->_flapParams[WING_SLAT].start;
bounds[7] = ws->_flapParams[WING_SLAT].end;
//and don't forget the root and the tip of the wing itself
bounds[8] = 0; bounds[9] = 1;
// Sort in increasing order
int i;
for(i=0; i<NUM_BOUNDS; i++) {
for(int i=0; i<NUM_BOUNDS; i++) {
int minIdx = i;
float minVal = bounds[i];
for(int j=i+1; j<NUM_BOUNDS; j++) {
@ -214,7 +285,7 @@ void Wing::compile()
// Uniqify
float last = bounds[0];
int nbounds = 1;
for(i=1; i<NUM_BOUNDS; i++) {
for(int i=1; i<NUM_BOUNDS; i++) {
if(bounds[i] != last)
bounds[nbounds++] = bounds[i];
last = bounds[i];
@ -222,48 +293,47 @@ void Wing::compile()
// Calculate a "nominal" segment length equal to an average chord,
// normalized to lie within 0-1 over the length of the wing.
float segLen = _meanChord / _length;
float segLen = ws->_meanChord / ws->_length;
// Now go through each boundary and make segments
for(i=0; i<(nbounds-1); i++) {
for(int i=0; i<(nbounds-1); i++) {
float start = bounds[i];
float end = bounds[i+1];
float mid = (start+end)/2;
bool hasFlap0=0, hasFlap1=0, hasSlat=0, hasSpoiler=0;
if(_flap0Start < mid && mid < _flap0End) hasFlap0 = 1;
if(_flap1Start < mid && mid < _flap1End) hasFlap1 = 1;
if(_slatStart < mid && mid < _slatEnd) hasSlat = 1;
if(_spoilerStart < mid && mid < _spoilerEnd) hasSpoiler = 1;
if(ws->_flapParams[WING_FLAP0].start < mid && mid < ws->_flapParams[WING_FLAP0].end)
hasFlap0 = 1;
if(ws->_flapParams[WING_FLAP1].start < mid && mid < ws->_flapParams[WING_FLAP1].end)
hasFlap1 = 1;
if(ws->_flapParams[WING_SLAT].start < mid && mid < ws->_flapParams[WING_SLAT].end)
hasSlat = 1;
if(ws->_flapParams[WING_SPOILER].start < mid && mid < ws->_flapParams[WING_SPOILER].end)
hasSpoiler = 1;
// FIXME: Should probably detect an error here if both flap0
// and flap1 are set. Right now flap1 overrides.
int nSegs = (int)Math::ceil((end-start)/segLen);
if (_twist != 0 && nSegs < 8) // more segments if twisted
if (ws->_twist != 0 && nSegs < 8) // more segments if twisted
nSegs = 8;
float segWid = _length * (end - start)/nSegs;
float segWid = ws->_length * (end - start)/nSegs;
int j;
for(j=0; j<nSegs; j++) {
for(int j=0; j<nSegs; j++) {
float frac = start + (j+0.5f) * (end-start)/nSegs;
float pos[3];
interp(_base, _tip, frac, pos);
float chord = _chord * (1 - (1-_taper)*frac);
float chord = ws->_rootChord.length * (1 - (1-ws->_taper)*frac);
float weight = chord * segWid;
float twist = _twist * frac;
float twist = ws->_twist * frac;
Surface *s = newSurface(pos, _orient, chord,
hasFlap0, hasFlap1, hasSlat, hasSpoiler);
addSurface(s, weight, twist);
ws->newSurface(_version, pos, ws->_orient, chord, hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist);
if(_mirror) {
pos[1] = -pos[1];
s = newSurface(pos, _rightOrient, chord,
hasFlap0, hasFlap1, hasSlat, hasSpoiler);
addSurface(s, weight, twist);
ws->newSurface(_version, pos, ws->_rightOrient, chord,
hasFlap0, hasFlap1, hasSlat, hasSpoiler, weight, twist);
}
}
}
@ -271,64 +341,94 @@ void Wing::compile()
// called before we were compiled.
setIncidence(_incidence);
}
void Wing::addSurface(Surface* s, float weight, float twist)
{
SurfRec *sr = new SurfRec();
sr->surface = s;
sr->weight = weight;
s->setTotalDrag(sr->weight);
s->setTwist(twist);
_surfs.add(sr);
writeInfoToProptree();
}
void Wing::setDragScale(float scale)
void Wing::multiplyLiftRatio(float factor)
{
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
ws->multiplyLiftRatio(factor);
}
}
void Wing::multiplyDragCoefficient(float factor)
{
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
ws->multiplyDragCoefficient(factor);
}
}
void Wing::setIncidence(float incidence)
{
WingSection* ws;
for (int section=0; section < _sections.size(); section++)
{
ws = (WingSection*)_sections.get(section);
ws->setIncidence(incidence);
}
}
void Wing::WingSection::setDragCoefficient(float scale)
{
_dragScale = scale;
for(int i=0; i<_surfs.size(); i++) {
SurfRec* s = (SurfRec*)_surfs.get(i);
s->surface->setTotalDrag(scale * s->weight);
s->surface->setTotalForceCoefficient(scale * s->weight);
}
}
void Wing::setLiftRatio(float ratio)
void Wing::WingSection::multiplyDragCoefficient(float factor)
{
setDragCoefficient(_dragScale * factor);
}
void Wing::WingSection::setLiftRatio(float ratio)
{
_liftRatio = ratio;
for(int i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setZDrag(ratio);
((SurfRec*)_surfs.get(i))->surface->setLiftCoefficient(ratio);
}
Surface* Wing::newSurface(float* pos, float* orient, float chord,
bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler)
void Wing::WingSection::multiplyLiftRatio(float factor)
{
Surface* s = new Surface(_version);
setLiftRatio(_liftRatio * factor);
}
void Wing::WingSection::newSurface(Version* _version, float* pos, float* orient, float chord, bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler, float weight, float twist)
{
Surface* s = new Surface(_version, pos, weight);
s->setPosition(pos);
s->setOrientation(orient);
s->setChord(chord);
// Camber is expressed as a fraction of stall peak, so convert.
s->setBaseZDrag(_camber*_stallPeak);
s->setZeroAlphaLift(_camber*_stallParams.peak);
// The "main" (i.e. normal) stall angle
float stallAoA = _stall - _stallWidth/4;
float stallAoA = _stallParams.aoa - _stallParams.width/4;
s->setStall(0, stallAoA);
s->setStallWidth(0, _stallWidth);
s->setStallPeak(0, _stallPeak);
s->setStallWidth(0, _stallParams.width);
s->setStallPeak(0, _stallParams.peak);
// The negative AoA stall is the same if we're using an symmetric
// airfoil, otherwise a "little worse".
if(_camber > 0) {
s->setStall(1, stallAoA * 0.8f);
s->setStallWidth(1, _stallWidth * 0.5f);
s->setStallWidth(1, _stallParams.width * 0.5f);
} else {
s->setStall(1, stallAoA);
if( _version->isVersionOrNewer( Version::YASIM_VERSION_2017_2 )) {
// what was presumably meant
s->setStallWidth(1, _stallWidth);
s->setStallWidth(1, _stallParams.width);
} else {
// old code; presumably a copy&paste error
s->setStall(1, _stallWidth);
s->setStall(1, _stallParams.width);
}
}
@ -341,19 +441,23 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
s->setStallWidth(i, 0.01);
}
if(hasFlap0) s->setFlapParams(_flap0Lift, _flap0Drag);
if(hasFlap1) s->setFlapParams(_flap1Lift, _flap1Drag);
if(hasSlat) s->setSlatParams(_slatAoA, _slatDrag);
if(hasSpoiler) s->setSpoilerParams(_spoilerLift, _spoilerDrag);
if(hasFlap0) s->setFlapParams(_flapParams[WING_FLAP0].lift, _flapParams[WING_FLAP0].drag);
if(hasFlap1) s->setFlapParams(_flapParams[WING_FLAP1].lift, _flapParams[WING_FLAP1].drag);
if(hasSlat) s->setSlatParams(_flapParams[WING_SLAT].aoa, _flapParams[WING_SLAT].drag);
if(hasSpoiler) s->setSpoilerParams(_flapParams[WING_SPOILER].lift, _flapParams[WING_SPOILER].drag);
if(hasFlap0) _flap0Surfs.add(s);
if(hasFlap1) _flap1Surfs.add(s);
if(hasSlat) _slatSurfs.add(s);
if(hasSpoiler) _spoilerSurfs.add(s);
if(hasFlap0) _flapSurfs[WING_FLAP0].add(s);
if(hasFlap1) _flapSurfs[WING_FLAP1].add(s);
if(hasSlat) _flapSurfs[WING_SLAT].add(s);
if(hasSpoiler) _flapSurfs[WING_SPOILER].add(s);
s->setInducedDrag(_inducedDrag);
return s;
SurfRec *sr = new SurfRec();
sr->surface = s;
sr->weight = weight;
s->setTwist(twist);
_surfs.add(sr);
}
void Wing::interp(const float* v1, const float* v2, const float frac, float* out)
@ -363,4 +467,81 @@ void Wing::interp(const float* v1, const float* v2, const float frac, float* out
out[2] = v1[2] + frac*(v2[2]-v1[2]);
}
void Wing::writeInfoToProptree()
{
if (_wingN == nullptr)
return;
WingSection* ws = (WingSection*)_sections.get(0);
float chord = ws->_rootChord.length;
ws = (WingSection*)_sections.get(_sections.size()-1);
float taper = ws->_rootChord.length * ws->_taper;
_wingN->getNode("tip-x", true)->setFloatValue(_tip[0]);
_wingN->getNode("tip-y", true)->setFloatValue(_tip[1]);
_wingN->getNode("tip-z", true)->setFloatValue(_tip[2]);
_wingN->getNode("base-x", true)->setFloatValue(_base[0]);
_wingN->getNode("base-y", true)->setFloatValue(_base[1]);
_wingN->getNode("base-z", true)->setFloatValue(_base[2]);
_wingN->getNode("chord", true)->setFloatValue(chord);
_wingN->getNode("taper", true)->setFloatValue(taper);
_wingN->getNode("wing-span", true)->setFloatValue(_wingspan);
_wingN->getNode("wing-area", true)->setFloatValue(_wingspan*_meanChord);
_wingN->getNode("aspect-ratio", true)->setFloatValue(_aspectRatio);
_wingN->getNode("standard-mean-chord", true)->setFloatValue(_meanChord);
_wingN->getNode("mac", true)->setFloatValue(_mac.length);
_wingN->getNode("mac-x", true)->setFloatValue(_mac.x);
_wingN->getNode("mac-y", true)->setFloatValue(_mac.y);
float wgt = 0;
float dragSum = 0;
for (int section=0; section < _sections.size(); section++) {
ws = (WingSection*)_sections.get(section);
for (int surf=0; surf < ws->numSurfaces(); surf++) {
Surface* s = ws->getSurface(surf);
float drag = s->getTotalForceCoefficient();
dragSum += drag;
float mass = ws->getSurfaceWeight(surf);
mass = mass * Math::sqrt(mass);
wgt += mass;
}
}
_wingN->getNode("weight", true)->setFloatValue(wgt);
_wingN->getNode("drag", true)->setFloatValue(dragSum);
}
// estimate a mass distibution and add masses to the model
// they will be scaled to match total mass of aircraft later
float Wing::updateModel(Model* model)
{
_weight = 0;
WingSection* ws;
for (int section=0; section < _sections.size(); section++) {
ws = (WingSection*)_sections.get(section);
for(int surf=0; surf < ws->numSurfaces(); surf++) {
Surface* s = ws->getSurface(surf);
model->addSurface(s);
float weight = ws->getSurfaceWeight(surf);
weight = weight * Math::sqrt(weight);
_weight += weight;
float pos[3];
s->getPosition(pos);
int mid = model->getBody()->addMass(weight, pos, true);
if (_wingN != nullptr) {
SGPropertyNode_ptr n = _wingN->getNode("surfaces", true)->getChild("surface", s->getID(), true);
n->getNode("c0", true)->setFloatValue(s->getTotalForceCoefficient());
n->getNode("cdrag", true)->setFloatValue(s->getDragCoefficient());
n->getNode("clift", true)->setFloatValue(s->getLiftCoefficient());
n->getNode("mass-id", true)->setIntValue(mid);
}
}
}
if (_wingN != nullptr) {
_wingN->getNode("weight", true)->setFloatValue(_weight);
}
return _weight;
}
}; // namespace yasim

268
src/FDM/YASim/Wing.hpp
View file

@ -4,159 +4,171 @@
#include "Vector.hpp"
#include "Version.hpp"
#include "Math.hpp"
#include <simgear/props/props.hxx>
namespace yasim {
class Surface;
class Model;
struct FlapParams {
float start {0};
float end {0};
float lift {0};
float drag {0};
float aoa {0};
};
struct StallParams {
float aoa {0};
float width {0};
float peak {0};
};
// position and length of a chord line
struct Chord {
float x {0};
float y {0};
float z {0};
float length {0};
};
enum WingFlaps {
WING_FLAP0,
WING_FLAP1,
WING_SPOILER,
WING_SLAT,
};
// FIXME: need to handle "inverted" controls for mirrored wings.
class Wing {
public:
Wing(Version *ver, bool mirror, float* base, float chord, float length,
float taper = 1, float sweep = 0, float dihedral = 0, float twist = 0);
~Wing();
SGPropertyNode_ptr _wingN {nullptr};
// Do we mirror ourselves about the XZ plane?
void setMirror(bool mirror) { _mirror = mirror; }
const bool isMirrored() { return _mirror; };
struct SurfRec {
Surface* surface;
float weight;
};
// Wing geometry in local coordinates:
// base point of wing
void getBase(float* base) const { Math::set3(_base, base); };
// dist. ALONG wing (not span!)
float getLength() const { return _length; };
// at base, measured along X axis
float getChord() const { return _chord; };
// fraction of chord at wing tip, 0..1
float getTaper() const { return _taper; };
// radians
float getSweep() const { return _sweep; };
// radians, positive is "up"
void setDihedral(float dihedral) { _dihedral = dihedral; }
float getDihedral() const { return _dihedral; };
void setIncidence(float incidence);
// parameters for stall curve
void setStall(float aoa) { _stall = aoa; }
void setStallWidth(float angle) { _stallWidth = angle; }
void setStallPeak(float fraction) { _stallPeak = fraction; }
void setCamber(float camber) { _camber = camber; }
void setInducedDrag(float drag) { _inducedDrag = drag; }
void setFlap0Params(float start, float end, float lift, float drag);
void setFlap1Params(float start, float end, float lift, float drag);
void setSpoilerParams(float start, float end, float lift, float drag);
void setSlatParams(float start, float end, float aoa, float drag);
// Set the control axes for the sub-surfaces
void setFlap0Pos(float lval, float rval);
void setFlap1Pos(float lval, float rval);
void setSpoilerPos(float lval, float rval);
void setSlatPos(float val);
void setFlap0Effectiveness(float lval);
void setFlap1Effectiveness(float lval);
// Compile the thing into a bunch of Surface objects
void compile();
void getTip(float* tip) const { Math::set3(_tip, tip);};
// valid only after Wing::compile() was called
float getSpan() const { return _wingspan; };
float getArea() const { return _wingspan*_meanChord; };
float getAspectRatio() const { return _aspectRatio; };
float getSMC() const { return _meanChord; };
float getMAC() const { return _mac; }; // get length of MAC
float getMACx() const { return _macX; }; // get x-coord of MAC leading edge
float getMACy() const { return _base[1]+_macRootDistance; }; // get y-coord of MAC leading edge
int numSurfaces() const { return _surfs.size(); }
Surface* getSurface(int n) { return ((SurfRec*)_surfs.get(n))->surface; }
float getSurfaceWeight(int n) const { return ((SurfRec*)_surfs.get(n))->weight; }
// The overall drag coefficient for the wing as a whole. Units are
// arbitrary.
void setDragScale(float scale);
float getDragScale() const { return _dragScale; }
// The ratio of force along the Z (lift) direction of each wing
// segment to that along the X (drag) direction.
void setLiftRatio(float ratio);
float getLiftRatio() const { return _liftRatio; }
private:
void interp(const float* v1, const float* v2, const float frac, float* out);
Surface* newSurface(float* pos, float* orient, float chord,
bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler);
void calculateWingCoordinateSystem();
void calculateTip();
void calculateSpan();
void calculateMAC();
void addSurface(Surface* s, float weight, float twist);
struct SurfRec { Surface * surface; float weight; };
Vector _surfs;
Vector _flap0Surfs;
Vector _flap1Surfs;
Vector _slatSurfs;
Vector _spoilerSurfs;
Version * _version;
bool _mirror {false};
float _base[3] {0,0,0};
float _chord {0};
struct WingSection {
int _id;
Chord _rootChord;
// length is distance from base to tip, not wing span
float _length {0};
float _taper {1};
float _sweep {0};
// sweep of center line, not leading edge!
float _sweepAngleCenterLine {0};
float _dihedral {0};
float _twist {0};
float _camber {0};
float _inducedDrag {1};
StallParams _stallParams;
//fixed incidence of section as given in config XML
float _sectionIncidence {0};
float _dragScale {1};
float _liftRatio {1};
FlapParams _flapParams[sizeof(WingFlaps)];
// calculated from above
float _orient[9];
float _rightOrient[9];
float _tip[3] {0,0,0};
Chord _tipChord;
float _meanChord {0}; // std. mean chord
float _mac {0}; // mean aerodynamic chord length
float _macRootDistance {0}; // y-distance of mac from root
float _macX {0}; // x-coordinate of mac (leading edge)
float _netSpan {0};
Chord _mac; // mean aerodynamic chord (x,y) leading edge
float _wingspan {0};
float _aspectRatio {1};
// all SurfRec of this wing
Vector _surfs;
// surfaces having a certain type of flap (flap, slat, spoiler)
Vector _flapSurfs[sizeof(WingFlaps)];
float _stall {0};
float _stallWidth {0};
float _stallPeak {0};
float _twist {0};
float _camber {0};
void calculateGeometry();
void calculateWingCoordinateSystem();
void calculateTipChord();
void calculateSpan();
void calculateMAC();
float calculateSweepAngleLeadingEdge();
//set incidence value to all surfaces of this section
void setIncidence(float incidence);
// parameters for stall curve
void setStallParams(StallParams sp) { _stallParams = sp; }
// valid only after Wing::compile() was called
Chord getMAC() const { return _mac; };
float getArea() const { return _wingspan*_meanChord; };
void setDragCoefficient(float scale);
void multiplyDragCoefficient(float factor);
// The ratio of force along the Z (lift) direction of each wing
// segment to that along the X (drag) direction.
void setLiftRatio(float ratio);
void multiplyLiftRatio(float factor);
void newSurface(Version* _version, float* pos, float* orient, float chord,
bool hasFlap0, bool hasFlap1, bool hasSlat, bool hasSpoiler,
float weight, float twist);
int numSurfaces() const { return _surfs.size(); }
Surface* getSurface(int n) { return ((SurfRec*)_surfs.get(n))->surface; }
float getSurfaceWeight(int n) const { return ((SurfRec*)_surfs.get(n))->weight; }
}; //struct WingSection
//-- wing member variables --
Version* _version;
bool _mirror {false};
Vector _sections;
Chord _mac;
float _base[3] {0,0,0};
float _tip[3] {0,0,0};
float _wingspan {0};
float _area {0};
float _aspectRatio {0};
float _meanChord {0};
float _incidence {0};
float _inducedDrag {1};
float _weight {0};
float _dragScale {1};
float _liftRatio {1};
//-- private methods
Chord _float2chord(float* pos, float lenght = 0);
void _chord2float(Chord c, float* pos);
void interp(const float* v1, const float* v2, const float frac, float* out);
void writeInfoToProptree();
float _flap0Start {0};
float _flap0End {0};
float _flap0Lift {0};
float _flap0Drag {0};
public:
Wing(Version* ver, bool mirror);
~Wing();
float _flap1Start {0};
float _flap1End {0};
float _flap1Lift {0};
float _flap1Drag {0};
int addWingSection(float* base, float chord, float wingLength,
float taper = 1, float sweep = 0, float dihedral = 0, float twist = 0, float camber = 0, float idrag = 1, float incidence = 0);
float _spoilerStart {0};
float _spoilerEnd {0};
float _spoilerLift {0};
float _spoilerDrag {0};
static Chord calculateMAC(Chord root, Chord tip);
float _slatStart {0};
float _slatEnd {0};
float _slatAoA {0};
float _slatDrag {0};
void setFlapParams(int section, WingFlaps type, FlapParams fp);
void setSectionDrag(int section, float pdrag);
void setSectionStallParams(int section, StallParams sp);
// Compile the thing into a bunch of Surface objects
void compile();
void multiplyLiftRatio(float factor);
void multiplyDragCoefficient(float factor);
void setIncidence(float incidence);
bool isMirrored() const { return _mirror; };
void getBase(float* base) const { Math::set3(_base, base); };
void getTip(float* tip) const { Math::set3(_tip, tip); };
float getSpan() const { return _wingspan; };
float getArea() const { return _area; };
float getAspectRatio() const { return _aspectRatio; };
float getSMC() const { return _meanChord; };
float getMACLength() const { return _mac.length; }; // get length of MAC
float getMACx() const { return _mac.x; }; // get x-coord of MAC leading edge
float getMACy() const { return _mac.y; }; // get y-coord of MAC leading edge
//-----------------------------
// propergate the control axes value for the sub-surfaces
void setFlapPos(WingFlaps type, float lval, float rval = 0);
void setFlapEffectiveness(WingFlaps f, float lval);
void setPropertyNode(SGPropertyNode_ptr n) { _wingN = n; };
float updateModel(Model* model);
};
}; // namespace yasim

1
src/FDM/YASim/proptest.cpp
View file

@ -5,6 +5,7 @@
#include <stdio.h>
#include <stdlib.h>
#include "yasim-common.hpp"
#include "Math.hpp"
#include "FGFDM.hpp"
#include "PropEngine.hpp"

5
src/FDM/YASim/yasim-common.cpp Normal file
View file

@ -0,0 +1,5 @@
#include "yasim-common.hpp"
namespace yasim {
}; //namespace yasim

6
src/FDM/YASim/yasim-common.hpp
View file

@ -1,6 +1,9 @@
#ifndef _YASIM_COMMON_HPP
#define _YASIM_COMMON_HPP
/*
common file for YASim wide constants and static helper functions
*/
namespace yasim {
static const float YASIM_PI = 3.14159265358979323846f;
static const float PI2 = YASIM_PI*2;
@ -28,7 +31,6 @@ namespace yasim {
static const float NM2FTLB = (1/(LBS2N*FT2M));
static const float SLUG2KG = 14.59390f;
};
}; //namespace yasim
#endif // ifndef _YASIM_COMMON_HPP

31
src/FDM/YASim/yasim-test.cpp
View file

@ -95,7 +95,7 @@ void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG
ld_max= ld;
ld_max_deg = deg;
}
printf("%d %g %g %g\n", deg, lift, drag, ld);
printf("%d %.4g %.4g %.4g\n", deg, lift, drag, ld);
}
printf("# cl_max %g at %d deg\n", cl_max, cl_max_deg);
printf("# cd_min %g at %d deg\n", cd_min, cd_min_deg);
@ -246,11 +246,12 @@ int main(int argc, char** argv)
float SI_inertia[9];
a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
float MAC = 0, MACx = 0, MACy = 0;
Wing* wing = a->getWing();
if (wing) {
MAC = a->getWing()->getMAC();
MACx = a->getWing()->getMACx();
MACy = a->getWing()->getMACy();
Wing* wing {nullptr};
if (a->hasWing()) {
wing = a->getWing();
MAC = wing->getMACLength();
MACx = wing->getMACx();
MACy = wing->getMACy();
}
printf(" Iterations: %d\n", a->getSolutionIterations());
printf(" Drag Coefficient: %.3f\n", drag);
@ -260,16 +261,18 @@ int main(int argc, char** argv)
printf("Approach Elevator: %.3f\n\n", a->getApproachElevator());
printf(" CG: x:%.3f, y:%.3f, z:%.3f\n", cg[0], cg[1], cg[2]);
if (wing) {
printf(" Wing MAC (*1): x:%.2f, y:%.2f, length:%.1f \n", MACx, MACy, MAC);
printf(" CG-x rel. MAC: %.3f\n", a->getCGMAC());
printf(" CG-x desired: %.3f < %.3f < %.3f \n", a->getCGSoftLimitXMin(), cg[0], a->getCGSoftLimitXMax());
printf(" Wing MAC: (x:%.2f, y:%.2f), length:%.1f \n", MACx, MACy, MAC);
printf(" hard limit CG-x: %.3f \n", a->getCGHardLimitXMax());
printf(" soft limit CG-x: %.3f \n", a->getCGSoftLimitXMax());
printf(" CG-x: %.3f \n", cg[0]);
printf(" CG-x rel. MAC: %3.0f%%\n", a->getCGMAC()*100);
printf(" soft limit CG-x: %.3f \n", a->getCGSoftLimitXMin());
printf(" hard limit CG-x: %.3f \n", a->getCGHardLimitXMin());
}
printf("\nInertia tensor [kg*m^2], origo at CG:\n\n");
printf(" %7.3f, %7.3f, %7.3f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
printf(" %7.3f, %7.3f, %7.3f\n", SI_inertia[3], SI_inertia[4], SI_inertia[5]);
printf(" %7.3f, %7.3f, %7.3f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
printf("\n(*1) MAC calculation works on <wing> only! Numbers will be wrong for segmented wings, e.g. <wing>+<mstab>.\n");
printf(" %7.0f, %7.0f, %7.0f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
printf(" %7.0f, %7.0f, %7.0f\n", SI_inertia[3], SI_inertia[4], SI_inertia[5]);
printf(" %7.0f, %7.0f, %7.0f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
}
delete fdm;
return 0;