fly/flightgear
1
0
Fork 0
Code Activity

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

Browse source 
https://sourceforge.net/p/flightgear/flightgear/merge-requests/78/
This commit is contained in:
Erik Hofman 2017-03-19 07:49:26 +00:00
commit 2393fd647d
22 changed files with 1228 additions and 1255 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -17,7 +17,6 @@
namespace yasim {
// gadgets
inline float norm(float f) { return f<1 ? 1/f : f; }
inline float abs(float f) { return f<0 ? -f : f; }
// Solver threshold. How close to the solution are we trying
@ -57,6 +56,9 @@ Airplane::Airplane()
_tailIncidence = 0;
_failureMsg = 0;
_wingsN = 0;
_cgMaxX = -1e6;
_cgMinX = 1e6;
}
Airplane::~Airplane()
@ -113,16 +115,6 @@ void Airplane::calcFuelWeights()
}
}
ControlMap* Airplane::getControlMap()
{
return &_controls;
}
Model* Airplane::getModel()
{
return &_model;
}
void Airplane::getPilotAccel(float* out)
{
State* s = _model.getState();
@ -145,43 +137,6 @@ void Airplane::getPilotAccel(float* out)
// FIXME: rotational & centripetal acceleration needed
}
void Airplane::setPilotPos(float* pos)
{
int i;
for(i=0; i<3; i++) _pilotPos[i] = pos[i];
}
void Airplane::getPilotPos(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _pilotPos[i];
}
int Airplane::numGear()
{
return _gears.size();
}
Gear* Airplane::getGear(int g)
{
return ((GearRec*)_gears.get(g))->gear;
}
Hook* Airplane::getHook()
{
return _model.getHook();
}
Launchbar* Airplane::getLaunchbar()
{
return _model.getLaunchbar();
}
Rotorgear* Airplane::getRotorgear()
{
return _model.getRotorgear();
}
void Airplane::updateGearState()
{
for(int i=0; i<_gears.size(); i++) {
@ -247,51 +202,6 @@ void Airplane::addSolutionWeight(bool approach, int idx, float wgt)
_solveWeights.add(w);
}
int Airplane::numTanks()
{
return _tanks.size();
}
float Airplane::getFuel(int tank)
{
return ((Tank*)_tanks.get(tank))->fill;
}
float Airplane::setFuel(int tank, float fuel)
{
return ((Tank*)_tanks.get(tank))->fill = fuel;
}
float Airplane::getFuelDensity(int tank)
{
return ((Tank*)_tanks.get(tank))->density;
}
float Airplane::getTankCapacity(int tank)
{
return ((Tank*)_tanks.get(tank))->cap;
}
void Airplane::setWeight(float weight)
{
_emptyWeight = weight;
}
void Airplane::setWing(Wing* wing)
{
_wing = wing;
}
void Airplane::setTail(Wing* tail)
{
_tail = tail;
}
void Airplane::addVStab(Wing* vstab)
{
_vstabs.add(vstab);
}
void Airplane::addFuselage(float* front, float* back, float width,
float taper, float mid,
float cx, float cy, float cz, float idrag)
@ -330,21 +240,10 @@ void Airplane::addGear(Gear* gear)
g->gear = gear;
g->surf = 0;
_gears.add(g);
}
void Airplane::addHook(Hook* hook)
{
_model.addHook(hook);
}
void Airplane::addHitch(Hitch* hitch)
{
_model.addHitch(hitch);
}
void Airplane::addLaunchbar(Launchbar* launchbar)
{
_model.addLaunchbar(launchbar);
float pos[3];
g->gear->getPosition(pos);
if (pos[0] > _cgMaxX) _cgMaxX = pos[0];
if (pos[0] < _cgMinX) _cgMinX = pos[0];
}
void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
@ -357,12 +256,14 @@ void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
_thrusters.add(t);
}
/// Use ballast to redistribute mass, this is NOT added to empty weight.
void Airplane::addBallast(float* pos, float mass)
{
_model.getBody()->addMass(mass, pos);
_model.getBody()->addMass(mass, pos, true);
_ballast += mass;
}
/// Masses configurable at runtime, e.g. cargo, pax
int Airplane::addWeight(float* pos, float size)
{
WeightRec* wr = new WeightRec();
@ -377,6 +278,7 @@ int Airplane::addWeight(float* pos, float size)
return _weights.add(wr);
}
/// Change weight of a previously added mass point
void Airplane::setWeight(int handle, float mass)
{
WeightRec* wr = (WeightRec*)_weights.get(handle);
@ -398,45 +300,14 @@ void Airplane::setWeight(int handle, float mass)
void Airplane::setFuelFraction(float frac)
{
int i;
for(i=0; i<_tanks.size(); i++) {
for(int i=0; i<_tanks.size(); i++) {
Tank* t = (Tank*)_tanks.get(i);
t->fill = frac * t->cap;
_model.getBody()->setMass(t->handle, t->cap * frac);
}
}
float Airplane::getDragCoefficient()
{
return _dragFactor;
}
float Airplane::getLiftRatio()
{
return _liftRatio;
}
float Airplane::getCruiseAoA()
{
return _cruiseAoA;
}
float Airplane::getTailIncidence()
{
return _tailIncidence;
}
const char* Airplane::getFailureMsg()
{
return _failureMsg;
}
int Airplane::getSolutionIterations()
{
return _solutionIterations;
}
void Airplane::setupState(float aoa, float speed, float gla, State* s)
void Airplane::setupState(const float aoa, const float speed, const float gla, State* s)
{
float cosAoA = Math::cos(aoa);
float sinAoA = Math::sin(aoa);
@ -444,10 +315,10 @@ void Airplane::setupState(float aoa, float speed, float gla, State* s)
s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0;
s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
//? what is gla? v[1]=y, v[2]=z? should sin go to v2 instead v1?
s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
int i;
for(i=0; i<3; i++)
for(int i=0; i<3; i++)
s->pos[i] = s->rot[i] = s->acc[i] = s->racc[i] = 0;
// Put us 1m above the origin, or else the gravity computation in
@ -455,39 +326,59 @@ void Airplane::setupState(float aoa, float speed, float gla, State* s)
s->pos[2] = 1;
}
/**
* @brief add contact point for crash detection
* used to add wingtips and fuselage nose and tail
*
* @param pos ...
* @return void
*/
void Airplane::addContactPoint(float* pos)
{
ContactRec* c = new ContactRec;
c->gear = 0;
c->p[0] = pos[0];
c->p[1] = pos[1];
c->p[2] = pos[2];
Math::set3(pos, c->p);
_contacts.add(c);
}
float Airplane::compileWing(Wing* w)
{
// The tip of the wing is a contact point
float tip[3];
w->getTip(tip);
addContactPoint(tip);
if(w->isMirrored()) {
tip[1] *= -1;
addContactPoint(tip);
}
// Make sure it's initialized. The surfaces will pop out with
// total drag coefficients equal to their areas, which is what we
// want.
w->compile();
float wgt = 0;
int i;
for(i=0; i<w->numSurfaces(); i++) {
Surface* s = (Surface*)w->getSurface(i);
// The tip of the wing is a contact point
float tip[3];
// need compile() before getTip()!
w->getTip(tip);
addContactPoint(tip);
if(w->isMirrored()) {
tip[1] *= -1;
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());
}
float td = s->getTotalDrag();
s->setTotalDrag(td);
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);
@ -495,8 +386,18 @@ float Airplane::compileWing(Wing* w)
mass = mass * Math::sqrt(mass);
float pos[3];
s->getPosition(pos);
_model.getBody()->addMass(mass, 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);
}
return wgt;
}
@ -547,7 +448,7 @@ float Airplane::compileFuselage(Fuselage* f)
// _Mass_ weighting goes as surface area^(3/2)
float mass = scale*segWgt * Math::sqrt(scale*segWgt);
_model.getBody()->addMass(mass, pos);
_model.getBody()->addMass(mass, pos, true);
wgt += mass;
// Make a Surface too
@ -635,6 +536,13 @@ void Airplane::compileGear(GearRec* gr)
_surfs.add(s);
}
/**
* @brief add "fake gear" per contact point
*
*
* @return void
*/
void Airplane::compileContactPoints()
{
// Figure it will compress by 20cm
@ -648,8 +556,7 @@ void Airplane::compileContactPoints()
float spring = (1/DIST) * 9.8f * 10.0f * mass;
float damp = 2 * Math::sqrt(spring * mass);
int i;
for(i=0; i<_contacts.size(); i++) {
for(int i=0; i<_contacts.size(); i++) {
ContactRec* c = (ContactRec*)_contacts.get(i);
Gear* g = new Gear();
@ -674,6 +581,8 @@ void Airplane::compile()
{
RigidBody* body = _model.getBody();
int firstMass = body->numMasses();
SGPropertyNode_ptr baseN = fgGetNode("/fdm/yasim/model/wings", true);
SGPropertyNode_ptr n;
// Generate the point masses for the plane. Just use unitless
// numbers for a first pass, then go back through and rescale to
@ -682,13 +591,21 @@ void Airplane::compile()
// The Wing objects
if (_wing)
{
if (baseN != 0) _wingsN = baseN->getChild("wing", 0, true);
aeroWgt += compileWing(_wing);
}
if (_tail)
{
if (baseN != 0) _wingsN = baseN->getChild("tail", 0, true);
aeroWgt += compileWing(_tail);
}
int i;
for(i=0; i<_vstabs.size(); i++)
aeroWgt += compileWing((Wing*)_vstabs.get(i));
{
if (baseN != 0) _wingsN = baseN->getChild("stab", i, true);
aeroWgt += compileWing((Wing*)_vstabs.get(i));
}
// The fuselage(s)
for(i=0; i<_fuselages.size(); i++)
@ -707,7 +624,7 @@ void Airplane::compile()
// Add the thruster masses
for(i=0; i<_thrusters.size(); i++) {
ThrustRec* t = (ThrustRec*)_thrusters.get(i);
body->addMass(t->mass, t->cg);
body->addMass(t->mass, t->cg, true);
}
// Add the tanks, empty for now.
@ -733,10 +650,23 @@ void Airplane::compile()
}
// Ground effect
// If a double tapered wing is modelled with wing and mstab, wing must
// be outboard to get correct wingspan.
if(_wing) {
float gepos[3];
float gespan = 0;
gespan = _wing->getGroundEffect(gepos);
gespan = _wing->getSpan();
_wing->getBase(gepos);
if(!isVersionOrNewer( Version::YASIM_VERSION_2017_2 )) {
//old code
//float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
//span = 2*(span + Math::abs(_base[2]));
gespan -= 2*gepos[1]; // cut away base (y-distance)
gespan += 2*Math::abs(gepos[2]); // add (wrong) z-distance
}
if (baseN != 0)
baseN->getChild("wing", 0)->getNode("gnd-eff-span", true)->setFloatValue(gespan);
// where does the hard coded factor 0.15 come from?
_model.setGroundEffect(gepos, gespan, 0.15f);
}
@ -822,11 +752,11 @@ void Airplane::initEngines()
void Airplane::stabilizeThrust()
{
int i;
for(i=0; i<_thrusters.size(); i++)
for(int i=0; i<_thrusters.size(); i++)
_model.getThruster(i)->stabilize();
}
/// Setup weights for cruise or approach during solve.
void Airplane::setupWeights(bool isApproach)
{
int i;
@ -839,6 +769,18 @@ void Airplane::setupWeights(bool isApproach)
}
}
/// load values for controls as defined in cruise configuration
void Airplane::loadCruiseControls()
{
_controls.reset();
for(int i=0; i<_cruiseControls.size(); i++) {
Control* c = (Control*)_cruiseControls.get(i);
_controls.setInput(c->control, c->val);
}
_controls.applyControls();
}
/// Helper for solve()
void Airplane::runCruise()
{
setupState(_cruiseAoA, _cruiseSpeed,_cruiseGlideAngle, &_cruiseState);
@ -847,14 +789,8 @@ void Airplane::runCruise()
Atmosphere::calcStdDensity(_cruiseP, _cruiseT));
// The control configuration
_controls.reset();
int i;
for(i=0; i<_cruiseControls.size(); i++) {
Control* c = (Control*)_cruiseControls.get(i);
_controls.setInput(c->control, c->val);
}
_controls.applyControls(1000000); // Huge dt value
loadCruiseControls();
// The local wind
float wind[3];
Math::mul3(-1, _cruiseState.v, wind);
@ -865,7 +801,7 @@ void Airplane::runCruise()
// Set up the thruster parameters and iterate until the thrust
// stabilizes.
for(i=0; i<_thrusters.size(); i++) {
for(int i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
t->setAir(_cruiseP, _cruiseT,
@ -882,6 +818,18 @@ void Airplane::runCruise()
_model.calcForces(&_cruiseState);
}
/// load values for controls as defined in approach configuration
void Airplane::loadApproachControls()
{
_controls.reset();
for(int i=0; i<_approachControls.size(); i++) {
Control* c = (Control*)_approachControls.get(i);
_controls.setInput(c->control, c->val);
}
_controls.applyControls();
}
/// Helper for solve()
void Airplane::runApproach()
{
setupState(_approachAoA, _approachSpeed,_approachGlideAngle, &_approachState);
@ -890,14 +838,8 @@ void Airplane::runApproach()
Atmosphere::calcStdDensity(_approachP, _approachT));
// The control configuration
_controls.reset();
int i;
for(i=0; i<_approachControls.size(); i++) {
Control* c = (Control*)_approachControls.get(i);
_controls.setInput(c->control, c->val);
}
_controls.applyControls(1000000);
loadApproachControls();
// The local wind
float wind[3];
Math::mul3(-1, _approachState.v, wind);
@ -909,7 +851,7 @@ void Airplane::runApproach()
// Run the thrusters until they get to a stable setting. FIXME:
// this is lots of wasted work.
for(i=0; i<_thrusters.size(); i++) {
for(int i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
t->setAir(_approachP, _approachT,
@ -926,6 +868,7 @@ void Airplane::runApproach()
_model.calcForces(&_approachState);
}
/// Used only in Airplane::solve() and solveHelicopter(), not at runtime
void Airplane::applyDragFactor(float factor)
{
float applied = Math::pow(factor, SOLVE_TWEAK);
@ -971,6 +914,7 @@ void Airplane::applyDragFactor(float factor)
}
}
/// Used only in Airplane::solve() and solveHelicopter(), not at runtime
void Airplane::applyLiftRatio(float factor)
{
float applied = Math::pow(factor, SOLVE_TWEAK);
@ -986,13 +930,7 @@ void Airplane::applyLiftRatio(float factor)
}
}
float Airplane::clamp(float val, float min, float max)
{
if(val < min) return min;
if(val > max) return max;
return val;
}
/// Helper for solve()
float Airplane::normFactor(float f)
{
if(f < 0) f = -f;
@ -1102,8 +1040,8 @@ void Airplane::solve()
_cruiseAoA += SOLVE_TWEAK*aoaDelta;
_tailIncidence += SOLVE_TWEAK*tailDelta;
_cruiseAoA = clamp(_cruiseAoA, -0.175f, 0.175f);
_tailIncidence = clamp(_tailIncidence, -0.175f, 0.175f);
_cruiseAoA = Math::clamp(_cruiseAoA, -0.175f, 0.175f);
_tailIncidence = Math::clamp(_tailIncidence, -0.175f, 0.175f);
if(abs(xforce/_cruiseWeight) < STHRESH*0.0001 &&
abs(alift/_approachWeight) < STHRESH*0.0001 &&

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

@ -7,6 +7,7 @@
#include "Rotor.hpp"
#include "Vector.hpp"
#include "Version.hpp"
#include <simgear/props/props.hxx>
namespace yasim {
@ -16,7 +17,9 @@ class Launchbar;
class Thruster;
class Hitch;
/// The Airplane class ties together the different components
class Airplane : public Version {
SGPropertyNode_ptr _wingsN;
public:
Airplane();
~Airplane();
@ -24,30 +27,30 @@ public:
void iterate(float dt);
void calcFuelWeights();
ControlMap* getControlMap();
Model* getModel();
ControlMap* getControlMap() { return &_controls; }
Model* getModel() { return &_model; }
void setPilotPos(float* pos);
void getPilotPos(float* out);
void setPilotPos(float* pos) { Math::set3(pos, _pilotPos); }
void getPilotPos(float* out) { Math::set3(_pilotPos, out); }
void getPilotAccel(float* out);
void setWeight(float weight);
void setEmptyWeight(float weight) { _emptyWeight = weight; }
void setWing(Wing* wing);
void setTail(Wing* tail);
void addVStab(Wing* vstab);
void setWing(Wing* wing) { _wing = wing; }
void setTail(Wing* tail) { _tail = tail; }
void addVStab(Wing* vstab) { _vstabs.add(vstab); }
void addFuselage(float* front, float* back, float width,
float taper=1, float mid=0.5,
float cx=1, float cy=1, float cz=1, float idrag=1);
int addTank(float* pos, float cap, float fuelDensity);
void addGear(Gear* g);
void addHook(Hook* h);
void addLaunchbar(Launchbar* l);
void addHook(Hook* h) { _model.addHook(h); }
void addLaunchbar(Launchbar* l) { _model.addLaunchbar(l); }
void addThruster(Thruster* t, float mass, float* cg);
void addBallast(float* pos, float mass);
void addHitch(Hitch* h);
void addHitch(Hitch* h) { _model.addHitch(h); }
int addWeight(float* pos, float size);
void setWeight(int handle, float mass);
@ -61,40 +64,48 @@ public:
void addSolutionWeight(bool approach, int idx, float wgt);
int numGear();
Gear* getGear(int g);
Hook* getHook();
int numGear() { return _gears.size(); }
Gear* getGear(int g) { return ((GearRec*)_gears.get(g))->gear; }
Hook* getHook() { return _model.getHook(); }
int numHitches() { return _hitches.size(); }
Hitch* getHitch(int h);
Rotorgear* getRotorgear();
Launchbar* getLaunchbar();
Rotorgear* getRotorgear() { return _model.getRotorgear(); }
Launchbar* getLaunchbar() { return _model.getLaunchbar(); }
int numThrusters() { return _thrusters.size(); }
Thruster* getThruster(int n) {
return ((ThrustRec*)_thrusters.get(n))->thruster; }
int numTanks();
int numTanks() { return _tanks.size(); }
void setFuelFraction(float frac); // 0-1, total amount of fuel
float getFuel(int tank); // in kg!
float setFuel(int tank, float fuel); // in kg!
float getFuelDensity(int tank); // kg/m^3
float getTankCapacity(int tank);
// get fuel in kg
float getFuel(int tank) { return ((Tank*)_tanks.get(tank))->fill; }
// set fuel in kg
float setFuel(int tank, float fuel) { return ((Tank*)_tanks.get(tank))->fill = fuel; }
// get fuel density in kg/m^3
float getFuelDensity(int tank) { return ((Tank*)_tanks.get(tank))->density; }
float getTankCapacity(int tank) { return ((Tank*)_tanks.get(tank))->cap; }
void compile(); // generate point masses & such, then solve
void initEngines();
void stabilizeThrust();
// Solution output values
int getSolutionIterations();
float getDragCoefficient();
float getLiftRatio();
float getCruiseAoA();
float getTailIncidence();
int getSolutionIterations() { return _solutionIterations; }
float getDragCoefficient() { return _dragFactor; }
float getLiftRatio() { return _liftRatio; }
float getCruiseAoA() { return _cruiseAoA; }
float getTailIncidence() { return _tailIncidence; }
float getApproachElevator() { return _approachElevator.val; }
const char* getFailureMsg();
static void setupState(float aoa, float speed, float gla, State* s); // utility
const char* getFailureMsg() { return _failureMsg; }
static void setupState(const float aoa, const float speed, const float gla, yasim::State* s); // utility
void loadApproachControls();
void loadCruiseControls();
float getCGMinX() { return _cgMinX; }
float getCGMaxX() { return _cgMaxX; }
private:
struct Tank { float pos[3]; float cap; float fill;
float density; int handle; };
@ -119,7 +130,6 @@ private:
void compileGear(GearRec* gr);
void applyDragFactor(float factor);
void applyLiftRatio(float factor);
float clamp(float val, float min, float max);
void addContactPoint(float* pos);
void compileContactPoints();
float normFactor(float f);
@ -175,6 +185,10 @@ private:
float _tailIncidence;
Control _approachElevator;
const char* _failureMsg;
// hard limits for cg from gear positions
float _cgMaxX;
float _cgMinX;
};
}; // namespace yasim

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

@ -21,25 +21,29 @@ namespace yasim {
ControlMap::~ControlMap()
{
int i;
for(i=0; i<_inputs.size(); i++) {
Vector* v = (Vector*)_inputs.get(i);
int j;
for(j=0; j<v->size(); j++)
int i;
for(i=0; i<_inputs.size(); i++) {
Vector* v = (Vector*)_inputs.get(i);
int j;
for(j=0; j<v->size(); j++)
delete (MapRec*)v->get(j);
delete v;
}
delete v;
}
for(i=0; i<_outputs.size(); i++)
delete (OutRec*)_outputs.get(i);
}
int ControlMap::newInput()
{
Vector* v = new Vector();
return _inputs.add(v);
for(i=0; i<_outputs.size(); i++)
delete (OutRec*)_outputs.get(i);
for(i=0; i<_properties.size(); i++) {
PropHandle* p = (PropHandle*)_properties.get(i);
delete[] p->name;
delete p;
}
}
/**
input : index to _inputs
type: identifier (see enum OutputType)
*/
void ControlMap::addMapping(int input, int type, void* object, int options,
float src0, float src1, float dst0, float dst1)
{
@ -55,6 +59,10 @@ void ControlMap::addMapping(int input, int type, void* object, int options,
m->dst1 = dst1;
}
/**
input : index to _inputs
type: identifier (see enum OutputType)
*/
void ControlMap::addMapping(int input, int type, void* object, int options)
{
// See if the output object already exists
@ -210,12 +218,12 @@ void ControlMap::applyControls(float dt)
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)->setSlat(lval); break;
case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); 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)->setFlap1(lval, rval); break;
case FLAP1: ((Wing*)obj)->setFlap1Pos(lval, rval); break;
case FLAP1EFFECTIVENESS: ((Wing*)obj)->setFlap1Effectiveness(lval); break;
case SPOILER: ((Wing*)obj)->setSpoiler(lval, rval); 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;
@ -278,4 +286,45 @@ 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)
{
for(int i=0; i < _properties.size(); i++) {
PropHandle* p = (PropHandle*)_properties.get(i);
if(eq(p->name, name))
return p->handle;
}
// create new
PropHandle* p = new PropHandle();
p->name = dup(name);
fgGetNode(p->name, true);
Vector* v = new Vector();
p->handle = _inputs.add(v);
_properties.add(p);
return p->handle;
}
} // namespace yasim

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

@ -1,6 +1,7 @@
#ifndef _CONTROL_MAP_HPP
#define _CONTROL_MAP_HPP
#include <simgear/props/props.hxx>
#include "Vector.hpp"
namespace yasim {
@ -26,9 +27,7 @@ public:
OPT_INVERT = 0x02,
OPT_SQUARE = 0x04 };
// Returns a new, not-yet-used "input handle" for addMapping and
// setInput. This typically corresponds to one user axis.
int newInput();
struct PropHandle { char* name; int handle; };
// Adds a mapping to between input handle and a particular setting
// on an output object. The value of output MUST match the type
@ -45,12 +44,13 @@ public:
// setInput() invokations.
void reset();
// Sets the specified input (as returned by newInput) to the
// Sets the specified input (as returned by propertyHandle) to the
// specified value.
void setInput(int input, float value);
void setInput(int propHandle, float value);
// Calculates and applies the settings received since the last reset().
void applyControls(float dt);
/// Calculates and applies the settings received since the last reset().
/// dt defaults to a large value used at solve time.
void applyControls(float dt=1e6);
// Returns the input/output range appropriate for the given
// control. Ailerons go from -1 to 1, while throttles are never
@ -72,6 +72,15 @@ public:
float getOutput(int handle);
float getOutputR(int handle);
// register property name, return handle
int propertyHandle(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; };
@ -84,6 +93,8 @@ private:
// An unordered list of output settings.
Vector _outputs;
// control properties
Vector _properties;
};
}; // namespace yasim

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

@ -21,6 +21,7 @@
#include "Rotor.hpp"
#include "Rotorpart.hpp"
#include "Hitch.hpp"
#include "Surface.hpp"
#include "FGFDM.hpp"
@ -28,6 +29,7 @@ namespace yasim {
// Some conversion factors
static const float KTS2MPS = 0.514444444444;
static const float KMH2MPS = 1/3.6;
static const float FT2M = 0.3048;
static const float DEG2RAD = 0.0174532925199;
static const float RPM2RAD = 0.10471975512;
@ -59,7 +61,7 @@ FGFDM::FGFDM()
// Map /controls/flight/elevator to the approach elevator control. This
// should probably be settable, but there are very few aircraft
// who trim their approaches using things other than elevator.
_airplane.setElevatorControl(parseAxis("/controls/flight/elevator-trim"));
_airplane.setElevatorControl(_airplane.getControlMap()->propertyHandle("/controls/flight/elevator-trim"));
// FIXME: read seed from somewhere?
int seed = 0;
@ -68,12 +70,6 @@ FGFDM::FGFDM()
FGFDM::~FGFDM()
{
for(int i=0; i<_axes.size(); i++) {
AxisRec* a = (AxisRec*)_axes.get(i);
delete[] a->name;
delete a;
}
for(int i=0; i<_thrusters.size(); i++) {
EngRec* er = (EngRec*)_thrusters.get(i);
delete[] er->prefix;
@ -113,7 +109,7 @@ void FGFDM::iterate(float dt)
_airplane.setFuel(i, LBS2KG * _tank_level_lbs[i]->getFloatValue());
}
_airplane.calcFuelWeights();
setOutputProperties(dt);
}
@ -124,19 +120,39 @@ Airplane* FGFDM::getAirplane()
void FGFDM::init()
{
//reset id generator, needed on simulator reset/re-init
Surface::resetIDgen();
_turb_magnitude_norm = fgGetNode("/environment/turbulence/magnitude-norm", true);
_turb_rate_hz = fgGetNode("/environment/turbulence/rate-hz", true);
SGPropertyNode_ptr yasimNode = fgGetNode("/fdm/yasim", true);
_gross_weight_lbs = yasimNode->getNode("gross-weight-lbs", true);
_yasimN = fgGetNode("/fdm/yasim", true);
_gross_weight_lbs = _yasimN->getNode("gross-weight-lbs", true);
// alias to older name
fgGetNode("/yasim/gross-weight-lbs", true)->alias(_gross_weight_lbs);
_cg_x = yasimNode->getNode("cg-x-m", true);
_cg_y = yasimNode->getNode("cg-y-m", true);
_cg_z = yasimNode->getNode("cg-z-m", true);
// write some compile time information to property tree
_yasimN->getNode("config-version",true)->setIntValue(_airplane.getVersion());
_yasimN->getNode("model/cg-x-min",true)->setFloatValue(_airplane.getCGMinX());
_yasimN->getNode("model/cg-x-max",true)->setFloatValue(_airplane.getCGMaxX());
// prepare nodes for write at runtime
_cg_x = _yasimN->getNode("cg-x-m", true);
_cg_y = _yasimN->getNode("cg-y-m", true);
_cg_z = _yasimN->getNode("cg-z-m", true);
_vxN = _yasimN->getNode("velocities/v-x", true);
_vyN = _yasimN->getNode("velocities/v-y", true);
_vzN = _yasimN->getNode("velocities/v-z", true);
_vrxN = _yasimN->getNode("velocities/vrot-x", true);
_vryN = _yasimN->getNode("velocities/vrot-y", true);
_vrzN = _yasimN->getNode("velocities/vrot-z", true);
_axN = _yasimN->getNode("accelerations/a-x", true);
_ayN = _yasimN->getNode("accelerations/a-y", true);
_azN = _yasimN->getNode("accelerations/a-z", true);
_arxN = _yasimN->getNode("accelerations/arot-x", true);
_aryN = _yasimN->getNode("accelerations/arot-y", true);
_arzN = _yasimN->getNode("accelerations/arot-z", true);
// Allows the user to start with something other than full fuel
_airplane.setFuelFraction(fgGetFloat("/sim/fuel-fraction", 1));
@ -225,32 +241,64 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
XMLAttributes* a = (XMLAttributes*)&atts;
float v[3];
char buf[64];
float f = 0;
if(eq(name, "airplane")) {
_airplane.setWeight(attrf(a, "mass") * LBS2KG);
if(a->hasAttribute("version")) {
_airplane.setVersion( a->getValue("version") );
}
if( !_airplane.isVersionOrNewer( Version::YASIM_VERSION_CURRENT ) ) {
SG_LOG(SG_FLIGHT, SG_DEV_ALERT, "This aircraft does not use the latest yasim configuration version.");
}
if(a->hasAttribute("mass")) { f = attrf(a, "mass") * LBS2KG; }
else if (a->hasAttribute("mass-lbs")) { f = attrf(a, "mass-lbs") * LBS2KG; }
else if (a->hasAttribute("mass-kg")) { f = attrf(a, "mass-kg"); }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, airplane needs one of {mass-lbs, mass-kg}");
exit(1);
}
_airplane.setEmptyWeight(f);
if(a->hasAttribute("version")) {
_airplane.setVersion( a->getValue("version") );
}
if( !_airplane.isVersionOrNewer( Version::YASIM_VERSION_CURRENT ) ) {
SG_LOG(SG_FLIGHT, SG_DEV_ALERT, "This aircraft does not use the latest yasim configuration version.");
}
} else if(eq(name, "approach")) {
float spd = attrf(a, "speed") * KTS2MPS;
float alt = attrf(a, "alt", 0) * FT2M;
float aoa = attrf(a, "aoa", 0) * DEG2RAD;
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2),gla);
_cruiseCurr = false;
float spd, alt = 0;
if (a->hasAttribute("speed")) { spd = attrf(a, "speed") * KTS2MPS; }
else if (a->hasAttribute("speed-kt")) { spd = attrf(a, "speed-kt") * KTS2MPS; }
else if (a->hasAttribute("speed-kmh")) { spd = attrf(a, "speed-kmh") * KMH2MPS; }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, approach needs one of {speed-kt, speed-kmh}");
exit(1);
}
if (a->hasAttribute("alt")) { alt = attrf(a, "alt") * FT2M; }
else if (a->hasAttribute("alt-ft")) { alt = attrf(a, "alt-ft") * FT2M; }
else if (a->hasAttribute("alt-m")) { alt = attrf(a, "alt-m"); }
float aoa = attrf(a, "aoa", 0) * DEG2RAD;
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setApproach(spd, alt, aoa, attrf(a, "fuel", 0.2), gla);
_cruiseCurr = false;
} else if(eq(name, "cruise")) {
float spd = attrf(a, "speed") * KTS2MPS;
float alt = attrf(a, "alt") * FT2M;
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
_cruiseCurr = true;
float spd, alt = 0;
if (a->hasAttribute("speed")) { spd = attrf(a, "speed") * KTS2MPS; }
else if (a->hasAttribute("speed-kt")) { spd = attrf(a, "speed-kt") * KTS2MPS; }
else if (a->hasAttribute("speed-kmh")) { spd = attrf(a, "speed-kmh") * KMH2MPS; }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, approach needs one of {speed-kt, speed-kmh}");
exit(1);
}
if (a->hasAttribute("alt")) { alt = attrf(a, "alt") * FT2M; }
else if (a->hasAttribute("alt-ft")) { alt = attrf(a, "alt-ft") * FT2M; }
else if (a->hasAttribute("alt-m")) { alt = attrf(a, "alt-m"); }
float gla = attrf(a, "glide-angle", 0) * DEG2RAD;
_airplane.setCruise(spd, alt, attrf(a, "fuel", 0.5),gla);
_cruiseCurr = true;
} else if(eq(name, "solve-weight")) {
int idx = attri(a, "idx");
float wgt = attrf(a, "weight") * LBS2KG;
_airplane.addSolutionWeight(!_cruiseCurr, idx, wgt);
if(a->hasAttribute("weight")) { f = attrf(a, "weight") * LBS2KG; }
else if(a->hasAttribute("weight-lbs")) { f = attrf(a, "weight-lbs") * LBS2KG; }
else if(a->hasAttribute("weight-kg")) { f = attrf(a, "weight-kg"); }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, solve-weight needs one of {weight-lbs, weight-kg}");
exit(1);
}
_airplane.addSolutionWeight(!_cruiseCurr, idx, f);
} else if(eq(name, "cockpit")) {
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
@ -300,8 +348,15 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
float mass = attrf(a, "mass") * LBS2KG;
j->setMaxThrust(attrf(a, "thrust") * LBS2N,
float mass;
if(a->hasAttribute("mass")) { mass = attrf(a, "mass") * LBS2KG; }
else if(a->hasAttribute("mass-lbs")) { mass = attrf(a, "mass-lbs") * LBS2KG; }
else if(a->hasAttribute("mass-kg")) { mass = attrf(a, "mass-kg"); }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, jet needs one of {mass-lbs, mass-kg}");
exit(1);
}
j->setMaxThrust(attrf(a, "thrust") * LBS2N,
attrf(a, "afterburner", 0) * LBS2N);
j->setVectorAngle(attrf(a, "rotate", 0) * DEG2RAD);
j->setReverseThrust(attrf(a, "reverse", 0.2));
@ -320,7 +375,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
j->setVMax(attrf(a, "exhaust-speed") * KTS2MPS);
if(a->hasAttribute("spool-time"))
j->setSpooling(attrf(a, "spool-time"));
j->setPosition(v);
_airplane.addThruster(j, mass, v);
sprintf(buf, "/engines/engine[%d]", _nextEngine++);
@ -459,21 +514,36 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
float cy = attrf(a, "cy", 1);
float cz = attrf(a, "cz", 1);
float idrag = attrf(a, "idrag", 1);
_airplane.addFuselage(v, b, attrf(a, "width"), taper, mid,
_airplane.addFuselage(v, b, attrf(a, "width"), taper, mid,
cx, cy, cz, idrag);
} else if(eq(name, "tank")) {
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
float density = 6.0; // gasoline, in lbs/gal
if(a->hasAttribute("jet")) density = 6.72;
if(a->hasAttribute("jet")) density = 6.72;
density *= LBS2KG*CM2GALS;
_airplane.addTank(v, attrf(a, "capacity") * LBS2KG, density);
float capacity = 0;
if(a->hasAttribute("capacity")) { capacity = attrf(a, "capacity") * LBS2KG; }
else if(a->hasAttribute("capacity-lbs")) { capacity = attrf(a, "capacity-lbs") * LBS2KG; }
else if(a->hasAttribute("capacity-kg")) { capacity = attrf(a, "capacity-kg"); }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, tank needs one of {capacity-lbs, capacity-kg}");
exit(1);
}
_airplane.addTank(v, capacity, density);
} else if(eq(name, "ballast")) {
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
_airplane.addBallast(v, attrf(a, "mass") * LBS2KG);
if(a->hasAttribute("mass")) { f = attrf(a, "mass") * LBS2KG; }
else if (a->hasAttribute("mass-lbs")) { f = attrf(a, "mass-lbs") * LBS2KG; }
else if (a->hasAttribute("mass-kg")) { f = attrf(a, "mass-kg"); }
else {
SG_LOG(SG_FLIGHT,SG_ALERT,"YASim fatal: missing attribute, airplane needs one of {mass-lbs, mass-kg}");
exit(1);
}
_airplane.addBallast(v, f);
} else if(eq(name, "weight")) {
parseWeight(a);
} else if(eq(name, "stall")) {
@ -482,22 +552,22 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
w->setStallWidth(attrf(a, "width", 2) * DEG2RAD);
w->setStallPeak(attrf(a, "peak", 1.5));
} else if(eq(name, "flap0")) {
((Wing*)_currObj)->setFlap0(attrf(a, "start"), attrf(a, "end"),
((Wing*)_currObj)->setFlap0Params(attrf(a, "start"), attrf(a, "end"),
attrf(a, "lift"), attrf(a, "drag"));
} else if(eq(name, "flap1")) {
((Wing*)_currObj)->setFlap1(attrf(a, "start"), attrf(a, "end"),
((Wing*)_currObj)->setFlap1Params(attrf(a, "start"), attrf(a, "end"),
attrf(a, "lift"), attrf(a, "drag"));
} else if(eq(name, "slat")) {
((Wing*)_currObj)->setSlat(attrf(a, "start"), attrf(a, "end"),
((Wing*)_currObj)->setSlatParams(attrf(a, "start"), attrf(a, "end"),
attrf(a, "aoa"), attrf(a, "drag"));
} else if(eq(name, "spoiler")) {
((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"),
((Wing*)_currObj)->setSpoilerParams(attrf(a, "start"), attrf(a, "end"),
attrf(a, "lift"), attrf(a, "drag"));
/* } else if(eq(name, "collective")) {
((Rotor*)_currObj)->setcollective(attrf(a, "min"), attrf(a, "max"));
} else if(eq(name, "cyclic")) {
((Rotor*)_currObj)->setcyclic(attrf(a, "ail"), attrf(a, "ele"));
*/
*/
} else if(eq(name, "actionpt")) {
v[0] = attrf(a, "x");
v[1] = attrf(a, "y");
@ -508,33 +578,32 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
v[1] = attrf(a, "y");
v[2] = attrf(a, "z");
((Thruster*)_currObj)->setDirection(v);
} else if(eq(name, "control-setting")) {
// A cruise or approach control setting
const char* axis = a->getValue("axis");
float value = attrf(a, "value", 0);
if(_cruiseCurr)
_airplane.addCruiseControl(parseAxis(axis), value);
else
_airplane.addApproachControl(parseAxis(axis), value);
} else if(eq(name, "control-input")) {
// A mapping of input property to a control
int axis = parseAxis(a->getValue("axis"));
int control = parseOutput(a->getValue("control"));
int opt = 0;
opt |= a->hasAttribute("split") ? ControlMap::OPT_SPLIT : 0;
opt |= a->hasAttribute("invert") ? ControlMap::OPT_INVERT : 0;
opt |= a->hasAttribute("square") ? ControlMap::OPT_SQUARE : 0;
ControlMap* cm = _airplane.getControlMap();
if(a->hasAttribute("src0")) {
cm->addMapping(axis, control, _currObj, opt,
attrf(a, "src0"), attrf(a, "src1"),
} else if(eq(name, "control-setting")) {
// A cruise or approach control setting
const char* axis = a->getValue("axis");
float value = attrf(a, "value", 0);
ControlMap* cm = _airplane.getControlMap();
if(_cruiseCurr)
_airplane.addCruiseControl(cm->propertyHandle(axis), value);
else
_airplane.addApproachControl(cm->propertyHandle(axis), value);
} else if(eq(name, "control-input")) {
ControlMap* cm = _airplane.getControlMap();
// A mapping of input property to a control
int axis = cm->propertyHandle(a->getValue("axis"));
int control = parseOutput(a->getValue("control"));
int opt = 0;
opt |= a->hasAttribute("split") ? ControlMap::OPT_SPLIT : 0;
opt |= a->hasAttribute("invert") ? ControlMap::OPT_INVERT : 0;
opt |= a->hasAttribute("square") ? ControlMap::OPT_SQUARE : 0;
if(a->hasAttribute("src0")) {
cm->addMapping(axis, control, _currObj, opt,
attrf(a, "src0"), attrf(a, "src1"),
attrf(a, "dst0"), attrf(a, "dst1"));
} else {
cm->addMapping(axis, control, _currObj, opt);
}
} else if(eq(name, "control-output")) {
} else {
cm->addMapping(axis, control, _currObj, opt);
}
} else if(eq(name, "control-output")) {
// A property output for a control on the current object
ControlMap* cm = _airplane.getControlMap();
int type = parseOutput(a->getValue("control"));
@ -555,7 +624,7 @@ void FGFDM::startElement(const char* name, const XMLAttributes &atts)
int type = parseOutput(a->getValue("control"));
int handle = cm->getOutputHandle(_currObj, type);
float time = attrf(a, "transition-time", 0);
cm->setTransitionTime(handle, time);
} else {
SG_LOG(SG_FLIGHT,SG_ALERT,"Unexpected tag '"
@ -575,10 +644,10 @@ void FGFDM::getExternalInput(float dt)
ControlMap* cm = _airplane.getControlMap();
cm->reset();
for(int i=0; i<_axes.size(); i++) {
AxisRec* a = (AxisRec*)_axes.get(i);
float val = fgGetFloat(a->name, 0);
cm->setInput(a->handle, val);
for(int i=0; i < cm->numProperties(); i++) {
ControlMap::PropHandle *p = cm->getProperty(i);
float val = fgGetFloat(p->name, 0);
cm->setInput(p->handle, val);
}
cm->applyControls(dt);
@ -623,6 +692,26 @@ void FGFDM::setOutputProperties(float dt)
_cg_y->setFloatValue(cg[1]);
_cg_z->setFloatValue(cg[2]);
State* s = _airplane.getModel()->getState();
float v[3], acc[3], rot[3], racc[3];
Math::vmul33(s->orient, s->v, v);
Math::vmul33(s->orient, s->acc, acc);
Math::vmul33(s->orient, s->rot, rot);
Math::vmul33(s->orient, s->racc, racc);
_vxN->setFloatValue(v[0]);
_vyN->setFloatValue(v[1]);
_vzN->setFloatValue(v[2]);
_vrxN->setFloatValue(rot[0]);
_vryN->setFloatValue(rot[1]);
_vrzN->setFloatValue(rot[2]);
_axN->setFloatValue(acc[0]);
_ayN->setFloatValue(acc[1]);
_azN->setFloatValue(acc[2]);
_arxN->setFloatValue(racc[0]);
_aryN->setFloatValue(racc[1]);
_arzN->setFloatValue(racc[2]);
ControlMap* cm = _airplane.getControlMap();
for(int i=0; i<_controlProps.size(); i++) {
PropOut* p = (PropOut*)_controlProps.get(i);
@ -716,7 +805,7 @@ void FGFDM::setOutputProperties(float dt)
// cockpit code can scale them to the right values.
float pnorm = j->getPerfNorm();
moveprop(node, "oilp-norm", pnorm, dt/3); // 3s seek time
moveprop(node, "oilt-norm", pnorm, dt/30); // 30s
moveprop(node, "oilt-norm", pnorm, dt/30); // 30s
moveprop(node, "itt-norm", pnorm, dt/1); // 1s
}
}
@ -824,9 +913,9 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
w->setTiltCenterZ(attrf(a,"tiltcenterz",0.0));
w->setDownwashFactor(attrf(a, "downwashfactor", 1));
if(attrb(a,"ccw"))
w->setCcw(1);
w->setCcw(1);
if(attrb(a,"sharedflaphinge"))
w->setSharedFlapHinge(true);
w->setSharedFlapHinge(true);
if(a->hasAttribute("name"))
w->setName(a->getValue("name") );
@ -845,7 +934,7 @@ Rotor* FGFDM::parseRotor(XMLAttributes* a, const char* type)
w->setPowerAtPitch0(attrf(a, "poweratpitch-0", 300));
w->setPowerAtPitchB(attrf(a, "poweratpitch-b", 3000));
if(attrb(a,"notorque"))
w->setNotorque(1);
w->setNotorque(1);
#define p(x) if (a->hasAttribute(#x)) w->setParameter((char *)#x,attrf(a,#x) );
#define p2(x,y) if (a->hasAttribute(y)) w->setParameter((char *)#x,attrf(a,y) );
@ -893,7 +982,7 @@ void FGFDM::parsePistonEngine(XMLAttributes* a)
eng->setDisplacement(attrf(a, "displacement") * CIN2CM);
if(a->hasAttribute("compression"))
eng->setCompression(attrf(a, "compression"));
eng->setCompression(attrf(a, "compression"));
if(a->hasAttribute("min-throttle"))
eng->setMinThrottle(attrf(a, "min-throttle"));
@ -944,7 +1033,7 @@ void FGFDM::parsePropeller(XMLAttributes* a)
if(a->hasAttribute("displacement"))
eng->setDisplacement(attrf(a, "displacement") * CIN2CM);
if(a->hasAttribute("compression"))
eng->setCompression(attrf(a, "compression"));
eng->setCompression(attrf(a, "compression"));
if(a->hasAttribute("turbo-mul")) {
float mul = attrf(a, "turbo-mul");
float mp = attrf(a, "wastegate-mp", 1e6) * INHG2PA;
@ -1005,26 +1094,7 @@ void FGFDM::parsePropeller(XMLAttributes* a)
_currObj = thruster;
}
// Turns a string axis name into an integer for use by the
// ControlMap. Creates a new axis if this one hasn't been defined
// yet.
int FGFDM::parseAxis(const char* name)
{
for(int i=0; i<_axes.size(); i++) {
AxisRec* a = (AxisRec*)_axes.get(i);
if(eq(a->name, name))
return a->handle;
}
// Not there, make a new one.
AxisRec* a = new AxisRec();
a->name = dup(name);
fgGetNode( a->name, true ); // make sure the property name exists
a->handle = _airplane.getControlMap()->newInput();
_axes.add(a);
return a->handle;
}
/// map identifier (string) to int (enum in ControlMap)
int FGFDM::parseOutput(const char* name)
{
if(eq(name, "THROTTLE")) return ControlMap::THROTTLE;
@ -1061,7 +1131,7 @@ int FGFDM::parseOutput(const char* name)
if(eq(name, "TILTYAW")) return ControlMap::TILTYAW;
if(eq(name, "ROTORGEARENGINEON")) return ControlMap::ROTORENGINEON;
if(eq(name, "ROTORBRAKE")) return ControlMap::ROTORBRAKE;
if(eq(name, "ROTORENGINEMAXRELTORQUE"))
if(eq(name, "ROTORENGINEMAXRELTORQUE"))
return ControlMap::ROTORENGINEMAXRELTORQUE;
if(eq(name, "ROTORRELTARGET")) return ControlMap::ROTORRELTARGET;
if(eq(name, "ROTORBALANCE")) return ControlMap::ROTORBALANCE;
@ -1143,7 +1213,7 @@ float FGFDM::attrf(XMLAttributes* atts, const char* attr, float def)
{
const char* val = atts->getValue(attr);
if(val == 0) return def;
else return (float)atof(val);
else return (float)atof(val);
}
double FGFDM::attrd(XMLAttributes* atts, const char* attr)

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

@ -31,7 +31,6 @@ public:
float getVehicleRadius(void) const { return _vehicle_radius; }
private:
struct AxisRec { char* name; int handle; };
struct EngRec { char* prefix; Thruster* eng; };
struct WeightRec { char* prop; float size; int handle; };
struct PropOut { SGPropertyNode* prop; int handle, type; bool left;
@ -41,7 +40,6 @@ private:
Rotor* parseRotor(XMLAttributes* a, const char* name);
Wing* parseWing(XMLAttributes* a, const char* name, Version * version);
int parseAxis(const char* name);
int parseOutput(const char* name);
void parseWeight(XMLAttributes* a);
void parseTurbineEngine(XMLAttributes* a);
@ -64,10 +62,6 @@ private:
// Aerodynamic turbulence model
Turbulence* _turb;
// The list of "axes" that we expect to find as input. These are
// typically property names.
Vector _axes;
// Settable weights
Vector _weights;
@ -107,9 +101,23 @@ private:
SGPropertyNode_ptr _cg_x;
SGPropertyNode_ptr _cg_y;
SGPropertyNode_ptr _cg_z;
SGPropertyNode_ptr _yasimN;
std::vector<SGPropertyNode_ptr> _tank_level_lbs;
std::vector<ThrusterProps> _thrust_props;
std::vector<FuelProps> _fuel_props;
SGPropertyNode_ptr _vxN;
SGPropertyNode_ptr _vyN;
SGPropertyNode_ptr _vzN;
SGPropertyNode_ptr _vrxN;
SGPropertyNode_ptr _vryN;
SGPropertyNode_ptr _vrzN;
SGPropertyNode_ptr _axN;
SGPropertyNode_ptr _ayN;
SGPropertyNode_ptr _azN;
SGPropertyNode_ptr _arxN;
SGPropertyNode_ptr _aryN;
SGPropertyNode_ptr _arzN;
};
}; // namespace yasim

161
src/FDM/YASim/Gear.cpp
View file

@ -53,98 +53,6 @@ Gear::Gear()
_global_ground[3] = -1e3;
}
void Gear::setPosition(float* position)
{
int i;
for(i=0; i<3; i++) _pos[i] = position[i];
}
void Gear::setCompression(float* compression)
{
int i;
for(i=0; i<3; i++) _cmpr[i] = compression[i];
}
void Gear::setSpring(float spring)
{
_spring = spring;
}
void Gear::setDamping(float damping)
{
_damp = damping;
}
void Gear::setStaticFriction(float sfric)
{
_sfric = sfric;
}
void Gear::setDynamicFriction(float dfric)
{
_dfric = dfric;
}
void Gear::setBrake(float brake)
{
_brake = Math::clamp(brake, 0, 1);
}
void Gear::setRotation(float rotation)
{
_rot = rotation;
}
void Gear::setExtension(float extension)
{
_extension = Math::clamp(extension, 0, 1);
}
void Gear::setCastering(bool c)
{
_castering = c;
}
void Gear::setContactPoint(bool c)
{
_isContactPoint=c;
}
void Gear::setOnWater(bool c)
{
_onWater = c;
}
void Gear::setOnSolid(bool c)
{
_onSolid = c;
}
void Gear::setIgnoreWhileSolving(bool c)
{
_ignoreWhileSolving = c;
}
void Gear::setSpringFactorNotPlaning(float f)
{
_spring_factor_not_planing = f;
}
void Gear::setSpeedPlaning(float s)
{
_speed_planing = s;
}
void Gear::setReduceFrictionByExtension(float s)
{
_reduceFrictionByExtension = s;
}
void Gear::setInitialLoad(float l)
{
_initialLoad = l;
}
void Gear::setGlobalGround(double *global_ground, float* global_vel,
double globalX, double globalY,
const simgear::BVHMaterial *material)
@ -183,57 +91,9 @@ void Gear::setGlobalGround(double *global_ground, float* global_vel,
}
void Gear::getPosition(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _pos[i];
}
void Gear::getCompression(float* out)
{
int i;
for(i=0; i<3; i++) out[i] = _cmpr[i];
}
void Gear::getGlobalGround(double* global_ground)
{
int i;
for(i=0; i<4; i++) global_ground[i] = _global_ground[i];
}
float Gear::getSpring()
{
return _spring;
}
float Gear::getDamping()
{
return _damp;
}
float Gear::getStaticFriction()
{
return _sfric;
}
float Gear::getDynamicFriction()
{
return _dfric;
}
float Gear::getBrake()
{
return _brake;
}
float Gear::getRotation()
{
return _rot;
}
float Gear::getExtension()
{
return _extension;
for(int i=0; i<4; i++) global_ground[i] = _global_ground[i];
}
void Gear::getForce(float* force, float* contact)
@ -242,25 +102,6 @@ void Gear::getForce(float* force, float* contact)
Math::set3(_contact, contact);
}
float Gear::getWoW()
{
return _wow;
}
float Gear::getCompressFraction()
{
return _frac;
}
bool Gear::getCastering()
{
return _castering;
}
bool Gear::getGroundIsSolid()
{
return _ground_isSolid;
}
float Gear::getBumpAltitude()
{

68
src/FDM/YASim/Gear.hpp
View file

@ -1,5 +1,6 @@
#ifndef _GEAR_HPP
#define _GEAR_HPP
#include "Math.hpp"
namespace simgear {
class BVHMaterial;
@ -33,42 +34,43 @@ public:
Gear();
// Externally set values
void setPosition(float* position);
void setCompression(float* compression);
void setSpring(float spring);
void setDamping(float damping);
void setStaticFriction(float sfric);
void setDynamicFriction(float dfric);
void setBrake(float brake);
void setRotation(float rotation);
void setExtension(float extension);
void setCastering(bool castering);
void setOnWater(bool c);
void setOnSolid(bool c);
void setSpringFactorNotPlaning(float f);
void setSpeedPlaning(float s);
void setReduceFrictionByExtension(float s);
void setInitialLoad(float l);
void setIgnoreWhileSolving(bool c);
void setPosition(float* position) { Math::set3(position, _pos); }
void getPosition(float* out) { Math::set3(_pos, out); }
void setCompression(float* compression) { Math::set3(compression, _cmpr); }
void getCompression(float* out) { Math::set3(_cmpr, out); }
void setSpring(float spring) { _spring = spring; }
float getSpring() { return _spring; }
void setDamping(float damping) { _damp = damping; }
float getDamping() {return _damp; }
void setStaticFriction(float sfric) { _sfric = sfric; }
float getStaticFriction() { return _sfric; }
void setDynamicFriction(float dfric) { _dfric = dfric; }
float getDynamicFriction() { return _dfric; }
void setBrake(float brake) { _brake = Math::clamp(brake, 0, 1); }
float getBrake() { return _brake; }
void setRotation(float rotation) { _rot = rotation; }
float getRotation() { return _rot; }
void setExtension(float extension) { _extension = Math::clamp(extension, 0, 1); }
float getExtension() { return _extension; }
void setCastering(bool c) { _castering = c; }
bool getCastering() { return _castering; }
void setOnWater(bool c) { _onWater = c; }
void setOnSolid(bool c) { _onSolid = c; }
void setSpringFactorNotPlaning(float f) { _spring_factor_not_planing = f; }
void setSpeedPlaning(float s) { _speed_planing = s; }
void setReduceFrictionByExtension(float s) { _reduceFrictionByExtension = s; }
void setInitialLoad(float l) { _initialLoad = l; }
float getInitialLoad() {return _initialLoad; }
void setIgnoreWhileSolving(bool c) { _ignoreWhileSolving = c; }
void setGlobalGround(double* global_ground, float* global_vel,
double globalX, double globalY,
const simgear::BVHMaterial *material);
void getPosition(float* out);
void getCompression(float* out);
const simgear::BVHMaterial *material);
void getGlobalGround(double* global_ground);
float getSpring();
float getDamping();
float getStaticFriction();
float getDynamicFriction();
float getBrake();
float getRotation();
float getExtension();
float getInitialLoad() {return _initialLoad; }
bool getCastering();
float getCasterAngle() { return _casterAngle; }
float getRollSpeed() { return _rollSpeed; }
float getBumpAltitude();
bool getGroundIsSolid();
bool getGroundIsSolid() { return _ground_isSolid; }
float getGroundFrictionFactor() { return (float)_ground_frictionFactor; }
void integrate(float dt);
@ -81,12 +83,12 @@ public:
// Computed values: total force, weight-on-wheels (force normal to
// ground) and compression fraction.
void getForce(float* force, float* contact);
float getWoW();
float getCompressFraction();
float getWoW() { return _wow; }
float getCompressFraction() { return _frac; }
float getCompressDist() { return _compressDist; }
bool getSubmergable() {return (!_ground_isSolid)&&(!_isContactPoint); }
bool getIgnoreWhileSolving() {return _ignoreWhileSolving; }
void setContactPoint(bool c);
void setContactPoint(bool c) { _isContactPoint=c; }
private:
float calcFriction(float wgt, float v);

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

@ -44,17 +44,17 @@ public:
// Some 3D vector stuff. In all cases, it is permissible for the
// "out" vector to be the same as one of the inputs.
static inline void set3(float* v, float* out) {
static inline void set3(const float* v, float* out) {
out[0] = v[0];
out[1] = v[1];
out[2] = v[2];
}
static inline float dot3(float* a, float* b) {
static inline float dot3(const float* a, const float* b) {
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}
static inline void cross3(float* a, float* b, float* out) {
static inline void cross3(const float* a, const float* b, float* out) {
float ax=a[0], ay=a[1], az=a[2];
float bx=b[0], by=b[1], bz=b[2];
out[0] = ay*bz - by*az;
@ -62,30 +62,30 @@ public:
out[2] = ax*by - bx*ay;
}
static inline void mul3(float scalar, float* v, float* out)
static inline void mul3(const float scalar, const float* v, float* out)
{
out[0] = scalar * v[0];
out[1] = scalar * v[1];
out[2] = scalar * v[2];
}
static inline void add3(float* a, float* b, float* out){
static inline void add3(const float* a, const float* b, float* out){
out[0] = a[0] + b[0];
out[1] = a[1] + b[1];
out[2] = a[2] + b[2];
}
static inline void sub3(float* a, float* b, float* out) {
static inline void sub3(const float* a, const float* b, float* out) {
out[0] = a[0] - b[0];
out[1] = a[1] - b[1];
out[2] = a[2] - b[2];
}
static inline float mag3(float* v) {
static inline float mag3(const float* v) {
return sqrt(dot3(v, v));
}
static inline void unit3(float* v, float* out) {
static inline void unit3(const float* v, float* out) {
float imag = 1/mag3(v);
mul3(imag, v, out);
}
@ -95,7 +95,7 @@ public:
// 6 7 8
// Multiply two matrices
static void mmul33(float* a, float* b, float* out) {
static void mmul33(const float* a, const float* b, float* out) {
float tmp[9];
tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
@ -114,7 +114,7 @@ public:
}
// Multiply by vector
static inline void vmul33(float* m, float* v, float* out) {
static inline void vmul33(const float* m, const float* v, float* out) {
float x = v[0], y = v[1], z = v[2];
out[0] = x*m[0] + y*m[1] + z*m[2];
out[1] = x*m[3] + y*m[4] + z*m[5];
@ -123,43 +123,41 @@ public:
// Multiply the vector by the matrix transpose. Or pre-multiply the
// matrix by v as a row vector. Same thing.
static inline void tmul33(float* m, float* v, float* out) {
static inline void tmul33(const float* m, const float* v, float* out) {
float x = v[0], y = v[1], z = v[2];
out[0] = x*m[0] + y*m[3] + z*m[6];
out[1] = x*m[1] + y*m[4] + z*m[7];
out[2] = x*m[2] + y*m[5] + z*m[8];
}
// Invert matrix
static void invert33(float* m, float* out) {
/// Invert symmetric matrix; ~1/3 less calculations due to symmetry
static void invert33_sym(const float* m, float* out) {
// Compute the inverse as the adjoint matrix times 1/(det M).
// A, B ... I are the cofactors of a b c
// d e f
// g h i
// symetric: d=b, g=c, h=f
float a=m[0], b=m[1], c=m[2];
float d=m[3], e=m[4], f=m[5];
float g=m[6], h=m[7], i=m[8];
float e=m[4], f=m[5];
float i=m[8];
float A = (e*i - h*f);
float B = -(d*i - g*f);
float C = (d*h - g*e);
float D = -(b*i - h*c);
float E = (a*i - g*c);
float F = -(a*h - g*b);
float G = (b*f - e*c);
float H = -(a*f - d*c);
float I = (a*e - d*b);
float A = (e*i - f*f);
float B = -(b*i - c*f);
float C = (b*f - c*e);
float E = (a*i - c*c);
float F = -(a*f - c*b);
float I = (a*e - b*b);
float id = 1/(a*A + b*B + c*C);
out[0] = id*A; out[1] = id*D; out[2] = id*G;
out[3] = id*B; out[4] = id*E; out[5] = id*H;
out[6] = id*C; out[7] = id*F; out[8] = id*I;
out[0] = id*A; out[1] = id*B; out[2] = id*C;
out[3] = out[1]; out[4] = id*E; out[5] = id*F;
out[6] = out[2]; out[7] = out[5]; out[8] = id*I;
}
// Transpose matrix (for an orthonormal orientation matrix, this
// is the same as the inverse).
static inline void trans33(float* m, float* out) {
static inline void trans33(const float* m, float* out) {
// 0 1 2 Elements 0, 4, and 8 are the same
// 3 4 5 Swap elements 1/3, 2/6, and 5/7
// 6 7 8
@ -184,7 +182,7 @@ public:
// xOut becomes the unit vector in the direction of x
// yOut is perpendicular to xOut in the x/y plane
// zOut becomes the unit vector: (xOut cross yOut)
static void ortho33(float* x, float* y,
static void ortho33(const float* x, const float* y,
float* xOut, float* yOut, float* zOut) {
float x0[3], y0[3];
set3(x, x0);

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

@ -66,13 +66,20 @@ Model::Model()
_hook = 0;
_launchbar = 0;
_groundEffectSpan = 0;
_wingSpan = 0;
_groundEffect = 0;
for(i=0; i<3; i++) _wingCenter[i] = 0;
for(i=0; i<3; i++) _geRefPoint[i] = 0;
_global_ground[0] = 0; _global_ground[1] = 0; _global_ground[2] = 1;
_global_ground[3] = -100000;
_modelN = fgGetNode("/fdm/yasim/forces", true);
_f0xN = _modelN->getNode("f0-aero-x-drag", true);
_f0yN = _modelN->getNode("f0-aero-y-side", true);
_f0zN = _modelN->getNode("f0-aero-z-lift", true);
_gefxN = _modelN->getNode("gndeff-f-x", true);
_gefyN = _modelN->getNode("gndeff-f-y", true);
_gefzN = _modelN->getNode("gndeff-f-z", true);
_wgdistN = _modelN->getNode("wing-gnd-dist", true);
}
Model::~Model()
@ -175,106 +182,12 @@ void Model::iterate()
_integrator.calcNewInterval();
}
bool Model::isCrashed()
{
return _crashed;
}
void Model::setCrashed(bool crashed)
{
_crashed = crashed;
}
float Model::getAGL()
{
return _agl;
}
State* Model::getState()
{
return _s;
}
void Model::setState(State* s)
{
_integrator.setState(s);
_s = _integrator.getState();
}
RigidBody* Model::getBody()
{
return &_body;
}
Integrator* Model::getIntegrator()
{
return &_integrator;
}
Surface* Model::getSurface(int handle)
{
return (Surface*)_surfaces.get(handle);
}
Rotorgear* Model::getRotorgear(void)
{
return &_rotorgear;
}
int Model::addThruster(Thruster* t)
{
return _thrusters.add(t);
}
Hook* Model::getHook(void)
{
return _hook;
}
Launchbar* Model::getLaunchbar(void)
{
return _launchbar;
}
int Model::numThrusters()
{
return _thrusters.size();
}
Thruster* Model::getThruster(int handle)
{
return (Thruster*)_thrusters.get(handle);
}
void Model::setThruster(int handle, Thruster* t)
{
_thrusters.set(handle, t);
}
int Model::addSurface(Surface* surf)
{
return _surfaces.add(surf);
}
int Model::addGear(Gear* gear)
{
return _gears.add(gear);
}
void Model::addHook(Hook* hook)
{
_hook = hook;
}
void Model::addLaunchbar(Launchbar* launchbar)
{
_launchbar = launchbar;
}
int Model::addHitch(Hitch* hitch)
{
return _hitches.add(hitch);
}
void Model::setGroundCallback(Ground* ground_cb)
{
@ -282,30 +195,20 @@ void Model::setGroundCallback(Ground* ground_cb)
_ground_cb = ground_cb;
}
Ground* Model::getGroundCallback(void)
void Model::setGroundEffect(const float* pos, const float span, const float mul)
{
return _ground_cb;
}
void Model::setGroundEffect(float* pos, float span, float mul)
{
Math::set3(pos, _wingCenter);
_groundEffectSpan = span;
Math::set3(pos, _geRefPoint);
_wingSpan = span;
_groundEffect = mul;
}
void Model::setAir(float pressure, float temp, float density)
void Model::setAir(const float pressure, const float temp, const float density)
{
_pressure = pressure;
_temp = temp;
_rho = density;
}
void Model::setWind(float* wind)
{
Math::set3(wind, _wind);
}
void Model::updateGround(State* s)
{
float dummy[3];
@ -390,11 +293,11 @@ void Model::calcForces(State* s)
_body.addTorque(_torque);
int i,j;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
float thrust[3], pos[3];
t->getThrust(thrust);
t->getPosition(pos);
_body.addForce(pos, thrust);
Thruster* t = (Thruster*)_thrusters.get(i);
float thrust[3], pos[3];
t->getThrust(thrust);
t->getPosition(pos);
_body.addForce(pos, thrust);
}
// Get a ground plane in local coordinates. The first three
@ -417,20 +320,26 @@ void Model::calcForces(State* s)
float faero[3];
faero[0] = faero[1] = faero[2] = 0;
for(i=0; i<_surfaces.size(); i++) {
Surface* sf = (Surface*)_surfaces.get(i);
Surface* sf = (Surface*)_surfaces.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
localWind(pos, s, vs, alt);
// Vsurf = wind - velocity + (rot cross (cg - pos))
float vs[3], pos[3];
sf->getPosition(pos);
localWind(pos, s, vs, alt);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
Math::add3(faero, force, faero);
float force[3], torque[3];
sf->calcForce(vs, _rho, force, torque);
Math::add3(faero, force, faero);
_body.addForce(pos, force);
_body.addTorque(torque);
_body.addForce(pos, force);
_body.addTorque(torque);
}
if (_modelN != 0) {
_f0xN->setFloatValue(faero[0]);
_f0yN->setFloatValue(faero[1]);
_f0zN->setFloatValue(faero[2]);
}
for (j=0; j<_rotorgear.getRotors()->size();j++)
{
Rotor* r = (Rotor *)_rotorgear.getRotors()->get(j);
@ -470,18 +379,30 @@ void Model::calcForces(State* s)
// Account for ground effect by multiplying the vertical force
// component by an amount linear with the fraction of the wingspan
// above the ground.
if ((_groundEffectSpan != 0) && (_groundEffect != 0 ))
if ((_wingSpan != 0) && (_groundEffect != 0 ))
{
float dist = ground[3] - Math::dot3(ground, _wingCenter);
if(dist > 0 && dist < _groundEffectSpan) {
float fz = Math::dot3(faero, ground);
fz *= (_groundEffectSpan - dist) / _groundEffectSpan;
fz *= _groundEffect;
Math::mul3(fz, ground, faero);
_body.addForce(faero);
}
// distance between ground and wing ref. point
float dist = ground[3] - Math::dot3(ground, _geRefPoint);
float fz = 0;
float geForce[3];
if(dist > 0 && dist < _wingSpan) {
fz = Math::dot3(faero, ground);
fz *= (_wingSpan - dist) / _wingSpan;
fz *= _groundEffect;
Math::mul3(fz, ground, geForce);
_body.addForce(geForce);
}
if (_modelN != 0) {
_gefxN->setFloatValue(geForce[0]);
_gefyN->setFloatValue(geForce[1]);
_gefzN->setFloatValue(geForce[2]);
_wgdistN->setFloatValue(dist);
//float ld0 = faero[2]/faero[0];
//float ld = (geForce[2]+faero[2])/(geForce[0]+faero[0]);
//n->getNode("gndeff-ld-ld0", true)->setFloatValue(ld/ld0);
}
}
// Convert the velocity and rotation vectors to local coordinates
float lrot[3], lv[3];
Math::vmul33(s->orient, s->rot, lrot);
@ -561,7 +482,7 @@ void Model::newState(State* s)
// Calculates the airflow direction at the given point and for the
// specified aircraft velocity.
void Model::localWind(float* pos, State* s, float* out, float alt, bool is_rotor)
void Model::localWind(const float* pos, yasim::State* s, float* out, float alt, bool is_rotor)
{
float tmp[3], lwind[3], lrot[3], lv[3];

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

@ -7,6 +7,7 @@
#include "Vector.hpp"
#include "Turbulence.hpp"
#include "Rotor.hpp"
#include <simgear/props/props.hxx>
namespace yasim {
@ -26,49 +27,49 @@ public:
Model();
virtual ~Model();
RigidBody* getBody();
Integrator* getIntegrator();
RigidBody* getBody() { return &_body; }
Integrator* getIntegrator() { return &_integrator; }
void setTurbulence(Turbulence* turb) { _turb = turb; }
State* getState();
State* getState() { return _s; }
void setState(State* s);
bool isCrashed();
void setCrashed(bool crashed);
float getAGL();
bool isCrashed() { return _crashed; }
void setCrashed(bool crashed) { _crashed = crashed; }
float getAGL() { return _agl; }
void iterate();
// Externally-managed subcomponents
int addThruster(Thruster* t);
int addSurface(Surface* surf);
int addGear(Gear* gear);
void addHook(Hook* hook);
void addLaunchbar(Launchbar* launchbar);
Surface* getSurface(int handle);
Rotorgear* getRotorgear(void);
int addThruster(Thruster* t) { return _thrusters.add(t); }
int addSurface(Surface* surf) { return _surfaces.add(surf); }
int addGear(Gear* gear) { return _gears.add(gear); }
void addHook(Hook* hook) { _hook = hook; }
void addLaunchbar(Launchbar* launchbar) { _launchbar = launchbar; }
Surface* getSurface(int handle) { return (Surface*)_surfaces.get(handle); }
Rotorgear* getRotorgear(void) { return &_rotorgear; }
Gear* getGear(int handle);
Hook* getHook(void);
int addHitch(Hitch* hitch);
Launchbar* getLaunchbar(void);
Hook* getHook(void) { return _hook; }
int addHitch(Hitch* hitch) { return _hitches.add(hitch); }
Launchbar* getLaunchbar(void) { return _launchbar; }
// Semi-private methods for use by the Airplane solver.
int numThrusters();
Thruster* getThruster(int handle);
void setThruster(int handle, Thruster* t);
int numThrusters() { return _thrusters.size(); }
Thruster* getThruster(int handle) { return (Thruster*)_thrusters.get(handle); }
void setThruster(int handle, Thruster* t) { _thrusters.set(handle, t); }
void initIteration();
void getThrust(float* out);
void setGroundCallback(Ground* ground_cb);
Ground* getGroundCallback(void);
Ground* getGroundCallback(void) { return _ground_cb; }
//
// Per-iteration settables
//
void setGroundEffect(float* pos, float span, float mul);
void setWind(float* wind);
void setAir(float pressure, float temp, float density);
void setGroundEffect(const float* pos, const float span, const float mul);
void setWind(float* wind) { Math::set3(wind, _wind); }
void setAir(const float pressure, const float temp, const float density);
void updateGround(State* s);
@ -80,8 +81,7 @@ private:
void initRotorIteration();
void calcGearForce(Gear* g, float* v, float* rot, float* ground);
float gearFriction(float wgt, float v, Gear* g);
void localWind(float* pos, State* s, float* out, float alt,
bool is_rotor = false);
void localWind(const float* pos, State* s, float* out, float alt, bool is_rotor = false);
Integrator _integrator;
RigidBody _body;
@ -96,9 +96,9 @@ private:
Launchbar* _launchbar;
Vector _hitches;
float _groundEffectSpan;
float _wingSpan;
float _groundEffect;
float _wingCenter[3];
float _geRefPoint[3];
Ground* _ground_cb;
double _global_ground[4];
@ -114,6 +114,14 @@ private:
State* _s;
bool _crashed;
float _agl;
SGPropertyNode_ptr _modelN;
SGPropertyNode_ptr _f0xN;
SGPropertyNode_ptr _f0yN;
SGPropertyNode_ptr _f0zN;
SGPropertyNode_ptr _gefxN;
SGPropertyNode_ptr _gefyN;
SGPropertyNode_ptr _gefzN;
SGPropertyNode_ptr _wgdistN;
};
}; // namespace yasim

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

@ -1,5 +1,6 @@
#include "Math.hpp"
#include <Main/fg_props.hxx>
#include "RigidBody.hpp"
namespace yasim {
RigidBody::RigidBody()
@ -11,6 +12,7 @@ RigidBody::RigidBody()
_masses = new Mass[_massesAlloced];
_gyro[0] = _gyro[1] = _gyro[2] = 0;
_spin[0] = _spin[1] = _spin[2] = 0;
_bodyN = fgGetNode("/fdm/yasim/model/masses", true);
}
RigidBody::~RigidBody()
@ -18,7 +20,9 @@ RigidBody::~RigidBody()
delete[] _masses;
}
int RigidBody::addMass(float mass, float* pos)
/// add new point mass to body
/// isStatic: set to true for masses that do not change per iteration (everything but fuel?)
int RigidBody::addMass(float mass, float* pos, bool isStatic)
{
// If out of space, reallocate twice as much
if(_nMasses == _massesAlloced) {
@ -30,31 +34,38 @@ int RigidBody::addMass(float mass, float* pos)
delete[] _masses;
_masses = m2;
}
_masses[_nMasses].m = mass;
Math::set3(pos, _masses[_nMasses].p);
setMass(_nMasses, mass, pos, isStatic);
return _nMasses++;
}
/// change mass
/// handle: returned by addMass
void RigidBody::setMass(int handle, float mass)
{
if (_masses[handle].m == mass)
return;
_masses[handle].m = mass;
// if static mass is changed, reset pre-calculated mass
// may apply to weights like cargo, pax, that usually do not change with FDM rate
if (_masses[handle].isStatic)
_staticMass.m = 0;
if (_bodyN != 0)
_bodyN->getChild("mass", handle, true)->getNode("mass", true)->setFloatValue(mass);
}
void RigidBody::setMass(int handle, float mass, float* pos)
void RigidBody::setMass(int handle, float mass, const float* pos, bool isStatic)
{
_masses[handle].m = mass;
_masses[handle].isStatic = isStatic;
Math::set3(pos, _masses[handle].p);
}
int RigidBody::numMasses()
{
return _nMasses;
}
float RigidBody::getMass(int handle)
{
return _masses[handle].m;
if (_bodyN != 0) {
SGPropertyNode_ptr n = _bodyN->getChild("mass", handle, true);
n->getNode("isStatic", true)->setValue(isStatic);
n->getNode("mass", true)->setFloatValue(mass);
n->getNode("pos-x", true)->setFloatValue(pos[0]);
n->getNode("pos-y", true)->setFloatValue(pos[1]);
n->getNode("pos-z", true)->setFloatValue(pos[2]);
}
}
void RigidBody::getMassPosition(int handle, float* out)
@ -64,60 +75,129 @@ void RigidBody::getMassPosition(int handle, float* out)
out[2] = _masses[handle].p[2];
}
float RigidBody::getTotalMass()
{
return _totalMass;
}
// Calcualtes the rotational velocity of a particular point. All
// coordinates are local!
void RigidBody::pointVelocity(float* pos, float* rot, float* out)
void RigidBody::pointVelocity(const float* pos, const float* rot, float* out)
{
Math::sub3(pos, _cg, out); // out = pos-cg
Math::cross3(rot, out, out); // = rot cross (pos-cg)
}
void RigidBody::setGyro(float* angularMomentum)
void RigidBody::_recalcStatic()
{
Math::set3(angularMomentum, _gyro);
// 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;
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];
}
}
Math::mul3(1/_staticMass.m, _staticMass.p, _staticMass.p);
if (_bodyN != 0) {
_bodyN->getNode("aggregated-mass", true)->setFloatValue(_staticMass.m);
_bodyN->getNode("aggregated-count", true)->setIntValue(s);
_bodyN->getNode("aggregated-pos-x", true)->setFloatValue(_staticMass.p[0]);
_bodyN->getNode("aggregated-pos-y", true)->setFloatValue(_staticMass.p[1]);
_bodyN->getNode("aggregated-pos-z", true)->setFloatValue(_staticMass.p[2]);
}
// Now the inertia tensor:
for(i=0; i<9; i++)
_tI_static[i] = 0;
for(i=0; i<_nMasses; i++) {
if (_masses[i].isStatic) {
float m = _masses[i].m;
float x = _masses[i].p[0] - _staticMass.p[0];
float y = _masses[i].p[1] - _staticMass.p[1];
float z = _masses[i].p[2] - _staticMass.p[2];
float xy = m*x*y; float yz = m*y*z; float zx = m*z*x;
float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z;
// tensor is symmetric, so we can save some calculations in the loop
_tI_static[0] += y2+z2; _tI_static[1] -= xy; _tI_static[2] -= zx;
_tI_static[4] += x2+z2; _tI_static[5] -= yz;
_tI_static[8] += x2+y2;
}
}
// copy symmetric elements
_tI_static[3] = _tI_static[1];
_tI_static[6] = _tI_static[2];
_tI_static[7] = _tI_static[5];
}
/// calculate the total mass, centre of gravity and inertia tensor
/**
recalc is used when compiling the model but more important it is called in
Model::iterate() e.g. at FDM rate (120 Hz)
We can save some CPU due to the symmetry of the tensor and by aggregating
masses that do not change during flight.
*/
void RigidBody::recalc()
{
// Calculate the c.g and total mass:
_totalMass = 0;
_cg[0] = _cg[1] = _cg[2] = 0;
//aggregate static masses into one mass
if (_staticMass.m == 0) _recalcStatic();
// 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];
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];
}
}
Math::mul3(1/_totalMass, _cg, _cg);
// Now the inertia tensor:
for(i=0; i<9; i++)
_tI[i] = 0;
_tI[i] = _tI_static[i];
for(i=0; i<_nMasses; i++) {
float m = _masses[i].m;
if (!_masses[i].isStatic) {
float m = _masses[i].m;
float x = _masses[i].p[0] - _cg[0];
float y = _masses[i].p[1] - _cg[1];
float z = _masses[i].p[2] - _cg[2];
float x = _masses[i].p[0] - _cg[0];
float y = _masses[i].p[1] - _cg[1];
float z = _masses[i].p[2] - _cg[2];
float mx = m*x;
float my = m*y;
float mz = m*z;
float xy = mx*y; float yz = my*z; float zx = mz*x;
float x2 = mx*x; float y2 = my*y; float z2 = mz*z;
float xy = m*x*y; float yz = m*y*z; float zx = m*z*x;
float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z;
_tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx;
_tI[3] -= xy; _tI[4] += x2+z2; _tI[5] -= yz;
_tI[6] -= zx; _tI[7] -= yz; _tI[8] += x2+y2;
_tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx;
_tI[4] += x2+z2; _tI[5] -= yz;
_tI[8] += x2+y2;
}
}
// copy symmetric elements
_tI[3] = _tI[1];
_tI[6] = _tI[2];
_tI[7] = _tI[5];
// And its inverse
Math::invert33(_tI, _invI);
//calculate inverse
Math::invert33_sym(_tI, _invI);
}
void RigidBody::reset()
@ -126,17 +206,7 @@ void RigidBody::reset()
_force[0] = _force[1] = _force[2] = 0;
}
void RigidBody::addForce(float* force)
{
Math::add3(_force, force, _force);
}
void RigidBody::addTorque(float* torque)
{
Math::add3(_torque, torque, _torque);
}
void RigidBody::addForce(float* pos, float* force)
void RigidBody::addForce(const float* pos, const float* force)
{
addForce(force);
@ -148,21 +218,6 @@ void RigidBody::addForce(float* pos, float* force)
addTorque(t);
}
void RigidBody::setBodySpin(float* rotation)
{
Math::set3(rotation, _spin);
}
void RigidBody::getCG(float* cgOut)
{
Math::set3(_cg, cgOut);
}
void RigidBody::getAccel(float* accelOut)
{
Math::mul3(1/_totalMass, _force, accelOut);
}
void RigidBody::getAccel(float* pos, float* accelOut)
{
getAccel(accelOut);

58
src/FDM/YASim/RigidBody.hpp
View file

@ -1,5 +1,8 @@
#ifndef _RIGIDBODY_HPP
#define _RIGIDBODY_HPP
#include <simgear/props/props.hxx>
#include "Vector.hpp"
#include "Math.hpp"
namespace yasim {
@ -20,27 +23,28 @@ namespace yasim {
//
class RigidBody
{
SGPropertyNode_ptr _bodyN;
public:
RigidBody();
~RigidBody();
// Adds a point mass to the system. Returns a handle so the gyro
// can be later modified via setMass().
int addMass(float mass, float* pos);
int addMass(float mass, float* pos, bool isStatic = false);
// Modifies a previously-added point mass (fuel tank running dry,
// gear going up, swing wing swinging, pilot bailing out, etc...)
void setMass(int handle, float mass);
void setMass(int handle, float mass, float* pos);
void setMass(int handle, float mass, const float* pos, bool isStatic = false);
int numMasses();
float getMass(int handle);
int numMasses() { return _nMasses; }
float getMass(int handle) { return _masses[handle].m; }
void getMassPosition(int handle, float* out);
float getTotalMass();
float getTotalMass() { return _totalMass; }
// The velocity, in local coordinates, of the specified point on a
// body rotating about its c.g. with velocity rot.
void pointVelocity(float* pos, float* rot, float* out);
void pointVelocity(const float* pos, const float* rot, float* out);
// Sets the "gyroscope" for the body. This is the total
// "intrinsic" angular momentum of the body; that is, rotations of
@ -48,27 +52,26 @@ public:
// frame. Because angular momentum is additive in this way, we
// don't need to specify specific gyro objects; just add all their
// momenta together and set it here.
void setGyro(float* angularMomentum);
void setGyro(const float* angularMomentum) { Math::set3(angularMomentum, _gyro); }
// When masses are moved or changed, this object needs to
// regenerate its internal tables. This step is expensive, so
// it's exposed to the client who can amortize the call across
// multiple changes.
// multiple changes. see also _recalcStatic()
void recalc();
// Resets the current force/torque parameters to zero.
void reset();
// Applies a force at the specified position.
void addForce(float* pos, float* force);
// Applies a force at the center of gravity.
void addForce(float* force);
void addForce(const float* force) { Math::add3(_force, force, _force); }
// Applies a force at the specified position.
void addForce(const float* pos, const float* force);
// Adds a torque with the specified axis and magnitude
void addTorque(float* torque);
void addTorque(const float* torque) { Math::add3(_torque, torque, _torque); }
// Sets the rotation rate of the body (about its c.g.) within the
// surrounding environment. This is needed to compute torque on
@ -76,17 +79,15 @@ public:
// rotation. NOTE: the rotation vector, like all other
// coordinates used here, is specified IN THE LOCAL COORDINATE
// SYSTEM.
void setBodySpin(float* rotation);
void setBodySpin(const float* rotation) { Math::set3(rotation, _spin); }
// Returns the center of gravity of the masses, in the body
// coordinate system.
void getCG(float* cgOut);
void getCG(float* cgOut) { Math::set3(_cg, cgOut); }
// Returns the acceleration of the body's c.g. relative to the
// rest of the world, specified in local coordinates.
void getAccel(float* accelOut);
void getAccel(float* accelOut) { Math::mul3(1/_totalMass, _force, accelOut); }
// Returns the acceleration of a specific location in local
// coordinates. If the body is rotating, this will be different
@ -102,17 +103,24 @@ public:
void getInertiaMatrix(float* inertiaOut);
private:
struct Mass { float m; float p[3]; };
/**
Most of the mass points do not change after compilation of the aircraft so
they can be replaced by one aggregated mass at the c.g. of the static masses.
The isStatic flag is used to mark those masses.
*/
struct Mass { float m; float p[3]; bool isStatic; };
void _recalcStatic(); /// aggregate static masses
Mass _staticMass; /// aggregated static masses, calculated once
Mass* _masses; /// mass elements
int _nMasses; /// number of masses
int _massesAlloced; /// counter for memory allocation
// Internal "rotational structure"
Mass* _masses;
int _nMasses;
int _massesAlloced;
float _totalMass;
float _cg[3];
float _gyro[3];
// Inertia tensor, and its inverse. Computed from the above.
float _tI_static[9];
float _tI[9];
float _invI[9];

6
src/FDM/YASim/Rotor.cpp
View file

@ -7,7 +7,7 @@
#include "Math.hpp"
#include <Main/fg_props.hxx>
#include "Surface.hpp"
//#include "Surface.hpp"
#include "Rotorpart.hpp"
#include "Glue.hpp"
#include "Ground.hpp"
@ -962,7 +962,7 @@ float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
);
}
void Rotor::getDownWash(float *pos, float *v_heli, float *downwash)
void Rotor::getDownWash(const float *pos, const float *v_heli, float *downwash)
{
float pos2rotor[3],tmp[3];
Math::sub3(_base,pos,pos2rotor);
@ -1632,7 +1632,7 @@ void Rotorgear::compile()
}
}
void Rotorgear::getDownWash(float *pos, float * v_heli, float *downwash)
void Rotorgear::getDownWash(const float *pos, const float * v_heli, float *downwash)
{
float tmp[3];
downwash[0]=downwash[1]=downwash[2]=0;

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

@ -9,7 +9,7 @@
namespace yasim {
class Surface;
//class Surface;
class Rotorpart;
class Ground;
const float rho_null=1.184f; //25DegC, 101325Pa
@ -107,7 +107,7 @@ public:
void updateDirectionsAndPositions(float *rot);
void getTip(float* tip);
void calcLiftFactor(float* v, float rho, State *s);
void getDownWash(float *pos, float * v_heli, float *downwash);
void getDownWash(const float* pos, const float* v_heli, float* downwash);
int getNumberOfBlades(){return _number_of_blades;}
void setDownwashFactor(float value);
@ -287,7 +287,7 @@ public:
float getEnginePropFactor() {return _engine_prop_factor;}
Vector* getRotors() { return &_rotors;}
void initRotorIteration(float *lrot,float dt);
void getDownWash(float *pos, float * v_heli, float *downwash);
void getDownWash(const float* pos, const float* v_heli, float* downwash);
int getValueforFGSet(int j,char *b,float *f);
};

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

@ -1,10 +1,14 @@
#include "Math.hpp"
#include <Main/fg_props.hxx>
#include "Surface.hpp"
namespace yasim {
int Surface::s_idGenerator = 0;
Surface::Surface( Version * version ) :
_version(version)
{
// create id for surface
_id = s_idGenerator++;
// Start in a "sane" mode, so unset stuff doesn't freak us out
_c0 = 1;
_cx = _cy = _cz = 1;
@ -30,91 +34,40 @@ Surface::Surface( Version * version ) :
_slatAlpha = 0;
_spoilerLift = 1;
_inducedDrag = 1;
_stallAlpha = 0;
_alpha = 0;
_surfN = fgGetNode("/fdm/yasim/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);
}
}
void Surface::setPosition(float* p)
void Surface::setPosition(const float* p)
{
int i;
for(i=0; i<3; i++) _pos[i] = p[i];
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]);
}
}
void Surface::getPosition(float* out)
void Surface::setOrientation(const float* o)
{
int i;
for(i=0; i<3; i++) out[i] = _pos[i];
for(int i=0; i<9; i++) _orient[i] = o[i];
}
void Surface::setChord(float chord)
{
_chord = chord;
}
void Surface::setTotalDrag(float c0)
{
_c0 = c0;
}
float Surface::getTotalDrag()
{
return _c0;
}
void Surface::setXDrag(float cx)
{
_cx = cx;
}
void Surface::setYDrag(float cy)
{
_cy = cy;
}
void Surface::setZDrag(float cz)
{
_cz = cz;
}
float Surface::getXDrag()
{
return _cx;
}
void Surface::setBaseZDrag(float cz0)
{
_cz0 = cz0;
}
void Surface::setStallPeak(int i, float peak)
{
_peaks[i] = peak;
}
void Surface::setStall(int i, float alpha)
{
_stalls[i] = alpha;
}
void Surface::setStallWidth(int i, float width)
{
_widths[i] = width;
}
void Surface::setOrientation(float* o)
{
int i;
for(i=0; i<9; i++)
_orient[i] = o[i];
}
void Surface::setIncidence(float angle)
{
_incidence = angle;
}
void Surface::setTwist(float angle)
{
_twist = angle;
}
void Surface::setSlatParams(float stallDelta, float dragPenalty)
{
@ -134,42 +87,39 @@ void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
_spoilerDrag = dragPenalty;
}
void Surface::setFlap(float pos)
void Surface::setFlapPos(float pos)
{
_flapPos = pos;
if (_flapPos != pos) {
_flapPos = pos;
if (_surfN != 0) _flapN->setFloatValue(pos);
}
}
void Surface::setFlapEffectiveness(float effectiveness)
{
_flapEffectiveness = effectiveness;
}
double Surface::getFlapEffectiveness()
{
return _flapEffectiveness;
}
void Surface::setSlat(float pos)
void Surface::setSlatPos(float pos)
{
if (_slatPos != pos) {
_slatPos = pos;
if (_surfN != 0) _slatN->setFloatValue(pos);
}
}
void Surface::setSpoiler(float 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, too.
void Surface::calcForce(float* v, float rho, float* out, float* torque)
void Surface::calcForce(const float* v, const float rho, float* out, float* torque)
{
// Split v into magnitude and direction:
float vel = Math::mag3(v);
// Handle the blowup condition. Zero velocity means zero force by
// definition.
// 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;
@ -182,10 +132,9 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
for(int i=0; i<3; i++) out[i] = torque[i] = 0.;
return;
}
// Normalize wind and convert to the surface's coordinates
Math::mul3(1/vel, v, out);
// Convert to the surface's coordinates
Math::vmul33(_orient, out, out);
// "Rotate" by the incidence angle. Assume small angles, so we
@ -247,6 +196,15 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
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
@ -257,9 +215,9 @@ void Surface::test()
float rho = Atmosphere::getStdDensity(0);
float spd = 30;
setFlap(0);
setSlat(0);
setSpoiler(0);
setFlapPos(0);
setSlatPos(0);
setSpoilerPos(0);
for(float angle = -90; angle<90; angle += 0.01) {
float rad = angle * DEG2RAD;
@ -281,39 +239,40 @@ float Surface::stallFunc(float* v)
// Sanity check to treat FPU psychopathology
if(v[0] == 0) return 1;
float alpha = Math::abs(v[2]/v[0]);
_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;
float stallAlpha = _stalls[i];
if(stallAlpha == 0)
_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;
_stallAlpha += _slatPos * _slatAlpha;
} else {
stallAlpha += _slatAlpha;
_stallAlpha += _slatAlpha;
}
}
// Beyond the stall
if(alpha > stallAlpha+_widths[i])
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)
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];
float frac = (_alpha - _stallAlpha) / _widths[i];
frac = frac*frac*(3-2*frac);
return scale*(1-frac) + frac;

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

@ -1,7 +1,9 @@
#ifndef _SURFACE_HPP
#define _SURFACE_HPP
#include <simgear/props/props.hxx>
#include "Version.hpp"
#include "Math.hpp"
namespace yasim {
@ -10,15 +12,21 @@ namespace yasim {
// front, and flaps act (in both lift and drag) toward the back.
class Surface
{
static int s_idGenerator;
int _id; //index for property tree
public:
Surface( Version * version );
int getID() { return _id; };
static void resetIDgen() { s_idGenerator = 0; };
// Position of this surface in local coords
void setPosition(float* p);
void getPosition(float* out);
void setPosition(const float* p);
void getPosition(float* out) { Math::set3(_pos, out); }
// Distance scale along the X axis
void setChord(float chord);
void setChord(float chord) { _chord = chord; }
// Slats act to move the stall peak by the specified angle, and
// increase drag by the multiplier specified.
@ -32,49 +40,54 @@ public:
// Positions for the controls, in the range [0:1]. [-1:1] for
// flaps, with positive meaning "force goes towards positive Z"
void setFlap(float pos);
void setSlat(float pos);
void setSpoiler(float pos);
void setFlapPos(float pos);
void setSlatPos(float pos);
void setSpoilerPos(float pos);
// Modifier for flap lift coefficient, useful for simulating flap blowing etc.
void setFlapEffectiveness(float effectiveness);
double getFlapEffectiveness();
void setFlapEffectiveness(float effectiveness) { _flapEffectiveness = effectiveness; }
double getFlapEffectiveness() { return _flapEffectiveness; }
// local -> Surface coords
void setOrientation(float* o);
void setOrientation(const float* o);
// For variable-incidence control surfaces. The angle is a
// negative rotation about the surface's Y axis, in radians, so
// positive is "up" (i.e. "positive AoA")
void setIncidence(float angle);
void setIncidence(float angle) { _incidence = angle; }
// The offset from base incidence for this surface.
void setTwist(float angle);
void setTwist(float angle) { _twist = angle; }
void setTotalDrag(float c0);
float getTotalDrag();
void setXDrag(float cx);
void setYDrag(float cy);
void setZDrag(float cz);
float getXDrag();
void setTotalDrag(float c0) { _c0 = c0; }
float getTotalDrag() { return _c0; }
void setXDrag(float cx) { _cx = cx; }
void setYDrag(float cy) { _cy = cy; }
void setZDrag(float cz) { _cz = cz; }
float getXDrag() { return _cx; }
// zero-alpha Z drag ("camber") specified as a fraction of cz
void setBaseZDrag(float cz0);
void setBaseZDrag(float cz0) { _cz0 = cz0; }
// i: 0 == forward, 1 == backwards
void setStallPeak(int i, float peak);
void setStallPeak(int i, float peak) { _peaks[i] = peak; }
// i: 0 == fwd/+z, 1 == fwd/-z, 2 == rev/+z, 3 == rev/-z
void setStall(int i, float alpha);
void setStallWidth(int i, float width);
void setStall(int i, float alpha) { _stalls[i] = alpha; }
void setStallWidth(int i, float width) { _widths[i] = width; }
// Induced drag multiplier
void setInducedDrag(float mul) { _inducedDrag = mul; }
void calcForce(float* v, float rho, float* forceOut, float* torqueOut);
void calcForce(const float* v, const float rho, float* out, float* torque);
float getAlpha() { return _alpha; };
float getStallAlpha() { return _stallAlpha; };
private:
SGPropertyNode_ptr _surfN;
float stallFunc(float* v);
float flapLift(float alpha);
float controlDrag(float lift, float drag);
@ -105,8 +118,21 @@ private:
float _incidence;
float _twist;
float _inducedDrag;
// used during calculations
float _stallAlpha;
float _alpha;
Version * _version;
SGPropertyNode* _fxN;
SGPropertyNode* _fyN;
SGPropertyNode* _fzN;
SGPropertyNode* _stallAlphaN;
SGPropertyNode* _alphaN;
SGPropertyNode* _flapN;
SGPropertyNode* _slatN;
SGPropertyNode* _spoilerN;
SGPropertyNode* _fabsN;
};
}; // namespace yasim

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

@ -5,6 +5,7 @@
#include "Version.hpp"
#include <simgear/debug/logstream.hxx>
#include <string>
#include <iostream>
namespace yasim {
void Version::setVersion( const char * version )
@ -15,11 +16,15 @@ void Version::setVersion( const char * version )
_version = YASIM_VERSION_ORIGINAL;
} else if( v == "YASIM_VERSION_32" ) {
_version = YASIM_VERSION_32;
} else if( v == "2017.2" ) {
_version = YASIM_VERSION_2017_2;
} else if( v == "YASIM_VERSION_CURRENT" ) {
_version = YASIM_VERSION_CURRENT;
} else {
SG_LOG(SG_FLIGHT,SG_ALERT,"unknown yasim version '" << version << "' ignored, using YASIM_VERSION_ORIGINAL");
return;
}
std::cout << "This aircraft uses yasim version '" << v << "'\n";
}
} // namespace yasim

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

@ -11,10 +11,12 @@ public:
typedef enum {
YASIM_VERSION_ORIGINAL = 0,
YASIM_VERSION_32,
YASIM_VERSION_CURRENT = YASIM_VERSION_32
YASIM_VERSION_2017_2,
YASIM_VERSION_CURRENT = YASIM_VERSION_2017_2
} YASIM_VERSION;
void setVersion( const char * version );
int getVersion() { return _version; }
bool isVersion( YASIM_VERSION version );
bool isVersionOrNewer( YASIM_VERSION version );

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

@ -1,9 +1,9 @@
#include "Math.hpp"
#include "Surface.hpp"
#include "Wing.hpp"
namespace yasim {
static const float RAD2DEG = 57.2957795131;
Wing::Wing( Version * version ) :
_version(version)
{
@ -39,6 +39,9 @@ Wing::Wing( Version * version ) :
_slatEnd = 0;
_slatAoA = 0;
_slatDrag = 0;
_meanChord = 0;
_wingspan = 0;
_aspectRatio = 1;
}
Wing::~Wing()
@ -51,82 +54,6 @@ Wing::~Wing()
}
}
int Wing::numSurfaces()
{
return _surfs.size();
}
Surface* Wing::getSurface(int n)
{
return ((SurfRec*)_surfs.get(n))->surface;
}
float Wing::getSurfaceWeight(int n)
{
return ((SurfRec*)_surfs.get(n))->weight;
}
void Wing::setMirror(bool mirror)
{
_mirror = mirror;
}
void Wing::setBase(float* base)
{
int i;
for(i=0; i<3; i++) _base[i] = base[i];
}
void Wing::setLength(float length)
{
_length = length;
}
void Wing::setChord(float chord)
{
_chord = chord;
}
void Wing::setTaper(float taper)
{
_taper = taper;
}
void Wing::setSweep(float sweep)
{
_sweep = sweep;
}
void Wing::setDihedral(float dihedral)
{
_dihedral = dihedral;
}
void Wing::setStall(float aoa)
{
_stall = aoa;
}
void Wing::setStallWidth(float angle)
{
_stallWidth = angle;
}
void Wing::setStallPeak(float fraction)
{
_stallPeak = fraction;
}
void Wing::setTwist(float angle)
{
_twist = angle;
}
void Wing::setCamber(float camber)
{
_camber = camber;
}
void Wing::setIncidence(float incidence)
{
_incidence = incidence;
@ -135,7 +62,7 @@ void Wing::setIncidence(float incidence)
((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
}
void Wing::setFlap0(float start, float end, float lift, float drag)
void Wing::setFlap0Params(float start, float end, float lift, float drag)
{
_flap0Start = start;
_flap0End = end;
@ -143,7 +70,7 @@ void Wing::setFlap0(float start, float end, float lift, float drag)
_flap0Drag = drag;
}
void Wing::setFlap1(float start, float end, float lift, float drag)
void Wing::setFlap1Params(float start, float end, float lift, float drag)
{
_flap1Start = start;
_flap1End = end;
@ -151,7 +78,7 @@ void Wing::setFlap1(float start, float end, float lift, float drag)
_flap1Drag = drag;
}
void Wing::setSlat(float start, float end, float aoa, float drag)
void Wing::setSlatParams(float start, float end, float aoa, float drag)
{
_slatStart = start;
_slatEnd = end;
@ -159,7 +86,7 @@ void Wing::setSlat(float start, float end, float aoa, float drag)
_slatDrag = drag;
}
void Wing::setSpoiler(float start, float end, float lift, float drag)
void Wing::setSpoilerParams(float start, float end, float lift, float drag)
{
_spoilerStart = start;
_spoilerEnd = end;
@ -167,14 +94,14 @@ void Wing::setSpoiler(float start, float end, float lift, float drag)
_spoilerDrag = drag;
}
void Wing::setFlap0(float lval, float rval)
void Wing::setFlap0Pos(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
int i;
for(i=0; i<_flap0Surfs.size(); i++) {
((Surface*)_flap0Surfs.get(i))->setFlap(lval);
if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
((Surface*)_flap0Surfs.get(i))->setFlapPos(lval);
if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlapPos(rval);
}
}
@ -184,18 +111,17 @@ void Wing::setFlap0Effectiveness(float lval)
int i;
for(i=0; i<_flap0Surfs.size(); i++) {
((Surface*)_flap0Surfs.get(i))->setFlapEffectiveness(lval);
// if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlapEffectiveness(rval);
}
}
void Wing::setFlap1(float lval, float rval)
void Wing::setFlap1Pos(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
int i;
for(i=0; i<_flap1Surfs.size(); i++) {
((Surface*)_flap1Surfs.get(i))->setFlap(lval);
if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
((Surface*)_flap1Surfs.get(i))->setFlapPos(lval);
if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlapPos(rval);
}
}
@ -205,51 +131,26 @@ void Wing::setFlap1Effectiveness(float lval)
int i;
for(i=0; i<_flap1Surfs.size(); i++) {
((Surface*)_flap1Surfs.get(i))->setFlapEffectiveness(lval);
// if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
}
}
void Wing::setSpoiler(float lval, float rval)
void Wing::setSpoilerPos(float lval, float rval)
{
lval = Math::clamp(lval, 0, 1);
rval = Math::clamp(rval, 0, 1);
int i;
for(i=0; i<_spoilerSurfs.size(); i++) {
((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
((Surface*)_spoilerSurfs.get(i))->setSpoilerPos(lval);
if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoilerPos(rval);
}
}
void Wing::setSlat(float val)
void Wing::setSlatPos(float val)
{
val = Math::clamp(val, 0, 1);
int i;
for(i=0; i<_slatSurfs.size(); i++)
((Surface*)_slatSurfs.get(i))->setSlat(val);
}
float Wing::getGroundEffect(float* posOut)
{
int i;
for(i=0; i<3; i++) posOut[i] = _base[i];
float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
span = 2*(span + Math::abs(_base[2]));
return span;
}
void Wing::getTip(float* tip)
{
tip[0] = -Math::tan(_sweep);
tip[1] = Math::cos(_dihedral);
tip[2] = Math::sin(_dihedral);
Math::unit3(tip, tip);
Math::mul3(_length, tip, tip);
Math::add3(_base, tip, tip);
}
bool Wing::isMirrored()
{
return _mirror;
((Surface*)_slatSurfs.get(i))->setSlatPos(val);
}
void Wing::compile()
@ -272,17 +173,17 @@ void Wing::compile()
// Sort in increasing order
int i;
for(i=0; i<10; i++) {
int minIdx = i;
float minVal = bounds[i];
int j;
for(j=i+1; j<10; j++) {
if(bounds[j] < minVal) {
minIdx = j;
minVal = bounds[j];
}
}
float tmp = bounds[i];
bounds[i] = minVal; bounds[minIdx] = tmp;
int minIdx = i;
float minVal = bounds[i];
int j;
for(j=i+1; j<10; j++) {
if(bounds[j] < minVal) {
minIdx = j;
minVal = bounds[j];
}
}
float tmp = bounds[i];
bounds[i] = minVal; bounds[minIdx] = tmp;
}
// Uniqify
@ -294,10 +195,9 @@ void Wing::compile()
last = bounds[i];
}
// Calculate a "nominal" segment length equal to an average chord,
// normalized to lie within 0-1 over the length of the wing.
float segLen = _chord * (0.5f*(_taper+1)) / _length;
// 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);
@ -306,12 +206,13 @@ void Wing::compile()
Math::unit3(left, left);
// Calculate coordinates for the root and tip of the wing
float root[3], tip[3];
Math::set3(_base, root);
Math::set3(left, tip);
Math::mul3(_length, tip, tip);
Math::add3(root, tip, tip);
Math::mul3(_length, left, _tip);
Math::add3(_base, _tip, _tip);
_meanChord = _chord*(_taper+1)*0.5f;
// wingspan in y-direction (not for vstab)
_wingspan = Math::abs(2*_tip[1]);
_aspectRatio = _wingspan / _meanChord;
// The wing's Y axis will be the "left" vector. The Z axis will
// be perpendicular to this and the local (!) X axis, because we
// want motion along the local X axis to be zero AoA (i.e. in the
@ -326,19 +227,23 @@ void Wing::compile()
Math::cross3(y, z, x);
if(_mirror) {
// Derive the right side orientation matrix from this one.
int i;
for(i=0; i<9; i++) rightOrient[i] = orient[i];
// Derive the right side orientation matrix from this one.
int i;
for(i=0; i<9; i++) rightOrient[i] = orient[i];
// Negate all Y coordinates, this gets us a valid basis, but
// it's left handed! So...
for(i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
// Negate all Y coordinates, this gets us a valid basis, but
// it's left handed! So...
for(i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
// Change the direction of the Y axis to get back to a
// right-handed system.
for(i=3; i<6; i++) rightOrient[i] = -rightOrient[i];
// Change the direction of the Y axis to get back to a
// right-handed system.
for(i=3; i<6; i++) rightOrient[i] = -rightOrient[i];
}
// 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;
// Now go through each boundary and make segments
for(i=0; i<(nbounds-1); i++) {
float start = bounds[i];
@ -363,7 +268,7 @@ void Wing::compile()
for(j=0; j<nSegs; j++) {
float frac = start + (j+0.5f) * (end-start)/nSegs;
float pos[3];
interp(root, tip, frac, pos);
interp(_base, _tip, frac, pos);
float chord = _chord * (1 - (1-_taper)*frac);
@ -378,7 +283,7 @@ void Wing::compile()
_surfs.add(sr);
if(_mirror) {
pos[1] = -pos[1];
pos[1] = -pos[1];
s = newSurface(pos, rightOrient, chord,
flap0, flap1, slat, spoiler);
sr = new SurfRec();
@ -390,22 +295,16 @@ void Wing::compile()
}
}
}
// Last of all, re-set the incidence in case setIncidence() was
// called before we were compiled.
setIncidence(_incidence);
}
float Wing::getDragScale()
{
return _dragScale;
}
void Wing::setDragScale(float scale)
{
_dragScale = scale;
int i;
for(i=0; i<_surfs.size(); i++) {
for(int i=0; i<_surfs.size(); i++) {
SurfRec* s = (SurfRec*)_surfs.get(i);
s->surface->setTotalDrag(scale * s->weight);
}
@ -414,16 +313,10 @@ void Wing::setDragScale(float scale)
void Wing::setLiftRatio(float ratio)
{
_liftRatio = ratio;
int i;
for(i=0; i<_surfs.size(); i++)
for(int i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setZDrag(ratio);
}
float Wing::getLiftRatio()
{
return _liftRatio;
}
Surface* Wing::newSurface(float* pos, float* orient, float chord,
bool flap0, bool flap1, bool slat, bool spoiler)
{
@ -442,14 +335,20 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
s->setStallWidth(0, _stallWidth);
s->setStallPeak(0, _stallPeak);
// The negative AoA stall is the same if we're using an uncambered
// airfoil, otherwise a "little badder".
// 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);
} else {
s->setStall(1, stallAoA);
s->setStall(1, stallAoA);
if( _version->isVersionOrNewer( Version::YASIM_VERSION_2017_2 )) {
// what was presumably meant
s->setStallWidth(1, _stallWidth);
} else {
// old code; presumably a copy&paste error
s->setStall(1, _stallWidth);
}
}
// The "reverse" stalls are unmeasurable junk. Just use 13deg and
@ -476,7 +375,7 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
return s;
}
void Wing::interp(float* v1, float* v2, float frac, float* out)
void Wing::interp(const float* v1, const float* v2, const float frac, float* out)
{
out[0] = v1[0] + frac*(v2[0]-v1[0]);
out[1] = v1[1] + frac*(v2[1]-v1[1]);

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

@ -3,6 +3,7 @@
#include "Vector.hpp"
#include "Version.hpp"
#include "Math.hpp"
namespace yasim {
@ -15,64 +16,81 @@ public:
~Wing();
// Do we mirror ourselves about the XZ plane?
void setMirror(bool mirror);
// Wing geometry:
void setBase(float* base); // in local coordinates
void setLength(float length); // dist. ALONG wing (not span!)
void setChord(float chord); // at base, measured along X axis
void setTaper(float taper); // fraction, 0-1
void setSweep(float sweep); // radians
void setDihedral(float dihedral); // radians, positive is "up"
void setStall(float aoa);
void setStallWidth(float angle);
void setStallPeak(float fraction);
void setTwist(float angle);
void setCamber(float camber);
void setMirror(bool mirror) { _mirror = mirror; }
bool isMirrored() { return _mirror; };
// Wing geometry in local coordinates:
// base point of wing
void setBase(const float* base) { Math::set3(base, _base); }
void getBase(float* base) { Math::set3(_base, base); };
// dist. ALONG wing (not span!)
void setLength(float length) { _length = length; }
float getLength() { return _length; };
// at base, measured along X axis
void setChord(float chord) { _chord = chord; }
float getChord() { return _chord; };
// fraction of chord at wing tip, 0..1
void setTaper(float taper) { _taper = taper; }
float getTaper() { return _taper; };
// radians
void setSweep(float sweep) { _sweep = sweep; }
float getSweep() { return _sweep; };
// radians, positive is "up"
void setDihedral(float dihedral) { _dihedral = dihedral; }
float getDihedral() { return _dihedral; };
void setIncidence(float incidence);
void setTwist(float angle) { _twist = angle; }
// 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 setFlap0(float start, float end, float lift, float drag);
void setFlap1(float start, float end, float lift, float drag);
void setSpoiler(float start, float end, float lift, float drag);
void setSlat(float start, float end, float aoa, float 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 setFlap0(float lval, float rval);
void setFlap1(float lval, float rval);
void setSpoiler(float lval, float rval);
void setSlat(float val);
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);
bool isMirrored();
// Ground effect information
float getGroundEffect(float* posOut);
void getTip(float* tip) { Math::set3(_tip, tip);};
// Query the list of Surface objects
int numSurfaces();
Surface* getSurface(int n);
float getSurfaceWeight(int n);
// valid only after Wing::compile() was called
float getSpan() { return _wingspan; };
float getArea() { return _wingspan*_meanChord; };
float getAspectRatio() { return _aspectRatio; };
float getSMC() { return _meanChord; };
int numSurfaces() { return _surfs.size(); }
Surface* getSurface(int n) { return ((SurfRec*)_surfs.get(n))->surface; }
float getSurfaceWeight(int n) { 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();
float getDragScale() { 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();
float getLiftRatio() { return _liftRatio; }
private:
void interp(float* v1, float* v2, float frac, float* out);
void interp(const float* v1, const float* v2, const float frac, float* out);
Surface* newSurface(float* pos, float* orient, float chord,
bool flap0, bool flap1, bool slat, bool spoiler);
@ -92,6 +110,12 @@ private:
float _taper;
float _sweep;
float _dihedral;
// calculated from above
float _tip[3];
float _meanChord; // std. mean chord
float _wingspan;
float _aspectRatio;
float _stall;
float _stallWidth;

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

@ -9,6 +9,7 @@
#include "FGFDM.hpp"
#include "Atmosphere.hpp"
#include "RigidBody.hpp"
#include "Airplane.hpp"
using namespace yasim;
@ -27,9 +28,17 @@ bool fgSetDouble (const char * name, double defaultValue = 0.0) { return 0; }
static const float RAD2DEG = 57.2957795131;
static const float DEG2RAD = 0.0174532925199;
/// knots 2 meters per second
static const float KTS2MPS = 0.514444444444;
enum Config
{
CONFIG_NONE,
CONFIG_APPROACH,
CONFIG_CRUISE,
};
// Generate a graph of lift, drag and L/D against AoA at the specified
// speed and altitude. The result is a space-separated file of
// numbers: "aoa lift drag LD" (aoa in degrees, lift and drag in
@ -40,92 +49,218 @@ static const float KTS2MPS = 0.514444444444;
"dat" using 1:3 with lines title 'drag', \
"dat" using 1:4 with lines title 'LD'
*/
void yasim_graph(Airplane* a, float alt, float kts)
void yasim_graph(Airplane* a, const float alt, const float kts, int cfg = CONFIG_NONE)
{
Model* m = a->getModel();
State s;
Model* m = a->getModel();
State s;
m->setAir(Atmosphere::getStdPressure(alt),
Atmosphere::getStdTemperature(alt),
Atmosphere::getStdDensity(alt));
m->getBody()->recalc();
m->setAir(Atmosphere::getStdPressure(alt),
Atmosphere::getStdTemperature(alt),
Atmosphere::getStdDensity(alt));
for(int deg=-179; deg<=179; deg++) {
float aoa = deg * DEG2RAD;
Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
switch (cfg) {
case CONFIG_APPROACH:
a->loadApproachControls();
break;
case CONFIG_CRUISE:
a->loadCruiseControls();
break;
case CONFIG_NONE:
break;
}
//if we fake the properties we could also use FGFDM::getExternalInput()
float acc[3];
m->getBody()->getAccel(acc);
Math::tmul33(s.orient, acc, acc);
m->getBody()->recalc();
float cl_max = 0, cd_min = 1e6, ld_max = 0;
int cl_max_deg = 0, cd_min_deg = 0, ld_max_deg = 0;
float drag = acc[0] * (-1/9.8);
float lift = 1 + acc[2] * (1/9.8);
for(int deg=-15; deg<=90; deg++) {
float aoa = deg * DEG2RAD;
Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
printf("%d %g %g %g\n", deg, lift, drag, lift/drag);
float acc[3];
m->getBody()->getAccel(acc);
Math::tmul33(s.orient, acc, acc);
float drag = acc[0] * (-1/9.8);
float lift = 1 + acc[2] * (1/9.8);
float ld = lift/drag;
if (cd_min > drag) {
cd_min = drag;
cd_min_deg = deg;
}
if (cl_max < lift) {
cl_max = lift;
cl_max_deg = deg;
}
if (ld_max < ld) {
ld_max= ld;
ld_max_deg = deg;
}
printf("%d %g %g %g\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);
printf("# ld_max %g at %d deg\n", ld_max, ld_max_deg);
}
void yasim_masses(Airplane* a)
{
RigidBody* body = a->getModel()->getBody();
int i, N = body->numMasses();
float pos[3];
float m, mass = 0;
printf("id posx posy posz mass\n");
for (i = 0; i < N; i++)
{
body->getMassPosition(i, pos);
m = body->getMass(i);
printf("%d %.3f %.3f %.3f %.3f\n", i, pos[0], pos[1], pos[2], m);
mass += m;
}
printf("Total mass: %g", mass);
}
void yasim_drag(Airplane* a, const float aoa, const float alt, int cfg = CONFIG_NONE)
{
fprintf(stderr,"yasim_drag");
Model* m = a->getModel();
State s;
m->setAir(Atmosphere::getStdPressure(alt),
Atmosphere::getStdTemperature(alt),
Atmosphere::getStdDensity(alt));
switch (cfg) {
case CONFIG_APPROACH:
a->loadApproachControls();
break;
case CONFIG_CRUISE:
a->loadCruiseControls();
break;
case CONFIG_NONE:
break;
}
m->getBody()->recalc();
float cd_min = 1e6;
int cd_min_kts = 0;
printf("#kts, drag\n");
for(int kts=15; kts<=150; kts++) {
Airplane::setupState(aoa, kts * KTS2MPS, 0 ,&s);
m->getBody()->reset();
m->initIteration();
m->calcForces(&s);
float acc[3];
m->getBody()->getAccel(acc);
Math::tmul33(s.orient, acc, acc);
float drag = acc[0] * (-1/9.8);
if (cd_min > drag) {
cd_min = drag;
cd_min_kts = kts;
}
printf("%d %g\n", kts, drag);
}
printf("# cd_min %g at %d kts\n", cd_min, cd_min_kts);
}
int usage()
{
fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts]]\n");
return 1;
fprintf(stderr, "Usage: yasim <ac.xml> [-g [-a alt] [-s kts] [-approach | -cruise] ]\n");
fprintf(stderr, " yasim <ac.xml> [-d [-a alt] [-approach | -cruise] ]\n");
fprintf(stderr, " yasim <ac.xml> [-m]\n");
fprintf(stderr, " -g print lift/drag table: aoa, lift, drag, lift/drag \n");
fprintf(stderr, " -d print drag over TAS: kts, drag\n");
fprintf(stderr, " -m print mass distribution table: id, x, y, z, mass \n");
return 1;
}
int main(int argc, char** argv)
{
FGFDM* fdm = new FGFDM();
Airplane* a = fdm->getAirplane();
FGFDM* fdm = new FGFDM();
Airplane* a = fdm->getAirplane();
if(argc < 2) return usage();
if(argc < 2) return usage();
// Read
try {
string file = argv[1];
readXML(SGPath(file), *fdm);
}
catch (const sg_exception &e) {
printf("XML parse error: %s (%s)\n", e.getFormattedMessage().c_str(), e.getOrigin());
}
// Read
try {
string file = argv[1];
readXML(SGPath(file), *fdm);
} catch (const sg_exception &e) {
printf("XML parse error: %s (%s)\n",
e.getFormattedMessage().c_str(), e.getOrigin());
}
// ... and run
a->compile();
if(a->getFailureMsg())
printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
if(!a->getFailureMsg() && argc > 2 && strcmp(argv[2], "-g") == 0) {
float alt = 5000, kts = 100;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) alt = std::atof(argv[++i]);
else if(std::strcmp(argv[i], "-s") == 0) kts = std::atof(argv[++i]);
else return usage();
// ... and run
a->compile();
if(a->getFailureMsg())
printf("SOLUTION FAILURE: %s\n", a->getFailureMsg());
if(!a->getFailureMsg() && argc > 2 ) {
if(strcmp(argv[2], "-g") == 0) {
float alt = 5000, kts = 100;
int cfg = CONFIG_NONE;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
yasim_graph(a, alt, kts);
} else {
float aoa = a->getCruiseAoA() * RAD2DEG;
float tail = -1 * a->getTailIncidence() * RAD2DEG;
float drag = 1000 * a->getDragCoefficient();
float cg[3];
a->getModel()->getBody()->getCG(cg);
a->getModel()->getBody()->recalc();
float SI_inertia[9];
a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
printf("Solution results:");
printf(" Iterations: %d\n", a->getSolutionIterations());
printf(" Drag Coefficient: %f\n", drag);
printf(" Lift Ratio: %f\n", a->getLiftRatio());
printf(" Cruise AoA: %f\n", aoa);
printf(" Tail Incidence: %f\n", tail);
printf("Approach Elevator: %f\n", a->getApproachElevator());
printf(" CG: x:%.3f, y:%.3f, z:%.3f\n\n", cg[0], cg[1], cg[2]);
printf(" Inertia tensor : %.3f, %.3f, %.3f\n", SI_inertia[0], SI_inertia[1], SI_inertia[2]);
printf(" [kg*m^2] %.3f, %.3f, %.3f\n", SI_inertia[3], SI_inertia[4], SI_inertia[5]);
printf(" Origo at CG %.3f, %.3f, %.3f\n", SI_inertia[6], SI_inertia[7], SI_inertia[8]);
else if(std::strcmp(argv[i], "-s") == 0) {
if(i+1 < argc) kts = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
else return usage();
}
yasim_graph(a, alt, kts, cfg);
}
else if(strcmp(argv[2], "-d") == 0) {
float alt = 2000, aoa = a->getCruiseAoA();
int cfg = CONFIG_NONE;
for(int i=3; i<argc; i++) {
if (std::strcmp(argv[i], "-a") == 0) {
if (i+1 < argc) alt = std::atof(argv[++i]);
}
else if(std::strcmp(argv[i], "-approach") == 0) cfg = CONFIG_APPROACH;
else if(std::strcmp(argv[i], "-cruise") == 0) cfg = CONFIG_CRUISE;
else return usage();
}
yasim_drag(a, aoa, alt, cfg);
}
else if(strcmp(argv[2], "-m") == 0) {
yasim_masses(a);
}
}
else {
printf("Solution results:");
float aoa = a->getCruiseAoA() * RAD2DEG;
float tail = -1 * a->getTailIncidence() * RAD2DEG;
float drag = 1000 * a->getDragCoefficient();
float cg[3];
a->getModel()->getBody()->getCG(cg);
a->getModel()->getBody()->recalc();
float SI_inertia[9];
a->getModel()->getBody()->getInertiaMatrix(SI_inertia);
printf(" Iterations: %d\n", a->getSolutionIterations());
printf(" Drag Coefficient: %f\n", drag);
printf(" Lift Ratio: %f\n", a->getLiftRatio());
printf(" Cruise AoA: %f\n", aoa);
printf(" Tail Incidence: %f\n", tail);
printf("Approach Elevator: %f\n", a->getApproachElevator());
printf(" CG: x:%.3f, y:%.3f, z:%.3f\n\n", cg[0], cg[1], cg[2]);
printf("Inertia tensor [kg*m^2], origo at CG:\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]);
}
delete fdm;
return 0;
}