Initial revision of Andy Ross's YASim code. This is (Y)et (A)nother Flight
Dynamics (Sim)ulator. Basically, this is a rough, first cut of a "different take" on FDM design. It's intended to be very simple to use, producing reasonable results for aircraft of all sorts and sizes, while maintaining simulation plausibility even in odd flight conditions like spins and aerobatics. It's at the point now where one can actually fly the planes around.
This commit is contained in:
curt
parent
bc391fb54e
commit
5b84ae51a5
39 changed files with 5667 additions and 1 deletions
|
|
@ -1,4 +1,4 @@
|
|||
SUBDIRS = Balloon JSBSim LaRCsim UIUCModel
|
||||
SUBDIRS = Balloon JSBSim LaRCsim UIUCModel YASim
|
||||
|
||||
noinst_LIBRARIES = libFlight.a
|
||||
|
||||
|
|
|
|||
799
src/FDM/YASim/Airplane.cpp
Normal file
799
src/FDM/YASim/Airplane.cpp
Normal file
|
|
@ -0,0 +1,799 @@
|
|||
#include "Atmosphere.hpp"
|
||||
#include "ControlMap.hpp"
|
||||
#include "Gear.hpp"
|
||||
#include "Math.hpp"
|
||||
#include "Glue.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
#include "Surface.hpp"
|
||||
#include "Thruster.hpp"
|
||||
|
||||
#include "Airplane.hpp"
|
||||
namespace yasim {
|
||||
|
||||
// FIXME: hook gear extension into the force calculation somehow...
|
||||
|
||||
Airplane::Airplane()
|
||||
{
|
||||
_emptyWeight = 0;
|
||||
_pilotPos[0] = _pilotPos[1] = _pilotPos[2] = 0;
|
||||
_wing = 0;
|
||||
_tail = 0;
|
||||
_ballast = 0;
|
||||
_cruiseRho = 0;
|
||||
_cruiseSpeed = 0;
|
||||
_cruiseWeight = 0;
|
||||
_approachRho = 0;
|
||||
_approachSpeed = 0;
|
||||
_approachAoA = 0;
|
||||
_approachWeight = 0;
|
||||
|
||||
_dragFactor = 1;
|
||||
_liftRatio = 1;
|
||||
_cruiseAoA = 0;
|
||||
_tailIncidence = 0;
|
||||
}
|
||||
|
||||
Airplane::~Airplane()
|
||||
{
|
||||
for(int i=0; i<_fuselages.size(); i++)
|
||||
delete (Fuselage*)_fuselages.get(i);
|
||||
for(int i=0; i<_tanks.size(); i++)
|
||||
delete (Tank*)_tanks.get(i);
|
||||
for(int i=0; i<_thrusters.size(); i++)
|
||||
delete (ThrustRec*)_thrusters.get(i);
|
||||
for(int i=0; i<_gears.size(); i++)
|
||||
delete (GearRec*)_gears.get(i);
|
||||
for(int i=0; i<_surfs.size(); i++)
|
||||
delete (Surface*)_surfs.get(i);
|
||||
}
|
||||
|
||||
void Airplane::iterate(float dt)
|
||||
{
|
||||
_model.iterate();
|
||||
|
||||
// Consume fuel
|
||||
// FIXME
|
||||
}
|
||||
|
||||
ControlMap* Airplane::getControlMap()
|
||||
{
|
||||
return &_controls;
|
||||
}
|
||||
|
||||
Model* Airplane::getModel()
|
||||
{
|
||||
return &_model;
|
||||
}
|
||||
|
||||
void Airplane::getPilotAccel(float* out)
|
||||
{
|
||||
State* s = _model.getState();
|
||||
|
||||
// Gravity
|
||||
Glue::geodUp(s->pos, out);
|
||||
Math::mul3(-9.8, out, out);
|
||||
|
||||
// The regular acceleration
|
||||
float tmp[3];
|
||||
Math::mul3(-1, s->acc, tmp);
|
||||
Math::add3(tmp, out, out);
|
||||
|
||||
// Convert to aircraft coordinates
|
||||
Math::vmul33(s->orient, out, out);
|
||||
|
||||
// FIXME: rotational & centripetal acceleration needed
|
||||
}
|
||||
|
||||
void Airplane::setPilotPos(float* pos)
|
||||
{
|
||||
for(int i=0; i<3; i++) _pilotPos[i] = pos[i];
|
||||
}
|
||||
|
||||
void Airplane::getPilotPos(float* out)
|
||||
{
|
||||
for(int 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;
|
||||
}
|
||||
|
||||
void Airplane::setGearState(bool down, float dt)
|
||||
{
|
||||
for(int i=0; i<_gears.size(); i++) {
|
||||
GearRec* gr = (GearRec*)_gears.get(i);
|
||||
if(gr->time == 0) {
|
||||
// Non-extensible
|
||||
gr->gear->setExtension(1);
|
||||
gr->surf->setXDrag(1);
|
||||
gr->surf->setYDrag(1);
|
||||
gr->surf->setZDrag(1);
|
||||
continue;
|
||||
}
|
||||
|
||||
float diff = dt / gr->time;
|
||||
if(!down) diff = -diff;
|
||||
float ext = gr->gear->getExtension() + diff;
|
||||
if(ext < 0) ext = 0;
|
||||
if(ext > 1) ext = 1;
|
||||
|
||||
gr->gear->setExtension(ext);
|
||||
gr->surf->setXDrag(ext);
|
||||
gr->surf->setYDrag(ext);
|
||||
gr->surf->setZDrag(ext);
|
||||
}
|
||||
}
|
||||
|
||||
void Airplane::setApproach(float speed, float altitude)
|
||||
{
|
||||
// The zero AoA will become a calculated stall AoA in compile()
|
||||
setApproach(speed, altitude, 0);
|
||||
}
|
||||
|
||||
void Airplane::setApproach(float speed, float altitude, float aoa)
|
||||
{
|
||||
_approachSpeed = speed;
|
||||
_approachRho = Atmosphere::getStdDensity(altitude);
|
||||
_approachAoA = aoa;
|
||||
}
|
||||
|
||||
void Airplane::setCruise(float speed, float altitude)
|
||||
{
|
||||
_cruiseSpeed = speed;
|
||||
_cruiseRho = Atmosphere::getStdDensity(altitude);
|
||||
_cruiseAoA = 0;
|
||||
_tailIncidence = 0;
|
||||
}
|
||||
|
||||
void Airplane::addApproachControl(int control, float val)
|
||||
{
|
||||
Control* c = new Control();
|
||||
c->control = control;
|
||||
c->val = val;
|
||||
_approachControls.add(c);
|
||||
}
|
||||
|
||||
void Airplane::addCruiseControl(int control, float val)
|
||||
{
|
||||
Control* c = new Control();
|
||||
c->control = control;
|
||||
c->val = val;
|
||||
_cruiseControls.add(c);
|
||||
}
|
||||
|
||||
int Airplane::numTanks()
|
||||
{
|
||||
return _tanks.size();
|
||||
}
|
||||
|
||||
float Airplane::getFuel(int tank)
|
||||
{
|
||||
return ((Tank*)_tanks.get(tank))->fill;
|
||||
}
|
||||
|
||||
float Airplane::getFuelDensity(int tank)
|
||||
{
|
||||
return ((Tank*)_tanks.get(tank))->density;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
Fuselage* f = new Fuselage();
|
||||
for(int i=0; i<3; i++) {
|
||||
f->front[i] = front[i];
|
||||
f->back[i] = back[i];
|
||||
}
|
||||
f->width = width;
|
||||
_fuselages.add(f);
|
||||
}
|
||||
|
||||
int Airplane::addTank(float* pos, float cap, float density)
|
||||
{
|
||||
Tank* t = new Tank();
|
||||
for(int i=0; i<3; i++) t->pos[i] = pos[i];
|
||||
t->cap = cap;
|
||||
t->fill = cap;
|
||||
t->density = density;
|
||||
t->handle = 0xffffffff;
|
||||
return _tanks.add(t);
|
||||
}
|
||||
|
||||
void Airplane::addGear(Gear* gear, float transitionTime)
|
||||
{
|
||||
GearRec* g = new GearRec();
|
||||
g->gear = gear;
|
||||
g->surf = 0;
|
||||
g->time = transitionTime;
|
||||
_gears.add(g);
|
||||
}
|
||||
|
||||
void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
|
||||
{
|
||||
ThrustRec* t = new ThrustRec();
|
||||
t->thruster = thruster;
|
||||
t->mass = mass;
|
||||
for(int i=0; i<3; i++) t->cg[i] = cg[i];
|
||||
_thrusters.add(t);
|
||||
}
|
||||
|
||||
void Airplane::addBallast(float* pos, float mass)
|
||||
{
|
||||
_model.getBody()->addMass(mass, pos);
|
||||
_ballast += mass;
|
||||
}
|
||||
|
||||
int Airplane::addWeight(float* pos, float size)
|
||||
{
|
||||
WeightRec* wr = new WeightRec();
|
||||
wr->handle = _model.getBody()->addMass(0, pos);
|
||||
|
||||
wr->surf = new Surface();
|
||||
wr->surf->setTotalDrag(size*size);
|
||||
_model.addSurface(wr->surf);
|
||||
_surfs.add(wr->surf);
|
||||
|
||||
return _weights.add(wr);
|
||||
}
|
||||
|
||||
void Airplane::setWeight(int handle, float mass)
|
||||
{
|
||||
WeightRec* wr = (WeightRec*)_weights.get(handle);
|
||||
|
||||
_model.getBody()->setMass(wr->handle, mass);
|
||||
|
||||
// Kill the aerodynamic drag if the mass is exactly zero. This is
|
||||
// how we simulate droppable stores.
|
||||
if(mass == 0) {
|
||||
wr->surf->setXDrag(0);
|
||||
wr->surf->setYDrag(0);
|
||||
wr->surf->setZDrag(0);
|
||||
} else {
|
||||
wr->surf->setXDrag(1);
|
||||
wr->surf->setYDrag(1);
|
||||
wr->surf->setZDrag(1);
|
||||
}
|
||||
}
|
||||
|
||||
void Airplane::setFuelFraction(float frac)
|
||||
{
|
||||
for(int i=0; i<_tanks.size(); i++) {
|
||||
Tank* t = (Tank*)_tanks.get(i);
|
||||
_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;
|
||||
}
|
||||
|
||||
char* Airplane::getFailureMsg()
|
||||
{
|
||||
return _failureMsg;
|
||||
}
|
||||
|
||||
int Airplane::getSolutionIterations()
|
||||
{
|
||||
return _solutionIterations;
|
||||
}
|
||||
|
||||
void Airplane::setupState(float aoa, float speed, State* s)
|
||||
{
|
||||
float cosAoA = Math::cos(aoa);
|
||||
float sinAoA = Math::sin(aoa);
|
||||
s->orient[0] = cosAoA; s->orient[1] = 0; s->orient[2] = sinAoA;
|
||||
s->orient[3] = 0; s->orient[4] = 1; s->orient[5] = 0;
|
||||
s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
|
||||
|
||||
s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
|
||||
|
||||
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
|
||||
// Model goes nuts
|
||||
s->pos[2] = 1;
|
||||
}
|
||||
|
||||
float Airplane::compileWing(Wing* w)
|
||||
{
|
||||
// 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;
|
||||
for(int i=0; i<w->numSurfaces(); i++) {
|
||||
Surface* s = (Surface*)w->getSurface(i);
|
||||
_model.addSurface(s);
|
||||
|
||||
float pos[3];
|
||||
s->getPosition(pos);
|
||||
_model.getBody()->addMass(w->getSurfaceWeight(i), pos);
|
||||
wgt += w->getSurfaceWeight(i);
|
||||
}
|
||||
return wgt;
|
||||
}
|
||||
|
||||
float Airplane::compileFuselage(Fuselage* f)
|
||||
{
|
||||
float wgt = 0;
|
||||
float fwd[3];
|
||||
Math::sub3(f->front, f->back, fwd);
|
||||
float len = Math::mag3(fwd);
|
||||
float wid = f->width;
|
||||
int segs = (int)Math::ceil(len/wid);
|
||||
float segWgt = wid*wid/segs;
|
||||
for(int j=0; j<segs; j++) {
|
||||
float frac = (j+0.5) / segs;
|
||||
float pos[3];
|
||||
Math::mul3(frac, fwd, pos);
|
||||
Math::add3(f->back, pos, pos);
|
||||
_model.getBody()->addMass(segWgt, pos);
|
||||
wgt += segWgt;
|
||||
|
||||
// Make a Surface too
|
||||
Surface* s = new Surface();
|
||||
s->setPosition(pos);
|
||||
float sideDrag = len/wid;
|
||||
s->setYDrag(sideDrag);
|
||||
s->setZDrag(sideDrag);
|
||||
s->setTotalDrag(segWgt);
|
||||
|
||||
// FIXME: fails for fuselages aligned along the Y axis
|
||||
float o[9];
|
||||
float *x=o, *y=o+3, *z=o+6; // nicknames for the axes
|
||||
Math::unit3(fwd, x);
|
||||
y[0] = 0; y[1] = 1; y[2] = 0;
|
||||
Math::cross3(x, y, z);
|
||||
s->setOrientation(o);
|
||||
|
||||
_model.addSurface(s);
|
||||
_surfs.add(s);
|
||||
}
|
||||
return wgt;
|
||||
}
|
||||
|
||||
// FIXME: should probably add a mass for the gear, too
|
||||
void Airplane::compileGear(GearRec* gr)
|
||||
{
|
||||
Gear* g = gr->gear;
|
||||
|
||||
// Make a Surface object for the aerodynamic behavior
|
||||
Surface* s = new Surface();
|
||||
gr->surf = s;
|
||||
|
||||
// Put the surface at the half-way point on the gear strut, give
|
||||
// it a drag coefficient equal to a square of the same dimension
|
||||
// (gear are really draggy) and make it symmetric.
|
||||
float pos[3], cmp[3];
|
||||
g->getCompression(cmp);
|
||||
float length = Math::mag3(cmp);
|
||||
g->getPosition(pos);
|
||||
Math::mul3(0.5, cmp, cmp);
|
||||
Math::add3(pos, cmp, pos);
|
||||
|
||||
s->setPosition(pos);
|
||||
s->setTotalDrag(length*length);
|
||||
|
||||
_model.addGear(g);
|
||||
_model.addSurface(s);
|
||||
_surfs.add(s);
|
||||
}
|
||||
|
||||
void Airplane::compile()
|
||||
{
|
||||
double ground[3];
|
||||
ground[0] = 0; ground[1] = 0; ground[2] = 1;
|
||||
_model.setGroundPlane(ground, -100000);
|
||||
|
||||
RigidBody* body = _model.getBody();
|
||||
int firstMass = body->numMasses();
|
||||
|
||||
// Generate the point masses for the plane. Just use unitless
|
||||
// numbers for a first pass, then go back through and rescale to
|
||||
// make the weight right.
|
||||
float aeroWgt = 0;
|
||||
|
||||
// The Wing objects
|
||||
aeroWgt += compileWing(_wing);
|
||||
aeroWgt += compileWing(_tail);
|
||||
for(int i=0; i<_vstabs.size(); i++) {
|
||||
aeroWgt += compileWing((Wing*)_vstabs.get(i));
|
||||
}
|
||||
|
||||
// The fuselage(s)
|
||||
for(int i=0; i<_fuselages.size(); i++) {
|
||||
aeroWgt += compileFuselage((Fuselage*)_fuselages.get(i));
|
||||
}
|
||||
|
||||
// Count up the absolute weight we have
|
||||
float nonAeroWgt = _ballast;
|
||||
for(int i=0; i<_thrusters.size(); i++)
|
||||
nonAeroWgt += ((ThrustRec*)_thrusters.get(i))->mass;
|
||||
|
||||
// Rescale to the specified empty weight
|
||||
float wscale = (_emptyWeight-nonAeroWgt)/aeroWgt;
|
||||
for(int i=firstMass; i<body->numMasses(); i++)
|
||||
body->setMass(i, body->getMass(i)*wscale);
|
||||
|
||||
// Add the thruster masses
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
ThrustRec* t = (ThrustRec*)_thrusters.get(i);
|
||||
body->addMass(t->mass, t->cg);
|
||||
}
|
||||
|
||||
// Add the tanks, empty for now.
|
||||
float totalFuel = 0;
|
||||
for(int i=0; i<_tanks.size(); i++) {
|
||||
Tank* t = (Tank*)_tanks.get(i);
|
||||
t->handle = body->addMass(0, t->pos);
|
||||
totalFuel += t->cap;
|
||||
}
|
||||
_cruiseWeight = _emptyWeight + totalFuel*0.5;
|
||||
_approachWeight = _emptyWeight + totalFuel*0.2;
|
||||
|
||||
body->recalc();
|
||||
|
||||
// Add surfaces for the landing gear.
|
||||
for(int i=0; i<_gears.size(); i++)
|
||||
compileGear((GearRec*)_gears.get(i));
|
||||
|
||||
// The Thruster objects
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
|
||||
tr->handle = _model.addThruster(tr->thruster);
|
||||
}
|
||||
|
||||
solveGear();
|
||||
solve();
|
||||
|
||||
// Drop the gear (use a really big dt)
|
||||
setGearState(true, 1000000);
|
||||
}
|
||||
|
||||
void Airplane::solveGear()
|
||||
{
|
||||
float cg[3], pos[3];
|
||||
_model.getBody()->getCG(cg);
|
||||
|
||||
// Calculate spring constant weightings for the gear. Weight by
|
||||
// the inverse of the distance to the c.g. in the XY plane, which
|
||||
// should be correct for most gear arrangements. Add 50cm of
|
||||
// "buffer" to keep things from blowing up with aircraft with a
|
||||
// single gear very near the c.g. (AV-8, for example).
|
||||
float total = 0;
|
||||
for(int i=0; i<_gears.size(); i++) {
|
||||
GearRec* gr = (GearRec*)_gears.get(i);
|
||||
Gear* g = gr->gear;
|
||||
g->getPosition(pos);
|
||||
Math::sub3(cg, pos, pos);
|
||||
gr->wgt = 1/(0.5+Math::sqrt(pos[0]*pos[0] + pos[1]*pos[1]));
|
||||
total += gr->wgt;
|
||||
}
|
||||
|
||||
// Renormalize so they sum to 1
|
||||
for(int i=0; i<_gears.size(); i++)
|
||||
((GearRec*)_gears.get(i))->wgt /= total;
|
||||
|
||||
// The force at max compression should be sufficient to stop a
|
||||
// plane moving downwards at 3x the approach descent rate. Assume
|
||||
// a 3 degree approach.
|
||||
float descentRate = 3*_approachSpeed/19.1;
|
||||
|
||||
// Spread the kinetic energy according to the gear weights. This
|
||||
// will results in an equal compression fraction (not distance) of
|
||||
// each gear.
|
||||
float energy = 0.5*_approachWeight*descentRate*descentRate;
|
||||
|
||||
for(int i=0; i<_gears.size(); i++) {
|
||||
GearRec* gr = (GearRec*)_gears.get(i);
|
||||
float e = energy * gr->wgt;
|
||||
float comp[3];
|
||||
gr->gear->getCompression(comp);
|
||||
float len = Math::mag3(comp);
|
||||
|
||||
// Energy in a spring: e = 0.5 * k * len^2
|
||||
float k = 2 * e / (len*len);
|
||||
|
||||
gr->gear->setSpring(k);
|
||||
|
||||
// Critically damped (too damped, too!)
|
||||
gr->gear->setDamping(2*Math::sqrt(k*_approachWeight*gr->wgt));
|
||||
|
||||
// These are pretty generic
|
||||
gr->gear->setStaticFriction(0.8);
|
||||
gr->gear->setDynamicFriction(0.7);
|
||||
}
|
||||
}
|
||||
|
||||
void Airplane::stabilizeThrust()
|
||||
{
|
||||
float thrust[3], tmp[3];
|
||||
float last = 0;
|
||||
while(1) {
|
||||
thrust[0] = thrust[1] = thrust[2] = 0;
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
Thruster* t = _model.getThruster(i);
|
||||
t->integrate(0.033);
|
||||
t->getThrust(tmp);
|
||||
Math::add3(thrust, tmp, thrust);
|
||||
}
|
||||
|
||||
float t = Math::mag3(thrust);
|
||||
float ratio = (t+0.1)/(last+0.1);
|
||||
if(ratio < 1.00001 && ratio > 0.99999)
|
||||
break;
|
||||
|
||||
last = t;
|
||||
}
|
||||
}
|
||||
|
||||
void Airplane::runCruise()
|
||||
{
|
||||
setupState(_cruiseAoA, _cruiseSpeed, &_cruiseState);
|
||||
_model.setState(&_cruiseState);
|
||||
_model.setAirDensity(_cruiseRho);
|
||||
|
||||
// The control configuration
|
||||
_controls.reset();
|
||||
for(int i=0; i<_cruiseControls.size(); i++) {
|
||||
Control* c = (Control*)_cruiseControls.get(i);
|
||||
_controls.setInput(c->control, c->val);
|
||||
}
|
||||
_controls.applyControls();
|
||||
|
||||
// The local wind
|
||||
float wind[3];
|
||||
Math::mul3(-1, _cruiseState.v, wind);
|
||||
Math::vmul33(_cruiseState.orient, wind, wind);
|
||||
|
||||
// Gear are up (if they're non-retractable, this is a noop)
|
||||
setGearState(false, 100000);
|
||||
|
||||
// Cruise is by convention at 50% tank capacity
|
||||
setFuelFraction(0.5);
|
||||
|
||||
// Set up the thruster parameters and iterate until the thrust
|
||||
// stabilizes.
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
|
||||
t->setWind(wind);
|
||||
t->setDensity(_cruiseRho);
|
||||
}
|
||||
stabilizeThrust();
|
||||
|
||||
// Precompute thrust in the model, and calculate aerodynamic forces
|
||||
_model.getBody()->reset();
|
||||
_model.initIteration();
|
||||
_model.calcForces(&_cruiseState);
|
||||
}
|
||||
|
||||
void Airplane::runApproach()
|
||||
{
|
||||
setupState(_approachAoA, _approachSpeed, &_approachState);
|
||||
_model.setState(&_approachState);
|
||||
_model.setAirDensity(_approachRho);
|
||||
|
||||
// The control configuration
|
||||
_controls.reset();
|
||||
for(int i=0; i<_approachControls.size(); i++) {
|
||||
Control* c = (Control*)_approachControls.get(i);
|
||||
_controls.setInput(c->control, c->val);
|
||||
}
|
||||
_controls.applyControls();
|
||||
|
||||
// The local wind
|
||||
float wind[3];
|
||||
Math::mul3(-1, _approachState.v, wind);
|
||||
Math::vmul33(_approachState.orient, wind, wind);
|
||||
|
||||
// Approach is by convention at 20% tank capacity
|
||||
setFuelFraction(0.2);
|
||||
|
||||
// Gear are down
|
||||
setGearState(true, 100000);
|
||||
|
||||
// Run the thrusters until they get to a stable setting. FIXME:
|
||||
// this is lots of wasted work.
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
|
||||
t->setWind(wind);
|
||||
t->setDensity(_approachRho);
|
||||
}
|
||||
stabilizeThrust();
|
||||
|
||||
// Precompute thrust in the model, and calculate aerodynamic forces
|
||||
_model.getBody()->reset();
|
||||
_model.initIteration();
|
||||
_model.calcForces(&_approachState);
|
||||
}
|
||||
|
||||
void Airplane::applyDragFactor(float factor)
|
||||
{
|
||||
float applied = Math::sqrt(factor);
|
||||
_dragFactor *= applied;
|
||||
_wing->setDragScale(_wing->getDragScale() * applied);
|
||||
_tail->setDragScale(_tail->getDragScale() * applied);
|
||||
for(int i=0; i<_vstabs.size(); i++) {
|
||||
Wing* w = (Wing*)_vstabs.get(i);
|
||||
w->setDragScale(w->getDragScale() * applied);
|
||||
}
|
||||
for(int i=0; i<_surfs.size(); i++) {
|
||||
Surface* s = (Surface*)_surfs.get(i);
|
||||
s->setTotalDrag(s->getTotalDrag() * applied);
|
||||
}
|
||||
}
|
||||
|
||||
void Airplane::applyLiftRatio(float factor)
|
||||
{
|
||||
float applied = Math::sqrt(factor);
|
||||
_liftRatio *= applied;
|
||||
_wing->setLiftRatio(_wing->getLiftRatio() * applied);
|
||||
_tail->setLiftRatio(_tail->getLiftRatio() * applied);
|
||||
for(int i=0; i<_vstabs.size(); i++) {
|
||||
Wing* w = (Wing*)_vstabs.get(i);
|
||||
w->setLiftRatio(w->getLiftRatio() * applied);
|
||||
}
|
||||
}
|
||||
|
||||
float Airplane::clamp(float val, float min, float max)
|
||||
{
|
||||
if(val < min) return min;
|
||||
if(val > max) return max;
|
||||
return val;
|
||||
}
|
||||
|
||||
float Airplane::normFactor(float f)
|
||||
{
|
||||
if(f < 0) f = -f;
|
||||
if(f < 1) f = 1/f;
|
||||
return f;
|
||||
}
|
||||
|
||||
void Airplane::solve()
|
||||
{
|
||||
static const float ARCMIN = 0.0002909;
|
||||
|
||||
float tmp[3];
|
||||
_solutionIterations = 0;
|
||||
_failureMsg = 0;
|
||||
while(1) {
|
||||
if(_solutionIterations++ > 10000) {
|
||||
_failureMsg = "Solution failed to converge after 10000 iterations";
|
||||
return;
|
||||
}
|
||||
|
||||
// Run an iteration at cruise, and extract the needed numbers:
|
||||
runCruise();
|
||||
|
||||
_model.getThrust(tmp);
|
||||
float thrust = tmp[0];
|
||||
|
||||
_model.getBody()->getAccel(tmp);
|
||||
float xforce = _cruiseWeight * tmp[0];
|
||||
float clift0 = _cruiseWeight * tmp[2];
|
||||
|
||||
_model.getBody()->getAngularAccel(tmp);
|
||||
float pitch0 = tmp[1];
|
||||
|
||||
// Run an approach iteration, and do likewise
|
||||
runApproach();
|
||||
|
||||
_model.getBody()->getAccel(tmp);
|
||||
float alift = _approachWeight * tmp[2];
|
||||
|
||||
// Modify the cruise AoA a bit to get a derivative
|
||||
_cruiseAoA += ARCMIN;
|
||||
runCruise();
|
||||
_cruiseAoA -= ARCMIN;
|
||||
|
||||
_model.getBody()->getAccel(tmp);
|
||||
float clift1 = _cruiseWeight * tmp[2];
|
||||
|
||||
// Do the same with the tail incidence
|
||||
_tail->setIncidence(_tailIncidence + ARCMIN);
|
||||
runCruise();
|
||||
_tail->setIncidence(_tailIncidence);
|
||||
|
||||
_model.getBody()->getAngularAccel(tmp);
|
||||
float pitch1 = tmp[1];
|
||||
|
||||
// Now calculate:
|
||||
float awgt = 9.8 * _approachWeight;
|
||||
|
||||
float dragFactor = thrust / (thrust-xforce);
|
||||
float liftFactor = awgt / (awgt+alift);
|
||||
float aoaDelta = -clift0 * (ARCMIN/(clift1-clift0));
|
||||
float tailDelta = -pitch0 * (ARCMIN/(pitch1-pitch0));
|
||||
|
||||
// Sanity:
|
||||
if(dragFactor <= 0) {
|
||||
_failureMsg = "Zero or negative drag adjustment.";
|
||||
return;
|
||||
} else if(liftFactor <= 0) {
|
||||
_failureMsg = "Zero or negative lift adjustment.";
|
||||
return;
|
||||
}
|
||||
|
||||
// And apply:
|
||||
applyDragFactor(dragFactor);
|
||||
applyLiftRatio(liftFactor);
|
||||
|
||||
// DON'T do the following until the above are sane
|
||||
if(normFactor(dragFactor) > 1.1
|
||||
|| normFactor(liftFactor) > 1.1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// OK, now we can adjust the minor variables
|
||||
_cruiseAoA += 0.5*aoaDelta;
|
||||
_tailIncidence += 0.5*tailDelta;
|
||||
|
||||
_cruiseAoA = clamp(_cruiseAoA, -.174, .174);
|
||||
_tailIncidence = clamp(_tailIncidence, -.174, .174);
|
||||
|
||||
if(dragFactor < 1.00001 && liftFactor < 1.00001 &&
|
||||
aoaDelta < .000017 && tailDelta < .000017)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if(_dragFactor < 1e-06 || _dragFactor > 1e6) {
|
||||
_failureMsg = "Drag factor beyond reasonable bounds.";
|
||||
return;
|
||||
} else if(_liftRatio < 1e-04 || _liftRatio > 1e4) {
|
||||
_failureMsg = "Lift ratio beyond reasonable bounds.";
|
||||
return;
|
||||
} else if(Math::abs(_cruiseAoA) >= .174) {
|
||||
_failureMsg = "Cruise AoA > 10 degrees";
|
||||
return;
|
||||
} else if(Math::abs(_tailIncidence) >= .174) {
|
||||
_failureMsg = "Tail incidence > 10 degrees";
|
||||
return;
|
||||
}
|
||||
}
|
||||
}; // namespace yasim
|
||||
135
src/FDM/YASim/Airplane.hpp
Normal file
135
src/FDM/YASim/Airplane.hpp
Normal file
|
|
@ -0,0 +1,135 @@
|
|||
#ifndef _AIRPLANE_HPP
|
||||
#define _AIRPLANE_HPP
|
||||
|
||||
#include "ControlMap.hpp"
|
||||
#include "Model.hpp"
|
||||
#include "Wing.hpp"
|
||||
#include "util/Vector.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Gear;
|
||||
class Thruster;
|
||||
|
||||
class Airplane {
|
||||
public:
|
||||
Airplane();
|
||||
~Airplane();
|
||||
|
||||
void iterate(float dt);
|
||||
|
||||
ControlMap* getControlMap();
|
||||
Model* getModel();
|
||||
|
||||
void setPilotPos(float* pos);
|
||||
void getPilotPos(float* out);
|
||||
|
||||
void getPilotAccel(float* out);
|
||||
|
||||
void setWeight(float weight);
|
||||
|
||||
void setWing(Wing* wing);
|
||||
void setTail(Wing* tail);
|
||||
void addVStab(Wing* vstab);
|
||||
|
||||
void addFuselage(float* front, float* back, float width);
|
||||
int addTank(float* pos, float cap, float fuelDensity);
|
||||
void addGear(Gear* g, float transitionTime);
|
||||
void addThruster(Thruster* t, float mass, float* cg);
|
||||
void addBallast(float* pos, float mass);
|
||||
|
||||
int addWeight(float* pos, float size);
|
||||
void setWeight(int handle, float mass);
|
||||
|
||||
void setApproach(float speed, float altitude);
|
||||
void setApproach(float speed, float altitude, float aoa);
|
||||
void setCruise(float speed, float altitude);
|
||||
|
||||
void addApproachControl(int control, float val);
|
||||
void addCruiseControl(int control, float val);
|
||||
|
||||
int numGear();
|
||||
Gear* getGear(int g);
|
||||
void setGearState(bool down, float dt);
|
||||
|
||||
int numTanks();
|
||||
void setFuelFraction(float frac); // 0-1, total amount of fuel
|
||||
float getFuel(int tank); // in kg!
|
||||
float getFuelDensity(int tank); // kg/m^3
|
||||
|
||||
void compile(); // generate point masses & such, then solve
|
||||
|
||||
// Solution output values
|
||||
int getSolutionIterations();
|
||||
float getDragCoefficient();
|
||||
float getLiftRatio();
|
||||
float getCruiseAoA();
|
||||
float getTailIncidence();
|
||||
char* getFailureMsg();
|
||||
|
||||
private:
|
||||
struct Tank { float pos[3]; float cap; float fill;
|
||||
float density; int handle; };
|
||||
struct Fuselage { float front[3], back[3], width; };
|
||||
struct GearRec { Gear* gear; Surface* surf; float wgt; float time; };
|
||||
struct ThrustRec { Thruster* thruster;
|
||||
int handle; float cg[3]; float mass; };
|
||||
struct Control { int control; float val; };
|
||||
struct WeightRec { int handle; Surface* surf; };
|
||||
|
||||
void runCruise();
|
||||
void runApproach();
|
||||
void setupState(float aoa, float speed, State* s);
|
||||
void solveGear();
|
||||
void solve();
|
||||
float compileWing(Wing* w);
|
||||
float compileFuselage(Fuselage* f);
|
||||
void compileGear(GearRec* gr);
|
||||
void stabilizeThrust();
|
||||
void applyDragFactor(float factor);
|
||||
void applyLiftRatio(float factor);
|
||||
float clamp(float val, float min, float max);
|
||||
float normFactor(float f);
|
||||
|
||||
Model _model;
|
||||
ControlMap _controls;
|
||||
|
||||
float _emptyWeight;
|
||||
float _pilotPos[3];
|
||||
|
||||
Wing* _wing;
|
||||
Wing* _tail;
|
||||
|
||||
Vector _fuselages;
|
||||
Vector _vstabs;
|
||||
Vector _tanks;
|
||||
Vector _thrusters;
|
||||
float _ballast;
|
||||
|
||||
Vector _gears;
|
||||
Vector _weights;
|
||||
Vector _surfs; // NON-wing Surfaces
|
||||
|
||||
Vector _cruiseControls;
|
||||
State _cruiseState;
|
||||
float _cruiseRho;
|
||||
float _cruiseSpeed;
|
||||
float _cruiseWeight;
|
||||
|
||||
Vector _approachControls;
|
||||
State _approachState;
|
||||
float _approachRho;
|
||||
float _approachSpeed;
|
||||
float _approachAoA;
|
||||
float _approachWeight;
|
||||
|
||||
int _solutionIterations;
|
||||
float _dragFactor;
|
||||
float _liftRatio;
|
||||
float _cruiseAoA;
|
||||
float _tailIncidence;
|
||||
char* _failureMsg;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _AIRPLANE_HPP
|
||||
120
src/FDM/YASim/Atmosphere.cpp
Normal file
120
src/FDM/YASim/Atmosphere.cpp
Normal file
|
|
@ -0,0 +1,120 @@
|
|||
#include "Math.hpp"
|
||||
#include "Atmosphere.hpp"
|
||||
namespace yasim {
|
||||
|
||||
// Copied from McCormick, who got it from "The ARDC Model Atmosphere"
|
||||
// Note that there's an error in the text in the first entry,
|
||||
// McCormick lists 299.16/101325/1.22500, but those don't agree with
|
||||
// R=287. I chose to correct the temperature to 288.20, since 79F is
|
||||
// pretty hot for a "standard" atmosphere.
|
||||
// meters kelvin kPa kg/m^3
|
||||
float Atmosphere::data[][4] = {{ 0, 288.20, 101325, 1.22500 },
|
||||
{ 900, 282.31, 90971, 1.12260 },
|
||||
{ 1800, 276.46, 81494, 1.02690 },
|
||||
{ 2700, 270.62, 72835, 0.93765 },
|
||||
{ 3600, 264.77, 64939, 0.85445 },
|
||||
{ 4500, 258.93, 57752, 0.77704 },
|
||||
{ 5400, 253.09, 51226, 0.70513 },
|
||||
{ 6300, 247.25, 45311, 0.63845 },
|
||||
{ 7200, 241.41, 39963, 0.57671 },
|
||||
{ 8100, 235.58, 35140, 0.51967 },
|
||||
{ 9000, 229.74, 30800, 0.46706 },
|
||||
{ 9900, 223.91, 26906, 0.41864 },
|
||||
{ 10800, 218.08, 23422, 0.37417 },
|
||||
{ 11700, 216.66, 20335, 0.32699 },
|
||||
{ 12600, 216.66, 17654, 0.28388 },
|
||||
{ 13500, 216.66, 15327, 0.24646 },
|
||||
{ 14400, 216.66, 13308, 0.21399 },
|
||||
{ 15300, 216.66, 11555, 0.18580 },
|
||||
{ 16200, 216.66, 10033, 0.16133 },
|
||||
{ 17100, 216.66, 8712, 0.14009 },
|
||||
{ 18000, 216.66, 7565, 0.12165 },
|
||||
{ 18900, 216.66, 6570, 0.10564 }};
|
||||
|
||||
float Atmosphere::getStdTemperature(float alt)
|
||||
{
|
||||
return getRecord(alt, 1);
|
||||
}
|
||||
|
||||
float Atmosphere::getStdPressure(float alt)
|
||||
{
|
||||
return getRecord(alt, 2);
|
||||
}
|
||||
|
||||
float Atmosphere::getStdDensity(float alt)
|
||||
{
|
||||
return getRecord(alt, 3);
|
||||
}
|
||||
|
||||
float Atmosphere::calcVEAS(float spd, float pressure, float temp)
|
||||
{
|
||||
return 0; //FIXME
|
||||
}
|
||||
|
||||
float Atmosphere::calcVCAS(float spd, float pressure, float temp)
|
||||
{
|
||||
// Stolen shamelessly from JSBSim. Constants that appear:
|
||||
// 2/5 == gamma-1
|
||||
// 5/12 == 1/(gamma+1)
|
||||
// 4/5 == 2*(gamma-1)
|
||||
// 14/5 == 2*gamma
|
||||
// 28/5 == 4*gamma
|
||||
// 144/25 == (gamma+1)^2
|
||||
|
||||
float m2 = calcMach(spd, temp);
|
||||
m2 = m2*m2; // mach^2
|
||||
|
||||
float cp; // pressure coefficient
|
||||
if(m2 < 1) {
|
||||
// (1+(mach^2)/5)^(gamma/(gamma-1))
|
||||
cp = Math::pow(1+0.2*m2, 3.5);
|
||||
} else {
|
||||
float tmp0 = ((144/25.) * m2) / (28/5.*m2 - 4/5.);
|
||||
float tmp1 = ((14/5.) * m2 - (2/5.)) * (5/12.);
|
||||
cp = Math::pow(tmp0, 3.5) * tmp1;
|
||||
}
|
||||
|
||||
// Conditions at sea level
|
||||
float p0 = getStdPressure(0);
|
||||
float rho0 = getStdDensity(0);
|
||||
|
||||
float tmp = Math::pow((pressure/p0)*(cp-1) + 1, (2/7.));
|
||||
return Math::sqrt((7*p0/rho0)*(tmp-1));
|
||||
}
|
||||
|
||||
float Atmosphere::calcDensity(float pressure, float temp)
|
||||
{
|
||||
// P = rho*R*T, R == 287 kPa*m^3 per kg*kelvin for air
|
||||
return pressure / (287 * temp);
|
||||
}
|
||||
|
||||
float Atmosphere::calcMach(float spd, float temp)
|
||||
{
|
||||
return spd / Math::sqrt(1.4 * 287 * temp);
|
||||
}
|
||||
|
||||
float Atmosphere::getRecord(float alt, int recNum)
|
||||
{
|
||||
int hi = (sizeof(data) / (4*sizeof(float))) - 1;
|
||||
int lo = 0;
|
||||
|
||||
// safety valve, clamp to the edges of the table
|
||||
if(alt < data[0][0]) hi=1;
|
||||
else if(alt > data[hi][0]) lo = hi-1;
|
||||
|
||||
// binary search
|
||||
while(1) {
|
||||
if(hi-lo == 1) break;
|
||||
int mid = (hi+lo)>>1;
|
||||
if(alt < data[mid][0]) hi = mid;
|
||||
else lo = mid;
|
||||
}
|
||||
|
||||
// interpolate
|
||||
float frac = (alt - data[lo][0])/(data[hi][0] - data[lo][0]);
|
||||
float a = data[lo][recNum];
|
||||
float b = data[hi][recNum];
|
||||
return a + frac * (b-a);
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
23
src/FDM/YASim/Atmosphere.hpp
Normal file
23
src/FDM/YASim/Atmosphere.hpp
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef _ATMOSPHERE_HPP
|
||||
#define _ATMOSPHERE_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Atmosphere {
|
||||
public:
|
||||
static float getStdTemperature(float alt);
|
||||
static float getStdPressure(float alt);
|
||||
static float getStdDensity(float alt);
|
||||
|
||||
static float calcVCAS(float spd, float pressure, float temp);
|
||||
static float calcVEAS(float spd, float pressure, float temp);
|
||||
static float calcMach(float spd, float temp);
|
||||
static float calcDensity(float pressure, float temp);
|
||||
|
||||
private:
|
||||
static float getRecord(float alt, int idx);
|
||||
static float data[][4];
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _ATMOSPHERE_HPP
|
||||
59
src/FDM/YASim/BodyEnvironment.hpp
Normal file
59
src/FDM/YASim/BodyEnvironment.hpp
Normal file
|
|
@ -0,0 +1,59 @@
|
|||
#ifndef _BODYENVIRONMENT_HPP
|
||||
#define _BODYENVIRONMENT_HPP
|
||||
|
||||
#include "RigidBody.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// The values for position and orientation within the global reference
|
||||
// frame, along with their first and second time derivatives. The
|
||||
// orientation is stored as a matrix for simplicity. Note that
|
||||
// because it is orthonormal, its inverse is simply its transpose.
|
||||
// You can get local->global transformations by calling Math::tmul33()
|
||||
// and using the same matrix.
|
||||
struct State {
|
||||
double pos[3]; // position
|
||||
float orient[9]; // global->local xform matrix
|
||||
float v[3]; // velocity
|
||||
float rot[3]; // rotational velocity
|
||||
float acc[3]; // acceleration
|
||||
float racc[3]; // rotational acceleration
|
||||
|
||||
// Simple initialization
|
||||
State() {
|
||||
for(int i=0; i<3; i++) {
|
||||
pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
|
||||
for(int j=0; j<3; j++)
|
||||
orient[3*i+j] = i==j ? 1 : 0;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
//
|
||||
// Objects implementing this interface are responsible for calculating
|
||||
// external forces on a RigidBody object. These will then be used by
|
||||
// an Integrator to decide on a new solution to the state equations,
|
||||
// which will be reported to the BodyEnvironment for further action.
|
||||
//
|
||||
class BodyEnvironment
|
||||
{
|
||||
public:
|
||||
// This method inspects the "environment" in which a RigidBody
|
||||
// exists, calculates forces and torques on the body, and sets
|
||||
// them via addForce()/addTorque(). Because this method is called
|
||||
// multiple times ("trials") as part of a Runge-Kutta integration,
|
||||
// this is NOT the place to make decisions about anything but the
|
||||
// forces on the object. Note that the acc and racc fields of the
|
||||
// passed-in State object are undefined! (They are calculed BY
|
||||
// this method).
|
||||
virtual void calcForces(State* state) = 0;
|
||||
|
||||
// Called when the RK4 integrator has determined a "real" new
|
||||
// point on the curve of life. Any side-effect producing checks
|
||||
// of body state vs. the environment can happen here (crashes,
|
||||
// etc...).
|
||||
virtual void newState(State* state) = 0;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _BODYENVIRONMENT_HPP
|
||||
130
src/FDM/YASim/ControlMap.cpp
Normal file
130
src/FDM/YASim/ControlMap.cpp
Normal file
|
|
@ -0,0 +1,130 @@
|
|||
#include "Jet.hpp"
|
||||
#include "Thruster.hpp"
|
||||
#include "Gear.hpp"
|
||||
#include "Wing.hpp"
|
||||
#include "Math.hpp"
|
||||
|
||||
#include "ControlMap.hpp"
|
||||
namespace yasim {
|
||||
|
||||
ControlMap::~ControlMap()
|
||||
{
|
||||
for(int i=0; i<_inputs.size(); i++) {
|
||||
Vector* v = (Vector*)_inputs.get(i);
|
||||
for(int j=0; j<v->size(); j++)
|
||||
delete (MapRec*)v->get(j);
|
||||
delete v;
|
||||
}
|
||||
|
||||
for(int i=0; i<_outputs.size(); i++) {
|
||||
OutRec* o = (OutRec*)_outputs.get(i);
|
||||
delete[] o->values;
|
||||
delete o;
|
||||
}
|
||||
}
|
||||
|
||||
int ControlMap::newInput()
|
||||
{
|
||||
Vector* v = new Vector();
|
||||
return _inputs.add(v);
|
||||
}
|
||||
|
||||
void ControlMap::addMapping(int input, int type, void* object, int options)
|
||||
{
|
||||
// See if the output object already exists
|
||||
OutRec* out = 0;
|
||||
for(int i=0; i<_outputs.size(); i++) {
|
||||
OutRec* o = (OutRec*)_outputs.get(i);
|
||||
if(o->object == object && o->type == type) {
|
||||
out = o;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Create one if it doesn't
|
||||
if(out == 0) {
|
||||
out = new OutRec();
|
||||
out->type = type;
|
||||
out->object = object;
|
||||
out->n = 0;
|
||||
out->values = 0;
|
||||
_outputs.add(out);
|
||||
}
|
||||
|
||||
// Make space for the new input value
|
||||
int idx = out->n++;
|
||||
delete[] out->values;
|
||||
out->values = new float[out->n];
|
||||
|
||||
// Add the new option tag
|
||||
out->options.add((void*)options);
|
||||
|
||||
// Make a new input record
|
||||
MapRec* map = new MapRec();
|
||||
map->out = out;
|
||||
map->idx = idx;
|
||||
|
||||
// And add it to the approproate vector.
|
||||
Vector* maps = (Vector*)_inputs.get(input);
|
||||
maps->add(map);
|
||||
}
|
||||
|
||||
void ControlMap::reset()
|
||||
{
|
||||
// Set all the values to zero
|
||||
for(int i=0; i<_outputs.size(); i++) {
|
||||
OutRec* o = (OutRec*)_outputs.get(i);
|
||||
for(int j=0; j<o->n; j++)
|
||||
o->values[j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void ControlMap::setInput(int input, float value)
|
||||
{
|
||||
Vector* maps = (Vector*)_inputs.get(input);
|
||||
for(int i=0; i<maps->size(); i++) {
|
||||
MapRec* map = (MapRec*)maps->get(i);
|
||||
map->out->values[map->idx] = value;
|
||||
}
|
||||
}
|
||||
|
||||
void ControlMap::applyControls()
|
||||
{
|
||||
for(int outrec=0; outrec<_outputs.size(); outrec++) {
|
||||
OutRec* o = (OutRec*)_outputs.get(outrec);
|
||||
|
||||
// Generate a summed value. Note the check for "split"
|
||||
// control axes like ailerons.
|
||||
float lval = 0, rval = 0;
|
||||
for(int i=0; i<o->n; i++) {
|
||||
float val = o->values[i];
|
||||
int opt = (int)o->options.get(i);
|
||||
if(opt & OPT_SQUARE)
|
||||
val = val * Math::abs(val);
|
||||
if(opt & OPT_INVERT)
|
||||
val = -val;
|
||||
lval += val;
|
||||
if(opt & OPT_SPLIT)
|
||||
rval -= val;
|
||||
else
|
||||
rval += val;
|
||||
}
|
||||
|
||||
void* obj = o->object;
|
||||
switch(o->type) {
|
||||
case THROTTLE: ((Thruster*)obj)->setThrottle(lval); break;
|
||||
case MIXTURE: ((Thruster*)obj)->setMixture(lval); break;
|
||||
case PROP: ((Thruster*)obj)->setPropAdvance(lval); break;
|
||||
case REHEAT: ((Jet*)obj)->setReheat(lval); break;
|
||||
case BRAKE: ((Gear*)obj)->setBrake(lval); break;
|
||||
case STEER: ((Gear*)obj)->setRotation(lval); break;
|
||||
case EXTEND: ((Gear*)obj)->setExtension(lval); break;
|
||||
case SLAT: ((Wing*)obj)->setSlat(lval); break;
|
||||
case FLAP0: ((Wing*)obj)->setFlap0(lval, rval); break;
|
||||
case FLAP1: ((Wing*)obj)->setFlap1(lval, rval); break;
|
||||
case SPOILER: ((Wing*)obj)->setSpoiler(lval, rval); break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
54
src/FDM/YASim/ControlMap.hpp
Normal file
54
src/FDM/YASim/ControlMap.hpp
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
#ifndef _CONTROL_MAP_HPP
|
||||
#define _CONTROL_MAP_HPP
|
||||
|
||||
#include "util/Vector.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class ControlMap {
|
||||
public:
|
||||
~ControlMap();
|
||||
|
||||
enum OutputType { THROTTLE, MIXTURE, REHEAT, PROP,
|
||||
BRAKE, STEER, EXTEND,
|
||||
INCIDENCE, FLAP0, FLAP1, SLAT, SPOILER };
|
||||
|
||||
static const int OPT_SPLIT = 0x01;
|
||||
static const int OPT_INVERT = 0x02;
|
||||
static const int OPT_SQUARE = 0x04;
|
||||
|
||||
// Returns a new, not-yet-used "input handle" for addMapping and
|
||||
// setInput. This typically corresponds to one user axis.
|
||||
int newInput();
|
||||
|
||||
// Adds a mapping to between input handle and a particular setting
|
||||
// on an output object. The value of output MUST match the type
|
||||
// of object!
|
||||
void addMapping(int input, int output, void* object, int options=0);
|
||||
|
||||
// Resets our accumulated input values. Call before any
|
||||
// setInput() invokations.
|
||||
void reset();
|
||||
|
||||
// Sets the specified input (as returned by newInput) to the
|
||||
// specified value.
|
||||
void setInput(int input, float value);
|
||||
|
||||
// Calculates and applies the settings received since the last reset().
|
||||
void applyControls();
|
||||
|
||||
private:
|
||||
struct OutRec { int type; void* object; int n;
|
||||
float* values; Vector options; };
|
||||
struct MapRec { OutRec* out; int idx; };
|
||||
|
||||
// A list of (sub)Vectors containing a bunch of MapRec objects for
|
||||
// each input handle.
|
||||
Vector _inputs;
|
||||
|
||||
// An unordered list of output settings.
|
||||
Vector _outputs;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _CONTROL_MAP_HPP
|
||||
458
src/FDM/YASim/FGFDM.cpp
Normal file
458
src/FDM/YASim/FGFDM.cpp
Normal file
|
|
@ -0,0 +1,458 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <Main/fg_props.hxx>
|
||||
|
||||
#include "Jet.hpp"
|
||||
#include "Gear.hpp"
|
||||
#include "Atmosphere.hpp"
|
||||
#include "PropEngine.hpp"
|
||||
#include "Propeller.hpp"
|
||||
#include "PistonEngine.hpp"
|
||||
|
||||
#include "FGFDM.hpp"
|
||||
namespace yasim {
|
||||
|
||||
// Some conversion factors
|
||||
static const float KTS2MPS = 0.514444444444;
|
||||
static const float FT2M = 0.3048;
|
||||
static const float DEG2RAD = 0.0174532925199;
|
||||
static const float RPM2RAD = 0.10471975512;
|
||||
static const float LBS2N = 4.44822;
|
||||
static const float LBS2KG = 0.45359237;
|
||||
static const float CM2GALS = 264.172037284;
|
||||
static const float HP2W = 745.700;
|
||||
|
||||
// Stubs, so that this can be compiled without the FlightGear
|
||||
// binary. What's the best way to handle this?
|
||||
|
||||
// float fgGetFloat(char* name, float def) { return 0; }
|
||||
// void fgSetFloat(char* name, float val) {}
|
||||
|
||||
FGFDM::FGFDM()
|
||||
{
|
||||
_nextEngine = 0;
|
||||
}
|
||||
|
||||
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<_pistons.size(); i++) {
|
||||
EngRec* er = (EngRec*)_pistons.get(i);
|
||||
delete[] er->prefix;
|
||||
delete (PropEngine*)er->eng;
|
||||
delete er;
|
||||
}
|
||||
for(int i=0; i<_jets.size(); i++) {
|
||||
EngRec* er = (EngRec*)_pistons.get(i);
|
||||
delete[] er->prefix;
|
||||
delete (Jet*)er->eng;
|
||||
delete er;
|
||||
}
|
||||
for(int i=0; i<_weights.size(); i++) {
|
||||
WeightRec* wr = (WeightRec*)_weights.get(i);
|
||||
delete[] wr->prop;
|
||||
delete wr;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void FGFDM::iterate(float dt)
|
||||
{
|
||||
getExternalInput(dt);
|
||||
_airplane.iterate(dt);
|
||||
setOutputProperties();
|
||||
}
|
||||
|
||||
Airplane* FGFDM::getAirplane()
|
||||
{
|
||||
return &_airplane;
|
||||
}
|
||||
|
||||
void FGFDM::init()
|
||||
{
|
||||
// We don't want to use these ties (we set the values ourselves,
|
||||
// and this works only for the first piston engine anyway).
|
||||
fgUntie("/engines/engine[0]/rpm");
|
||||
fgUntie("/engines/engine[0]/egt-degf");
|
||||
fgUntie("/engines/engine[0]/cht-degf");
|
||||
fgUntie("/engines/engine[0]/oil-temperature-degf");
|
||||
fgUntie("/engines/engine[0]/mp-osi");
|
||||
fgUntie("/engines/engine[0]/fuel-flow-gph");
|
||||
fgUntie("/engines/engine[0]/running");
|
||||
fgUntie("/engines/engine[0]/cranking");
|
||||
fgUntie("/consumables/fuel/tank[0]/level-gal_us");
|
||||
fgUntie("/consumables/fuel/tank[1]/level-gal_us");
|
||||
|
||||
// Set these to sane values. We don't support engine start yet.
|
||||
fgSetBool("/engines/engine[0]/running", true);
|
||||
fgSetBool("/engines/engine[0]/cranking", false);
|
||||
|
||||
// Allows the user to start with something other than full fuel
|
||||
_airplane.setFuelFraction(fgGetFloat("/yasim/fuel-fraction", 1));
|
||||
|
||||
// This has a nasty habit of being false at startup. That's not
|
||||
// good.
|
||||
fgSetBool("/controls/gear-down", true);
|
||||
}
|
||||
|
||||
// Not the worlds safest parser. But it's short & sweet.
|
||||
void FGFDM::startElement(const char* name, const XMLAttributes &atts)
|
||||
{
|
||||
XMLAttributes* a = (XMLAttributes*)&atts;
|
||||
float v[3];
|
||||
char buf[64];
|
||||
|
||||
if(eq(name, "airplane")) {
|
||||
_airplane.setWeight(attrf(a, "mass") * LBS2KG);
|
||||
} 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;
|
||||
_airplane.setApproach(spd, alt, aoa);
|
||||
_cruiseCurr = false;
|
||||
} else if(eq(name, "cruise")) {
|
||||
float spd = attrf(a, "speed") * KTS2MPS;
|
||||
float alt = attrf(a, "alt") * FT2M;
|
||||
_airplane.setCruise(spd, alt);
|
||||
_cruiseCurr = true;
|
||||
} else if(eq(name, "cockpit")) {
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
_airplane.setPilotPos(v);
|
||||
} else if(eq(name, "wing")) {
|
||||
_airplane.setWing(parseWing(a, name));
|
||||
} else if(eq(name, "hstab")) {
|
||||
_airplane.setTail(parseWing(a, name));
|
||||
} else if(eq(name, "vstab")) {
|
||||
_airplane.addVStab(parseWing(a, name));
|
||||
} else if(eq(name, "propeller")) {
|
||||
parsePropeller(a);
|
||||
} else if(eq(name, "jet")) {
|
||||
Jet* j = new Jet();
|
||||
_currObj = j;
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
float mass = attrf(a, "mass") * LBS2KG;
|
||||
j->setDryThrust(attrf(a, "thrust") * LBS2N);
|
||||
j->setPosition(v);
|
||||
_airplane.addThruster(j, mass, v);
|
||||
sprintf(buf, "/engines/engine[%d]", _nextEngine++);
|
||||
EngRec* er = new EngRec();
|
||||
er->eng = j;
|
||||
er->prefix = dup(buf);
|
||||
_jets.add(er);
|
||||
} else if(eq(name, "gear")) {
|
||||
Gear* g = new Gear();
|
||||
_currObj = g;
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
g->setPosition(v);
|
||||
v[0] = 0;
|
||||
v[1] = 0;
|
||||
v[2] = attrf(a, "compression", 1);
|
||||
g->setCompression(v);
|
||||
g->setStaticFriction(attrf(a, "sfric", 0.8));
|
||||
g->setDynamicFriction(attrf(a, "dfric", 0.7));
|
||||
float transitionTime = attrf(a, "retract-time", 0);
|
||||
_airplane.addGear(g, transitionTime);
|
||||
} else if(eq(name, "fuselage")) {
|
||||
float b[3];
|
||||
v[0] = attrf(a, "ax");
|
||||
v[1] = attrf(a, "ay");
|
||||
v[2] = attrf(a, "az");
|
||||
b[0] = attrf(a, "bx");
|
||||
b[1] = attrf(a, "by");
|
||||
b[2] = attrf(a, "bz");
|
||||
_airplane.addFuselage(v, b, attrf(a, "width"));
|
||||
} 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;
|
||||
density *= LBS2KG/CM2GALS;
|
||||
_airplane.addTank(v, attrf(a, "capacity") * LBS2KG, 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);
|
||||
} else if(eq(name, "weight")) {
|
||||
parseWeight(a);
|
||||
} else if(eq(name, "stall")) {
|
||||
Wing* w = (Wing*)_currObj;
|
||||
w->setStall(attrf(a, "aoa") * DEG2RAD);
|
||||
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"),
|
||||
attrf(a, "lift"), attrf(a, "drag"));
|
||||
} else if(eq(name, "flap1")) {
|
||||
((Wing*)_currObj)->setFlap1(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"),
|
||||
attrf(a, "aoa"), attrf(a, "drag"));
|
||||
} else if(eq(name, "spoiler")) {
|
||||
((Wing*)_currObj)->setSpoiler(attrf(a, "start"), attrf(a, "end"),
|
||||
attrf(a, "lift"), attrf(a, "drag"));
|
||||
} else if(eq(name, "actionpt")) {
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
((Thruster*)_currObj)->setPosition(v);
|
||||
} else if(eq(name, "dir")) {
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
((Thruster*)_currObj)->setDirection(v);
|
||||
} else if(eq(name, "control")) {
|
||||
const char* axis = a->getValue("axis");
|
||||
if(a->hasAttribute("output")) {
|
||||
// assert: output type must match _currObj type!
|
||||
const char* output = a->getValue("output");
|
||||
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;
|
||||
_airplane.getControlMap()->addMapping(parseAxis(axis),
|
||||
parseOutput(output),
|
||||
_currObj,
|
||||
opt);
|
||||
} else {
|
||||
// assert: must be under a "cruise" or "approach" tag
|
||||
float value = attrf(a, "value", 0);
|
||||
if(_cruiseCurr)
|
||||
_airplane.addCruiseControl(parseAxis(axis), value);
|
||||
else
|
||||
_airplane.addApproachControl(parseAxis(axis), value);
|
||||
}
|
||||
} else {
|
||||
*(int*)0=0; // unexpected tag, boom
|
||||
}
|
||||
}
|
||||
|
||||
void FGFDM::getExternalInput(float dt)
|
||||
{
|
||||
// The control axes
|
||||
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);
|
||||
}
|
||||
cm->applyControls();
|
||||
|
||||
// Weights
|
||||
for(int i=0; i<_weights.size(); i++) {
|
||||
WeightRec* wr = (WeightRec*)_weights.get(i);
|
||||
_airplane.setWeight(wr->handle, fgGetFloat(wr->prop));
|
||||
}
|
||||
|
||||
// Gear state
|
||||
_airplane.setGearState(fgGetBool("/controls/gear-down"), dt);
|
||||
}
|
||||
|
||||
void FGFDM::setOutputProperties()
|
||||
{
|
||||
char buf[256];
|
||||
for(int i=0; i<_airplane.numTanks(); i++) {
|
||||
sprintf(buf, "/consumables/fuel/tank[%d]/level-gal_us", i);
|
||||
fgSetFloat(buf,
|
||||
CM2GALS*_airplane.getFuel(i)/_airplane.getFuelDensity(i));
|
||||
}
|
||||
|
||||
for(int i=0; i<_pistons.size(); i++) {
|
||||
EngRec* er = (EngRec*)_pistons.get(i);
|
||||
PropEngine* p = (PropEngine*)er->eng;
|
||||
|
||||
sprintf(buf, "%s/rpm", er->prefix);
|
||||
fgSetFloat(buf, p->getOmega() * (30/3.15149265358979));
|
||||
|
||||
sprintf(buf, "%s/fuel-flow-gph", er->prefix);
|
||||
fgSetFloat(buf, p->getFuelFlow() * (3600*2.2/5)); // FIXME, wrong
|
||||
}
|
||||
|
||||
for(int i=0; i<_jets.size(); i++) {
|
||||
EngRec* er = (EngRec*)_jets.get(i);
|
||||
Jet* j = (Jet*)er->eng;
|
||||
|
||||
sprintf(buf, "%s/fuel-flow-gph", er->prefix);
|
||||
fgSetFloat(buf, j->getFuelFlow() * (3600*2.2/6)); // FIXME, wrong
|
||||
}
|
||||
}
|
||||
|
||||
Wing* FGFDM::parseWing(XMLAttributes* a, const char* type)
|
||||
{
|
||||
Wing* w = new Wing();
|
||||
|
||||
float defDihed = 0;
|
||||
if(eq(type, "vstab"))
|
||||
defDihed = 90;
|
||||
else
|
||||
w->setMirror(true);
|
||||
|
||||
float pos[3];
|
||||
pos[0] = attrf(a, "x");
|
||||
pos[1] = attrf(a, "y");
|
||||
pos[2] = attrf(a, "z");
|
||||
w->setBase(pos);
|
||||
|
||||
w->setLength(attrf(a, "length"));
|
||||
w->setChord(attrf(a, "chord"));
|
||||
w->setSweep(attrf(a, "sweep", 0) * DEG2RAD);
|
||||
w->setTaper(attrf(a, "taper", 1));
|
||||
w->setDihedral(attrf(a, "dihedral", defDihed) * DEG2RAD);
|
||||
w->setCamber(attrf(a, "camber", 0));
|
||||
w->setIncidence(attrf(a, "incidence", 0) * DEG2RAD);
|
||||
|
||||
float effect = attrf(a, "effectiveness", 1);
|
||||
w->setDragScale(w->getDragScale()*effect);
|
||||
|
||||
_currObj = w;
|
||||
return w;
|
||||
}
|
||||
|
||||
void FGFDM::parsePropeller(XMLAttributes* a)
|
||||
{
|
||||
float cg[3];
|
||||
cg[0] = attrf(a, "x");
|
||||
cg[1] = attrf(a, "y");
|
||||
cg[2] = attrf(a, "z");
|
||||
float mass = attrf(a, "mass") * LBS2KG;
|
||||
float moment = attrf(a, "moment");
|
||||
float radius = attrf(a, "radius");
|
||||
float speed = attrf(a, "cruise-speed") * KTS2MPS;
|
||||
float omega = attrf(a, "cruise-rpm") * RPM2RAD;
|
||||
float rho = Atmosphere::getStdDensity(attrf(a, "alt") * FT2M);
|
||||
float power = attrf(a, "takeoff-power") * HP2W;
|
||||
float omega0 = attrf(a, "takeoff-rpm") * RPM2RAD;
|
||||
|
||||
// FIXME: this is a hack. Find a better way to ask the engine how
|
||||
// much power it can produce under cruise conditions.
|
||||
float cruisePower = (power * (rho/Atmosphere::getStdDensity(0))
|
||||
* (omega/omega0));
|
||||
|
||||
Propeller* prop = new Propeller(radius, speed, omega, rho, cruisePower,
|
||||
omega0, power);
|
||||
PistonEngine* eng = new PistonEngine(power, omega0);
|
||||
PropEngine* thruster = new PropEngine(prop, eng, moment);
|
||||
_airplane.addThruster(thruster, mass, cg);
|
||||
|
||||
char buf[64];
|
||||
sprintf(buf, "/engines/engine[%d]", _nextEngine++);
|
||||
EngRec* er = new EngRec();
|
||||
er->eng = thruster;
|
||||
er->prefix = dup(buf);
|
||||
_pistons.add(er);
|
||||
|
||||
_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);
|
||||
a->handle = _airplane.getControlMap()->newInput();
|
||||
_axes.add(a);
|
||||
return a->handle;
|
||||
}
|
||||
|
||||
int FGFDM::parseOutput(const char* name)
|
||||
{
|
||||
if(eq(name, "THROTTLE")) return ControlMap::THROTTLE;
|
||||
if(eq(name, "MIXTURE")) return ControlMap::MIXTURE;
|
||||
if(eq(name, "REHEAT")) return ControlMap::REHEAT;
|
||||
if(eq(name, "PROP")) return ControlMap::PROP;
|
||||
if(eq(name, "BRAKE")) return ControlMap::BRAKE;
|
||||
if(eq(name, "STEER")) return ControlMap::STEER;
|
||||
if(eq(name, "EXTEND")) return ControlMap::EXTEND;
|
||||
if(eq(name, "INCIDENCE")) return ControlMap::INCIDENCE;
|
||||
if(eq(name, "FLAP0")) return ControlMap::FLAP0;
|
||||
if(eq(name, "FLAP1")) return ControlMap::FLAP1;
|
||||
if(eq(name, "SLAT")) return ControlMap::SLAT;
|
||||
if(eq(name, "SPOILER")) return ControlMap::SPOILER;
|
||||
// error here...
|
||||
return -1;
|
||||
}
|
||||
|
||||
void FGFDM::parseWeight(XMLAttributes* a)
|
||||
{
|
||||
WeightRec* wr = new WeightRec();
|
||||
|
||||
float v[3];
|
||||
v[0] = attrf(a, "x");
|
||||
v[1] = attrf(a, "y");
|
||||
v[2] = attrf(a, "z");
|
||||
|
||||
wr->prop = dup(a->getValue("mass-prop"));
|
||||
wr->size = attrf(a, "size", 0);
|
||||
wr->handle = _airplane.addWeight(v, wr->size);
|
||||
|
||||
_weights.add(wr);
|
||||
}
|
||||
|
||||
bool FGFDM::eq(const char* a, const char* b)
|
||||
{
|
||||
// Figure it out for yourself. :)
|
||||
while(*a && *b && *a++ == *b++);
|
||||
return !(*a || *b);
|
||||
}
|
||||
|
||||
char* FGFDM::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;
|
||||
}
|
||||
|
||||
int FGFDM::attri(XMLAttributes* atts, char* attr)
|
||||
{
|
||||
if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
|
||||
return attri(atts, attr, 0);
|
||||
}
|
||||
|
||||
int FGFDM::attri(XMLAttributes* atts, char* attr, int def)
|
||||
{
|
||||
const char* val = atts->getValue(attr);
|
||||
if(val == 0) return def;
|
||||
else return atol(val);
|
||||
}
|
||||
|
||||
float FGFDM::attrf(XMLAttributes* atts, char* attr)
|
||||
{
|
||||
if(!atts->hasAttribute(attr)) *(int*)0=0; // boom
|
||||
return attrf(atts, attr, 0);
|
||||
}
|
||||
|
||||
float FGFDM::attrf(XMLAttributes* atts, char* attr, float def)
|
||||
{
|
||||
const char* val = atts->getValue(attr);
|
||||
if(val == 0) return def;
|
||||
else return (float)atof(val);
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
69
src/FDM/YASim/FGFDM.hpp
Normal file
69
src/FDM/YASim/FGFDM.hpp
Normal file
|
|
@ -0,0 +1,69 @@
|
|||
#ifndef _FGFDM_HPP
|
||||
#define _FGFDM_HPP
|
||||
|
||||
#include <simgear/easyxml.hxx>
|
||||
|
||||
#include "Airplane.hpp"
|
||||
#include "util/Vector.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Wing;
|
||||
|
||||
// This class forms the "glue" to the FlightGear codebase. It handles
|
||||
// parsing of XML airplane files, interfacing to the properties
|
||||
// system, and providing data for the use of the FGInterface object.
|
||||
class FGFDM : public XMLVisitor {
|
||||
public:
|
||||
FGFDM();
|
||||
~FGFDM();
|
||||
void init();
|
||||
void iterate(float dt);
|
||||
|
||||
Airplane* getAirplane();
|
||||
|
||||
// XML parsing callback from XMLVisitor
|
||||
virtual void startElement(const char* name, const XMLAttributes &atts);
|
||||
|
||||
private:
|
||||
struct AxisRec { char* name; int handle; };
|
||||
struct EngRec { char* prefix; void* eng; };
|
||||
struct WeightRec { char* prop; float size; int handle; };
|
||||
|
||||
void getExternalInput(float dt);
|
||||
void setOutputProperties();
|
||||
|
||||
Wing* parseWing(XMLAttributes* a, const char* name);
|
||||
int parseAxis(const char* name);
|
||||
int parseOutput(const char* name);
|
||||
void parseWeight(XMLAttributes* a);
|
||||
void parsePropeller(XMLAttributes* a);
|
||||
bool eq(const char* a, const char* b);
|
||||
char* dup(const char* s);
|
||||
int attri(XMLAttributes* atts, char* attr);
|
||||
int attri(XMLAttributes* atts, char* attr, int def);
|
||||
float attrf(XMLAttributes* atts, char* attr);
|
||||
float attrf(XMLAttributes* atts, char* attr, float def);
|
||||
|
||||
// The core Airplane object we manage.
|
||||
Airplane _airplane;
|
||||
|
||||
// The list of "axes" that we expect to find as input. These are
|
||||
// typically property names.
|
||||
Vector _axes;
|
||||
|
||||
// Settable weights
|
||||
Vector _weights;
|
||||
|
||||
// Engine types. Contains an EngRec structure.
|
||||
Vector _jets;
|
||||
Vector _pistons;
|
||||
|
||||
// Parsing temporaries
|
||||
void* _currObj;
|
||||
bool _cruiseCurr;
|
||||
int _nextEngine;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _FGFDM_HPP
|
||||
240
src/FDM/YASim/Gear.cpp
Normal file
240
src/FDM/YASim/Gear.cpp
Normal file
|
|
@ -0,0 +1,240 @@
|
|||
#include "Math.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
|
||||
#include "Gear.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Gear::Gear()
|
||||
{
|
||||
for(int i=0; i<3; i++)
|
||||
_pos[i] = _cmpr[i] = 0;
|
||||
_spring = 1;
|
||||
_damp = 0;
|
||||
_sfric = 0.8;
|
||||
_dfric = 0.7;
|
||||
_brake = 0;
|
||||
_rot = 0;
|
||||
_extension = 1;
|
||||
}
|
||||
|
||||
void Gear::setPosition(float* position)
|
||||
{
|
||||
for(int i=0; i<3; i++) _pos[i] = position[i];
|
||||
}
|
||||
|
||||
void Gear::setCompression(float* compression)
|
||||
{
|
||||
for(int 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 = brake;
|
||||
}
|
||||
|
||||
void Gear::setRotation(float rotation)
|
||||
{
|
||||
_rot = rotation;
|
||||
}
|
||||
|
||||
void Gear::setExtension(float extension)
|
||||
{
|
||||
_extension = extension;
|
||||
}
|
||||
|
||||
void Gear::getPosition(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _pos[i];
|
||||
}
|
||||
|
||||
void Gear::getCompression(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _cmpr[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;
|
||||
}
|
||||
|
||||
void Gear::getForce(float* force, float* contact)
|
||||
{
|
||||
Math::set3(_force, force);
|
||||
Math::set3(_contact, contact);
|
||||
}
|
||||
|
||||
float Gear::getWoW()
|
||||
{
|
||||
return _wow;
|
||||
}
|
||||
|
||||
float Gear::getCompressFraction()
|
||||
{
|
||||
return _frac;
|
||||
}
|
||||
|
||||
void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
|
||||
{
|
||||
// Init the return values
|
||||
for(int i=0; i<3; i++) _force[i] = _contact[i] = 0;
|
||||
|
||||
// Don't bother if it's not down
|
||||
if(_extension < 1)
|
||||
return;
|
||||
|
||||
float tmp[3];
|
||||
|
||||
// First off, make sure that the gear "tip" is below the ground.
|
||||
// If it's not, there's no force.
|
||||
float a = ground[3] - Math::dot3(_pos, ground);
|
||||
if(a > 0)
|
||||
return;
|
||||
|
||||
// Now a is the distance from the tip to ground, so make b the
|
||||
// distance from the base to ground. We can get the fraction
|
||||
// (0-1) of compression from a/(a-b). Note the minus sign -- stuff
|
||||
// above ground is negative.
|
||||
Math::add3(_cmpr, _pos, tmp);
|
||||
float b = ground[3] - Math::dot3(tmp, ground);
|
||||
|
||||
// Calculate the point of ground _contact.
|
||||
_frac = a/(a-b);
|
||||
if(b < 0) _frac = 1;
|
||||
for(int i=0; i<3; i++)
|
||||
_contact[i] = _pos[i] + _frac*_cmpr[i];
|
||||
|
||||
// Turn _cmpr into a unit vector and a magnitude
|
||||
float cmpr[3];
|
||||
float clen = Math::mag3(_cmpr);
|
||||
Math::mul3(1/clen, _cmpr, cmpr);
|
||||
|
||||
// Now get the velocity of the point of contact
|
||||
float cv[3];
|
||||
body->pointVelocity(_contact, rot, cv);
|
||||
Math::add3(cv, v, cv);
|
||||
|
||||
// Finally, we can start adding up the forces. First the
|
||||
// spring compression (note the clamping of _frac to 1):
|
||||
_frac = (_frac > 1) ? 1 : _frac;
|
||||
float fmag = _frac*clen*_spring;
|
||||
Math::mul3(fmag, cmpr, _force);
|
||||
|
||||
// Then the damping. Use the only the velocity into the ground
|
||||
// (projection along "ground") projected along the compression
|
||||
// axis. So Vdamp = ground*(ground dot cv) dot cmpr
|
||||
Math::mul3(Math::dot3(ground, cv), ground, tmp);
|
||||
float dv = Math::dot3(cmpr, tmp);
|
||||
float damp = _damp * dv;
|
||||
if(damp > fmag) damp = fmag; // can't pull the plane down!
|
||||
if(damp < -fmag) damp = -fmag; // sanity
|
||||
Math::mul3(-damp, cmpr, tmp);
|
||||
Math::add3(_force, tmp, _force);
|
||||
|
||||
_wow = fmag - damp;
|
||||
|
||||
// Wheels are funky. Split the velocity along the ground plane
|
||||
// into rolling and skidding components. Assuming small angles,
|
||||
// we generate "forward" and "left" unit vectors (the compression
|
||||
// goes "up") for the gear, make a "steer" direction from these,
|
||||
// and then project it onto the ground plane. Project the
|
||||
// velocity onto the ground plane too, and extract the "steer"
|
||||
// component. The remainder is the skid velocity.
|
||||
|
||||
float gup[3]; // "up" unit vector from the ground
|
||||
Math::set3(ground, gup);
|
||||
Math::mul3(-1, gup, gup);
|
||||
|
||||
float xhat[] = {1,0,0};
|
||||
float steer[3], skid[3];
|
||||
Math::cross3(gup, xhat, skid); // up cross xhat =~ skid
|
||||
Math::unit3(skid, skid); // == skid
|
||||
|
||||
Math::cross3(skid, gup, steer); // skid cross up == steer
|
||||
|
||||
if(_rot != 0) {
|
||||
// Correct for a (small) rotation
|
||||
Math::mul3(_rot, steer, tmp);
|
||||
Math::add3(tmp, skid, skid);
|
||||
Math::unit3(skid, skid);
|
||||
Math::cross3(skid, gup, steer);
|
||||
}
|
||||
|
||||
float vsteer = Math::dot3(cv, steer);
|
||||
float vskid = Math::dot3(cv, skid);
|
||||
float wgt = Math::dot3(_force, gup); // force into the ground
|
||||
|
||||
float fsteer = _brake * calcFriction(wgt, vsteer);
|
||||
float fskid = calcFriction(wgt, vskid);
|
||||
if(vsteer > 0) fsteer = -fsteer;
|
||||
if(vskid > 0) fskid = -fskid;
|
||||
|
||||
// Phoo! All done. Add it up and get out of here.
|
||||
Math::mul3(fsteer, steer, tmp);
|
||||
Math::add3(tmp, _force, _force);
|
||||
|
||||
Math::mul3(fskid, skid, tmp);
|
||||
Math::add3(tmp, _force, _force);
|
||||
}
|
||||
|
||||
float Gear::calcFriction(float wgt, float v)
|
||||
{
|
||||
// How slow is stopped? 50 cm/second?
|
||||
const float STOP = 0.5;
|
||||
const float iSTOP = 1/STOP;
|
||||
v = Math::abs(v);
|
||||
if(v < STOP) return v*iSTOP * wgt * _sfric;
|
||||
else return wgt * _dfric;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
|
||||
82
src/FDM/YASim/Gear.hpp
Normal file
82
src/FDM/YASim/Gear.hpp
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
#ifndef _GEAR_HPP
|
||||
#define _GEAR_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class RigidBody;
|
||||
|
||||
// A landing gear has the following parameters:
|
||||
//
|
||||
// position: a point in the aircraft's local coordinate system where the
|
||||
// fully-extended wheel will be found.
|
||||
// compression: a vector from position where a fully-compressed wheel
|
||||
// will be, also in aircraft-local coordinates.
|
||||
// spring coefficient: force coefficient along the compression axis, in
|
||||
// Newtons per meter.
|
||||
// damping coefficient: force per compression speed, in Ns/m
|
||||
// static coefficient of friction: force along the ground plane exerted
|
||||
// by a motionless wheel. A unitless scalar.
|
||||
// dynamic coefficient of friction: force along the ground plane exerted
|
||||
// by a sliding/skidding wheel.
|
||||
// braking fraction: fraction of the dynamic friction that will be
|
||||
// actually exerted by a rolling wheel
|
||||
// rotation: the angle from "forward" by which the wheel is rotated
|
||||
// around its compression axis. In radians.
|
||||
//
|
||||
class Gear {
|
||||
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 getPosition(float* out);
|
||||
void getCompression(float* out);
|
||||
float getSpring();
|
||||
float getDamping();
|
||||
float getStaticFriction();
|
||||
float getDynamicFriction();
|
||||
float getBrake();
|
||||
float getRotation();
|
||||
float getExtension();
|
||||
|
||||
// Takes a velocity of the aircraft relative to ground, a rotation
|
||||
// vector, and a ground plane (all specified in local coordinates)
|
||||
// and make a force and point of application (i.e. ground contact)
|
||||
// available via getForce().
|
||||
void calcForce(RigidBody* body, float* v, float* rot, float* ground);
|
||||
|
||||
// Computed values: total force, weight-on-wheels (force normal to
|
||||
// ground) and compression fraction.
|
||||
void getForce(float* force, float* contact);
|
||||
float getWoW();
|
||||
float getCompressFraction();
|
||||
|
||||
private:
|
||||
float calcFriction(float wgt, float v);
|
||||
|
||||
float _pos[3];
|
||||
float _cmpr[3];
|
||||
float _spring;
|
||||
float _damp;
|
||||
float _sfric;
|
||||
float _dfric;
|
||||
float _brake;
|
||||
float _rot;
|
||||
float _extension;
|
||||
float _force[3];
|
||||
float _contact[3];
|
||||
float _wow;
|
||||
float _frac;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _GEAR_HPP
|
||||
235
src/FDM/YASim/Glue.cpp
Normal file
235
src/FDM/YASim/Glue.cpp
Normal file
|
|
@ -0,0 +1,235 @@
|
|||
#include "Math.hpp"
|
||||
#include "Glue.hpp"
|
||||
namespace yasim {
|
||||
|
||||
void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
|
||||
{
|
||||
// Convert the velocity to the aircraft frame.
|
||||
float v[3];
|
||||
Math::vmul33(s->orient, s->v, v);
|
||||
|
||||
// By convention, positive alpha is an up pitch, and a positive
|
||||
// beta is yawed to the right.
|
||||
*alpha = -Math::atan2(v[2], v[0]);
|
||||
*beta = Math::atan2(v[1], v[0]);
|
||||
}
|
||||
|
||||
void Glue::calcEulerRates(State* s, float* roll, float* pitch, float* hdg)
|
||||
{
|
||||
// This one is easy, the projection of the rotation vector around
|
||||
// the "up" axis.
|
||||
float up[3];
|
||||
geodUp(s->pos, up);
|
||||
*hdg = -Math::dot3(up, s->rot); // negate for "NED" conventions
|
||||
|
||||
// A bit harder: the X component of the rotation vector expressed
|
||||
// in airframe coordinates.
|
||||
float lr[3];
|
||||
Math::vmul33(s->orient, s->rot, lr);
|
||||
*roll = lr[0];
|
||||
|
||||
// Hardest: the component of rotation along the direction formed
|
||||
// by the cross product of (and thus perpendicular to) the
|
||||
// aircraft's forward vector (i.e. the first three elements of the
|
||||
// orientation matrix) and the "up" axis.
|
||||
float pitchAxis[3];
|
||||
Math::cross3(s->orient, up, pitchAxis);
|
||||
Math::unit3(pitchAxis, pitchAxis);
|
||||
*pitch = Math::dot3(pitchAxis, s->rot);
|
||||
}
|
||||
|
||||
void Glue::xyz2geoc(double* xyz, double* lat, double* lon, double* alt)
|
||||
{
|
||||
double x=xyz[0], y=xyz[1], z=xyz[2];
|
||||
|
||||
// Cylindrical radius from the polar axis
|
||||
double rcyl = Math::sqrt(x*x + y*y);
|
||||
|
||||
// In geocentric coordinates, these are just the angles in
|
||||
// cartesian space.
|
||||
*lon = Math::atan2(y, x);
|
||||
*lat = Math::atan2(z, rcyl);
|
||||
|
||||
// To get XYZ coordinate of "ground", we "squash" the cylindric
|
||||
// radius into a coordinate system where the earth is a sphere,
|
||||
// find the fraction of the xyz vector that is above ground.
|
||||
double rsquash = SQUASH * rcyl;
|
||||
double frac = POLRAD/Math::sqrt(rsquash*rsquash + z*z);
|
||||
double len = Math::sqrt(x*x + y*y + z*z);
|
||||
|
||||
*alt = len * (1-frac);
|
||||
}
|
||||
|
||||
void Glue::geoc2xyz(double lat, double lon, double alt, double* out)
|
||||
{
|
||||
// Generate a unit vector
|
||||
double rcyl = Math::cos(lat);
|
||||
double x = rcyl*Math::cos(lon);
|
||||
double y = rcyl*Math::sin(lon);
|
||||
double z = Math::sin(lat);
|
||||
|
||||
// Convert to "squashed" space, renormalize the unit vector,
|
||||
// multiply by the polar radius, and back convert to get us the
|
||||
// point of intersection of the unit vector with the surface.
|
||||
// Then just add the altitude.
|
||||
double rtmp = rcyl*SQUASH;
|
||||
double renorm = POLRAD/Math::sqrt(rtmp*rtmp + z*z);
|
||||
double ztmp = z*renorm;
|
||||
rtmp *= renorm*STRETCH;
|
||||
double len = Math::sqrt(rtmp*rtmp + ztmp*ztmp);
|
||||
len += alt;
|
||||
|
||||
out[0] = x*len;
|
||||
out[1] = y*len;
|
||||
out[2] = z*len;
|
||||
}
|
||||
|
||||
double Glue::geod2geocLat(double lat)
|
||||
{
|
||||
double r = Math::cos(lat)*STRETCH*STRETCH;
|
||||
double z = Math::sin(lat);
|
||||
return Math::atan2(z, r);
|
||||
}
|
||||
|
||||
double Glue::geoc2geodLat(double lat)
|
||||
{
|
||||
double r = Math::cos(lat)*SQUASH*SQUASH;
|
||||
double z = Math::sin(lat);
|
||||
return Math::atan2(z, r);
|
||||
}
|
||||
|
||||
void Glue::xyz2geod(double* xyz, double* lat, double* lon, double* alt)
|
||||
{
|
||||
xyz2geoc(xyz, lat, lon, alt);
|
||||
*lat = geoc2geodLat(*lat);
|
||||
}
|
||||
|
||||
void Glue::geod2xyz(double lat, double lon, double alt, double* out)
|
||||
{
|
||||
lat = geod2geocLat(lat);
|
||||
geoc2xyz(lat, lon, alt, out);
|
||||
}
|
||||
|
||||
void Glue::xyz2nedMat(double lat, double lon, float* out)
|
||||
{
|
||||
// Shorthand for our output vectors:
|
||||
float *north = out, *east = out+3, *down = out+6;
|
||||
|
||||
float slat = Math::sin(lat);
|
||||
float clat = Math::cos(lat);
|
||||
float slon = Math::sin(lon);
|
||||
float clon = Math::cos(lon);
|
||||
|
||||
north[0] = -clon * slat;
|
||||
north[1] = -slon * slat;
|
||||
north[2] = clat;
|
||||
|
||||
east[0] = -slon;
|
||||
east[1] = clon;
|
||||
east[2] = 0;
|
||||
|
||||
down[0] = -clon * clat;
|
||||
down[1] = -slon * clat;
|
||||
down[2] = -slat;
|
||||
}
|
||||
|
||||
void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
|
||||
{
|
||||
// To translate a point in aircraft space to the output "NED"
|
||||
// frame, first negate the Y and Z axes (ugh), then roll around
|
||||
// the X axis, then pitch around Y, then point to the correct
|
||||
// heading about Z. Expressed as a matrix multiplication, then,
|
||||
// the transformation from aircraft to local is HPRN. And our
|
||||
// desired output is the inverse (i.e. transpose) of that. Since
|
||||
// all rotations are 2D, they have a simpler form than a generic
|
||||
// rotation and are done out longhand below for efficiency.
|
||||
|
||||
// Init to the identity matrix
|
||||
for(int i=0; i<3; i++)
|
||||
for(int j=0; j<3; j++)
|
||||
out[3*i+j] = (i==j) ? 1 : 0;
|
||||
|
||||
// Negate Y and Z
|
||||
out[4] = out[8] = -1;
|
||||
|
||||
float s = Math::sin(roll);
|
||||
float c = Math::cos(roll);
|
||||
for(int col=0; col<3; col++) {
|
||||
float y=out[col+3], z=out[col+6];
|
||||
out[col+3] = c*y - s*z;
|
||||
out[col+6] = s*y + c*z;
|
||||
}
|
||||
|
||||
s = Math::sin(pitch);
|
||||
c = Math::cos(pitch);
|
||||
for(int col=0; col<3; col++) {
|
||||
float x=out[col], z=out[col+6];
|
||||
out[col] = c*x + s*z;
|
||||
out[col+6] = c*z - s*x;
|
||||
}
|
||||
|
||||
s = Math::sin(hdg);
|
||||
c = Math::cos(hdg);
|
||||
for(int col=0; col<3; col++) {
|
||||
float x=out[col], y=out[col+3];
|
||||
out[col] = c*x - s*y;
|
||||
out[col+3] = s*x + c*y;
|
||||
}
|
||||
|
||||
// Invert:
|
||||
Math::trans33(out, out);
|
||||
}
|
||||
|
||||
void Glue::orient2euler(float* o, float* roll, float* pitch, float* hdg)
|
||||
{
|
||||
// The airplane's "pointing" direction in NED coordinates is vx,
|
||||
// and it's y (left-right) axis is vy.
|
||||
float vx[3], vy[3];
|
||||
vx[0]=o[0], vx[1]=o[1], vx[2]=o[2];
|
||||
vy[0]=o[3], vy[1]=o[4], vy[2]=o[5];
|
||||
|
||||
// The heading is simply the rotation of the projection onto the
|
||||
// XY plane
|
||||
*hdg = Math::atan2(vx[1], vx[0]);
|
||||
|
||||
// The pitch is the angle between the XY plane and vx, remember
|
||||
// that rotations toward positive Z are _negative_
|
||||
float projmag = Math::sqrt(vx[0]*vx[0]+vx[1]*vx[1]);
|
||||
*pitch = -Math::atan2(vx[2], projmag);
|
||||
|
||||
// Roll is a bit harder. Construct an "unrolled" orientation,
|
||||
// where the X axis is the same as the "rolled" one, and the Y
|
||||
// axis is parallel to the XY plane. These two can give you an
|
||||
// "unrolled" Z axis as their cross product. Now, take the "vy"
|
||||
// axis, which points out the left wing. The projections of this
|
||||
// along the "unrolled" YZ plane will give you the roll angle via
|
||||
// atan().
|
||||
float* ux = vx;
|
||||
float uy[3], uz[3];
|
||||
|
||||
uz[0] = 0; uz[1] = 0; uz[2] = 1;
|
||||
Math::cross3(uz, ux, uy);
|
||||
Math::unit3(uy, uy);
|
||||
Math::cross3(ux, uy, uz);
|
||||
|
||||
float py = -Math::dot3(vy, uy);
|
||||
float pz = -Math::dot3(vy, uz);
|
||||
*roll = Math::atan2(pz, py);
|
||||
}
|
||||
|
||||
void Glue::geodUp(double* pos, float* out)
|
||||
{
|
||||
double lat, lon, alt;
|
||||
xyz2geod(pos, &lat, &lon, &alt);
|
||||
|
||||
float slat = Math::sin(lat);
|
||||
float clat = Math::cos(lat);
|
||||
float slon = Math::sin(lon);
|
||||
float clon = Math::cos(lon);
|
||||
out[0] = clon * clat;
|
||||
out[1] = slon * clat;
|
||||
out[2] = slat;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
|
||||
60
src/FDM/YASim/Glue.hpp
Normal file
60
src/FDM/YASim/Glue.hpp
Normal file
|
|
@ -0,0 +1,60 @@
|
|||
#ifndef _GLUE_HPP
|
||||
#define _GLUE_HPP
|
||||
|
||||
#include "BodyEnvironment.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// The XYZ coordinate system has Z as the earth's axis, the Y axis
|
||||
// pointing out the equator at zero longitude, and the X axis pointing
|
||||
// out the middle of the western hemisphere.
|
||||
class Glue {
|
||||
public:
|
||||
static void calcAlphaBeta(State* s, float* alpha, float* beta);
|
||||
|
||||
// Calculates the instantaneous rotation velocities about each
|
||||
// axis.
|
||||
static void calcEulerRates(State* s,
|
||||
float* roll, float* pitch, float* hdg);
|
||||
|
||||
static void xyz2geoc(double* xyz,
|
||||
double* lat, double* lon, double* alt);
|
||||
static void geoc2xyz(double lat, double lon, double alt,
|
||||
double* out);
|
||||
static void xyz2geod(double* xyz,
|
||||
double* lat, double* lon, double* alt);
|
||||
static void geod2xyz(double lat, double lon, double alt,
|
||||
double* out);
|
||||
|
||||
static double geod2geocLat(double lat);
|
||||
static double geoc2geodLat(double lat);
|
||||
|
||||
// Returns a global to "local" (north, east, down) matrix. Note
|
||||
// that the latitude passed in is geoDETic.
|
||||
static void xyz2nedMat(double lat, double lon, float* out);
|
||||
|
||||
// Conversion between a euler triplet and a matrix that transforms
|
||||
// "local" (north/east/down) coordinates to the aircraft frame.
|
||||
static void euler2orient(float roll, float pitch, float hdg,
|
||||
float* out);
|
||||
static void orient2euler(float* o,
|
||||
float* roll, float* pitch, float* hdg);
|
||||
|
||||
// Returns a geodetic (i.e. gravitational, "level", etc...) "up"
|
||||
// vector for the specified xyz position.
|
||||
static void geodUp(double* pos, float* out);
|
||||
|
||||
private:
|
||||
|
||||
// WGS84 numbers
|
||||
static const double EQURAD = 6378137; // equatorial radius
|
||||
static const double STRETCH = 1.003352810665; // equ./polar radius
|
||||
|
||||
// Derived from the above
|
||||
static const double SQUASH = 0.99665839311; // 1/STRETCH
|
||||
static const double POLRAD = 6356823.77346; // EQURAD*SQUASH
|
||||
static const double iPOLRAD = 1.57311266701e-07; // 1/POLRAD
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _GLUE_HPP
|
||||
311
src/FDM/YASim/Integrator.cpp
Normal file
311
src/FDM/YASim/Integrator.cpp
Normal file
|
|
@ -0,0 +1,311 @@
|
|||
#include "Math.hpp"
|
||||
#include "Integrator.hpp"
|
||||
namespace yasim {
|
||||
|
||||
void Integrator::setBody(RigidBody* body)
|
||||
{
|
||||
_body = body;
|
||||
}
|
||||
|
||||
void Integrator::setEnvironment(BodyEnvironment* env)
|
||||
{
|
||||
_env = env;
|
||||
}
|
||||
|
||||
void Integrator::setInterval(float dt)
|
||||
{
|
||||
_dt = dt;
|
||||
}
|
||||
|
||||
float Integrator::getInterval()
|
||||
{
|
||||
return _dt;
|
||||
}
|
||||
|
||||
void Integrator::setState(State* s)
|
||||
{
|
||||
_s = *s;
|
||||
}
|
||||
|
||||
State* Integrator::getState()
|
||||
{
|
||||
return &_s;
|
||||
}
|
||||
|
||||
// Transforms a "local" vector to a "global" vector (not coordinate!)
|
||||
// using the specified orientation.
|
||||
void Integrator::l2gVector(float* orient, float* v, float* out)
|
||||
{
|
||||
Math::tmul33(_s.orient, v, out);
|
||||
}
|
||||
|
||||
// Updates a position vector for a body c.g. motion with velocity v
|
||||
// over time dt, from orientation o0 to o1. Because the position
|
||||
// references the local coordinate origin, but the velocity is that of
|
||||
// the c.g., this gets a bit complicated.
|
||||
void Integrator::extrapolatePosition(double* pos, float* v, float dt,
|
||||
float* o1, float* o2)
|
||||
{
|
||||
// Remember that it's the c.g. that's moving, so account for
|
||||
// changes in orientation. The motion of the coordinate
|
||||
// frame will be l2gOLD(cg) + deltaCG - l2gNEW(cg)
|
||||
float cg[3], tmp[3];
|
||||
|
||||
_body->getCG(cg);
|
||||
l2gVector(o1, cg, cg); // cg = l2gOLD(cg) ("cg0")
|
||||
Math::set3(v, tmp); // tmp = vel
|
||||
Math::mul3(dt, tmp, tmp); // = vel*dt ("deltaCG")
|
||||
Math::add3(cg, tmp, tmp); // = cg0 + deltaCG
|
||||
_body->getCG(cg);
|
||||
l2gVector(o2, cg, cg); // cg = l2gNEW(cg) ("cg1")
|
||||
Math::sub3(tmp, cg, tmp); // tmp = cg0 + deltaCG - cg1
|
||||
|
||||
pos[0] += tmp[0]; // p1 = p0 + (cg0+deltaCG-cg1)
|
||||
pos[1] += tmp[1]; // (positions are doubles, so we
|
||||
pos[2] += tmp[2]; // can't use Math::add3)
|
||||
}
|
||||
|
||||
#if 0
|
||||
// A straight euler integration, for reference. Don't use.
|
||||
void Integrator::calcNewInterval()
|
||||
{
|
||||
float tmp[3];
|
||||
State s = _s;
|
||||
|
||||
float dt = _dt / 4;
|
||||
|
||||
orthonormalize(_s.orient);
|
||||
for(int i=0; i<4; i++) {
|
||||
_body->reset();
|
||||
_env->calcForces(&s);
|
||||
|
||||
_body->getAccel(s.acc);
|
||||
l2gVector(_s.orient, s.acc, s.acc);
|
||||
|
||||
_body->getAngularAccel(s.racc);
|
||||
l2gVector(_s.orient, s.racc, s.racc);
|
||||
|
||||
float rotmat[9];
|
||||
rotMatrix(s.rot, dt, rotmat);
|
||||
Math::mmul33(_s.orient, rotmat, s.orient);
|
||||
|
||||
extrapolatePosition(s.pos, s.v, dt, _s.orient, s.orient);
|
||||
|
||||
Math::mul3(dt, s.acc, tmp);
|
||||
Math::add3(tmp, s.v, s.v);
|
||||
|
||||
Math::mul3(dt, s.racc, tmp);
|
||||
Math::add3(tmp, s.rot, s.rot);
|
||||
|
||||
_s = s;
|
||||
}
|
||||
|
||||
_env->newState(&_s);
|
||||
}
|
||||
#endif
|
||||
|
||||
void Integrator::calcNewInterval()
|
||||
{
|
||||
// In principle, these could be changed for something other than
|
||||
// a 4th order integration. I doubt if anyone cares.
|
||||
const static int NITER=4;
|
||||
static float TIMESTEP[] = { 1.0, 0.5, 0.5, 1.0 };
|
||||
static float WEIGHTS[] = { 6.0, 3.0, 3.0, 6.0 };
|
||||
|
||||
// Scratch pads:
|
||||
double pos[NITER][3]; float vel[NITER][3]; float acc[NITER][3];
|
||||
float ori[NITER][9]; float rot[NITER][3]; float rac[NITER][3];
|
||||
|
||||
float *currVel = _s.v;
|
||||
float *currAcc = _s.acc;
|
||||
float *currRot = _s.rot;
|
||||
float *currRac = _s.racc;
|
||||
|
||||
// First off, sanify the initial orientation
|
||||
orthonormalize(_s.orient);
|
||||
|
||||
for(int i=0; i<NITER; i++) {
|
||||
//
|
||||
// extrapolate forward based on current values of the
|
||||
// derivatives and the ORIGINAL values of the
|
||||
// position/orientation.
|
||||
//
|
||||
float dt = _dt * TIMESTEP[i];
|
||||
float tmp[3];
|
||||
|
||||
// "add" rotation to orientation (generate a rotation matrix)
|
||||
float rotmat[9];
|
||||
rotMatrix(currRot, dt, rotmat);
|
||||
Math::mmul33(_s.orient, rotmat, ori[i]);
|
||||
|
||||
// add velocity to (original!) position
|
||||
for(int j=0; j<3; j++) pos[i][j] = _s.pos[j];
|
||||
extrapolatePosition(pos[i], currVel, dt, _s.orient, ori[i]);
|
||||
|
||||
// add acceleration to (original!) velocity
|
||||
Math::set3(currAcc, tmp);
|
||||
Math::mul3(dt, tmp, tmp);
|
||||
Math::add3(_s.v, tmp, vel[i]);
|
||||
|
||||
// add rotational acceleration to rotation
|
||||
Math::set3(currRac, tmp);
|
||||
Math::mul3(dt, tmp, tmp);
|
||||
Math::add3(_s.rot, tmp, rot[i]);
|
||||
|
||||
//
|
||||
// Tell the environment to generate new forces on the body,
|
||||
// extract the accelerations, and convert to vectors in the
|
||||
// global frame.
|
||||
//
|
||||
_body->reset();
|
||||
|
||||
// FIXME. Copying into a state object is clumsy! The
|
||||
// per-coordinate arrays should be changed into a single array
|
||||
// of State objects. Ick.
|
||||
State stmp;
|
||||
for(int j=0; j<3; j++) {
|
||||
stmp.pos[j] = pos[i][j];
|
||||
stmp.v[j] = vel[i][j];
|
||||
stmp.rot[j] = rot[i][j];
|
||||
}
|
||||
for(int j=0; j<9; j++)
|
||||
stmp.orient[j] = ori[i][j];
|
||||
_env->calcForces(&stmp);
|
||||
|
||||
_body->getAccel(acc[i]);
|
||||
_body->getAngularAccel(rac[i]);
|
||||
l2gVector(_s.orient, acc[i], acc[i]);
|
||||
l2gVector(_s.orient, rac[i], rac[i]);
|
||||
|
||||
//
|
||||
// Save the resulting derivatives for the next iteration
|
||||
//
|
||||
currVel = vel[i]; currAcc = acc[i];
|
||||
currRot = rot[i]; currRac = rac[i];
|
||||
}
|
||||
|
||||
// Average the resulting derivatives together according to their
|
||||
// weights. Yes, we're "averaging" rotations, which isn't
|
||||
// stricly correct -- rotations live in a non-cartesian space.
|
||||
// But the space is "locally" cartesian.
|
||||
State derivs;
|
||||
float tot = 0;
|
||||
for(int i=0; i<NITER; i++) {
|
||||
float wgt = WEIGHTS[i];
|
||||
tot += wgt;
|
||||
for(int j=0; j<3; j++) {
|
||||
derivs.v[j] += wgt*vel[i][j]; derivs.rot[j] += wgt*rot[i][j];
|
||||
derivs.acc[j] += wgt*acc[i][j]; derivs.racc[j] += wgt*rac[i][j];
|
||||
}
|
||||
}
|
||||
float itot = 1/tot;
|
||||
for(int i=0; i<3; i++) {
|
||||
derivs.v[i] *= itot; derivs.rot[i] *= itot;
|
||||
derivs.acc[i] *= itot; derivs.racc[i] *= itot;
|
||||
}
|
||||
|
||||
// And finally extrapolate once more, using the averaged
|
||||
// derivatives, to the final position and orientation. This code
|
||||
// is essentially identical to the position extrapolation step
|
||||
// inside the loop.
|
||||
|
||||
// save the starting orientation
|
||||
float orient0[9];
|
||||
for(int i=0; i<9; i++) orient0[i] = _s.orient[i];
|
||||
|
||||
float rotmat[9];
|
||||
rotMatrix(derivs.rot, _dt, rotmat);
|
||||
Math::mmul33(orient0, rotmat, _s.orient);
|
||||
|
||||
extrapolatePosition(_s.pos, derivs.v, _dt, orient0, _s.orient);
|
||||
|
||||
float tmp[3];
|
||||
Math::mul3(_dt, derivs.acc, tmp);
|
||||
Math::add3(_s.v, tmp, _s.v);
|
||||
|
||||
Math::mul3(_dt, derivs.racc, tmp);
|
||||
Math::add3(_s.rot, tmp, _s.rot);
|
||||
|
||||
for(int i=0; i<3; i++) {
|
||||
_s.acc[i] = derivs.acc[i];
|
||||
_s.racc[i] = derivs.racc[i];
|
||||
}
|
||||
|
||||
// Tell the environment about our decision
|
||||
_env->newState(&_s);
|
||||
}
|
||||
|
||||
// Generates a matrix that rotates about axis r through an angle equal
|
||||
// to (|r| * dt). That is, a rotation effected by rotating with rate
|
||||
// r for dt "seconds" (or whatever time unit is in use).
|
||||
// Implementation shamelessly cribbed from the OpenGL specification.
|
||||
//
|
||||
// NOTE: we're actually returning the _transpose_ of the rotation
|
||||
// matrix! This is becuase we store orientations as global-to-local
|
||||
// transformations. Thus, we want to rotate the ROWS of the old
|
||||
// matrix to get the new one.
|
||||
void Integrator::rotMatrix(float* r, float dt, float* out)
|
||||
{
|
||||
// Normalize the vector, and extract the angle through which we'll
|
||||
// rotate.
|
||||
float mag = Math::mag3(r);
|
||||
float angle = dt*mag;
|
||||
|
||||
// Tiny rotations result in degenerate (zero-length) rotation
|
||||
// vectors, so clamp to an identity matrix. 1e-06 radians
|
||||
// per 1/30th of a second (typical dt unit) corresponds to one
|
||||
// revolution per 2.4 days, or significantly less than the
|
||||
// coriolis rotation. And it's still preserves half the floating
|
||||
// point precision of a radian-per-iteration rotation.
|
||||
if(angle < 1e-06) {
|
||||
out[0] = 1; out[1] = 0; out[2] = 0;
|
||||
out[3] = 0; out[4] = 1; out[5] = 0;
|
||||
out[6] = 0; out[7] = 0; out[8] = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
float runit[3];
|
||||
Math::mul3(1/mag, r, runit);
|
||||
|
||||
float s = Math::sin(angle);
|
||||
float c = Math::cos(angle);
|
||||
float c1 = 1-c;
|
||||
float c1rx = c1*runit[0];
|
||||
float c1ry = c1*runit[1];
|
||||
float xx = c1rx*runit[0];
|
||||
float xy = c1rx*runit[1];
|
||||
float xz = c1rx*runit[2];
|
||||
float yy = c1ry*runit[1];
|
||||
float yz = c1ry*runit[2];
|
||||
float zz = c1*runit[2]*runit[2];
|
||||
float xs = runit[0]*s;
|
||||
float ys = runit[1]*s;
|
||||
float zs = runit[2]*s;
|
||||
|
||||
out[0] = xx+c ; out[3] = xy-zs; out[6] = xz+ys;
|
||||
out[1] = xy+zs; out[4] = yy+c ; out[7] = yz-xs;
|
||||
out[2] = xz-ys; out[5] = yz+xs; out[8] = zz+c ;
|
||||
}
|
||||
|
||||
// Does a Gram-Schmidt orthonormalization on the rows of a
|
||||
// global-to-local orientation matrix. The order of normalization
|
||||
// here is x, z, y. This is because of the convention of "x" being
|
||||
// the forward vector and "z" being up in the body frame. These two
|
||||
// vectors are the most important to keep correct.
|
||||
void Integrator::orthonormalize(float* m)
|
||||
{
|
||||
// The 1st, 2nd and 3rd rows of the matrix store the global frame
|
||||
// equivalents of the x, y, and z unit vectors in the local frame.
|
||||
float *x = m, *y = m+3, *z = m+6;
|
||||
|
||||
Math::unit3(x, x); // x = x/|x|
|
||||
|
||||
float v[3];
|
||||
Math::mul3(Math::dot3(x, z), z, v); // v = z(x dot z)
|
||||
Math::sub3(z, v, z); // z = z - z*(x dot z)
|
||||
Math::unit3(z, z); // z = z/|z|
|
||||
|
||||
Math::cross3(z, x, y); // y = z cross x
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
58
src/FDM/YASim/Integrator.hpp
Normal file
58
src/FDM/YASim/Integrator.hpp
Normal file
|
|
@ -0,0 +1,58 @@
|
|||
#ifndef _INTEGRATOR_HPP
|
||||
#define _INTEGRATOR_HPP
|
||||
|
||||
#include "BodyEnvironment.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
//
|
||||
// These objects are responsible for extracting force data from a
|
||||
// BodyEnvironment object, using a RigidBody object to calculate
|
||||
// accelerations, and then tying that all together into a new
|
||||
// "solution" of position/orientation/etc... for the body. The method
|
||||
// used is a fourth-order Runge-Kutta integration.
|
||||
//
|
||||
class Integrator
|
||||
{
|
||||
public:
|
||||
// Sets the RigidBody that will be integrated.
|
||||
void setBody(RigidBody* body);
|
||||
|
||||
// Sets the BodyEnvironment object used to calculate the second
|
||||
// derivatives.
|
||||
void setEnvironment(BodyEnvironment* env);
|
||||
|
||||
// Sets the time interval between steps. Units can be anything,
|
||||
// but they must match the other values (if you specify speed in
|
||||
// m/s, then angular acceleration had better be in 1/s^2 and the
|
||||
// time interval should be in seconds, etc...)
|
||||
void setInterval(float dt);
|
||||
float getInterval();
|
||||
|
||||
// The current state, i.e. initial conditions for the next
|
||||
// integration iteration. Note that the acceleration parameters
|
||||
// in the State object are ignored.
|
||||
State* getState();
|
||||
void setState(State* s);
|
||||
|
||||
// Do a 4th order Runge-Kutta integration over one time interval.
|
||||
// This is the top level of the simulation.
|
||||
void calcNewInterval();
|
||||
|
||||
private:
|
||||
void orthonormalize(float* m);
|
||||
void rotMatrix(float* r, float dt, float* out);
|
||||
void l2gVector(float* orient, float* v, float* out);
|
||||
void extrapolatePosition(double* pos, float* v, float dt,
|
||||
float* o1, float* o2);
|
||||
|
||||
BodyEnvironment* _env;
|
||||
RigidBody* _body;
|
||||
float _dt;
|
||||
|
||||
State _s;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _INTEGRATOR_HPP
|
||||
59
src/FDM/YASim/Jet.cpp
Normal file
59
src/FDM/YASim/Jet.cpp
Normal file
|
|
@ -0,0 +1,59 @@
|
|||
#include "Atmosphere.hpp"
|
||||
#include "Math.hpp"
|
||||
#include "Jet.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Jet::Jet()
|
||||
{
|
||||
_rho0 = Atmosphere::getStdDensity(0);
|
||||
_thrust = 0;
|
||||
_reheat = 0;
|
||||
}
|
||||
|
||||
Thruster* Jet::clone()
|
||||
{
|
||||
Jet* j = new Jet();
|
||||
j->_thrust = _thrust;
|
||||
j->_rho0 = _rho0;
|
||||
return j;
|
||||
}
|
||||
|
||||
void Jet::setDryThrust(float thrust)
|
||||
{
|
||||
_thrust = thrust;
|
||||
}
|
||||
|
||||
void Jet::setReheat(float reheat)
|
||||
{
|
||||
_reheat = reheat;
|
||||
}
|
||||
|
||||
void Jet::getThrust(float* out)
|
||||
{
|
||||
float t = _thrust * _throttle * (_rho / _rho0);
|
||||
Math::mul3(t, _dir, out);
|
||||
}
|
||||
|
||||
void Jet::getTorque(float* out)
|
||||
{
|
||||
out[0] = out[1] = out[2] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
void Jet::getGyro(float* out)
|
||||
{
|
||||
out[0] = out[1] = out[2] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
float Jet::getFuelFlow()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void Jet::integrate(float dt)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
32
src/FDM/YASim/Jet.hpp
Normal file
32
src/FDM/YASim/Jet.hpp
Normal file
|
|
@ -0,0 +1,32 @@
|
|||
#ifndef _JET_HPP
|
||||
#define _JET_HPP
|
||||
|
||||
#include "Thruster.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// Incredibly dumb placeholder for a Jet implementation. But it does
|
||||
// what's important, which is provide thrust.
|
||||
class Jet : public Thruster {
|
||||
public:
|
||||
Jet();
|
||||
|
||||
virtual Thruster* clone();
|
||||
|
||||
void setDryThrust(float thrust);
|
||||
void setReheat(float reheat);
|
||||
|
||||
virtual void getThrust(float* out);
|
||||
virtual void getTorque(float* out);
|
||||
virtual void getGyro(float* out);
|
||||
virtual float getFuelFlow();
|
||||
virtual void integrate(float dt);
|
||||
|
||||
private:
|
||||
float _thrust;
|
||||
float _rho0;
|
||||
float _reheat;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _JET_HPP
|
||||
9
src/FDM/YASim/Makefile.am
Normal file
9
src/FDM/YASim/Makefile.am
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
noinst_LIBRARIES = libYASim.a
|
||||
|
||||
libYASim_a_SOURCES = YASim.cxx Airplane.cpp Atmosphere.cpp ControlMap.cpp \
|
||||
FGFDM.cpp Gear.cpp Glue.cpp Integrator.cpp Jet.cpp \
|
||||
Math.cpp Model.cpp PistonEngine.cpp Propeller.cpp \
|
||||
PropEngine.cpp RigidBody.cpp Surface.cpp \
|
||||
Thruster.cpp Wing.cpp
|
||||
|
||||
INCLUDES += -I$(top_srcdir) -I$(top_srcdir)/src
|
||||
237
src/FDM/YASim/Math.cpp
Normal file
237
src/FDM/YASim/Math.cpp
Normal file
|
|
@ -0,0 +1,237 @@
|
|||
#include <math.h>
|
||||
|
||||
#include "Math.hpp"
|
||||
namespace yasim {
|
||||
|
||||
float Math::abs(float f)
|
||||
{
|
||||
return ::fabs(f);
|
||||
}
|
||||
|
||||
float Math::sqrt(float f)
|
||||
{
|
||||
return ::sqrt(f);
|
||||
}
|
||||
|
||||
float Math::pow(float base, float exp)
|
||||
{
|
||||
return ::pow(base, exp);
|
||||
}
|
||||
|
||||
float Math::ceil(float f)
|
||||
{
|
||||
return ::ceil(f);
|
||||
}
|
||||
|
||||
float Math::cos(float f)
|
||||
{
|
||||
return ::cos(f);
|
||||
}
|
||||
|
||||
float Math::sin(float f)
|
||||
{
|
||||
return ::sin(f);
|
||||
}
|
||||
|
||||
float Math::tan(float f)
|
||||
{
|
||||
return ::tan(f);
|
||||
}
|
||||
|
||||
float Math::atan2(float y, float x)
|
||||
{
|
||||
return ::atan2(y, x);
|
||||
}
|
||||
|
||||
double Math::abs(double f)
|
||||
{
|
||||
return ::fabs(f);
|
||||
}
|
||||
|
||||
double Math::sqrt(double f)
|
||||
{
|
||||
return ::sqrt(f);
|
||||
}
|
||||
|
||||
double Math::pow(double base, double exp)
|
||||
{
|
||||
return ::pow(base, exp);
|
||||
}
|
||||
|
||||
double Math::ceil(double f)
|
||||
{
|
||||
return ::ceil(f);
|
||||
}
|
||||
|
||||
double Math::cos(double f)
|
||||
{
|
||||
return ::cos(f);
|
||||
}
|
||||
|
||||
double Math::sin(double f)
|
||||
{
|
||||
return ::sin(f);
|
||||
}
|
||||
|
||||
double Math::tan(double f)
|
||||
{
|
||||
return ::tan(f);
|
||||
}
|
||||
|
||||
double Math::atan2(double y, double x)
|
||||
{
|
||||
return ::atan2(y, x);
|
||||
}
|
||||
|
||||
void Math::set3(float* v, float* out)
|
||||
{
|
||||
out[0] = v[0];
|
||||
out[1] = v[1];
|
||||
out[2] = v[2];
|
||||
}
|
||||
|
||||
float Math::dot3(float* a, float* b)
|
||||
{
|
||||
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
|
||||
}
|
||||
|
||||
void Math::mul3(float scalar, float* v, float* out)
|
||||
{
|
||||
out[0] = scalar * v[0];
|
||||
out[1] = scalar * v[1];
|
||||
out[2] = scalar * v[2];
|
||||
}
|
||||
|
||||
void Math::add3(float* a, float* b, float* out)
|
||||
{
|
||||
out[0] = a[0] + b[0];
|
||||
out[1] = a[1] + b[1];
|
||||
out[2] = a[2] + b[2];
|
||||
}
|
||||
|
||||
void Math::sub3(float* a, float* b, float* out)
|
||||
{
|
||||
out[0] = a[0] - b[0];
|
||||
out[1] = a[1] - b[1];
|
||||
out[2] = a[2] - b[2];
|
||||
}
|
||||
|
||||
float Math::mag3(float* v)
|
||||
{
|
||||
return sqrt(dot3(v, v));
|
||||
}
|
||||
|
||||
|
||||
void Math::unit3(float* v, float* out)
|
||||
{
|
||||
float imag = 1/mag3(v);
|
||||
mul3(imag, v, out);
|
||||
}
|
||||
|
||||
void Math::cross3(float* a, 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;
|
||||
out[1] = az*bx - bz*ax;
|
||||
out[2] = ax*by - bx*ay;
|
||||
}
|
||||
|
||||
void Math::mmul33(float* a, 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];
|
||||
tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
|
||||
|
||||
tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
|
||||
tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
|
||||
tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
|
||||
|
||||
tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
|
||||
tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
|
||||
tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
|
||||
|
||||
for(int i=0; i<9; i++)
|
||||
out[i] = tmp[i];
|
||||
}
|
||||
|
||||
void Math::vmul33(float* m, 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];
|
||||
out[2] = x*m[6] + y*m[7] + z*m[8];
|
||||
}
|
||||
|
||||
void Math::tmul33(float* m, 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];
|
||||
}
|
||||
|
||||
void Math::invert33(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
|
||||
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 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 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;
|
||||
}
|
||||
|
||||
void Math::trans33(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
|
||||
out[0] = m[0];
|
||||
out[4] = m[4];
|
||||
out[8] = m[8];
|
||||
|
||||
float tmp = m[1];
|
||||
out[1] = m[3];
|
||||
out[3] = tmp;
|
||||
|
||||
tmp = m[2];
|
||||
out[2] = m[6];
|
||||
out[6] = tmp;
|
||||
|
||||
tmp = m[5];
|
||||
out[5] = m[7];
|
||||
out[7] = tmp;
|
||||
}
|
||||
|
||||
void Math::ortho33(float* x, float* y,
|
||||
float* xOut, float* yOut, float* zOut)
|
||||
{
|
||||
float x0[3], y0[3];
|
||||
set3(x, x0);
|
||||
set3(y, y0);
|
||||
|
||||
unit3(x0, xOut);
|
||||
cross3(xOut, y0, zOut);
|
||||
unit3(zOut, zOut);
|
||||
cross3(zOut, xOut, yOut);
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
70
src/FDM/YASim/Math.hpp
Normal file
70
src/FDM/YASim/Math.hpp
Normal file
|
|
@ -0,0 +1,70 @@
|
|||
#ifndef _MATH_HPP
|
||||
#define _MATH_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Math
|
||||
{
|
||||
public:
|
||||
// Simple wrappers around library routines
|
||||
static float abs(float f);
|
||||
static float sqrt(float f);
|
||||
static float pow(float base, float exp);
|
||||
static float ceil(float f);
|
||||
static float sin(float f);
|
||||
static float cos(float f);
|
||||
static float tan(float f);
|
||||
static float atan2(float y, float x);
|
||||
|
||||
// double variants of the above
|
||||
static double abs(double f);
|
||||
static double sqrt(double f);
|
||||
static double pow(double base, double exp);
|
||||
static double ceil(double f);
|
||||
static double sin(double f);
|
||||
static double cos(double f);
|
||||
static double tan(double f);
|
||||
static double atan2(double y, double x);
|
||||
|
||||
// Some 3D vector stuff. In all cases, it is permissible for the
|
||||
// "out" vector to be the same as one of the inputs.
|
||||
static void set3(float* v, float* out);
|
||||
static float dot3(float* a, float* b);
|
||||
static void cross3(float* a, float* b, float* out);
|
||||
static void mul3(float scalar, float* v, float* out);
|
||||
static void add3(float* a, float* b, float* out);
|
||||
static void sub3(float* a, float* b, float* out);
|
||||
static float mag3(float* v);
|
||||
static void unit3(float* v, float* out);
|
||||
|
||||
// Matrix array convention: 0 1 2
|
||||
// 3 4 5
|
||||
// 6 7 8
|
||||
|
||||
// Multiply two matrices
|
||||
static void mmul33(float* a, float* b, float* out);
|
||||
|
||||
// Multiply by vector
|
||||
static void vmul33(float* m, float* v, float* out);
|
||||
|
||||
// Multiply the vector by the matrix transpose. Or pre-multiply the
|
||||
// matrix by v as a row vector. Same thing.
|
||||
static void tmul33(float* m, float* v, float* out);
|
||||
|
||||
// Invert matrix
|
||||
static void invert33(float* m, float* out);
|
||||
|
||||
// Transpose matrix (for an orthonormal orientation matrix, this
|
||||
// is the same as the inverse).
|
||||
static void trans33(float* m, float* out);
|
||||
|
||||
// Generates an orthonormal basis:
|
||||
// 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,
|
||||
float* xOut, float* yOut, float* zOut);
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _MATH_HPP
|
||||
296
src/FDM/YASim/Model.cpp
Normal file
296
src/FDM/YASim/Model.cpp
Normal file
|
|
@ -0,0 +1,296 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include "Atmosphere.hpp"
|
||||
#include "Thruster.hpp"
|
||||
#include "Math.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
#include "Integrator.hpp"
|
||||
#include "Propeller.hpp"
|
||||
#include "PistonEngine.hpp"
|
||||
#include "Gear.hpp"
|
||||
#include "Surface.hpp"
|
||||
#include "Glue.hpp"
|
||||
|
||||
#include "Model.hpp"
|
||||
namespace yasim {
|
||||
|
||||
void printState(State* s)
|
||||
{
|
||||
State tmp = *s;
|
||||
Math::vmul33(tmp.orient, tmp.v, tmp.v);
|
||||
Math::vmul33(tmp.orient, tmp.acc, tmp.acc);
|
||||
Math::vmul33(tmp.orient, tmp.rot, tmp.rot);
|
||||
Math::vmul33(tmp.orient, tmp.racc, tmp.racc);
|
||||
|
||||
printf("\nNEW STATE (LOCAL COORDS)\n");
|
||||
printf("pos: %10.2f %10.2f %10.2f\n", tmp.pos[0], tmp.pos[1], tmp.pos[2]);
|
||||
printf("o: ");
|
||||
for(int i=0; i<3; i++) {
|
||||
if(i != 0) printf(" ");
|
||||
printf("%6.2f %6.2f %6.2f\n",
|
||||
tmp.orient[3*i+0], tmp.orient[3*i+1], tmp.orient[3*i+2]);
|
||||
}
|
||||
printf("v: %6.2f %6.2f %6.2f\n", tmp.v[0], tmp.v[1], tmp.v[2]);
|
||||
printf("acc: %6.2f %6.2f %6.2f\n", tmp.acc[0], tmp.acc[1], tmp.acc[2]);
|
||||
printf("rot: %6.2f %6.2f %6.2f\n", tmp.rot[0], tmp.rot[1], tmp.rot[2]);
|
||||
printf("rac: %6.2f %6.2f %6.2f\n", tmp.racc[0], tmp.racc[1], tmp.racc[2]);
|
||||
}
|
||||
|
||||
Model::Model()
|
||||
{
|
||||
for(int i=0; i<3; i++) _wind[i] = 0;
|
||||
|
||||
_integrator.setBody(&_body);
|
||||
_integrator.setEnvironment(this);
|
||||
|
||||
// Default value of 30 Hz
|
||||
_integrator.setInterval(1.0/30.0);
|
||||
}
|
||||
|
||||
Model::~Model()
|
||||
{
|
||||
// FIXME: who owns these things? Need a policy
|
||||
}
|
||||
|
||||
void Model::getThrust(float* out)
|
||||
{
|
||||
float tmp[3];
|
||||
out[0] = out[1] = out[2] = 0;
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
Thruster* t = (Thruster*)_thrusters.get(i);
|
||||
t->getThrust(tmp);
|
||||
Math::add3(tmp, out, out);
|
||||
}
|
||||
}
|
||||
|
||||
void Model::initIteration()
|
||||
{
|
||||
// Precompute torque and angular momentum for the thrusters
|
||||
for(int i=0; i<3; i++)
|
||||
_gyro[i] = _torque[i] = 0;
|
||||
for(int i=0; i<_thrusters.size(); i++) {
|
||||
Thruster* t = (Thruster*)_thrusters.get(i);
|
||||
|
||||
// Get the wind velocity at the thruster location
|
||||
float pos[3], v[3];
|
||||
t->getPosition(pos);
|
||||
localWind(pos, _s, v);
|
||||
|
||||
t->setWind(v);
|
||||
t->setDensity(_rho);
|
||||
t->integrate(_integrator.getInterval());
|
||||
|
||||
t->getTorque(v);
|
||||
Math::add3(v, _torque, _torque);
|
||||
|
||||
t->getGyro(v);
|
||||
Math::add3(v, _gyro, _gyro);
|
||||
}
|
||||
}
|
||||
|
||||
void Model::iterate()
|
||||
{
|
||||
initIteration();
|
||||
_body.recalc(); // FIXME: amortize this, somehow
|
||||
_integrator.calcNewInterval();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int Model::addThruster(Thruster* t)
|
||||
{
|
||||
return _thrusters.add(t);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// The first three elements are a unit vector pointing from the global
|
||||
// origin to the plane, the final element is the distance from the
|
||||
// origin (the radius of the earth, in most implementations). So
|
||||
// (v dot _ground)-_ground[3] gives the distance AGL.
|
||||
void Model::setGroundPlane(double* planeNormal, double fromOrigin)
|
||||
{
|
||||
for(int i=0; i<3; i++) _ground[i] = planeNormal[i];
|
||||
_ground[3] = fromOrigin;
|
||||
}
|
||||
|
||||
void Model::setAirDensity(float rho)
|
||||
{
|
||||
_rho = rho;
|
||||
}
|
||||
|
||||
void Model::setAir(float pressure, float temp)
|
||||
{
|
||||
_rho = Atmosphere::calcDensity(pressure, temp);
|
||||
}
|
||||
|
||||
void Model::setWind(float* wind)
|
||||
{
|
||||
Math::set3(wind, _wind);
|
||||
}
|
||||
|
||||
void Model::calcForces(State* s)
|
||||
{
|
||||
// Add in the pre-computed stuff. These values aren't part of the
|
||||
// Runge-Kutta integration (they don't depend on position or
|
||||
// velocity), and are therefore constant across the four calls to
|
||||
// calcForces. They get computed before we begin the integration
|
||||
// step.
|
||||
_body.setGyro(_gyro);
|
||||
_body.addTorque(_torque);
|
||||
for(int 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);
|
||||
}
|
||||
|
||||
// Gravity, convert to a force, then to local coordinates
|
||||
float grav[3];
|
||||
Glue::geodUp(s->pos, grav);
|
||||
Math::mul3(-9.8 * _body.getTotalMass(), grav, grav);
|
||||
Math::vmul33(s->orient, grav, grav);
|
||||
_body.addForce(grav);
|
||||
|
||||
// Do each surface, remembering that the local velocity at each
|
||||
// point is different due to rotation.
|
||||
for(int i=0; i<_surfaces.size(); 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);
|
||||
|
||||
float force[3], torque[3];
|
||||
sf->calcForce(vs, _rho, force, torque);
|
||||
_body.addForce(pos, force);
|
||||
_body.addTorque(torque);
|
||||
}
|
||||
|
||||
// Get a ground plane in local coordinates. The first three
|
||||
// elements are the normal vector, the final one is the distance
|
||||
// from the local origin along that vector to the ground plane
|
||||
// (negative for objects "above" the ground)
|
||||
float ground[4];
|
||||
ground[3] = localGround(s, ground);
|
||||
|
||||
// Convert the velocity and rotation vectors to local coordinates
|
||||
float lrot[3], lv[3];
|
||||
Math::vmul33(s->orient, s->rot, lrot);
|
||||
Math::vmul33(s->orient, s->v, lv);
|
||||
|
||||
// The landing gear
|
||||
for(int i=0; i<_gears.size(); i++) {
|
||||
float force[3], contact[3];
|
||||
Gear* g = (Gear*)_gears.get(i);
|
||||
g->calcForce(&_body, lv, lrot, ground);
|
||||
g->getForce(force, contact);
|
||||
_body.addForce(contact, force);
|
||||
}
|
||||
}
|
||||
|
||||
void Model::newState(State* s)
|
||||
{
|
||||
_s = s;
|
||||
|
||||
//printState(s);
|
||||
|
||||
// Some simple collision detection
|
||||
float ground[4], pos[3], cmpr[3];
|
||||
ground[3] = localGround(s, ground);
|
||||
for(int i=0; i<_gears.size(); i++) {
|
||||
Gear* g = (Gear*)_gears.get(i);
|
||||
g->getPosition(pos);
|
||||
g->getCompression(cmpr);
|
||||
Math::add3(cmpr, pos, pos);
|
||||
float dist = ground[3] - Math::dot3(pos, ground);
|
||||
if(dist < 0) {
|
||||
printf("CRASH: gear %d\n", i);
|
||||
*(int*)0=0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Returns a unit "down" vector for the ground in out, and the
|
||||
// distance from the local origin to the ground as the return value.
|
||||
// So for a given position V, "dist - (V dot out)" will be the height
|
||||
// AGL.
|
||||
float Model::localGround(State* s, float* out)
|
||||
{
|
||||
// Get the ground's "down" vector, this can be in floats, because
|
||||
// we don't need positioning accuracy. The direction has plenty
|
||||
// of accuracy after truncation.
|
||||
out[0] = -(float)_ground[0];
|
||||
out[1] = -(float)_ground[1];
|
||||
out[2] = -(float)_ground[2];
|
||||
Math::vmul33(s->orient, out, out);
|
||||
|
||||
// The distance from the ground to the Aircraft's origin:
|
||||
double dist = (s->pos[0]*_ground[0]
|
||||
+ s->pos[1]*_ground[1]
|
||||
+ s->pos[2]*_ground[2] - _ground[3]);
|
||||
|
||||
return (float)dist;
|
||||
}
|
||||
|
||||
// Calculates the airflow direction at the given point and for the
|
||||
// specified aircraft velocity.
|
||||
void Model::localWind(float* pos, State* s, float* out)
|
||||
{
|
||||
// Most of the input is in global coordinates. Fix that.
|
||||
float lwind[3], lrot[3], lv[3];
|
||||
Math::vmul33(s->orient, _wind, lwind);
|
||||
Math::vmul33(s->orient, s->rot, lrot);
|
||||
Math::vmul33(s->orient, s->v, lv);
|
||||
|
||||
_body.pointVelocity(pos, lrot, out); // rotational velocity
|
||||
Math::mul3(-1, out, out); // (negated)
|
||||
Math::add3(lwind, out, out); // + wind
|
||||
Math::sub3(out, lv, out); // - velocity
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
80
src/FDM/YASim/Model.hpp
Normal file
80
src/FDM/YASim/Model.hpp
Normal file
|
|
@ -0,0 +1,80 @@
|
|||
#ifndef _MODEL_HPP
|
||||
#define _MODEL_HPP
|
||||
|
||||
#include "Integrator.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
#include "BodyEnvironment.hpp"
|
||||
#include "util/Vector.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// Declare the types whose pointers get passed around here
|
||||
class Integrator;
|
||||
class Thruster;
|
||||
class Surface;
|
||||
class Gear;
|
||||
|
||||
class Model : public BodyEnvironment {
|
||||
public:
|
||||
Model();
|
||||
virtual ~Model();
|
||||
|
||||
RigidBody* getBody();
|
||||
Integrator* getIntegrator();
|
||||
|
||||
State* getState();
|
||||
void setState(State* s);
|
||||
|
||||
void iterate();
|
||||
|
||||
// Externally-managed subcomponents
|
||||
int addThruster(Thruster* t);
|
||||
int addSurface(Surface* surf);
|
||||
int addGear(Gear* gear);
|
||||
Surface* getSurface(int handle);
|
||||
Gear* getGear(int handle);
|
||||
|
||||
// Semi-private methods for use by the Airplane solver.
|
||||
Thruster* getThruster(int handle);
|
||||
void setThruster(int handle, Thruster* t);
|
||||
void initIteration();
|
||||
void getThrust(float* out);
|
||||
|
||||
//
|
||||
// Per-iteration settables
|
||||
//
|
||||
void setGroundPlane(double* planeNormal, double fromOrigin);
|
||||
void setWind(float* wind);
|
||||
void setAirDensity(float rho);
|
||||
void setAir(float pressure, float temp);
|
||||
|
||||
// BodyEnvironment callbacks
|
||||
virtual void calcForces(State* s);
|
||||
virtual void newState(State* s);
|
||||
|
||||
private:
|
||||
void calcGearForce(Gear* g, float* v, float* rot, float* ground);
|
||||
float gearFriction(float wgt, float v, Gear* g);
|
||||
float localGround(State* s, float* out);
|
||||
void localWind(float* pos, State* s, float* out);
|
||||
|
||||
Integrator _integrator;
|
||||
RigidBody _body;
|
||||
|
||||
Vector _thrusters;
|
||||
Vector _surfaces;
|
||||
Vector _gears;
|
||||
|
||||
double _ground[4];
|
||||
float _rho;
|
||||
float _wind[3];
|
||||
|
||||
// Accumulators for the total internal gyro and engine torque
|
||||
float _gyro[3];
|
||||
float _torque[3];
|
||||
|
||||
State* _s;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _MODEL_HPP
|
||||
67
src/FDM/YASim/PistonEngine.cpp
Normal file
67
src/FDM/YASim/PistonEngine.cpp
Normal file
|
|
@ -0,0 +1,67 @@
|
|||
#include "PistonEngine.hpp"
|
||||
namespace yasim {
|
||||
|
||||
PistonEngine::PistonEngine(float power, float speed)
|
||||
{
|
||||
// Presume a BSFC (in lb/hour per HP) of 0.45. In SI that becomes
|
||||
// (2.2 lb/kg, 745.7 W/hp, 3600 sec/hour) 3.69e-07 kg/Ws.
|
||||
_f0 = power * 3.69e-07;
|
||||
|
||||
_P0 = power;
|
||||
_omega0 = speed;
|
||||
|
||||
// We must be at sea level under standard conditions
|
||||
_rho0 = 1.225; // kg/m^3
|
||||
|
||||
// Further presume that takeoff is (duh) full throttle and
|
||||
// peak-power, that means that by our efficiency function, we are
|
||||
// at 11/8 of "ideal" fuel flow.
|
||||
float realFlow = _f0 * (11.0/8.0);
|
||||
_mixCoeff = realFlow * 1.1 / _omega0;
|
||||
}
|
||||
|
||||
void PistonEngine::setThrottle(float t)
|
||||
{
|
||||
_throttle = t;
|
||||
}
|
||||
|
||||
void PistonEngine::setMixture(float m)
|
||||
{
|
||||
_mixture = m;
|
||||
}
|
||||
|
||||
void PistonEngine::calc(float density, float speed,
|
||||
float* torqueOut, float* fuelFlowOut)
|
||||
{
|
||||
// The actual fuel flow
|
||||
float fuel = _mixture * _mixCoeff * speed;
|
||||
|
||||
// manifold air density
|
||||
float rho = density * _throttle;
|
||||
|
||||
// How much fuel could be burned with ideal (i.e. uncorrected!)
|
||||
// combustion.
|
||||
float burnable = _f0 * (rho/_rho0) * (speed/_omega0);
|
||||
|
||||
// Calculate the fuel that actually burns to produce work. The
|
||||
// idea is that less than 5/8 of ideal, we get complete
|
||||
// combustion. We use up all the oxygen at 1 3/8 of ideal (that
|
||||
// is, you need to waste fuel to use all your O2). In between,
|
||||
// interpolate. This vaguely matches a curve I copied out of a
|
||||
// book for a single engine. Shrug.
|
||||
float burned;
|
||||
float r = fuel/burnable;
|
||||
if (burnable == 0) burned = 0;
|
||||
else if(r < .625) burned = fuel;
|
||||
else if(r > 1.375) burned = burnable;
|
||||
else burned = fuel + (burnable-fuel)*(r-.625)*(4.0/3.0);
|
||||
|
||||
// And finally the power is just the reference power scaled by the
|
||||
// amount of fuel burned.
|
||||
float power = _P0 * burned/_f0;
|
||||
|
||||
*torqueOut = power/speed;
|
||||
*fuelFlowOut = fuel;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
35
src/FDM/YASim/PistonEngine.hpp
Normal file
35
src/FDM/YASim/PistonEngine.hpp
Normal file
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef _PISTONENGINE_HPP
|
||||
#define _PISTONENGINE_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class PistonEngine {
|
||||
public:
|
||||
// Initializes an engine from known "takeoff" parameters.
|
||||
PistonEngine(float power, float spd);
|
||||
|
||||
void setThrottle(float throttle);
|
||||
void setMixture(float mixture);
|
||||
|
||||
// Calculates power output and fuel flow, based on a given
|
||||
// throttle setting (0-1 corresponding to the fraction of
|
||||
// "available" manifold pressure), mixture (fuel flux per rpm,
|
||||
// 0-1, where 1 is "max rich", or a little bit more than needed
|
||||
// for rated power at sea level)
|
||||
void calc(float density, float speed,
|
||||
float* powerOut, float* fuelFlowOut);
|
||||
|
||||
private:
|
||||
float _P0; // reference power setting
|
||||
float _omega0; // " engine speed
|
||||
float _rho0; // " manifold air density
|
||||
float _f0; // "ideal" fuel flow at P0/omega0
|
||||
float _mixCoeff; // fuel flow per omega at full mixture
|
||||
|
||||
// Runtime settables:
|
||||
float _throttle;
|
||||
float _mixture;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _PISTONENGINE_HPP
|
||||
94
src/FDM/YASim/PropEngine.cpp
Normal file
94
src/FDM/YASim/PropEngine.cpp
Normal file
|
|
@ -0,0 +1,94 @@
|
|||
#include "Math.hpp"
|
||||
#include "Propeller.hpp"
|
||||
#include "PistonEngine.hpp"
|
||||
#include "PropEngine.hpp"
|
||||
namespace yasim {
|
||||
|
||||
PropEngine::PropEngine(Propeller* prop, PistonEngine* eng, float moment)
|
||||
{
|
||||
// Start off at 500rpm, because the start code doesn't exist yet
|
||||
_omega = 52.3;
|
||||
_dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
|
||||
|
||||
_prop = prop;
|
||||
_eng = eng;
|
||||
_moment = moment;
|
||||
}
|
||||
|
||||
PropEngine::~PropEngine()
|
||||
{
|
||||
delete _prop;
|
||||
delete _eng;
|
||||
}
|
||||
|
||||
float PropEngine::getOmega()
|
||||
{
|
||||
return _omega;
|
||||
}
|
||||
|
||||
Thruster* PropEngine::clone()
|
||||
{
|
||||
// FIXME: bloody mess
|
||||
PropEngine* p = new PropEngine(_prop, _eng, _moment);
|
||||
cloneInto(p);
|
||||
p->_prop = new Propeller(*_prop);
|
||||
p->_eng = new PistonEngine(*_eng);
|
||||
return p;
|
||||
}
|
||||
|
||||
void PropEngine::getThrust(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _thrust[i];
|
||||
}
|
||||
|
||||
void PropEngine::getTorque(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _torque[i];
|
||||
}
|
||||
|
||||
void PropEngine::getGyro(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _gyro[i];
|
||||
}
|
||||
|
||||
float PropEngine::getFuelFlow()
|
||||
{
|
||||
return _fuelFlow;
|
||||
}
|
||||
|
||||
void PropEngine::integrate(float dt)
|
||||
{
|
||||
float speed = -Math::dot3(_wind, _dir);
|
||||
|
||||
float propTorque, engTorque, thrust;
|
||||
|
||||
_eng->setThrottle(_throttle);
|
||||
_eng->setMixture(_mixture);
|
||||
|
||||
_prop->calc(_rho, speed, _omega,
|
||||
&thrust, &propTorque);
|
||||
_eng->calc(_rho, _omega, &engTorque, &_fuelFlow);
|
||||
|
||||
// Turn the thrust into a vector and save it
|
||||
Math::mul3(thrust, _dir, _thrust);
|
||||
|
||||
// Euler-integrate the RPM. This doesn't need the full-on
|
||||
// Runge-Kutta stuff.
|
||||
_omega += dt*(engTorque-propTorque)/Math::abs(_moment);
|
||||
|
||||
// FIXME: Integrate the propeller governor here, when that gets
|
||||
// implemented...
|
||||
|
||||
// Store the total angular momentum into _gyro
|
||||
Math::mul3(_omega*_moment, _dir, _gyro);
|
||||
|
||||
// Accumulate the engine torque, it acta on the body as a whole.
|
||||
// (Note: engine torque, not propeller torque. They can be
|
||||
// different, but the difference goes to accelerating the
|
||||
// rotation. It is the engine torque that is felt at the shaft
|
||||
// and works on the body.)
|
||||
float tau = _moment < 0 ? engTorque : -engTorque;
|
||||
Math::mul3(tau, _dir, _torque);
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
42
src/FDM/YASim/PropEngine.hpp
Normal file
42
src/FDM/YASim/PropEngine.hpp
Normal file
|
|
@ -0,0 +1,42 @@
|
|||
#ifndef _PROPENGINE_HPP
|
||||
#define _PROPENGINE_HPP
|
||||
|
||||
#include "Thruster.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Propeller;
|
||||
class PistonEngine;
|
||||
|
||||
class PropEngine : public Thruster {
|
||||
public:
|
||||
PropEngine(Propeller* prop, PistonEngine* eng, float moment);
|
||||
virtual ~PropEngine();
|
||||
|
||||
virtual Thruster* clone();
|
||||
|
||||
// Dynamic output
|
||||
virtual void getThrust(float* out);
|
||||
virtual void getTorque(float* out);
|
||||
virtual void getGyro(float* out);
|
||||
virtual float getFuelFlow();
|
||||
|
||||
// Runtime instructions
|
||||
virtual void integrate(float dt);
|
||||
|
||||
float getOmega();
|
||||
|
||||
private:
|
||||
float _moment;
|
||||
Propeller* _prop;
|
||||
PistonEngine* _eng;
|
||||
|
||||
float _omega; // RPM, in radians/sec
|
||||
float _thrust[3];
|
||||
float _torque[3];
|
||||
float _gyro[3];
|
||||
float _fuelFlow;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _PROPENGINE_HPP
|
||||
89
src/FDM/YASim/Propeller.cpp
Normal file
89
src/FDM/YASim/Propeller.cpp
Normal file
|
|
@ -0,0 +1,89 @@
|
|||
#include "Atmosphere.hpp"
|
||||
#include "Math.hpp"
|
||||
#include "Propeller.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Propeller::Propeller(float radius, float v, float omega,
|
||||
float rho, float power, float omega0,
|
||||
float power0)
|
||||
{
|
||||
// Initialize numeric constants:
|
||||
_lambdaPeak = Math::pow(9.0, -1.0/8.0);
|
||||
_beta = 1.0/(Math::pow(9.0, -1.0/8.0) - Math::pow(9.0, -9.0/8.0));
|
||||
|
||||
_r = radius;
|
||||
_etaC = 0.85; // make this settable?
|
||||
|
||||
_J0 = v/(omega*_lambdaPeak);
|
||||
|
||||
float V2 = v*v + (_r*omega)*(_r*omega);
|
||||
_F0 = 2*_etaC*power/(rho*v*V2);
|
||||
|
||||
float stallAngle = 0.25;
|
||||
_lambdaS = _r*(_J0/_r - stallAngle) / _J0;
|
||||
|
||||
// Now compute a correction for zero forward speed to make the
|
||||
// takeoff performance correct.
|
||||
float torque0 = power0/omega0;
|
||||
float thrust, torque;
|
||||
_takeoffCoef = 1;
|
||||
calc(Atmosphere::getStdDensity(0), 0, omega0, &thrust, &torque);
|
||||
_takeoffCoef = torque/torque0;
|
||||
}
|
||||
|
||||
void Propeller::calc(float density, float v, float omega,
|
||||
float* thrustOut, float* torqueOut)
|
||||
{
|
||||
float tipspd = _r*omega;
|
||||
float V2 = v*v + tipspd*tipspd;
|
||||
|
||||
// Clamp v (forward velocity) to zero, now that we've used it to
|
||||
// calculate V (propeller "speed")
|
||||
if(v < 0) v = 0;
|
||||
|
||||
float J = v/omega;
|
||||
float lambda = J/_J0;
|
||||
|
||||
float torque = 0;
|
||||
if(lambda > 1) {
|
||||
lambda = 1.0/lambda;
|
||||
torque = (density*V2*_F0*_J0)/(8*_etaC*_beta*(1-_lambdaPeak));
|
||||
}
|
||||
|
||||
// There's an undefined point at 1. Just offset by a tiny bit to
|
||||
// fix (note: the discontinuity is at EXACTLY one, this is about
|
||||
// the only time in history you'll see me use == on a floating
|
||||
// point number!)
|
||||
if(lambda == 1) lambda = 0.9999;
|
||||
|
||||
// Compute thrust, remembering to clamp lambda to the stall point
|
||||
float lambda2 = lambda < _lambdaS ? _lambdaS : lambda;
|
||||
float thrust = (0.5*density*V2*_F0/(1-_lambdaPeak))*(1-lambda2);
|
||||
|
||||
// Calculate lambda^8
|
||||
float l8 = lambda*lambda; l8 = l8*l8; l8 = l8*l8;
|
||||
|
||||
// thrust/torque ratio
|
||||
float gamma = (_etaC*_beta/_J0)*(1-l8);
|
||||
|
||||
// Correct slow speeds to get the takeoff parameters correct
|
||||
if(lambda < _lambdaPeak) {
|
||||
// This will interpolate takeoffCoef along a descending from 1
|
||||
// at lambda==0 to 0 at the peak, fairing smoothly into the
|
||||
// flat peak.
|
||||
float frac = (lambda - _lambdaPeak)/_lambdaPeak;
|
||||
gamma *= 1 + (_takeoffCoef - 1)*frac*frac;
|
||||
}
|
||||
|
||||
if(torque > 0) {
|
||||
torque -= thrust/gamma;
|
||||
thrust = -thrust;
|
||||
} else {
|
||||
torque = thrust/gamma;
|
||||
}
|
||||
|
||||
*thrustOut = thrust;
|
||||
*torqueOut = torque;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
35
src/FDM/YASim/Propeller.hpp
Normal file
35
src/FDM/YASim/Propeller.hpp
Normal file
|
|
@ -0,0 +1,35 @@
|
|||
#ifndef _PROPELLER_HPP
|
||||
#define _PROPELLER_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// A generic propeller model. See the TeX documentation for
|
||||
// implementation details, this is too hairy to explain in code
|
||||
// comments.
|
||||
class Propeller
|
||||
{
|
||||
public:
|
||||
// Initializes a propeller with the specified "cruise" numbers
|
||||
// for airspeed, RPM, power and air density, and two "takeoff"
|
||||
// numbers for RPM and power (with air speed and density being
|
||||
// zero and sea level). RPM values are in radians per second, of
|
||||
// course.
|
||||
Propeller(float radius, float v, float omega, float rho, float power,
|
||||
float omega0, float power0);
|
||||
|
||||
void calc(float density, float v, float omega,
|
||||
float* thrustOut, float* torqueOut);
|
||||
|
||||
private:
|
||||
float _r; // characteristic radius
|
||||
float _J0; // zero-thrust advance ratio
|
||||
float _lambdaS; // "propeller stall" normalized advance ratio
|
||||
float _F0; // thrust coefficient
|
||||
float _etaC; // Peak efficiency
|
||||
float _lambdaPeak; // constant, ~0.759835;
|
||||
float _beta; // constant, ~1.48058;
|
||||
float _takeoffCoef; // correction to get the zero-speed torque right
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _PROPELLER_HPP
|
||||
204
src/FDM/YASim/RigidBody.cpp
Normal file
204
src/FDM/YASim/RigidBody.cpp
Normal file
|
|
@ -0,0 +1,204 @@
|
|||
#include "Math.hpp"
|
||||
#include "RigidBody.hpp"
|
||||
namespace yasim {
|
||||
|
||||
RigidBody::RigidBody()
|
||||
{
|
||||
// Allocate space for 16 masses initially. More space will be
|
||||
// allocated dynamically.
|
||||
_nMasses = 0;
|
||||
_massesAlloced = 16;
|
||||
_masses = new Mass[_massesAlloced];
|
||||
_gyro[0] = _gyro[1] = _gyro[2] = 0;
|
||||
_spin[0] = _spin[1] = _spin[2] = 0;
|
||||
}
|
||||
|
||||
RigidBody::~RigidBody()
|
||||
{
|
||||
delete[] _masses;
|
||||
}
|
||||
|
||||
int RigidBody::addMass(float mass, float* pos)
|
||||
{
|
||||
// If out of space, reallocate twice as much
|
||||
if(_nMasses == _massesAlloced) {
|
||||
_massesAlloced *= 2;
|
||||
Mass *m2 = new Mass[_massesAlloced];
|
||||
for(int i=0; i<_nMasses; i++)
|
||||
m2[i] = _masses[i];
|
||||
delete[] _masses;
|
||||
_masses = m2;
|
||||
}
|
||||
|
||||
_masses[_nMasses].m = mass;
|
||||
Math::set3(pos, _masses[_nMasses].p);
|
||||
return _nMasses++;
|
||||
}
|
||||
|
||||
void RigidBody::setMass(int handle, float mass)
|
||||
{
|
||||
_masses[handle].m = mass;
|
||||
}
|
||||
|
||||
void RigidBody::setMass(int handle, float mass, float* pos)
|
||||
{
|
||||
_masses[handle].m = mass;
|
||||
Math::set3(pos, _masses[handle].p);
|
||||
}
|
||||
|
||||
int RigidBody::numMasses()
|
||||
{
|
||||
return _nMasses;
|
||||
}
|
||||
|
||||
float RigidBody::getMass(int handle)
|
||||
{
|
||||
return _masses[handle].m;
|
||||
}
|
||||
|
||||
void RigidBody::getMassPosition(int handle, float* out)
|
||||
{
|
||||
out[0] = _masses[handle].p[0];
|
||||
out[1] = _masses[handle].p[1];
|
||||
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)
|
||||
{
|
||||
Math::sub3(pos, _cg, out); // out = pos-cg
|
||||
Math::cross3(rot, out, out); // = rot cross (pos-cg)
|
||||
}
|
||||
|
||||
void RigidBody::setGyro(float* angularMomentum)
|
||||
{
|
||||
Math::set3(angularMomentum, _gyro);
|
||||
}
|
||||
|
||||
void RigidBody::recalc()
|
||||
{
|
||||
// Calculate the c.g and total mass:
|
||||
_totalMass = 0;
|
||||
_cg[0] = _cg[1] = _cg[2] = 0;
|
||||
for(int i=0; i<_nMasses; i++) {
|
||||
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(int i=0; i<9; i++)
|
||||
_I[i] = 0;
|
||||
|
||||
for(int i=0; i<_nMasses; i++) {
|
||||
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 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;
|
||||
|
||||
_I[0] += y2+z2; _I[1] -= xy; _I[2] -= zx;
|
||||
_I[3] -= xy; _I[4] += x2+z2; _I[5] -= yz;
|
||||
_I[6] -= zx; _I[7] -= yz; _I[8] += x2+y2;
|
||||
}
|
||||
|
||||
// And its inverse
|
||||
Math::invert33(_I, _invI);
|
||||
}
|
||||
|
||||
void RigidBody::reset()
|
||||
{
|
||||
_torque[0] = _torque[1] = _torque[2] = 0;
|
||||
_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)
|
||||
{
|
||||
addForce(force);
|
||||
|
||||
// For a force F at position X, the torque about the c.g C is:
|
||||
// torque = F cross (C - X)
|
||||
float v[3], t[3];
|
||||
Math::sub3(_cg, pos, v);
|
||||
Math::cross3(force, v, t);
|
||||
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);
|
||||
|
||||
// Turn the "spin" vector into a normalized spin axis "a" and a
|
||||
// radians/sec scalar "rate".
|
||||
float a[3];
|
||||
float rate = Math::mag3(_spin);
|
||||
Math::set3(_spin, a);
|
||||
Math::mul3(1/rate, a, a);
|
||||
|
||||
float v[3];
|
||||
Math::sub3(_cg, pos, v); // v = cg - pos
|
||||
Math::mul3(Math::dot3(v, a), a, a); // a = a * (v dot a)
|
||||
Math::add3(v, a, v); // v = v + a
|
||||
|
||||
// Now v contains the vector from pos to the rotation axis.
|
||||
// Multiply by the square of the rotation rate to get the linear
|
||||
// acceleration.
|
||||
Math::mul3(rate*rate, v, v);
|
||||
Math::add3(v, accelOut, accelOut);
|
||||
}
|
||||
|
||||
void RigidBody::getAngularAccel(float* accelOut)
|
||||
{
|
||||
// Compute "tau" as the externally applied torque, plus the
|
||||
// counter-torque due to the internal gyro.
|
||||
float tau[3]; // torque
|
||||
Math::cross3(_gyro, _spin, tau);
|
||||
Math::add3(_torque, tau, tau);
|
||||
|
||||
// Now work the equation of motion. Use "v" as a notational
|
||||
// shorthand, as the value isn't an acceleration until the end.
|
||||
float *v = accelOut;
|
||||
Math::vmul33(_I, _spin, v); // v = I*omega
|
||||
Math::cross3(_spin, v, v); // v = omega X I*omega
|
||||
Math::add3(tau, v, v); // v = tau + (omega X I*omega)
|
||||
Math::vmul33(_invI, v, v); // v = invI*(tau + (omega X I*omega))
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
123
src/FDM/YASim/RigidBody.hpp
Normal file
123
src/FDM/YASim/RigidBody.hpp
Normal file
|
|
@ -0,0 +1,123 @@
|
|||
#ifndef _RIGIDBODY_HPP
|
||||
#define _RIGIDBODY_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
//
|
||||
// A RigidBody object maintains all "internal" state about an object,
|
||||
// accumulates force and torque information from external sources, and
|
||||
// calculates the resulting accelerations.
|
||||
//
|
||||
//
|
||||
// Units note: obviously, the choice of mass, time and distance units
|
||||
// is up to the user. If you provide mass in kilograms, forces in
|
||||
// newtons, and torques in newton-meters, you'll get your
|
||||
// accelerations back in m/s^2. The angular units, however, are
|
||||
// UNIFORMLY radians. Angular velocities are radians per <time unit>,
|
||||
// the angular acceleration you get back is radians per <time unit>^2,
|
||||
// and the angular momenta supplied to setGyro must be in radians,
|
||||
// too. Radians, not degrees. Don't forget.
|
||||
//
|
||||
class RigidBody
|
||||
{
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
int numMasses();
|
||||
float getMass(int handle);
|
||||
void getMassPosition(int handle, float* out);
|
||||
float getTotalMass();
|
||||
|
||||
// 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);
|
||||
|
||||
// Sets the "gyroscope" for the body. This is the total
|
||||
// "intrinsic" angular momentum of the body; that is, rotations of
|
||||
// sub-objects, NOT rotation of the whole body within the global
|
||||
// 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);
|
||||
|
||||
|
||||
// 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.
|
||||
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);
|
||||
|
||||
// Adds a torque with the specified axis and magnitude
|
||||
void addTorque(float* torque);
|
||||
|
||||
// Sets the rotation rate of the body (about its c.g.) within the
|
||||
// surrounding environment. This is needed to compute torque on
|
||||
// the body due to the centripetal forces involved in the
|
||||
// rotation. NOTE: the rotation vector, like all other
|
||||
// coordinates used here, is specified IN THE LOCAL COORDINATE
|
||||
// SYSTEM.
|
||||
void setBodySpin(float* rotation);
|
||||
|
||||
|
||||
|
||||
// Returns the center of gravity of the masses, in the body
|
||||
// coordinate system.
|
||||
void getCG(float* 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);
|
||||
|
||||
// Returns the acceleration of a specific location in local
|
||||
// coordinates. If the body is rotating, this will be different
|
||||
// from the c.g. acceleration due to the centripetal accelerations
|
||||
// of points not on the rotation axis.
|
||||
void getAccel(float* pos, float* accelOut);
|
||||
|
||||
// Returns the instantaneous rate of change of the angular
|
||||
// velocity, as a vector in local coordinates.
|
||||
void getAngularAccel(float* accelOut);
|
||||
|
||||
private:
|
||||
struct Mass { float m; float p[3]; };
|
||||
|
||||
// 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 _I[9];
|
||||
float _invI[9];
|
||||
|
||||
// Externally determined quantities
|
||||
float _force[3];
|
||||
float _torque[3];
|
||||
float _spin[3];
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _RIGIDBODY_HPP
|
||||
245
src/FDM/YASim/Surface.cpp
Normal file
245
src/FDM/YASim/Surface.cpp
Normal file
|
|
@ -0,0 +1,245 @@
|
|||
#include "Math.hpp"
|
||||
#include "Surface.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Surface::Surface()
|
||||
{
|
||||
// Start in a "sane" mode, so unset stuff doesn't freak us out
|
||||
_c0 = 1;
|
||||
_cx = _cy = _cz = 1;
|
||||
_cz0 = 0;
|
||||
_peaks[0] = _peaks[1] = 1;
|
||||
for(int i=0; i<4; i++)
|
||||
_stalls[i] = _widths[i] = 0;
|
||||
_orient[0] = 1; _orient[1] = 0; _orient[2] = 0;
|
||||
_orient[3] = 0; _orient[4] = 1; _orient[5] = 0;
|
||||
_orient[6] = 0; _orient[7] = 0; _orient[8] = 1;
|
||||
|
||||
_incidence = 0;
|
||||
_slatPos = _spoilerPos = _flapPos = 0;
|
||||
_slatDrag = _spoilerDrag = _flapDrag = 1;
|
||||
_flapLift = 0;
|
||||
_slatAlpha = 0;
|
||||
_spoilerLift = 1;
|
||||
}
|
||||
|
||||
void Surface::setPosition(float* p)
|
||||
{
|
||||
for(int i=0; i<3; i++) _pos[i] = p[i];
|
||||
}
|
||||
|
||||
void Surface::getPosition(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _pos[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;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
for(int i=0; i<9; i++)
|
||||
_orient[i] = o[i];
|
||||
}
|
||||
|
||||
void Surface::setIncidence(float angle)
|
||||
{
|
||||
_incidence = angle;
|
||||
}
|
||||
|
||||
void Surface::setSlatParams(float stallDelta, float dragPenalty)
|
||||
{
|
||||
_slatAlpha = stallDelta;
|
||||
_slatDrag = dragPenalty;
|
||||
}
|
||||
|
||||
void Surface::setFlapParams(float liftAdd, float dragPenalty)
|
||||
{
|
||||
_flapLift = liftAdd;
|
||||
_flapDrag = dragPenalty;
|
||||
}
|
||||
|
||||
void Surface::setSpoilerParams(float liftPenalty, float dragPenalty)
|
||||
{
|
||||
_spoilerLift = liftPenalty;
|
||||
_spoilerDrag = dragPenalty;
|
||||
}
|
||||
|
||||
void Surface::setFlap(float pos)
|
||||
{
|
||||
_flapPos = pos;
|
||||
}
|
||||
|
||||
void Surface::setSlat(float pos)
|
||||
{
|
||||
_slatPos = pos;
|
||||
}
|
||||
|
||||
void Surface::setSpoiler(float pos)
|
||||
{
|
||||
_spoilerPos = 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)
|
||||
{
|
||||
// Split v into magnitude and direction:
|
||||
float vel = Math::mag3(v);
|
||||
|
||||
// Handle the blowup condition. Zero velocity means zero force by
|
||||
// definition.
|
||||
if(vel == 0) {
|
||||
for(int i=0; i<3; i++) out[i] = torque[i] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
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
|
||||
// need to diddle only the Z component, X is relatively unchanged
|
||||
// by small rotations.
|
||||
out[2] += _incidence * out[0]; // z' = z + incidence * x
|
||||
|
||||
// Diddle the Z force according to our configuration
|
||||
float stallMul = stallFunc(out);
|
||||
stallMul *= 1 + _spoilerPos * (_spoilerLift - 1);
|
||||
float stallLift = (stallMul - 1) * _cz * out[2];
|
||||
float flapLift = _cz * _flapPos * (_flapLift-1);
|
||||
|
||||
out[2] *= _cz; // scaling factor
|
||||
out[2] += _cz*_cz0; // zero-alpha lift
|
||||
out[2] += stallLift;
|
||||
out[2] += flapLift;
|
||||
|
||||
// Airfoil lift (pre-stall and zero-alpha) torques "up" (negative
|
||||
// torque) around the Y axis, while flap lift pushes down. Both
|
||||
// forces are considered to act at one third chord from the
|
||||
// center. Convert to local (i.e. airplane) coordiantes and store
|
||||
// into "torque".
|
||||
torque[0] = 0;
|
||||
torque[1] = 0.33 * _chord * (flapLift - (_cz*_cz0 + stallLift));
|
||||
torque[2] = 0;
|
||||
Math::tmul33(_orient, torque, torque);
|
||||
|
||||
// Diddle X (drag) and Y (side force) in the same manner
|
||||
out[0] *= _cx * controlDrag();
|
||||
out[1] *= _cy;
|
||||
|
||||
// Reverse the incidence rotation to get back to surface
|
||||
// coordinates.
|
||||
out[2] -= _incidence * out[0];
|
||||
|
||||
// Convert back to external coordinates
|
||||
Math::tmul33(_orient, out, out);
|
||||
|
||||
// Add in the units to make a real force:
|
||||
float scale = 0.5*rho*vel*vel*_c0;
|
||||
Math::mul3(scale, out, out);
|
||||
Math::mul3(scale, torque, torque);
|
||||
}
|
||||
|
||||
// Returns a multiplier for the "plain" force equations that
|
||||
// approximates an airfoil's lift/stall curve.
|
||||
float Surface::stallFunc(float* v)
|
||||
{
|
||||
// Sanity check to treat FPU psychopathology
|
||||
if(v[0] == 0) return 1;
|
||||
|
||||
float 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 lift is toward -z
|
||||
int i = (fwdBak<<1) | posNeg;
|
||||
|
||||
float stallAlpha = _stalls[i];
|
||||
if(stallAlpha == 0)
|
||||
return 1;
|
||||
|
||||
if(i == 0)
|
||||
stallAlpha += _slatAlpha;
|
||||
|
||||
// Beyond the stall
|
||||
if(alpha > stallAlpha+_widths[i])
|
||||
return 1;
|
||||
|
||||
// (note mask: we want to use the "positive" stall angle here)
|
||||
float scale = 0.5*_peaks[fwdBak]/_stalls[i&2];
|
||||
|
||||
// Before the stall
|
||||
if(alpha <= stallAlpha)
|
||||
return scale;
|
||||
|
||||
// Inside the stall. Compute a cubic interpolation between the
|
||||
// pre-stall "scale" value and the post-stall unity.
|
||||
float frac = (alpha - stallAlpha) / _widths[i];
|
||||
frac = frac*frac*(3-2*frac);
|
||||
|
||||
return scale*(1-frac) + frac;
|
||||
}
|
||||
|
||||
float Surface::controlDrag()
|
||||
{
|
||||
float d = 1;
|
||||
d *= 1 + _spoilerPos * (_spoilerDrag - 1);
|
||||
d *= 1 + _slatPos * (_slatDrag - 1);
|
||||
|
||||
// FIXME: flaps should REDUCE drag when the reduce lift!
|
||||
d *= 1 + Math::abs(_flapPos) * (_flapDrag - 1);
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
94
src/FDM/YASim/Surface.hpp
Normal file
94
src/FDM/YASim/Surface.hpp
Normal file
|
|
@ -0,0 +1,94 @@
|
|||
#ifndef _SURFACE_HPP
|
||||
#define _SURFACE_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
// FIXME: need a "chord" member for calculating moments. Generic
|
||||
// forces act at the center, but "pre-stall" lift acts towards the
|
||||
// front, and flaps act (in both lift and drag) toward the back.
|
||||
class Surface
|
||||
{
|
||||
public:
|
||||
Surface();
|
||||
|
||||
// Position of this surface in local coords
|
||||
void setPosition(float* p);
|
||||
void getPosition(float* out);
|
||||
|
||||
// Distance scale along the X axis
|
||||
void setChord(float chord);
|
||||
|
||||
// Slats act to move the stall peak by the specified angle, and
|
||||
// increase drag by the multiplier specified.
|
||||
void setSlatParams(float stallDelta, float dragPenalty);
|
||||
|
||||
// Flaps add to lift coefficient, and multiply drag.
|
||||
void setFlapParams(float liftAdd, float dragPenalty);
|
||||
|
||||
// Spoilers reduce the pre-stall lift, and multiply drag.
|
||||
void setSpoilerParams(float liftPenalty, float dragPenalty);
|
||||
|
||||
// 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);
|
||||
|
||||
// local -> Surface coords
|
||||
void setOrientation(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 setTotalDrag(float c0);
|
||||
float getTotalDrag();
|
||||
|
||||
void setXDrag(float cx);
|
||||
void setYDrag(float cy);
|
||||
void setZDrag(float cz);
|
||||
|
||||
// zero-alpha Z drag ("camber") specified as a fraction of cz
|
||||
void setBaseZDrag(float cz0);
|
||||
|
||||
// i: 0 == forward, 1 == backwards
|
||||
void setStallPeak(int i, float 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 calcForce(float* v, float rho, float* forceOut, float* torqueOut);
|
||||
|
||||
private:
|
||||
float stallFunc(float* v);
|
||||
float controlDrag();
|
||||
|
||||
float _chord; // X-axis size
|
||||
float _c0; // total force coefficient
|
||||
float _cx; // X-axis force coefficient
|
||||
float _cy; // Y-axis force coefficient
|
||||
float _cz; // Z-axis force coefficient
|
||||
float _cz0; // Z-axis force offset
|
||||
float _peaks[2]; // Stall peak coefficients (fwd, back)
|
||||
float _stalls[4]; // Stall angles (fwd/back, pos/neg)
|
||||
float _widths[4]; // Stall widths " "
|
||||
float _pos[3]; // position in local coords
|
||||
float _orient[9]; // local->surface orthonormal matrix
|
||||
|
||||
float _slatAlpha;
|
||||
float _slatDrag;
|
||||
float _flapLift;
|
||||
float _flapDrag;
|
||||
float _spoilerLift;
|
||||
float _spoilerDrag;
|
||||
|
||||
float _slatPos;
|
||||
float _flapPos;
|
||||
float _spoilerPos;
|
||||
float _incidence;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _SURFACE_HPP
|
||||
75
src/FDM/YASim/Thruster.cpp
Normal file
75
src/FDM/YASim/Thruster.cpp
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
#include "Math.hpp"
|
||||
#include "Thruster.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Thruster::Thruster()
|
||||
{
|
||||
_dir[0] = 1; _dir[1] = 0; _dir[2] = 0;
|
||||
for(int i=0; i<3; i++) _pos[i] = _wind[i] = 0;
|
||||
_throttle = 0;
|
||||
_mixture = 0;
|
||||
_propAdvance = 0;
|
||||
_rho = 0;
|
||||
}
|
||||
|
||||
Thruster::~Thruster()
|
||||
{
|
||||
}
|
||||
|
||||
void Thruster::getPosition(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _pos[i];
|
||||
}
|
||||
|
||||
void Thruster::setPosition(float* pos)
|
||||
{
|
||||
for(int i=0; i<3; i++) _pos[i] = pos[i];
|
||||
}
|
||||
|
||||
void Thruster::getDirection(float* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) out[i] = _dir[i];
|
||||
}
|
||||
|
||||
void Thruster::setDirection(float* dir)
|
||||
{
|
||||
Math::unit3(dir, _dir);
|
||||
}
|
||||
|
||||
void Thruster::setThrottle(float throttle)
|
||||
{
|
||||
_throttle = throttle;
|
||||
}
|
||||
|
||||
void Thruster::setMixture(float mixture)
|
||||
{
|
||||
_mixture = mixture;
|
||||
}
|
||||
|
||||
void Thruster::setPropAdvance(float propAdvance)
|
||||
{
|
||||
_propAdvance = propAdvance;
|
||||
}
|
||||
|
||||
void Thruster::setWind(float* wind)
|
||||
{
|
||||
for(int i=0; i<3; i++) _wind[i] = wind[i];
|
||||
}
|
||||
|
||||
void Thruster::setDensity(float rho)
|
||||
{
|
||||
_rho = rho;
|
||||
}
|
||||
|
||||
void Thruster::cloneInto(Thruster* out)
|
||||
{
|
||||
for(int i=0; i<3; i++) {
|
||||
out->_pos[i] = _pos[i];
|
||||
out->_dir[i] = _dir[i];
|
||||
}
|
||||
out->_throttle = _throttle;
|
||||
out->_mixture = _mixture;
|
||||
out->_propAdvance = _propAdvance;
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
50
src/FDM/YASim/Thruster.hpp
Normal file
50
src/FDM/YASim/Thruster.hpp
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
#ifndef _THRUSTER_HPP
|
||||
#define _THRUSTER_HPP
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Thruster {
|
||||
public:
|
||||
Thruster();
|
||||
virtual ~Thruster();
|
||||
|
||||
// Static data
|
||||
void getPosition(float* out);
|
||||
void setPosition(float* pos);
|
||||
void getDirection(float* out);
|
||||
void setDirection(float* dir);
|
||||
|
||||
virtual Thruster* clone()=0;
|
||||
|
||||
// Controls
|
||||
void setThrottle(float throttle);
|
||||
void setMixture(float mixture);
|
||||
void setPropAdvance(float advance);
|
||||
|
||||
// Dynamic output
|
||||
virtual void getThrust(float* out)=0;
|
||||
virtual void getTorque(float* out)=0;
|
||||
virtual void getGyro(float* out)=0;
|
||||
virtual float getFuelFlow()=0;
|
||||
|
||||
// Runtime instructions
|
||||
void setWind(float* wind);
|
||||
void setDensity(float rho);
|
||||
virtual void integrate(float dt)=0;
|
||||
|
||||
protected:
|
||||
void cloneInto(Thruster* out);
|
||||
|
||||
float _pos[3];
|
||||
float _dir[3];
|
||||
float _throttle;
|
||||
float _mixture;
|
||||
float _propAdvance;
|
||||
|
||||
float _wind[3];
|
||||
float _rho;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _THRUSTER_HPP
|
||||
|
||||
399
src/FDM/YASim/Wing.cpp
Normal file
399
src/FDM/YASim/Wing.cpp
Normal file
|
|
@ -0,0 +1,399 @@
|
|||
#include "Math.hpp"
|
||||
#include "Surface.hpp"
|
||||
#include "Wing.hpp"
|
||||
namespace yasim {
|
||||
|
||||
Wing::Wing()
|
||||
{
|
||||
_mirror = false;
|
||||
_base[0] = _base[1] = _base[2] = 0;
|
||||
_length = 0;
|
||||
_chord = 0;
|
||||
_taper = 0;
|
||||
_sweep = 0;
|
||||
_dihedral = 0;
|
||||
_stall = 0;
|
||||
_stallWidth = 0;
|
||||
_stallPeak = 0;
|
||||
_camber = 0;
|
||||
_incidence = 0;
|
||||
_dragScale = 1;
|
||||
_liftRatio = 1;
|
||||
_flap0Start = 0;
|
||||
_flap0End = 0;
|
||||
_flap0Lift = 0;
|
||||
_flap0Drag = 0;
|
||||
_flap1Start = 0;
|
||||
_flap1End = 0;
|
||||
_flap1Lift = 0;
|
||||
_flap1Drag = 0;
|
||||
_spoilerStart = 0;
|
||||
_spoilerEnd = 0;
|
||||
_spoilerLift = 0;
|
||||
_spoilerDrag = 0;
|
||||
_slatStart = 0;
|
||||
_slatEnd = 0;
|
||||
_slatAoA = 0;
|
||||
_slatDrag = 0;
|
||||
}
|
||||
|
||||
Wing::~Wing()
|
||||
{
|
||||
for(int i=0; i<_surfs.size(); i++) {
|
||||
SurfRec* s = (SurfRec*)_surfs.get(i);
|
||||
delete s->surface;
|
||||
delete s;
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
for(int 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::setCamber(float camber)
|
||||
{
|
||||
_camber = camber;
|
||||
}
|
||||
|
||||
void Wing::setIncidence(float incidence)
|
||||
{
|
||||
_incidence = incidence;
|
||||
for(int i=0; i<_surfs.size(); i++)
|
||||
((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
|
||||
}
|
||||
|
||||
void Wing::setFlap0(float start, float end, float lift, float drag)
|
||||
{
|
||||
_flap0Start = start;
|
||||
_flap0End = end;
|
||||
_flap0Lift = lift;
|
||||
_flap0Drag = drag;
|
||||
}
|
||||
|
||||
void Wing::setFlap1(float start, float end, float lift, float drag)
|
||||
{
|
||||
_flap1Start = start;
|
||||
_flap1End = end;
|
||||
_flap1Lift = lift;
|
||||
_flap1Drag = drag;
|
||||
}
|
||||
|
||||
void Wing::setSlat(float start, float end, float aoa, float drag)
|
||||
{
|
||||
_slatStart = start;
|
||||
_slatEnd = end;
|
||||
_slatAoA = aoa;
|
||||
_slatDrag = drag;
|
||||
}
|
||||
|
||||
void Wing::setSpoiler(float start, float end, float lift, float drag)
|
||||
{
|
||||
_spoilerStart = start;
|
||||
_spoilerEnd = end;
|
||||
_spoilerLift = lift;
|
||||
_spoilerDrag = drag;
|
||||
}
|
||||
|
||||
void Wing::setFlap0(float lval, float rval)
|
||||
{
|
||||
for(int i=0; i<_flap0Surfs.size(); i++) {
|
||||
((Surface*)_flap0Surfs.get(i))->setFlap(lval);
|
||||
if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
|
||||
}
|
||||
}
|
||||
|
||||
void Wing::setFlap1(float lval, float rval)
|
||||
{
|
||||
for(int i=0; i<_flap1Surfs.size(); i++) {
|
||||
((Surface*)_flap1Surfs.get(i))->setFlap(lval);
|
||||
if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
|
||||
}
|
||||
}
|
||||
|
||||
void Wing::setSpoiler(float lval, float rval)
|
||||
{
|
||||
for(int i=0; i<_spoilerSurfs.size(); i++) {
|
||||
((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
|
||||
if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
|
||||
}
|
||||
}
|
||||
|
||||
void Wing::setSlat(float val)
|
||||
{
|
||||
for(int i=0; i<_slatSurfs.size(); i++)
|
||||
((Surface*)_slatSurfs.get(i))->setSlat(val);
|
||||
}
|
||||
|
||||
void Wing::compile()
|
||||
{
|
||||
// Have we already been compiled?
|
||||
if(_surfs.size() != 0) return;
|
||||
|
||||
// Assemble the start/end coordinates into an array, sort them,
|
||||
// and remove duplicates. This gives us the boundaries of our
|
||||
// segments.
|
||||
float bounds[8];
|
||||
bounds[0] = _flap0Start; bounds[1] = _flap0End;
|
||||
bounds[2] = _flap1Start; bounds[3] = _flap1End;
|
||||
bounds[4] = _spoilerStart; bounds[5] = _spoilerEnd;
|
||||
bounds[6] = _slatStart; bounds[7] = _slatEnd;
|
||||
|
||||
// Sort in increasing order
|
||||
for(int i=0; i<8; i++) {
|
||||
int minIdx = i;
|
||||
float minVal = bounds[i];
|
||||
for(int j=i+1; j<8; j++) {
|
||||
if(bounds[j] < minVal) {
|
||||
minIdx = j;
|
||||
minVal = bounds[j];
|
||||
}
|
||||
}
|
||||
float tmp = bounds[i];
|
||||
bounds[i] = minVal; bounds[minIdx] = tmp;
|
||||
}
|
||||
|
||||
// Uniqify
|
||||
float last = bounds[0];
|
||||
int nbounds = 1;
|
||||
for(int i=1; i<8; i++) {
|
||||
if(bounds[i] != last)
|
||||
bounds[nbounds++] = bounds[i];
|
||||
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.5*(_taper+1)) / _length;
|
||||
|
||||
// Generating a unit vector pointing out the left wing.
|
||||
float left[3];
|
||||
left[0] = -Math::tan(_sweep);
|
||||
left[1] = Math::cos(_dihedral);
|
||||
left[2] = Math::sin(_dihedral);
|
||||
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);
|
||||
|
||||
// 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
|
||||
// wing's XY plane) by definition. Then the local X coordinate is
|
||||
// just Y cross Z.
|
||||
float orient[9], rightOrient[9];
|
||||
float *x = orient, *y = orient+3, *z = orient+6;
|
||||
x[0] = 1; x[1] = 0; x[2] = 0;
|
||||
Math::set3(left, y);
|
||||
Math::cross3(x, y, z);
|
||||
Math::unit3(z, z);
|
||||
Math::cross3(y, z, x);
|
||||
|
||||
if(_mirror) {
|
||||
// Derive the right side orientation matrix from this one.
|
||||
for(int 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(int 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(int i=3; i<6; i++) rightOrient[i] = -rightOrient[i];
|
||||
}
|
||||
|
||||
// Now go through each boundary and make segments
|
||||
for(int i=0; i<(nbounds-1); i++) {
|
||||
float start = bounds[i];
|
||||
float end = bounds[i+1];
|
||||
float mid = (start+end)/2;
|
||||
|
||||
bool flap0=0, flap1=0, slat=0, spoiler=0;
|
||||
if(_flap0Start < mid && mid < _flap0End) flap0 = 1;
|
||||
if(_flap1Start < mid && mid < _flap1End) flap1 = 1;
|
||||
if(_slatStart < mid && mid < _slatEnd) slat = 1;
|
||||
if(_spoilerStart < mid && mid < _spoilerEnd) spoiler = 1;
|
||||
|
||||
// FIXME: Should probably detect an error here if both flap0
|
||||
// and flap1 are set. Right now flap1 overrides.
|
||||
|
||||
int nSegs = (int)Math::ceil((end-start)/segLen);
|
||||
float segWid = _length * (end - start)/nSegs;
|
||||
|
||||
for(int j=0; j<nSegs; j++) {
|
||||
float frac = start + (j+0.5) * (end-start)/nSegs;
|
||||
float pos[3];
|
||||
interp(root, tip, frac, pos);
|
||||
|
||||
float chord = _chord * (1 - (1-_taper)*frac);
|
||||
|
||||
Surface *s = newSurface(pos, orient, chord,
|
||||
flap0, flap1, slat, spoiler);
|
||||
|
||||
SurfRec *sr = new SurfRec();
|
||||
sr->surface = s;
|
||||
sr->weight = chord * segWid;
|
||||
s->setTotalDrag(sr->weight);
|
||||
_surfs.add(sr);
|
||||
|
||||
if(_mirror) {
|
||||
pos[1] = -pos[1];
|
||||
s = newSurface(pos, rightOrient, chord,
|
||||
flap0, flap1, slat, spoiler);
|
||||
sr = new SurfRec();
|
||||
sr->surface = s;
|
||||
sr->weight = chord * segWid;
|
||||
s->setTotalDrag(sr->weight);
|
||||
_surfs.add(sr);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float Wing::getDragScale()
|
||||
{
|
||||
return _dragScale;
|
||||
}
|
||||
|
||||
void Wing::setDragScale(float scale)
|
||||
{
|
||||
_dragScale = scale;
|
||||
for(int i=0; i<_surfs.size(); i++) {
|
||||
SurfRec* s = (SurfRec*)_surfs.get(i);
|
||||
s->surface->setTotalDrag(scale * s->weight);
|
||||
}
|
||||
}
|
||||
|
||||
void Wing::setLiftRatio(float ratio)
|
||||
{
|
||||
_liftRatio = ratio;
|
||||
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)
|
||||
{
|
||||
Surface* s = new Surface();
|
||||
|
||||
s->setPosition(pos);
|
||||
s->setOrientation(orient);
|
||||
s->setChord(chord);
|
||||
|
||||
// Camber is expressed as a fraction of stall peak, so convert.
|
||||
s->setBaseZDrag(_camber*_stallPeak);
|
||||
|
||||
// The "main" (i.e. normal) stall angle
|
||||
float stallAoA = _stall - _stallWidth/4;
|
||||
s->setStall(0, stallAoA);
|
||||
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".
|
||||
if(_camber > 0) {
|
||||
s->setStall(1, stallAoA * 0.8);
|
||||
s->setStallWidth(1, _stallWidth * 0.5);
|
||||
} else {
|
||||
s->setStall(1, stallAoA);
|
||||
s->setStall(1, _stallWidth);
|
||||
}
|
||||
|
||||
// The "reverse" stalls are unmeasurable junk. Just use 13deg and
|
||||
// "sharp".
|
||||
s->setStallPeak(1, 1);
|
||||
for(int i=2; i<4; i++) {
|
||||
s->setStall(i, 0.2267);
|
||||
s->setStallWidth(i, 1);
|
||||
}
|
||||
|
||||
if(flap0) s->setFlapParams(_flap0Lift, _flap0Drag);
|
||||
if(flap1) s->setFlapParams(_flap1Lift, _flap1Drag);
|
||||
if(slat) s->setSlatParams(_slatAoA, _slatDrag);
|
||||
if(spoiler) s->setSpoilerParams(_spoilerLift, _spoilerDrag);
|
||||
|
||||
if(flap0) _flap0Surfs.add(s);
|
||||
if(flap1) _flap1Surfs.add(s);
|
||||
if(slat) _slatSurfs.add(s);
|
||||
if(spoiler) _spoilerSurfs.add(s);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
void Wing::interp(float* v1, float* v2, float frac, float* out)
|
||||
{
|
||||
out[0] = v1[0] + frac*(v2[0]-v1[0]);
|
||||
out[1] = v1[1] + frac*(v2[1]-v1[1]);
|
||||
out[2] = v1[2] + frac*(v2[2]-v1[2]);
|
||||
}
|
||||
|
||||
}; // namespace yasim
|
||||
115
src/FDM/YASim/Wing.hpp
Normal file
115
src/FDM/YASim/Wing.hpp
Normal file
|
|
@ -0,0 +1,115 @@
|
|||
#ifndef _WING_HPP
|
||||
#define _WING_HPP
|
||||
|
||||
#include "util/Vector.hpp"
|
||||
|
||||
namespace yasim {
|
||||
|
||||
class Surface;
|
||||
|
||||
// FIXME: need to handle "inverted" controls for mirrored wings.
|
||||
class Wing {
|
||||
public:
|
||||
Wing();
|
||||
~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 setCamber(float camber);
|
||||
void setIncidence(float incidence);
|
||||
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// Compile the thing into a bunch of Surface objects
|
||||
void compile();
|
||||
|
||||
// Query the list of Surface objects
|
||||
int numSurfaces();
|
||||
Surface* getSurface(int n);
|
||||
float getSurfaceWeight(int n);
|
||||
|
||||
// The overall drag coefficient for the wing as a whole. Units are
|
||||
// arbitrary.
|
||||
void setDragScale(float scale);
|
||||
float getDragScale();
|
||||
|
||||
// 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();
|
||||
|
||||
private:
|
||||
void interp(float* v1, float* v2, float frac, float* out);
|
||||
Surface* newSurface(float* pos, float* orient, float chord,
|
||||
bool flap0, bool flap1, bool slat, bool spoiler);
|
||||
|
||||
struct SurfRec { Surface * surface; float weight; };
|
||||
|
||||
Vector _surfs;
|
||||
Vector _flap0Surfs;
|
||||
Vector _flap1Surfs;
|
||||
Vector _slatSurfs;
|
||||
Vector _spoilerSurfs;
|
||||
|
||||
bool _mirror;
|
||||
|
||||
float _base[3];
|
||||
float _length;
|
||||
float _chord;
|
||||
float _taper;
|
||||
float _sweep;
|
||||
float _dihedral;
|
||||
|
||||
float _stall;
|
||||
float _stallWidth;
|
||||
float _stallPeak;
|
||||
float _camber;
|
||||
float _incidence;
|
||||
|
||||
float _dragScale;
|
||||
float _liftRatio;
|
||||
|
||||
float _flap0Start;
|
||||
float _flap0End;
|
||||
float _flap0Lift;
|
||||
float _flap0Drag;
|
||||
|
||||
float _flap1Start;
|
||||
float _flap1End;
|
||||
float _flap1Lift;
|
||||
float _flap1Drag;
|
||||
|
||||
float _spoilerStart;
|
||||
float _spoilerEnd;
|
||||
float _spoilerLift;
|
||||
float _spoilerDrag;
|
||||
|
||||
float _slatStart;
|
||||
float _slatEnd;
|
||||
float _slatAoA;
|
||||
float _slatDrag;
|
||||
};
|
||||
|
||||
}; // namespace yasim
|
||||
#endif // _WING_HPP
|
||||
353
src/FDM/YASim/YASim.cxx
Normal file
353
src/FDM/YASim/YASim.cxx
Normal file
|
|
@ -0,0 +1,353 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <simgear/misc/sg_path.hxx>
|
||||
#include <simgear/debug/logstream.hxx>
|
||||
#include <simgear/easyxml.hxx>
|
||||
#include <Main/globals.hxx>
|
||||
#include <Main/fg_props.hxx>
|
||||
#include <Scenery/scenery.hxx>
|
||||
|
||||
#include "FGFDM.hpp"
|
||||
#include "Atmosphere.hpp"
|
||||
#include "Math.hpp"
|
||||
#include "Airplane.hpp"
|
||||
#include "Model.hpp"
|
||||
#include "Integrator.hpp"
|
||||
#include "Glue.hpp"
|
||||
#include "Gear.hpp"
|
||||
#include "PropEngine.hpp"
|
||||
|
||||
#include "YASim.hxx"
|
||||
|
||||
using namespace yasim;
|
||||
|
||||
static const float RAD2DEG = 180/3.14159265358979323846;
|
||||
static const float M2FT = 3.2808399;
|
||||
static const float FT2M = 0.3048;
|
||||
static const float MPS2KTS = 3600.0/1852.0;
|
||||
static const float CM2GALS = 264.172037284; // gallons/cubic meter
|
||||
|
||||
void YASim::printDEBUG()
|
||||
{
|
||||
static int debugCount = 0;
|
||||
|
||||
debugCount++;
|
||||
if(debugCount >= 3) {
|
||||
debugCount = 0;
|
||||
|
||||
// printf("RPM %.1f FF %.1f\n",
|
||||
// fgGetFloat("/engines/engine[0]/rpm"),
|
||||
// fgGetFloat("/engines/engine[0]/fuel-flow-gph"));
|
||||
|
||||
// printf("gear: %f\n", fgGetFloat("/controls/gear-down"));
|
||||
|
||||
// printf("alpha %5.1f beta %5.1f\n", get_Alpha()*57.3, get_Beta()*57.3);
|
||||
|
||||
// printf("pilot: %f %f %f\n",
|
||||
// fgGetDouble("/sim/view/pilot/x-offset-m"),
|
||||
// fgGetDouble("/sim/view/pilot/y-offset-m"),
|
||||
// fgGetDouble("/sim/view/pilot/z-offset-m"));
|
||||
}
|
||||
}
|
||||
|
||||
YASim::YASim(double dt)
|
||||
{
|
||||
set_delta_t(dt);
|
||||
_fdm = new FGFDM();
|
||||
|
||||
// Because the integration method is via fourth-order Runge-Kutta,
|
||||
// we only get an "output" state for every 4 times the internal
|
||||
// forces are calculated. So divide dt by four to account for
|
||||
// this, and only run an iteration every fourth time through
|
||||
// update.
|
||||
_dt = dt * 4;
|
||||
_fdm->getAirplane()->getModel()->getIntegrator()->setInterval(_dt);
|
||||
_updateCount = 0;
|
||||
}
|
||||
|
||||
void YASim::report()
|
||||
{
|
||||
Airplane* a = _fdm->getAirplane();
|
||||
|
||||
float aoa = a->getCruiseAoA() * RAD2DEG;
|
||||
float tail = -1 * a->getTailIncidence() * RAD2DEG;
|
||||
float drag = 1000 * a->getDragCoefficient();
|
||||
|
||||
SG_LOG(SG_FLIGHT,SG_INFO,"YASim solution results:");
|
||||
SG_LOG(SG_FLIGHT,SG_INFO," Iterations: "<<a->getSolutionIterations());
|
||||
SG_LOG(SG_FLIGHT,SG_INFO,"Drag Coefficient: "<< drag);
|
||||
SG_LOG(SG_FLIGHT,SG_INFO," Lift Ratio: "<<a->getLiftRatio());
|
||||
SG_LOG(SG_FLIGHT,SG_INFO," Cruise AoA: "<< aoa);
|
||||
SG_LOG(SG_FLIGHT,SG_INFO," Tail Incidence: "<< tail);
|
||||
|
||||
float cg[3];
|
||||
char buf[256];
|
||||
a->getModel()->getBody()->getCG(cg);
|
||||
sprintf(buf, " CG: %.1f, %.1f, %.1f", cg[0], cg[1], cg[2]);
|
||||
SG_LOG(SG_FLIGHT, SG_INFO, buf);
|
||||
|
||||
if(a->getFailureMsg()) {
|
||||
SG_LOG(SG_FLIGHT, SG_ALERT, "YASim SOLUTION FAILURE:");
|
||||
SG_LOG(SG_FLIGHT, SG_ALERT, a->getFailureMsg());
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
void YASim::init()
|
||||
{
|
||||
Airplane* a = _fdm->getAirplane();
|
||||
Model* m = a->getModel();
|
||||
|
||||
// Superclass hook
|
||||
common_init();
|
||||
|
||||
// Build a filename and parse it
|
||||
SGPath f(globals->get_fg_root());
|
||||
f.append("Aircraft");
|
||||
f.append(fgGetString("/sim/aircraft"));
|
||||
f.concat(".xml");
|
||||
readXML(f.str(), *_fdm);
|
||||
|
||||
// Compile it into a real airplane, and tell the user what they got
|
||||
a->compile();
|
||||
report();
|
||||
|
||||
_fdm->init();
|
||||
|
||||
// Lift the plane up so the gear clear the ground
|
||||
float minGearZ = 1e18;
|
||||
for(int i=0; i<a->numGear(); i++) {
|
||||
Gear* g = a->getGear(i);
|
||||
float pos[3];
|
||||
g->getPosition(pos);
|
||||
if(pos[2] < minGearZ)
|
||||
minGearZ = pos[2];
|
||||
}
|
||||
_set_Altitude(get_Altitude() - minGearZ*M2FT);
|
||||
|
||||
// The pilot's eyepoint
|
||||
float pilot[3];
|
||||
a->getPilotPos(pilot);
|
||||
fgSetFloat("/sim/view/pilot/x-offset-m", -pilot[0]);
|
||||
fgSetFloat("/sim/view/pilot/y-offset-m", -pilot[1]);
|
||||
fgSetFloat("/sim/view/pilot/z-offset-m", pilot[2]);
|
||||
|
||||
// Blank the state, and copy in ours
|
||||
State s;
|
||||
m->setState(&s);
|
||||
|
||||
copyToYASim(true);
|
||||
set_inited(true);
|
||||
}
|
||||
|
||||
bool YASim::update(int iterations)
|
||||
{
|
||||
for(int i=0; i<iterations; i++) {
|
||||
// Remember, update only every 4th call
|
||||
_updateCount++;
|
||||
if(_updateCount >= 4) {
|
||||
|
||||
copyToYASim(false);
|
||||
_fdm->iterate(_dt);
|
||||
copyFromYASim();
|
||||
|
||||
printDEBUG();
|
||||
|
||||
_updateCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return true; // what does this mean?
|
||||
}
|
||||
|
||||
void YASim::copyToYASim(bool copyState)
|
||||
{
|
||||
// Physical state
|
||||
float lat = get_Latitude();
|
||||
float lon = get_Longitude();
|
||||
float alt = get_Altitude() * FT2M;
|
||||
float roll = get_Phi();
|
||||
float pitch = get_Theta();
|
||||
float hdg = get_Psi();
|
||||
|
||||
// Environment
|
||||
float wind[3];
|
||||
wind[0] = get_V_north_airmass() * FT2M;
|
||||
wind[1] = get_V_east_airmass() * FT2M;
|
||||
wind[2] = get_V_down_airmass() * FT2M;
|
||||
double ground = get_Runway_altitude() * FT2M;
|
||||
|
||||
// You'd this this would work, but it doesn't. These values are
|
||||
// always zero. Use a "standard" pressure intstead.
|
||||
//
|
||||
// float pressure = get_Static_pressure();
|
||||
// float temp = get_Static_temperature();
|
||||
float pressure = Atmosphere::getStdPressure(alt);
|
||||
float temp = Atmosphere::getStdTemperature(alt);
|
||||
|
||||
// Convert and set:
|
||||
Model* model = _fdm->getAirplane()->getModel();
|
||||
State s;
|
||||
float xyz2ned[9];
|
||||
Glue::xyz2nedMat(lat, lon, xyz2ned);
|
||||
|
||||
// position
|
||||
Glue::geod2xyz(lat, lon, alt, s.pos);
|
||||
|
||||
// orientation
|
||||
Glue::euler2orient(roll, pitch, hdg, s.orient);
|
||||
Math::mmul33(s.orient, xyz2ned, s.orient);
|
||||
|
||||
// Copy in the existing velocity for now...
|
||||
Math::set3(model->getState()->v, s.v);
|
||||
|
||||
if(copyState)
|
||||
model->setState(&s);
|
||||
|
||||
// wind
|
||||
Math::tmul33(xyz2ned, wind, wind);
|
||||
model->setWind(wind);
|
||||
|
||||
// ground. Calculate a cartesian coordinate for the ground under
|
||||
// us, find the (geodetic) up vector normal to the ground, then
|
||||
// use that to find the final (radius) term of the plane equation.
|
||||
double xyz[3], gplane[3]; float up[3];
|
||||
Glue::geod2xyz(lat, lon, ground, xyz);
|
||||
Glue::geodUp(xyz, up); // FIXME, needless reverse computation...
|
||||
for(int i=0; i<3; i++) gplane[i] = up[i];
|
||||
double rad = gplane[0]*xyz[0] + gplane[1]*xyz[1] + gplane[2]*xyz[2];
|
||||
model->setGroundPlane(gplane, rad);
|
||||
|
||||
// air
|
||||
model->setAir(pressure, temp);
|
||||
}
|
||||
|
||||
// All the settables:
|
||||
//
|
||||
// These are set below:
|
||||
// _set_Accels_Local
|
||||
// _set_Accels_Body
|
||||
// _set_Accels_CG_Body
|
||||
// _set_Accels_Pilot_Body
|
||||
// _set_Accels_CG_Body_N
|
||||
// _set_Velocities_Local
|
||||
// _set_Velocities_Ground
|
||||
// _set_Velocities_Wind_Body
|
||||
// _set_Omega_Body
|
||||
// _set_Euler_Rates
|
||||
// _set_Euler_Angles
|
||||
// _set_V_rel_wind
|
||||
// _set_V_ground_speed
|
||||
// _set_V_equiv_kts
|
||||
// _set_V_calibrated_kts
|
||||
// _set_Alpha
|
||||
// _set_Beta
|
||||
// _set_Mach_number
|
||||
// _set_Climb_Rate
|
||||
// _set_Tank1Fuel
|
||||
// _set_Tank2Fuel
|
||||
// _set_Altitude_AGL
|
||||
// _set_Geodetic_Position
|
||||
// _set_Runway_altitude
|
||||
|
||||
// Ignoring these, because they're unused:
|
||||
// _set_Geocentric_Position
|
||||
// _set_Geocentric_Rates
|
||||
// _set_Cos_phi
|
||||
// _set_Cos_theta
|
||||
// _set_Earth_position_angle (WTF?)
|
||||
// _set_Gamma_vert_rad
|
||||
// _set_Inertias
|
||||
// _set_T_Local_to_Body
|
||||
// _set_CG_Position
|
||||
// _set_Sea_Level_Radius
|
||||
|
||||
// Externally set via the weather code:
|
||||
// _set_Velocities_Local_Airmass
|
||||
// _set_Density
|
||||
// _set_Static_pressure
|
||||
// _set_Static_temperature
|
||||
void YASim::copyFromYASim()
|
||||
{
|
||||
Model* model = _fdm->getAirplane()->getModel();
|
||||
State* s = model->getState();
|
||||
|
||||
// position
|
||||
double lat, lon, alt;
|
||||
Glue::xyz2geod(s->pos, &lat, &lon, &alt);
|
||||
_set_Geodetic_Position(lat, lon, alt*M2FT);
|
||||
|
||||
// UNUSED
|
||||
//_set_Geocentric_Position(Glue::geod2geocLat(lat), lon, alt*M2FT);
|
||||
|
||||
// FIXME: there's a normal vector available too, use it.
|
||||
float groundMSL = scenery.get_cur_elev();
|
||||
_set_Runway_altitude(groundMSL*M2FT);
|
||||
_set_Altitude_AGL((alt - groundMSL)*M2FT);
|
||||
|
||||
// useful conversion matrix
|
||||
float xyz2ned[9];
|
||||
Glue::xyz2nedMat(lat, lon, xyz2ned);
|
||||
|
||||
// velocity
|
||||
float v[3];
|
||||
Math::vmul33(xyz2ned, s->v, v);
|
||||
_set_Velocities_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
|
||||
_set_V_ground_speed(Math::sqrt(M2FT*v[0]*M2FT*v[0] +
|
||||
M2FT*v[1]*M2FT*v[1]));
|
||||
_set_Climb_Rate(-M2FT*v[2]);
|
||||
|
||||
// The HUD uses this, but inverts down (?!)
|
||||
_set_Velocities_Ground(M2FT*v[0], M2FT*v[1], -M2FT*v[2]);
|
||||
|
||||
// _set_Geocentric_Rates(M2FT*v[0], M2FT*v[1], M2FT*v[2]); // UNUSED
|
||||
|
||||
Math::vmul33(s->orient, s->v, v);
|
||||
_set_Velocities_Wind_Body(v[0]*M2FT, -v[1]*M2FT, -v[2]*M2FT);
|
||||
_set_V_rel_wind(Math::mag3(v)*M2FT); // units?
|
||||
|
||||
// These don't work, use a dummy based on altitude
|
||||
// float P = get_Static_pressure();
|
||||
// float T = get_Static_temperature();
|
||||
float P = Atmosphere::getStdPressure(alt);
|
||||
float T = Atmosphere::getStdTemperature(alt);
|
||||
_set_V_equiv_kts(Atmosphere::calcVEAS(v[0], P, T)*MPS2KTS);
|
||||
_set_V_calibrated_kts(Atmosphere::calcVCAS(v[0], P, T)*MPS2KTS);
|
||||
_set_Mach_number(Atmosphere::calcMach(v[0], T));
|
||||
|
||||
// acceleration
|
||||
Math::vmul33(xyz2ned, s->acc, v);
|
||||
_set_Accels_Local(M2FT*v[0], M2FT*v[1], M2FT*v[2]);
|
||||
|
||||
Math::vmul33(s->orient, s->acc, v);
|
||||
_set_Accels_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
|
||||
_set_Accels_CG_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
|
||||
|
||||
_fdm->getAirplane()->getPilotAccel(v);
|
||||
_set_Accels_Pilot_Body(M2FT*v[0], -M2FT*v[1], -M2FT*v[2]);
|
||||
|
||||
// The one appears (!) to want inverted pilot acceleration
|
||||
// numbers, in G's...
|
||||
Math::mul3(1.0/9.8, v, v);
|
||||
_set_Accels_CG_Body_N(v[0], -v[1], -v[2]);
|
||||
|
||||
// orientation
|
||||
float alpha, beta;
|
||||
Glue::calcAlphaBeta(s, &alpha, &beta);
|
||||
_set_Alpha(alpha);
|
||||
_set_Beta(beta);
|
||||
|
||||
float tmp[9];
|
||||
Math::trans33(xyz2ned, tmp);
|
||||
Math::mmul33(s->orient, tmp, tmp);
|
||||
float roll, pitch, hdg;
|
||||
Glue::orient2euler(tmp, &roll, &pitch, &hdg);
|
||||
_set_Euler_Angles(roll, pitch, hdg);
|
||||
|
||||
// rotation
|
||||
Math::vmul33(s->orient, s->rot, v);
|
||||
_set_Omega_Body(v[0], -v[1], -v[2]);
|
||||
|
||||
Glue::calcEulerRates(s, &roll, &pitch, &hdg);
|
||||
_set_Euler_Rates(roll, pitch, hdg);
|
||||
}
|
||||
30
src/FDM/YASim/YASim.hxx
Normal file
30
src/FDM/YASim/YASim.hxx
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#ifndef _YASIM_HXX
|
||||
#define _YASIM_HXX
|
||||
|
||||
#include <FDM/flight.hxx>
|
||||
|
||||
namespace yasim { class FGFDM; };
|
||||
|
||||
class YASim : public FGInterface {
|
||||
public:
|
||||
YASim(double dt);
|
||||
|
||||
// Load externally set stuff into the FDM
|
||||
virtual void init();
|
||||
|
||||
// Run an iteration
|
||||
virtual bool update(int iterations);
|
||||
|
||||
private:
|
||||
void report();
|
||||
void copyFromYASim();
|
||||
void copyToYASim(bool copyState);
|
||||
|
||||
void printDEBUG();
|
||||
|
||||
yasim::FGFDM* _fdm;
|
||||
int _updateCount;
|
||||
float _dt;
|
||||
};
|
||||
|
||||
#endif // _YASIM_HXX
|
||||
Loading…
Add table
Reference in a new issue