fly/flightgear
1
0
Fork 0
Code Activity

Updated to YASim-0.1.2

Browse source
This commit is contained in:
curt 2001-12-07 20:00:59 +00:00
parent 01d13f797a
commit 48260480b3
24 changed files with 330 additions and 181 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -35,15 +35,16 @@ Airplane::Airplane()
Airplane::~Airplane()
{
for(int i=0; i<_fuselages.size(); i++)
int i;
for(i=0; i<_fuselages.size(); i++)
delete (Fuselage*)_fuselages.get(i);
for(int i=0; i<_tanks.size(); i++)
for(i=0; i<_tanks.size(); i++)
delete (Tank*)_tanks.get(i);
for(int i=0; i<_thrusters.size(); i++)
for(i=0; i<_thrusters.size(); i++)
delete (ThrustRec*)_thrusters.get(i);
for(int i=0; i<_gears.size(); i++)
for(i=0; i<_gears.size(); i++)
delete (GearRec*)_gears.get(i);
for(int i=0; i<_surfs.size(); i++)
for(i=0; i<_surfs.size(); i++)
delete (Surface*)_surfs.get(i);
}
@ -85,12 +86,14 @@ void Airplane::getPilotAccel(float* out)
void Airplane::setPilotPos(float* pos)
{
for(int i=0; i<3; i++) _pilotPos[i] = pos[i];
int i;
for(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 i;
for(i=0; i<3; i++) out[i] = _pilotPos[i];
}
int Airplane::numGear()
@ -105,7 +108,8 @@ Gear* Airplane::getGear(int g)
void Airplane::setGearState(bool down, float dt)
{
for(int i=0; i<_gears.size(); i++) {
int i;
for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
if(gr->time == 0) {
// Non-extensible
@ -206,7 +210,8 @@ void Airplane::addVStab(Wing* vstab)
void Airplane::addFuselage(float* front, float* back, float width)
{
Fuselage* f = new Fuselage();
for(int i=0; i<3; i++) {
int i;
for(i=0; i<3; i++) {
f->front[i] = front[i];
f->back[i] = back[i];
}
@ -217,7 +222,8 @@ void Airplane::addFuselage(float* front, float* back, float width)
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];
int i;
for(i=0; i<3; i++) t->pos[i] = pos[i];
t->cap = cap;
t->fill = cap;
t->density = density;
@ -239,7 +245,8 @@ 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];
int i;
for(i=0; i<3; i++) t->cg[i] = cg[i];
_thrusters.add(t);
}
@ -284,7 +291,8 @@ void Airplane::setWeight(int handle, float mass)
void Airplane::setFuelFraction(float frac)
{
for(int i=0; i<_tanks.size(); i++) {
int i;
for(i=0; i<_tanks.size(); i++) {
Tank* t = (Tank*)_tanks.get(i);
_model.getBody()->setMass(t->handle, t->cap * frac);
}
@ -330,7 +338,8 @@ void Airplane::setupState(float aoa, float speed, State* s)
s->v[0] = speed; s->v[1] = 0; s->v[2] = 0;
for(int i=0; i<3; i++)
int i;
for(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
@ -346,7 +355,8 @@ float Airplane::compileWing(Wing* w)
w->compile();
float wgt = 0;
for(int i=0; i<w->numSurfaces(); i++) {
int i;
for(i=0; i<w->numSurfaces(); i++) {
Surface* s = (Surface*)w->getSurface(i);
_model.addSurface(s);
@ -367,7 +377,8 @@ float Airplane::compileFuselage(Fuselage* f)
float wid = f->width;
int segs = (int)Math::ceil(len/wid);
float segWgt = len*wid/segs;
for(int j=0; j<segs; j++) {
int j;
for(j=0; j<segs; j++) {
float frac = (j+0.5) / segs;
float pos[3];
Math::mul3(frac, fwd, pos);
@ -442,34 +453,35 @@ void Airplane::compile()
// The Wing objects
aeroWgt += compileWing(_wing);
aeroWgt += compileWing(_tail);
for(int i=0; i<_vstabs.size(); i++) {
int i;
for(i=0; i<_vstabs.size(); i++) {
aeroWgt += compileWing((Wing*)_vstabs.get(i));
}
// The fuselage(s)
for(int i=0; i<_fuselages.size(); i++) {
for(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++)
for(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++)
for(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++) {
for(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++) {
for(i=0; i<_tanks.size(); i++) {
Tank* t = (Tank*)_tanks.get(i);
t->handle = body->addMass(0, t->pos);
totalFuel += t->cap;
@ -480,11 +492,11 @@ void Airplane::compile()
body->recalc();
// Add surfaces for the landing gear.
for(int i=0; i<_gears.size(); i++)
for(i=0; i<_gears.size(); i++)
compileGear((GearRec*)_gears.get(i));
// The Thruster objects
for(int i=0; i<_thrusters.size(); i++) {
for(i=0; i<_thrusters.size(); i++) {
ThrustRec* tr = (ThrustRec*)_thrusters.get(i);
tr->handle = _model.addThruster(tr->thruster);
}
@ -512,7 +524,8 @@ void Airplane::solveGear()
// "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++) {
int i;
for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
Gear* g = gr->gear;
g->getPosition(pos);
@ -522,7 +535,7 @@ void Airplane::solveGear()
}
// Renormalize so they sum to 1
for(int i=0; i<_gears.size(); i++)
for(i=0; i<_gears.size(); i++)
((GearRec*)_gears.get(i))->wgt /= total;
// The force at max compression should be sufficient to stop a
@ -535,7 +548,7 @@ void Airplane::solveGear()
// each gear.
float energy = 0.5*_approachWeight*descentRate*descentRate;
for(int i=0; i<_gears.size(); i++) {
for(i=0; i<_gears.size(); i++) {
GearRec* gr = (GearRec*)_gears.get(i);
float e = energy * gr->wgt;
float comp[3];
@ -558,7 +571,8 @@ void Airplane::solveGear()
void Airplane::stabilizeThrust()
{
for(int i=0; i<_thrusters.size(); i++)
int i;
for(i=0; i<_thrusters.size(); i++)
_model.getThruster(i)->stabilize();
}
@ -570,7 +584,8 @@ void Airplane::runCruise()
// The control configuration
_controls.reset();
for(int i=0; i<_cruiseControls.size(); i++) {
int i;
for(i=0; i<_cruiseControls.size(); i++) {
Control* c = (Control*)_cruiseControls.get(i);
_controls.setInput(c->control, c->val);
}
@ -589,7 +604,7 @@ void Airplane::runCruise()
// Set up the thruster parameters and iterate until the thrust
// stabilizes.
for(int i=0; i<_thrusters.size(); i++) {
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
t->setAir(_cruiseP, _cruiseT);
@ -610,7 +625,8 @@ void Airplane::runApproach()
// The control configuration
_controls.reset();
for(int i=0; i<_approachControls.size(); i++) {
int i;
for(i=0; i<_approachControls.size(); i++) {
Control* c = (Control*)_approachControls.get(i);
_controls.setInput(c->control, c->val);
}
@ -629,7 +645,7 @@ void Airplane::runApproach()
// 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++) {
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = ((ThrustRec*)_thrusters.get(i))->thruster;
t->setWind(wind);
t->setAir(_approachP, _approachT);
@ -648,11 +664,12 @@ void Airplane::applyDragFactor(float factor)
_dragFactor *= applied;
_wing->setDragScale(_wing->getDragScale() * applied);
_tail->setDragScale(_tail->getDragScale() * applied);
for(int i=0; i<_vstabs.size(); i++) {
int i;
for(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++) {
for(i=0; i<_surfs.size(); i++) {
Surface* s = (Surface*)_surfs.get(i);
s->setTotalDrag(s->getTotalDrag() * applied);
}
@ -664,7 +681,8 @@ void Airplane::applyLiftRatio(float factor)
_liftRatio *= applied;
_wing->setLiftRatio(_wing->getLiftRatio() * applied);
_tail->setLiftRatio(_tail->getLiftRatio() * applied);
for(int i=0; i<_vstabs.size(); i++) {
int i;
for(i=0; i<_vstabs.size(); i++) {
Wing* w = (Wing*)_vstabs.get(i);
w->setLiftRatio(w->getLiftRatio() * applied);
}

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

@ -21,9 +21,11 @@ struct State {
// Simple initialization
State() {
for(int i=0; i<3; i++) {
int i;
for(i=0; i<3; i++) {
pos[i] = v[i] = rot[i] = acc[i] = racc[i] = 0;
for(int j=0; j<3; j++)
int j;
for(j=0; j<3; j++)
orient[3*i+j] = i==j ? 1 : 0;
}
}

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

@ -10,14 +10,16 @@ namespace yasim {
ControlMap::~ControlMap()
{
for(int i=0; i<_inputs.size(); i++) {
int i;
for(i=0; i<_inputs.size(); i++) {
Vector* v = (Vector*)_inputs.get(i);
for(int j=0; j<v->size(); j++)
int j;
for(j=0; j<v->size(); j++)
delete (MapRec*)v->get(j);
delete v;
}
for(int i=0; i<_outputs.size(); i++) {
for(i=0; i<_outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
delete[] o->values;
delete o;
@ -34,7 +36,8 @@ 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++) {
int i;
for(i=0; i<_outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
if(o->object == object && o->type == type) {
out = o;
@ -73,9 +76,11 @@ void ControlMap::addMapping(int input, int type, void* object, int options)
void ControlMap::reset()
{
// Set all the values to zero
for(int i=0; i<_outputs.size(); i++) {
int i;
for(i=0; i<_outputs.size(); i++) {
OutRec* o = (OutRec*)_outputs.get(i);
for(int j=0; j<o->n; j++)
int j;
for(j=0; j<o->n; j++)
o->values[j] = 0;
}
}
@ -83,7 +88,8 @@ void ControlMap::reset()
void ControlMap::setInput(int input, float value)
{
Vector* maps = (Vector*)_inputs.get(input);
for(int i=0; i<maps->size(); i++) {
int i;
for(i=0; i<maps->size(); i++) {
MapRec* map = (MapRec*)maps->get(i);
map->out->values[map->idx] = value;
}
@ -91,13 +97,15 @@ void ControlMap::setInput(int input, float value)
void ControlMap::applyControls()
{
for(int outrec=0; outrec<_outputs.size(); outrec++) {
int outrec;
for(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++) {
int i;
for(i=0; i<o->n; i++) {
float val = o->values[i];
int opt = (int)o->options.get(i);
if(opt & OPT_SQUARE)

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

@ -13,9 +13,9 @@ public:
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;
enum { OPT_SPLIT = 0x01,
OPT_INVERT = 0x02,
OPT_SQUARE = 0x04 };
// Returns a new, not-yet-used "input handle" for addMapping and
// setInput. This typically corresponds to one user axis.

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

@ -37,24 +37,25 @@ FGFDM::FGFDM()
FGFDM::~FGFDM()
{
for(int i=0; i<_axes.size(); i++) {
int i;
for(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++) {
for(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++) {
for(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++) {
for(i=0; i<_weights.size(); i++) {
WeightRec* wr = (WeightRec*)_weights.get(i);
delete[] wr->prop;
delete wr;
@ -247,7 +248,8 @@ void FGFDM::getExternalInput(float dt)
// The control axes
ControlMap* cm = _airplane.getControlMap();
cm->reset();
for(int i=0; i<_axes.size(); i++) {
int i;
for(i=0; i<_axes.size(); i++) {
AxisRec* a = (AxisRec*)_axes.get(i);
float val = fgGetFloat(a->name, 0);
cm->setInput(a->handle, val);
@ -255,7 +257,7 @@ void FGFDM::getExternalInput(float dt)
cm->applyControls();
// Weights
for(int i=0; i<_weights.size(); i++) {
for(i=0; i<_weights.size(); i++) {
WeightRec* wr = (WeightRec*)_weights.get(i);
_airplane.setWeight(wr->handle, fgGetFloat(wr->prop));
}
@ -267,24 +269,25 @@ void FGFDM::getExternalInput(float dt)
void FGFDM::setOutputProperties()
{
char buf[256];
for(int i=0; i<_airplane.numTanks(); i++) {
int i;
for(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++) {
for(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));
fgSetFloat(buf, p->getOmega() / RPM2RAD);
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++) {
for(i=0; i<_jets.size(); i++) {
EngRec* er = (EngRec*)_jets.get(i);
Jet* j = (Jet*)er->eng;
@ -380,7 +383,8 @@ void FGFDM::parsePropeller(XMLAttributes* a)
// yet.
int FGFDM::parseAxis(const char* name)
{
for(int i=0; i<_axes.size(); i++) {
int i;
for(i=0; i<_axes.size(); i++) {
AxisRec* a = (AxisRec*)_axes.get(i);
if(eq(a->name, name))
return a->handle;

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

@ -6,7 +6,8 @@ namespace yasim {
Gear::Gear()
{
for(int i=0; i<3; i++)
int i;
for(i=0; i<3; i++)
_pos[i] = _cmpr[i] = 0;
_spring = 1;
_damp = 0;
@ -19,12 +20,14 @@ Gear::Gear()
void Gear::setPosition(float* position)
{
for(int i=0; i<3; i++) _pos[i] = position[i];
int i;
for(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];
int i;
for(i=0; i<3; i++) _cmpr[i] = compression[i];
}
void Gear::setSpring(float spring)
@ -64,12 +67,14 @@ void Gear::setExtension(float extension)
void Gear::getPosition(float* out)
{
for(int i=0; i<3; i++) out[i] = _pos[i];
int i;
for(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];
int i;
for(i=0; i<3; i++) out[i] = _cmpr[i];
}
float Gear::getSpring()
@ -126,7 +131,8 @@ float Gear::getCompressFraction()
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;
int i;
for(i=0; i<3; i++) _force[i] = _contact[i] = 0;
// Don't bother if it's not down
if(_extension < 1)
@ -150,7 +156,7 @@ void Gear::calcForce(RigidBody* body, float* v, float* rot, float* ground)
// Calculate the point of ground _contact.
_frac = a/(a-b);
if(b < 0) _frac = 1;
for(int i=0; i<3; i++)
for(i=0; i<3; i++)
_contact[i] = _pos[i] + _frac*_cmpr[i];
// Turn _cmpr into a unit vector and a magnitude

21
src/FDM/YASim/Glue.cpp
View file

@ -2,6 +2,15 @@
#include "Glue.hpp"
namespace yasim {
// 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
void Glue::calcAlphaBeta(State* s, float* alpha, float* beta)
{
// Convert the velocity to the aircraft frame.
@ -145,8 +154,9 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
// 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++)
int i, j;
for(i=0; i<3; i++)
for(j=0; j<3; j++)
out[3*i+j] = (i==j) ? 1 : 0;
// Negate Y and Z
@ -154,7 +164,8 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
float s = Math::sin(roll);
float c = Math::cos(roll);
for(int col=0; col<3; col++) {
int col;
for(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;
@ -162,7 +173,7 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
s = Math::sin(pitch);
c = Math::cos(pitch);
for(int col=0; col<3; col++) {
for(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;
@ -170,7 +181,7 @@ void Glue::euler2orient(float roll, float pitch, float hdg, float* out)
s = Math::sin(hdg);
c = Math::cos(hdg);
for(int col=0; col<3; col++) {
for(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;

11
src/FDM/YASim/Glue.hpp
View file

@ -43,17 +43,6 @@ public:
// 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

24
src/FDM/YASim/Integrator.cpp
View file

@ -75,7 +75,8 @@ void Integrator::calcNewInterval()
float dt = _dt / 4;
orthonormalize(_s.orient);
for(int i=0; i<4; i++) {
int i;
for(i=0; i<4; i++) {
_body->reset();
_env->calcForces(&s);
@ -124,7 +125,8 @@ void Integrator::calcNewInterval()
// First off, sanify the initial orientation
orthonormalize(_s.orient);
for(int i=0; i<NITER; i++) {
int i;
for(i=0; i<NITER; i++) {
//
// extrapolate forward based on current values of the
// derivatives and the ORIGINAL values of the
@ -139,7 +141,8 @@ void Integrator::calcNewInterval()
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];
int j;
for(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
@ -163,12 +166,12 @@ void Integrator::calcNewInterval()
// per-coordinate arrays should be changed into a single array
// of State objects. Ick.
State stmp;
for(int j=0; j<3; j++) {
for(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++)
for(j=0; j<9; j++)
stmp.orient[j] = ori[i][j];
_env->calcForces(&stmp);
@ -190,16 +193,17 @@ void Integrator::calcNewInterval()
// But the space is "locally" cartesian.
State derivs;
float tot = 0;
for(int i=0; i<NITER; i++) {
for(i=0; i<NITER; i++) {
float wgt = WEIGHTS[i];
tot += wgt;
for(int j=0; j<3; j++) {
int j;
for(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++) {
for(i=0; i<3; i++) {
derivs.v[i] *= itot; derivs.rot[i] *= itot;
derivs.acc[i] *= itot; derivs.racc[i] *= itot;
}
@ -211,7 +215,7 @@ void Integrator::calcNewInterval()
// save the starting orientation
float orient0[9];
for(int i=0; i<9; i++) orient0[i] = _s.orient[i];
for(i=0; i<9; i++) orient0[i] = _s.orient[i];
float rotmat[9];
rotMatrix(derivs.rot, _dt, rotmat);
@ -226,7 +230,7 @@ void Integrator::calcNewInterval()
Math::mul3(_dt, derivs.racc, tmp);
Math::add3(_s.rot, tmp, _s.rot);
for(int i=0; i<3; i++) {
for(i=0; i<3; i++) {
_s.acc[i] = derivs.acc[i];
_s.racc[i] = derivs.racc[i];
}

3
src/FDM/YASim/Math.cpp
View file

@ -159,7 +159,8 @@ void Math::mmul33(float* a, float* b, float* out)
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++)
int i;
for(i=0; i<9; i++)
out[i] = tmp[i];
}

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

@ -25,7 +25,8 @@ void printState(State* s)
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++) {
int i;
for(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]);
@ -38,7 +39,8 @@ void printState(State* s)
Model::Model()
{
for(int i=0; i<3; i++) _wind[i] = 0;
int i;
for(i=0; i<3; i++) _wind[i] = 0;
_integrator.setBody(&_body);
_integrator.setEnvironment(this);
@ -56,7 +58,8 @@ void Model::getThrust(float* out)
{
float tmp[3];
out[0] = out[1] = out[2] = 0;
for(int i=0; i<_thrusters.size(); i++) {
int i;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
t->getThrust(tmp);
Math::add3(tmp, out, out);
@ -66,9 +69,10 @@ void Model::getThrust(float* out)
void Model::initIteration()
{
// Precompute torque and angular momentum for the thrusters
for(int i=0; i<3; i++)
int i;
for(i=0; i<3; i++)
_gyro[i] = _torque[i] = 0;
for(int i=0; i<_thrusters.size(); i++) {
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
// Get the wind velocity at the thruster location
@ -77,7 +81,7 @@ void Model::initIteration()
localWind(pos, _s, v);
t->setWind(v);
t->setAir(_P, _T);
t->setAir(_pressure, _temp);
t->integrate(_integrator.getInterval());
t->getTorque(v);
@ -164,14 +168,15 @@ void Model::setGroundEffect(float* pos, float span, float mul)
// (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];
int i;
for(i=0; i<3; i++) _ground[i] = planeNormal[i];
_ground[3] = fromOrigin;
}
void Model::setAir(float pressure, float temp)
{
_P = pressure;
_T = temp;
_pressure = pressure;
_temp = temp;
_rho = Atmosphere::calcDensity(pressure, temp);
}
@ -189,7 +194,8 @@ void Model::calcForces(State* s)
// step.
_body.setGyro(_gyro);
_body.addTorque(_torque);
for(int i=0; i<_thrusters.size(); i++) {
int i;
for(i=0; i<_thrusters.size(); i++) {
Thruster* t = (Thruster*)_thrusters.get(i);
float thrust[3], pos[3];
t->getThrust(thrust);
@ -208,7 +214,7 @@ void Model::calcForces(State* s)
// point is different due to rotation.
float faero[3];
faero[0] = faero[1] = faero[2] = 0;
for(int i=0; i<_surfaces.size(); i++) {
for(i=0; i<_surfaces.size(); i++) {
Surface* sf = (Surface*)_surfaces.get(i);
// Vsurf = wind - velocity + (rot cross (cg - pos))
@ -247,7 +253,7 @@ void Model::calcForces(State* s)
Math::vmul33(s->orient, s->v, lv);
// The landing gear
for(int i=0; i<_gears.size(); i++) {
for(i=0; i<_gears.size(); i++) {
float force[3], contact[3];
Gear* g = (Gear*)_gears.get(i);
g->calcForce(&_body, lv, lrot, ground);
@ -265,7 +271,8 @@ void Model::newState(State* 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++) {
int i;
for(i=0; i<_gears.size(); i++) {
Gear* g = (Gear*)_gears.get(i);
g->getPosition(pos);
g->getCompression(cmpr);

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

@ -71,8 +71,8 @@ private:
float _wingCenter[3];
double _ground[4];
float _P;
float _T;
float _pressure;
float _temp;
float _rho;
float _wind[3];

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

@ -9,7 +9,7 @@ PistonEngine::PistonEngine(float power, float speed)
// (2.2 lb/kg, 745.7 W/hp, 3600 sec/hour) 3.69e-07 kg/Ws.
_f0 = power * 3.69e-07;
_P0 = power;
_power0 = power;
_omega0 = speed;
// We must be at sea level under standard conditions
@ -40,7 +40,7 @@ void PistonEngine::setTurboParams(float turbo, float maxMP)
float PistonEngine::getPower()
{
return _P0;
return _power0;
}
void PistonEngine::setThrottle(float t)
@ -89,7 +89,7 @@ void PistonEngine::calc(float P, float T, float speed,
// And finally the power is just the reference power scaled by the
// amount of fuel burned.
float power = _P0 * burned/_f0;
float power = _power0 * burned/_f0;
*torqueOut = power/speed;
*fuelFlowOut = fuel;

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

@ -23,7 +23,7 @@ public:
float* powerOut, float* fuelFlowOut);
private:
float _P0; // reference power setting
float _power0; // reference power setting
float _omega0; // " engine speed
float _rho0; // " manifold air density
float _f0; // "ideal" fuel flow at P0/omega0

13
src/FDM/YASim/PropEngine.cpp
View file

@ -42,17 +42,20 @@ float PropEngine::getOmega()
void PropEngine::getThrust(float* out)
{
for(int i=0; i<3; i++) out[i] = _thrust[i];
int i;
for(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];
int i;
for(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];
int i;
for(i=0; i<3; i++) out[i] = _gyro[i];
}
float PropEngine::getFuelFlow()
@ -78,7 +81,7 @@ void PropEngine::stabilize()
while(true) {
float etau, ptau, dummy;
_prop->calc(_rho, speed, _omega, &dummy, &ptau);
_eng->calc(_P, _T, _omega, &etau, &dummy);
_eng->calc(_pressure, _temp, _omega, &etau, &dummy);
float tdiff = etau - ptau;
if(Math::abs(tdiff/_moment) < 0.1)
@ -109,7 +112,7 @@ void PropEngine::integrate(float dt)
_prop->calc(_rho, speed, _omega,
&thrust, &propTorque);
_eng->calc(_P, _T, _omega, &engTorque, &_fuelFlow);
_eng->calc(_pressure, _temp, _omega, &engTorque, &_fuelFlow);
// Turn the thrust into a vector and save it
Math::mul3(thrust, _dir, _thrust);

24
src/FDM/YASim/Propeller.cpp
View file

@ -15,11 +15,11 @@ Propeller::Propeller(float radius, float v, float omega,
_r = radius;
_etaC = 0.85; // make this settable?
_J0 = v/(omega*_lambdaPeak);
_baseJ0 = _J0;
_j0 = v/(omega*_lambdaPeak);
_baseJ0 = _j0;
float V2 = v*v + (_r*omega)*(_r*omega);
_F0 = 2*_etaC*power/(rho*v*V2);
_f0 = 2*_etaC*power/(rho*v*V2);
_matchTakeoff = false;
}
@ -29,17 +29,17 @@ void Propeller::setTakeoff(float omega0, float power0)
// Takeoff thrust coefficient at lambda==0
_matchTakeoff = true;
float V2 = _r*omega0 * _r*omega0;
float gamma = _etaC * _beta / _J0;
float gamma = _etaC * _beta / _j0;
float torque = power0 / omega0;
float density = Atmosphere::getStdDensity(0);
_tc0 = (torque * gamma) / (0.5 * density * V2 * _F0);
_tc0 = (torque * gamma) / (0.5 * density * V2 * _f0);
}
void Propeller::modPitch(float mod)
{
_J0 *= mod;
if(_J0 < 0.25*_baseJ0) _J0 = 0.25*_baseJ0;
if(_J0 > 4*_baseJ0) _J0 = 4*_baseJ0;
_j0 *= mod;
if(_j0 < 0.25*_baseJ0) _j0 = 0.25*_baseJ0;
if(_j0 > 4*_baseJ0) _j0 = 4*_baseJ0;
}
@ -54,12 +54,12 @@ void Propeller::calc(float density, float v, float omega,
if(v < 0) v = 0;
float J = v/omega;
float lambda = J/_J0;
float lambda = J/_j0;
float torque = 0;
if(lambda > 1) {
lambda = 1.0/lambda;
torque = (density*V2*_F0*_J0)/(4*_etaC*_beta*(1-_lambdaPeak));
torque = (density*V2*_f0*_j0)/(4*_etaC*_beta*(1-_lambdaPeak));
}
// There's an undefined point at 1. Just offset by a tiny bit to
@ -72,14 +72,14 @@ void Propeller::calc(float density, float v, float omega,
float l4 = lambda*lambda; l4 = l4*l4;
// thrust/torque ratio
float gamma = (_etaC*_beta/_J0)*(1-l4);
float gamma = (_etaC*_beta/_j0)*(1-l4);
// Compute a thrust, clamp to takeoff thrust to prevend huge
// numbers at slow speeds.
float tc = (1 - lambda) / (1 - _lambdaPeak);
if(_matchTakeoff && tc > _tc0) tc = _tc0;
float thrust = 0.5 * density * V2 * _F0 * tc;
float thrust = 0.5 * density * V2 * _f0 * tc;
if(torque > 0) {
torque -= thrust/gamma;

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

@ -25,9 +25,9 @@ public:
private:
float _r; // characteristic radius
float _J0; // zero-thrust advance ratio
float _j0; // zero-thrust advance ratio
float _baseJ0; // ... uncorrected for prop advance
float _F0; // thrust coefficient
float _f0; // thrust coefficient
float _etaC; // Peak efficiency
float _lambdaPeak; // constant, ~0.759835;
float _beta; // constant, ~1.48058;

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

@ -24,7 +24,8 @@ int RigidBody::addMass(float mass, float* pos)
if(_nMasses == _massesAlloced) {
_massesAlloced *= 2;
Mass *m2 = new Mass[_massesAlloced];
for(int i=0; i<_nMasses; i++)
int i;
for(i=0; i<_nMasses; i++)
m2[i] = _masses[i];
delete[] _masses;
_masses = m2;
@ -86,7 +87,8 @@ 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++) {
int i;
for(i=0; i<_nMasses; i++) {
float m = _masses[i].m;
_totalMass += m;
_cg[0] += m * _masses[i].p[0];
@ -96,10 +98,10 @@ void RigidBody::recalc()
Math::mul3(1/_totalMass, _cg, _cg);
// Now the inertia tensor:
for(int i=0; i<9; i++)
_I[i] = 0;
for(i=0; i<9; i++)
_tI[i] = 0;
for(int i=0; i<_nMasses; i++) {
for(i=0; i<_nMasses; i++) {
float m = _masses[i].m;
float x = _masses[i].p[0] - _cg[0];
@ -109,13 +111,13 @@ void RigidBody::recalc()
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;
_tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx;
_tI[3] -= xy; _tI[4] += x2+z2; _tI[5] -= yz;
_tI[6] -= zx; _tI[7] -= yz; _tI[8] += x2+y2;
}
// And its inverse
Math::invert33(_I, _invI);
Math::invert33(_tI, _invI);
}
void RigidBody::reset()
@ -195,7 +197,7 @@ void RigidBody::getAngularAccel(float* accelOut)
// 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::vmul33(_tI, _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))

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

@ -110,7 +110,7 @@ private:
float _gyro[3];
// Inertia tensor, and its inverse. Computed from the above.
float _I[9];
float _tI[9];
float _invI[9];
// Externally determined quantities

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

@ -9,7 +9,8 @@ Surface::Surface()
_cx = _cy = _cz = 1;
_cz0 = 0;
_peaks[0] = _peaks[1] = 1;
for(int i=0; i<4; i++)
int i;
for(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;
@ -25,12 +26,14 @@ Surface::Surface()
void Surface::setPosition(float* p)
{
for(int i=0; i<3; i++) _pos[i] = p[i];
int i;
for(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];
int i;
for(i=0; i<3; i++) out[i] = _pos[i];
}
void Surface::setChord(float chord)
@ -85,7 +88,8 @@ void Surface::setStallWidth(int i, float width)
void Surface::setOrientation(float* o)
{
for(int i=0; i<9; i++)
int i;
for(i=0; i<9; i++)
_orient[i] = o[i];
}
@ -138,7 +142,8 @@ void Surface::calcForce(float* v, float rho, float* out, float* torque)
// 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;
int i;
for(i=0; i<3; i++) out[i] = torque[i] = 0;
return;
}

23
src/FDM/YASim/Thruster.cpp
View file

@ -5,10 +5,11 @@ 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;
int i;
for(i=0; i<3; i++) _pos[i] = _wind[i] = 0;
_throttle = 0;
_mixture = 0;
_P = _T = _rho = 0;
_pressure = _temp = _rho = 0;
}
Thruster::~Thruster()
@ -17,17 +18,20 @@ Thruster::~Thruster()
void Thruster::getPosition(float* out)
{
for(int i=0; i<3; i++) out[i] = _pos[i];
int i;
for(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];
int i;
for(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];
int i;
for(i=0; i<3; i++) out[i] = _dir[i];
}
void Thruster::setDirection(float* dir)
@ -47,14 +51,15 @@ void Thruster::setMixture(float mixture)
void Thruster::setWind(float* wind)
{
for(int i=0; i<3; i++) _wind[i] = wind[i];
int i;
for(i=0; i<3; i++) _wind[i] = wind[i];
}
void Thruster::setAir(float pressure, float temp)
{
_P = pressure;
_T = temp;
_rho = _P / (287.1 * _T);
_pressure = pressure;
_temp = temp;
_rho = _pressure / (287.1 * _temp);
}
}; // namespace yasim

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

@ -50,8 +50,8 @@ protected:
float _mixture;
float _wind[3];
float _P;
float _T;
float _pressure;
float _temp;
float _rho;
};

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

@ -39,7 +39,8 @@ Wing::Wing()
Wing::~Wing()
{
for(int i=0; i<_surfs.size(); i++) {
int i;
for(i=0; i<_surfs.size(); i++) {
SurfRec* s = (SurfRec*)_surfs.get(i);
delete s->surface;
delete s;
@ -68,7 +69,8 @@ void Wing::setMirror(bool mirror)
void Wing::setBase(float* base)
{
for(int i=0; i<3; i++) _base[i] = base[i];
int i;
for(i=0; i<3; i++) _base[i] = base[i];
}
void Wing::setLength(float length)
@ -119,7 +121,8 @@ void Wing::setCamber(float camber)
void Wing::setIncidence(float incidence)
{
_incidence = incidence;
for(int i=0; i<_surfs.size(); i++)
int i;
for(i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setIncidence(incidence);
}
@ -159,7 +162,8 @@ void Wing::setFlap0(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
for(int i=0; i<_flap0Surfs.size(); i++) {
int i;
for(i=0; i<_flap0Surfs.size(); i++) {
((Surface*)_flap0Surfs.get(i))->setFlap(lval);
if(_mirror) ((Surface*)_flap0Surfs.get(++i))->setFlap(rval);
}
@ -169,7 +173,8 @@ void Wing::setFlap1(float lval, float rval)
{
lval = Math::clamp(lval, -1, 1);
rval = Math::clamp(rval, -1, 1);
for(int i=0; i<_flap1Surfs.size(); i++) {
int i;
for(i=0; i<_flap1Surfs.size(); i++) {
((Surface*)_flap1Surfs.get(i))->setFlap(lval);
if(_mirror) ((Surface*)_flap1Surfs.get(++i))->setFlap(rval);
}
@ -179,7 +184,8 @@ void Wing::setSpoiler(float lval, float rval)
{
lval = Math::clamp(lval, 0, 1);
rval = Math::clamp(rval, 0, 1);
for(int i=0; i<_spoilerSurfs.size(); i++) {
int i;
for(i=0; i<_spoilerSurfs.size(); i++) {
((Surface*)_spoilerSurfs.get(i))->setSpoiler(lval);
if(_mirror) ((Surface*)_spoilerSurfs.get(++i))->setSpoiler(rval);
}
@ -188,13 +194,15 @@ void Wing::setSpoiler(float lval, float rval)
void Wing::setSlat(float val)
{
val = Math::clamp(val, 0, 1);
for(int i=0; i<_slatSurfs.size(); i++)
int i;
for(i=0; i<_slatSurfs.size(); i++)
((Surface*)_slatSurfs.get(i))->setSlat(val);
}
float Wing::getGroundEffect(float* posOut)
{
for(int i=0; i<3; i++) posOut[i] = _base[i];
int i;
for(i=0; i<3; i++) posOut[i] = _base[i];
float span = _length * Math::cos(_sweep) * Math::cos(_dihedral);
span = 2*(span + Math::abs(_base[2]));
return span;
@ -215,10 +223,12 @@ void Wing::compile()
bounds[6] = _slatStart; bounds[7] = _slatEnd;
// Sort in increasing order
for(int i=0; i<8; i++) {
int i;
for(i=0; i<8; i++) {
int minIdx = i;
float minVal = bounds[i];
for(int j=i+1; j<8; j++) {
int j;
for(j=i+1; j<8; j++) {
if(bounds[j] < minVal) {
minIdx = j;
minVal = bounds[j];
@ -231,7 +241,7 @@ void Wing::compile()
// Uniqify
float last = bounds[0];
int nbounds = 1;
for(int i=1; i<8; i++) {
for(i=1; i<8; i++) {
if(bounds[i] != last)
bounds[nbounds++] = bounds[i];
last = bounds[i];
@ -270,19 +280,20 @@ void Wing::compile()
if(_mirror) {
// Derive the right side orientation matrix from this one.
for(int i=0; i<9; i++) rightOrient[i] = orient[i];
int i;
for(i=0; i<9; i++) rightOrient[i] = orient[i];
// Negate all Y coordinates, this gets us a valid basis, but
// it's left handed! So...
for(int i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
for(i=1; i<9; i+=3) rightOrient[i] = -rightOrient[i];
// Change the direction of the Y axis to get back to a
// right-handed system.
for(int i=3; i<6; i++) rightOrient[i] = -rightOrient[i];
for(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++) {
for(i=0; i<(nbounds-1); i++) {
float start = bounds[i];
float end = bounds[i+1];
float mid = (start+end)/2;
@ -299,7 +310,8 @@ void Wing::compile()
int nSegs = (int)Math::ceil((end-start)/segLen);
float segWid = _length * (end - start)/nSegs;
for(int j=0; j<nSegs; j++) {
int j;
for(j=0; j<nSegs; j++) {
float frac = start + (j+0.5) * (end-start)/nSegs;
float pos[3];
interp(root, tip, frac, pos);
@ -337,7 +349,8 @@ float Wing::getDragScale()
void Wing::setDragScale(float scale)
{
_dragScale = scale;
for(int i=0; i<_surfs.size(); i++) {
int i;
for(i=0; i<_surfs.size(); i++) {
SurfRec* s = (SurfRec*)_surfs.get(i);
s->surface->setTotalDrag(scale * s->weight);
}
@ -346,7 +359,8 @@ void Wing::setDragScale(float scale)
void Wing::setLiftRatio(float ratio)
{
_liftRatio = ratio;
for(int i=0; i<_surfs.size(); i++)
int i;
for(i=0; i<_surfs.size(); i++)
((SurfRec*)_surfs.get(i))->surface->setZDrag(ratio);
}
@ -386,7 +400,8 @@ Surface* Wing::newSurface(float* pos, float* orient, float chord,
// The "reverse" stalls are unmeasurable junk. Just use 13deg and
// "sharp".
s->setStallPeak(1, 1);
for(int i=2; i<4; i++) {
int i;
for(i=2; i<4; i++) {
s->setStall(i, 0.2267);
s->setStallWidth(i, 1);
}

77
src/FDM/YASim/YASim.cxx
View file

@ -16,16 +16,20 @@
#include "Glue.hpp"
#include "Gear.hpp"
#include "PropEngine.hpp"
#include "PistonEngine.hpp"
#include "YASim.hxx"
using namespace yasim;
static const float RAD2DEG = 180/3.14159265358979323846;
static const float RAD2RPM = 9.54929658551;
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
static const float KG2LBS = 2.20462262185;
static const float W2HP = 1.3416e-3;
void YASim::printDEBUG()
{
@ -114,9 +118,32 @@ void YASim::init()
_fdm->init();
// Create some FG{Eng|Gear}Interface objects
int i;
for(i=0; i<a->numGear(); i++) {
Gear* g = a->getGear(i);
FGGearInterface fgg;
float pos[3];
g->getPosition(pos);
fgg.SetX(pos[0]); fgg.SetY(-pos[1]); fgg.SetZ(-pos[2]);
add_gear_unit(fgg);
}
for(i=0; i<m->numThrusters(); i++) {
FGEngInterface fge;
add_engine(fge);
// Sanify the initial input conditions
char buf[64];
sprintf(buf, "/controls/throttle[%d]", i); fgSetFloat(buf, 0);
sprintf(buf, "/controls/mixture[%d]", i); fgSetFloat(buf, 1);
sprintf(buf, "/controls/propeller-pitch[%d]", i); fgSetFloat(buf, 1);
sprintf(buf, "/controls/afterburner[%d]", i); fgSetFloat(buf, 0);
}
// Lift the plane up so the gear clear the ground
float minGearZ = 1e18;
for(int i=0; i<a->numGear(); i++) {
for(i=0; i<a->numGear(); i++) {
Gear* g = a->getGear(i);
float pos[3];
g->getPosition(pos);
@ -142,7 +169,8 @@ void YASim::init()
bool YASim::update(int iterations)
{
for(int i=0; i<iterations; i++) {
int i;
for(i=0; i<iterations; i++) {
// Remember, update only every 4th call
_updateCount++;
if(_updateCount >= 4) {
@ -214,7 +242,8 @@ void YASim::copyToYASim(bool copyState)
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];
int i;
for(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);
@ -269,7 +298,8 @@ void YASim::copyToYASim(bool copyState)
// _set_Static_temperature
void YASim::copyFromYASim()
{
Model* model = _fdm->getAirplane()->getModel();
Airplane* airplane = _fdm->getAirplane();
Model* model = airplane->getModel();
State* s = model->getState();
// position
@ -350,4 +380,43 @@ void YASim::copyFromYASim()
Glue::calcEulerRates(s, &roll, &pitch, &hdg);
_set_Euler_Rates(roll, pitch, hdg);
// Fill out our engine and gear objects
int i;
for(i=0; i<get_num_gear(); i++) {
FGGearInterface* fgg = get_gear_unit(i);
Gear* g = airplane->getGear(i);
if(g->getBrake() != 0)
fgg->SetBrake(true);
if(g->getCompressFraction() != 0)
fgg->SetWoW(true);
fgg->SetPosition(g->getExtension());
}
for(i=0; i<get_num_engines(); i++) {
FGEngInterface* fge = get_engine(i);
Thruster* t = model->getThruster(i);
fge->set_Running_Flag(true);
// Note: assumes all tanks have the same fuel density!
fge->set_Fuel_Flow(CM2GALS * t->getFuelFlow()
/ airplane->getFuelDensity(0));
float tmp[3];
t->getThrust(tmp);
fge->set_prop_thrust(Math::mag3(tmp) * KG2LBS / 9.8);
PropEngine* pe = t->getPropEngine();
if(pe) {
fge->set_RPM(pe->getOmega() * RAD2RPM);
pe->getTorque(tmp);
float power = Math::mag3(tmp) * pe->getOmega();
float maxPower = pe->getPistonEngine()->getPower();
fge->set_MaxHP(maxPower * W2HP);
fge->set_Percentage_Power(100 * power/maxPower);
}
}
}